//! Turns one raw sensor [`Frame`] plus [`Calibration`] data into a 16x16
//! object-temperature heatmap, following the compensation pipeline described
//! in the HTPA16x16dR2 datasheet, section 12:
//!
//! 1. ambient temperature, from PTAT                (12.1)
//! 2. thermal offset compensation                   (12.2)
//! 3. electrical offset compensation                 (12.3)
//! 4. VDD compensation                               (12.4)
//! 5. sensitivity (PixC) compensation + lookup table (12.5)

use crate::calibration::{Calibration, GRID};
use crate::lookup_table::LookupTable;
use crate::sensor::{header_word, pixel, Frame, RawHalf};

/// The computed heatmap for one frame, plus a couple of handy summary stats.
pub struct Heatmap {
    pub ambient_c: f32,
    pub min_c: f32,
    pub max_c: f32,
    pub temperature_c: [[f32; GRID]; GRID],
}

/// Compensate one raw frame into a full object-temperature heatmap.
pub fn process_frame<const ROWS: usize, const COLS: usize>(
    cal: &Calibration,
    frame: &Frame,
    table: &LookupTable<ROWS, COLS>,
) -> Heatmap {
    let ptat_av = average_header_word(&frame.pixel_top, &frame.pixel_bottom);
    let vdd_av = average_header_word(&frame.vdd_top, &frame.vdd_bottom);

    let ambient_dk = ptat_av as f32 * cal.ptat_gradient + cal.ptat_offset;
    let ambient_c = dk_to_c(ambient_dk);
    let ambient_column = table.locate_ambient_column(ambient_dk as i32);

    let mut temperature_c = [[0f32; GRID]; GRID];
    let mut min_c = f32::MAX;
    let mut max_c = f32::MIN;

    for col in 0..GRID {
        let raw_pixels = assemble_raw_column(frame, col);
        let electrical_offsets = electrical_offset_column(&frame.electrical_offset, col);

        for row in 0..GRID {
            let profile = row_profile(row);

            let thermal_compensated =
                raw_pixels[row] - thermal_offset_correction(cal, row, col, ptat_av);
            let electrical_compensated =
                thermal_compensated - electrical_offsets[profile] as i32;
            let vdd_compensated = electrical_compensated
                - vdd_compensation(cal, profile, col, ptat_av, vdd_av);

            let pixc = sensitivity_coefficient(cal, row, col);
            let signal_digits = (vdd_compensated as f32 * table.pixel_signal_scale / pixc) as i32;

            let object_dk = table.lookup(&ambient_column, signal_digits);
            let object_c = dk_to_c(object_dk as f32);

            temperature_c[row][col] = object_c;
            min_c = min_c.min(object_c);
            max_c = max_c.max(object_c);
        }
    }

    Heatmap { ambient_c, min_c, max_c, temperature_c }
}

/// Convert deci-Kelvin (as used throughout the datasheet's calibration data)
/// to degrees Celsius.
fn dk_to_c(dk: f32) -> f32 {
    dk / 10.0 - 273.15
}

/// Which of the 8 stored calibration row-profiles a physical row uses.
/// Electrical offset and VDD compensation are only calibrated for 8 rows;
/// each profile is reused for two of the array's 16 physical rows (the
/// bottom half is wired up in reverse row order, see the datasheet's
/// "Readout Order" figure).
fn row_profile(row: usize) -> usize {
    if row < 8 {
        row % 4
    } else {
        row % 4 + 4
    }
}

/// Average of the header word (PTAT or VDD) across a block-0/block-1 pair of
/// top/bottom half buffers (datasheet 12.1/12.4 both recommend averaging all
/// four samples for a more stable reading).
fn average_header_word(top: &[RawHalf; 2], bottom: &[RawHalf; 2]) -> i32 {
    let sum = header_word(&top[0]) + header_word(&top[1]) + header_word(&bottom[0]) + header_word(&bottom[1]);
    (sum / 4) as i32
}

