/*
 * @file protocol.cpp
 * @brief Protocol framing, CRC, and command handler implementations.
 *
 * Created: 07.05.2026
 * Author: ThePetrovich
 *
 * Copyright YKSA - Sakha Aerospace Systems, LLC.
 * See the LICENSE file for details.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <Arduino.h>
#include <stdint.h>
#include <string.h>
#include <util/atomic.h>

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

extern bool g_rsense_enabled; /* from rsense.cpp */

uint16_t protocol_crc16_update(uint16_t crc, const uint8_t *data, uint16_t len)
{
	if (data == NULL) {
		return crc;
	}
	while (len--) {
		crc ^= (uint16_t)(*data++) << 8;
		for (uint8_t i = 0; i < 8; i++) {
			crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : crc << 1;
		}
	}
	return crc;
}

uint16_t protocol_crc16_xmodem(const uint8_t *data, uint16_t len) { return protocol_crc16_update(0, data, len); }

/**
 * @brief Write a frame header (CMD + LEN) to the serial port.
 *        Internal helper, not exported.
 */
static void send_header_bytes(uint16_t cmd, uint16_t length)
{
	Serial.write((uint8_t)(cmd & 0xFF));
	Serial.write((uint8_t)(cmd >> 8));
	Serial.write((uint8_t)(length & 0xFF));
	Serial.write((uint8_t)(length >> 8));
}

/**
 * @brief Write a 16-bit CRC trailer (LSB first) and flush the TX queue.
 */
static void send_crc_trailer(uint16_t crc)
{
	Serial.write((uint8_t)(crc & 0xFF));
	Serial.write((uint8_t)(crc >> 8));
	Serial.flush();
}

void protocol_send_message_v2(uint16_t cmd, const void *frag1, uint16_t len1, const void *frag2, uint16_t len2)
{
	protocol_header_t hdr;
	hdr.cmd = cmd;
	hdr.length = (uint16_t)(len1 + len2);

	uint16_t crc = protocol_crc16_update(0, (const uint8_t *)&hdr, sizeof(hdr));
	crc = protocol_crc16_update(crc, (const uint8_t *)frag1, len1);
	crc = protocol_crc16_update(crc, (const uint8_t *)frag2, len2);

	send_header_bytes(hdr.cmd, hdr.length);
	if (frag1 != NULL && len1 > 0) {
		Serial.write((const uint8_t *)frag1, len1);
	}
	if (frag2 != NULL && len2 > 0) {
		Serial.write((const uint8_t *)frag2, len2);
	}
	send_crc_trailer(crc);
}

void protocol_send_message(uint16_t cmd, const void *payload, uint16_t length)
{
	protocol_send_message_v2(cmd, payload, length, NULL, 0);
}

void protocol_send_ack(void) { protocol_send_message(PROTOCOL_RESP_ACK, NULL, 0); }
void protocol_send_nak(void) { protocol_send_message(PROTOCOL_RESP_NAK, NULL, 0); }
void protocol_send_error(void) { protocol_send_message(PROTOCOL_RESP_ERR, NULL, 0); }

/**
 * @brief Validate that a received payload is at least @p expected bytes long.
 *        Sends a NAK on the wire and returns false if the check fails.
 */
static bool require_payload(uint16_t length, uint16_t expected)
{
	if (length < expected) {
		protocol_send_nak();
		return false;
	}
	return true;
}

void protocol_version_handler(void *payload, uint16_t length)
{
	(void)payload;
	(void)length;
	protocol_firmware_version_response_t resp;
	resp.major = FIRMWARE_VERSION_MAJOR;
	resp.minor = FIRMWARE_VERSION_MINOR;
	protocol_send_message(CMD_FIRMWARE_VERSION, &resp, sizeof(resp));
}

void protocol_light_sensor_presence_handler(void *payload, uint16_t length)
{
	(void)payload;
	(void)length;
	lsense_detect_all();

	protocol_light_sensor_presence_response_t resp;
	lsense_get_presence(&resp);
	protocol_send_message(CMD_LIGHT_SENSOR_PRESENCE, &resp, sizeof(resp));
}

void protocol_read_light_sensors_handler(void *payload, uint16_t length)
{
	if (!require_payload(length, sizeof(protocol_read_light_sensors_handler_t))) {
		return;
	}
	protocol_read_light_sensors_handler_t *cmd = (protocol_read_light_sensors_handler_t *)payload;
	if (cmd->channel > 6) {
		protocol_send_nak();
		return;
	}

	protocol_read_light_sensors_response_t resp;
	lsense_read_channel(cmd->channel, &resp);
	protocol_send_message(CMD_READ_LIGHT_SENSORS, &resp, sizeof(resp));
}

void protocol_get_configuration_handler(void *payload, uint16_t length)
{
	(void)payload;
	(void)length;
	protocol_send_message(CMD_GET_CONFIGURATION, &config, sizeof(config));
}

void protocol_set_configuration_handler(void *payload, uint16_t length)
{
	if (!require_payload(length, sizeof(protocol_set_configuration_handler_t))) {
		return;
	}
	protocol_set_configuration_handler_t *cmd = (protocol_set_configuration_handler_t *)payload;
	if (cmd->magic1 != 0xA5 || cmd->magic2 != 0x5A) {
		protocol_send_nak();
		return;
	}
	config = *cmd;
	eeprom_save_config();
	rsense_apply_potentiometers();
	protocol_send_ack();
}

void protocol_set_potentiometers_handler(void *payload, uint16_t length)
{
	if (!require_payload(length, sizeof(protocol_set_potentiometers_handler_t))) {
		return;
	}
	config.pots = *(protocol_set_potentiometers_handler_t *)payload;
	rsense_apply_potentiometers();
	eeprom_save_config();
	protocol_send_ack();
}

