/*
 * @file rsense.cpp
 * @brief Radiation sensor implementation
 *
 * Created: 21.09.2025
 * Author: ThePetrovich
 *
 * Copyright YKSA - Sakha Aerospace Systems, LLC.
 * See the LICENSE file for details.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <Arduino.h>
#include <Event.h>
#include <Logic.h>
#include <SPI.h>
#include <stdint.h>
#include <util/atomic.h>

#include "adc.h"
#include "config.h"
#include "eeprom.h"
#include "iodefs.h"
#include "rsense.h"

static volatile union __attribute__((packed)) {
	uint16_t channels_16[RSENSE_CHANNEL_COUNT];
} g_detector_counts = {0};

static volatile uint32_t g_counts_cps = 0; /* counts per second accumulator, reset by periodic tick */
static uint32_t g_cps_time = 0;
static uint16_t g_cps_last = 0;
static uint32_t g_total_counts = 0;
static volatile uint32_t g_counts_delta = 0; /* counts since last CMD_GET_COUNTS */

static struct {
	uint8_t index;
	uint16_t cps[10]; /* Circular buffer of the last 10 CPS values, updated by periodic tick */
} g_cp10s_data;

/*
 * Drift compensation state.
 *
 * Three independent reference temperatures:
 *   det_ref — DET_TEMP (SiPM carrier), drives pot_hv via temp_drift.
 *   hv_ref  — HV_TEMP  (+28V supply),  drives pot_hv via vbias_drift.
 *   amp_ref — AMP_TEMP (amplifier),    drives pot_amp and pot_det.
 *
 * pot_hv adjustment = temp_drift*delta_det + vbias_drift*delta_hv (summed).
 */
static uint32_t g_drift_last_ms = 0;
static int16_t g_drift_ref_det = 0; /* DET_TEMP reference in 0.1 °C */
static int16_t g_drift_ref_hv = 0;  /* HV_TEMP  reference in 0.1 °C */
static int16_t g_drift_ref_amp = 0; /* AMP_TEMP reference in 0.1 °C */
static bool g_drift_ref_valid = false;
bool g_rsense_enabled = false; /* true when HV/AMP are powered on */

static void apply_potentiometer_settings(void);
static void initialize_event_system(void);
static inline void analog_read_interrupt(void);
static void rsense_drift_compensation_tick(void);

void rsense_init(void)
{
	pinMode(HV_EN, OUTPUT);
	pinMode(DET_EN, OUTPUT);
	pinMode(DET_RST, OUTPUT);

	digitalWrite(HV_EN, LOW);
	digitalWrite(DET_EN, LOW);

	pinMode(HV_CS, OUTPUT);
	pinMode(DET_CS, OUTPUT);
	pinMode(AMP_CS, OUTPUT);

	digitalWrite(HV_CS, HIGH);
	digitalWrite(DET_CS, HIGH);
	digitalWrite(AMP_CS, HIGH);

	apply_potentiometer_settings();

	pinMode(DET_TRIG_PIN, INPUT);

	initialize_event_system();
}

static void initialize_event_system(void)
{
	Event2.set_generator(event::gen2::pin_pc7);
	Event2.set_user(event::user::adc0_start);
	Event2.start();
}

static inline void analog_read_interrupt(void)
{
	digitalWriteFast(DET_RST, HIGH);
	uint16_t channel = ADC0.RES;
	digitalWriteFast(STATUS_LED, LOW);

	channel = channel >> (ADC_OVERSAMPLING_BITS + 2);
	g_detector_counts.channels_16[channel & RSENSE_CHANNEL_MASK]++;

	g_total_counts++;
	g_counts_cps++;
	g_counts_delta++;
	digitalWriteFast(DET_RST, LOW);
}

ISR(ADC0_RESRDY_vect) { analog_read_interrupt(); }

/**
 * @brief Clamp a signed value into the unsigned interval [lo, hi].
 */
static uint8_t clamp_u8(int16_t v, uint8_t lo, uint8_t hi)
{
	if (v < (int16_t)lo)
		return lo;
	if (v > (int16_t)hi)
		return hi;
	return (uint8_t)v;
}