/// Reconstruct one column (16 rows) of raw pixel digits from the pixel
/// blocks, in the physical row order described in the datasheet's "Readout
/// Order" figure: block 0 and block 1 each contribute 8 rows, with the
/// bottom half read out column-major and row-reversed.
fn assemble_raw_column(frame: &Frame, col: usize) -> [i32; GRID] {
    let top0 = &frame.pixel_top[0];
    let top1 = &frame.pixel_top[1];
    let bottom1 = &frame.pixel_bottom[1];
    let bottom0 = &frame.pixel_bottom[0];

    [
        pixel(top0, col) as i32,
        pixel(top0, col + 16) as i32,
        pixel(top0, col + 32) as i32,
        pixel(top0, col + 48) as i32,
        pixel(top1, col) as i32,
        pixel(top1, col + 16) as i32,
        pixel(top1, col + 32) as i32,
        pixel(top1, col + 48) as i32,
        pixel(bottom1, col + 48) as i32,
        pixel(bottom1, col + 32) as i32,
        pixel(bottom1, col + 16) as i32,
        pixel(bottom1, col) as i32,
        pixel(bottom0, col + 48) as i32,
        pixel(bottom0, col + 32) as i32,
        pixel(bottom0, col + 16) as i32,
        pixel(bottom0, col) as i32,
    ]
}

/// One column of the 8 electrical-offset row profiles (datasheet 12.3).
fn electrical_offset_column(halves: &(RawHalf, RawHalf), col: usize) -> [u16; 8] {
    let (top, bottom) = halves;
    [
        pixel(top, col),
        pixel(top, col + 16),
        pixel(top, col + 32),
        pixel(top, col + 48),
        pixel(bottom, col + 48),
        pixel(bottom, col + 32),
        pixel(bottom, col + 16),
        pixel(bottom, col),
    ]
}

/// Thermal offset correction to subtract from one pixel (datasheet 12.2):
/// `ThGrad * Ta / 2^gradScale + ThOffset`.
fn thermal_offset_correction(cal: &Calibration, row: usize, col: usize, ptat_av: i32) -> i32 {
    (cal.thermal_gradient[row][col] as i32 * ptat_av) / cal.gradient_scale_div
        + cal.thermal_offset[row][col] as i32
}

/// VDD compensation to subtract from one pixel (datasheet 12.4). Compares
/// the measured VDD against what it should be at the current ambient
/// temperature (interpolated between the two calibration points), scaled by
/// this pixel's VDD sensitivity.
fn vdd_compensation(cal: &Calibration, profile: usize, col: usize, ptat_av: i32, vdd_av: i32) -> i32 {
    let gradient = cal.vdd_comp_gradient[profile][col] as i32;
    let offset = cal.vdd_comp_offset[profile][col] as i32;

    let [vdd_at_cal1, vdd_at_cal2] = cal.vdd_at_calibration;
    let [ptat_at_cal1, ptat_at_cal2] = cal.ptat_at_calibration;
    let expected_vdd = vdd_at_cal1
        + (vdd_at_cal2 - vdd_at_cal1) * (ptat_av - ptat_at_cal1) / (ptat_at_cal2 - ptat_at_cal1);
    let vdd_delta = vdd_av - expected_vdd;

    let scaled_gradient = gradient * ptat_av / cal.vdd_scale_gradient_div + offset;
    scaled_gradient * vdd_delta / cal.vdd_scale_offset_div
}

/// Per-pixel sensitivity coefficient, PixC (datasheet 12.5):
/// `(Pij * (PixCmax - PixCmin) / 65535 + PixCmin) * epsilon/100 * GlobalGain/10000`.
fn sensitivity_coefficient(cal: &Calibration, row: usize, col: usize) -> f32 {
    let scaled = cal.sensitivity[row][col] as f32 * (cal.pixc_max - cal.pixc_min) / 65535.0 + cal.pixc_min;
    scaled * (cal.epsilon / 100.0) * (cal.global_gain / 10000.0)
}