void protocol_set_calibration_handler(void *payload, uint16_t length)
{
	if (!require_payload(length, sizeof(protocol_set_calibration_handler_t))) {
		return;
	}
	config.calibration = *(protocol_set_calibration_handler_t *)payload;
	eeprom_save_config();
	protocol_send_ack();
}

void protocol_set_flags_handler(void *payload, uint16_t length)
{
	if (!require_payload(length, sizeof(protocol_set_flags_handler_t))) {
		return;
	}
	config.flags = *(protocol_set_flags_handler_t *)payload;
	eeprom_save_config();
	protocol_send_ack();
}

void protocol_set_temp_drift_compensation_handler(void *payload, uint16_t length)
{
	if (!require_payload(length, sizeof(protocol_set_temp_drift_compensation_handler_t))) {
		return;
	}
	config.temp_drift_compensation = *(protocol_set_temp_drift_compensation_handler_t *)payload;
	eeprom_save_config();
	protocol_send_ack();
}

void protocol_set_drift_compensation_enable_handler(void *payload, uint16_t length)
{
	if (!require_payload(length, sizeof(protocol_set_drift_compensation_enable_handler_t))) {
		return;
	}
	protocol_set_drift_compensation_enable_handler_t *cmd =
	    (protocol_set_drift_compensation_enable_handler_t *)payload;
	config.flags.fields.temperature_drift_compensation = (cmd->enable != 0) ? 1 : 0;
	eeprom_save_config();
	protocol_send_ack();
}

void protocol_get_telemetry_handler(void *payload, uint16_t length)
{
	(void)payload;
	(void)length;

	adc_restore_default();

	protocol_get_telemetry_response_t resp;
	resp.hv_temp_c = (uint16_t)adc_to_temperature_c(adc_read_oversampled(HV_TEMP_PIN, 16));
	resp.amp_temp_c = (uint16_t)adc_to_temperature_c(adc_read_oversampled(AMP_TEMP_PIN, 16));
	resp.sipm_temp_c = (uint16_t)adc_to_temperature_c(adc_read_oversampled(DET_TEMP_PIN, 16));
	resp.mcu_temp_c = adc_read_tempsense();
	resp.vbias_mv = adc_to_voltage_mv(adc_read_oversampled(V28V0_FB_PIN, 16));
	resp.pots = config.pots;
	resp.cps = rsense_get_cps() / 60;
	resp.cp10s = rsense_get_cp10s();
	resp.total_counts = rsense_get_total_counts();
    resp.flags = (1 << 0) | /* power on ok */
                 ((g_rsense_enabled ? 1 : 0) << 1) |
                 ((config.flags.fields.temperature_drift_compensation ? 1 : 0) << 2);

	adc_enable_fast();

	protocol_send_message(CMD_GET_TELEMETRY, &resp, sizeof(resp));
}

void protocol_flush_counters_handler(void *payload, uint16_t length)
{
	(void)payload;
	(void)length;
	rsense_flush_counters();
	protocol_send_ack();
}

void protocol_flush_spectrum_handler(void *payload, uint16_t length)
{
	(void)payload;
	(void)length;
	rsense_flush_spectrum();
	protocol_send_ack();
}

void protocol_rsense_enable_handler(void *payload, uint16_t length)
{
	(void)payload;
	(void)length;
	rsense_enable();
	protocol_send_ack();
}

void protocol_rsense_disable_handler(void *payload, uint16_t length)
{
	(void)payload;
	(void)length;
	rsense_disable();
	protocol_send_ack();
}

void protocol_spectrum_freeze_handler(void *payload, uint16_t length)
{
	(void)payload;
	(void)length;
	/* Disable ADC result-ready interrupt so the ISR stops accumulating. */
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { ADC0.INTCTRL &= ~ADC_RESRDY_bm; }
	protocol_send_ack();
}

void protocol_spectrum_unfreeze_handler(void *payload, uint16_t length)
{
	(void)payload;
	(void)length;
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { ADC0.INTCTRL |= ADC_RESRDY_bm; }
	protocol_send_ack();
}

void protocol_get_counts_handler(void *payload, uint16_t length)
{
	(void)payload;
	(void)length;
	protocol_get_counts_response_t resp;
	resp.counts = rsense_get_counts_since_last();
	protocol_send_message(CMD_GET_COUNTS, &resp, sizeof(resp));
}

void protocol_read_spectrum_chunk_handler(void *payload, uint16_t length)
{
	if (!require_payload(length, sizeof(protocol_read_spectrum_chunk_handler_t))) {
		return;
	}
	protocol_read_spectrum_chunk_handler_t *cmd = (protocol_read_spectrum_chunk_handler_t *)payload;

	if (cmd->offset >= RSENSE_CHANNEL_COUNT * sizeof(uint16_t)) {
		protocol_send_nak();
		return;
	}

	uint16_t available = (RSENSE_CHANNEL_COUNT * sizeof(uint16_t)) - cmd->offset;
	uint16_t send_len = (cmd->length < available) ? cmd->length : available;

	const uint8_t *spectrum = (const uint8_t *)rsense_get_spectrum_ptr();

	protocol_read_spectrum_chunk_response_t resp_hdr;
	resp_hdr.offset = cmd->offset;
	resp_hdr.length = send_len;

	protocol_send_message_v2(CMD_READ_SPECTRUM_DATA, &resp_hdr, sizeof(resp_hdr), spectrum + cmd->offset, send_len);
}