static void rsense_drift_compensation_tick(void)
{
	if (!config.flags.fields.temperature_drift_compensation || !g_rsense_enabled)
		return;

	uint32_t period = config.temp_drift_compensation.drift_period_ms;
	if (period == 0)
		period =
		    300000; /* Default to 5 minutes if not set, to avoid excessive compensation when misconfigured. */

	if (millis() - g_drift_last_ms < period)
		return;

	/* Read all three temperature sensors (each ~10 ms due to oversampling) */
	int16_t det_temp = adc_to_temperature_c(adc_read_oversampled(DET_TEMP_PIN, 16));
	int16_t hv_temp = adc_to_temperature_c(adc_read_oversampled(HV_TEMP_PIN, 16));
	int16_t amp_temp = adc_to_temperature_c(adc_read_oversampled(AMP_TEMP_PIN, 16));

	if (!g_drift_ref_valid) {
		g_drift_ref_det = det_temp;
		g_drift_ref_hv = hv_temp;
		g_drift_ref_amp = amp_temp;
		g_drift_ref_valid = true;
		g_drift_last_ms = millis();
		return;
	}

	/* Deltas in 0.1°C units */
	int16_t delta_det = det_temp - g_drift_ref_det;
	int16_t delta_hv = hv_temp - g_drift_ref_hv;
	int16_t delta_amp = amp_temp - g_drift_ref_amp;

	/* Drift coefficients in pot_units per °C, scaled by 256.
	 * adj = coeff * delta_0.1C / 10 / 256 = coeff * delta_0.1C / 2560 */
	bool needs_update = false;

	int16_t new_hv = config.pots.pot_hv;
	int16_t new_amp = config.pots.pot_amp;
	int16_t new_det = config.pots.pot_det;

	const temp_drift_compensation_t *dc = &config.temp_drift_compensation;

	if (dc->pot_hv_low != dc->pot_hv_high) {
		/* pot_hv: SiPM carrier (DET_TEMP) + 28V supply (HV_TEMP) contributions. */
		int16_t adj =
		    (int16_t)(((int32_t)dc->temp_drift * delta_det + (int32_t)dc->vbias_drift * delta_hv) / 2560L);
		if (adj != 0) {
			new_hv = (int16_t)config.pots.pot_hv + adj;
			needs_update = true;
		}
	}

	if (dc->pot_amp_low != dc->pot_amp_high) {
		int16_t adj = (int16_t)(((int32_t)dc->gain_drift * delta_amp) / 2560L);
		if (adj != 0) {
			new_amp = (int16_t)config.pots.pot_amp + adj;
			needs_update = true;
		}
	}

	if (dc->pot_det_low != dc->pot_det_high) {
		int16_t adj = (int16_t)(((int32_t)dc->threshold_drift * delta_amp) / 2560L);
		if (adj != 0) {
			new_det = (int16_t)config.pots.pot_det + adj;
			needs_update = true;
		}
	}

	if (!needs_update) {
		g_drift_last_ms = millis();
		return;
	}

	config.pots.pot_hv = clamp_u8(new_hv, dc->pot_hv_low, dc->pot_hv_high);
	config.pots.pot_amp = clamp_u8(new_amp, dc->pot_amp_low, dc->pot_amp_high);
	config.pots.pot_det = clamp_u8(new_det, dc->pot_det_low, dc->pot_det_high);

	rsense_apply_potentiometers();

	g_drift_ref_det = det_temp;
	g_drift_ref_hv = hv_temp;
	g_drift_ref_amp = amp_temp;
	g_drift_last_ms = millis();
}

void rsense_periodic(void)
{
	if (millis() - g_cps_time > RSENSE_UPDATE_PERIOD) {
		uint32_t elapsed_time = millis() - g_cps_time;
		g_cps_last = (uint16_t)(((uint32_t)g_counts_cps * 1000UL) / elapsed_time);
		g_counts_cps = 0;
		g_cps_time = millis();

		/* Update 10-second CPS circular buffer for moving average */
		g_cp10s_data.cps[g_cp10s_data.index] = g_cps_last;
		g_cp10s_data.index = (g_cp10s_data.index + 1) % 10;

		rsense_drift_compensation_tick();
	}
}

/**
 * @brief Clock @c value out to one AD5160 selected by @p cs_pin.
 */
static inline void write_pot(uint8_t cs_pin, uint8_t value)
{
	digitalWrite(cs_pin, LOW);
	SPI.transfer(value);
	digitalWrite(cs_pin, HIGH);
	delay(1);
}

/**
 * @brief Push the current @c config.pots values to the AD5160 digital
 *        potentiometers over SPI. Does not touch power-enable lines;
 *        see #rsense_apply_potentiometers for the safe re-sequencing version.
 */
static void apply_potentiometer_settings(void)
{
	write_pot(HV_CS, config.pots.pot_hv);
	write_pot(AMP_CS, config.pots.pot_amp);
	write_pot(DET_CS, config.pots.pot_det);
}

void rsense_apply_potentiometers(void)
{
	if (!g_rsense_enabled) {
		/* System is off — just write SPI registers without power cycling */
		apply_potentiometer_settings();
		return;
	}

	/* Power sequence: freeze → disable amp → disable HV → set pots → enable HV → enable amp → unfreeze */
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { ADC0.INTCTRL &= ~ADC_RESRDY_bm; }
	digitalWrite(DET_EN, LOW);
	digitalWrite(HV_EN, LOW);
	delay(10);
	apply_potentiometer_settings();
	delay(10);
	digitalWrite(HV_EN, HIGH);
	delay(100);
	digitalWrite(DET_EN, HIGH);
	delay(100);
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { ADC0.INTCTRL |= ADC_RESRDY_bm; }
}

void rsense_enable(void)
{
	g_rsense_enabled = true;
	digitalWrite(HV_EN, HIGH);
	digitalWrite(DET_EN, HIGH);
	adc_enable_fast();
}

void rsense_disable(void)
{
	g_rsense_enabled = false;
	digitalWrite(DET_EN, LOW);
	digitalWrite(HV_EN, LOW);
	adc_restore_default();
}

void rsense_flush_counters(void)
{
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
	{
		g_total_counts = 0;
		g_counts_cps = 0;
		g_counts_delta = 0;
		g_cps_time = millis();
	}
}

void rsense_flush_spectrum(void)
{
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
	{
		for (int i = 0; i < RSENSE_CHANNEL_COUNT; i++) {
			g_detector_counts.channels_16[i] = 0;
		}
	}
	rsense_flush_counters();
}

uint32_t rsense_get_total_counts(void)
{
	uint32_t v;
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { v = g_total_counts; }
	return v;
}

uint16_t rsense_get_cps(void) { return g_cps_last; }

uint32_t rsense_get_cp10s(void)
{
	uint32_t sum = 0;
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
	{
		for (int i = 0; i < 10; i++) {
			sum += g_cp10s_data.cps[i];
		}
	}
	return sum / 10;
}

uint32_t rsense_get_counts_since_last(void)
{
	uint32_t v;
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
	{
		v = g_counts_delta;
		g_counts_delta = 0;
	}
	return v;
}

const volatile void *rsense_get_spectrum_ptr(void) { return (const volatile void *)&g_detector_counts; }