/*
 * @file sbc_fw.ino
 * @brief Main firmware for sensor board controller, YKSA PL/EDU16 RSENSE.
 *
 * 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 <Event.h>
#include <SPI.h>
#include <SoftwareI2C.h>
#include <stdint.h>
#include <string.h>

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

/*
 * Frame format: [CMD 2B][LEN 2B][PAYLOAD LEN bytes][CRC16 2B].
 * Maximum payload size chosen to fit the largest command (config_t ~22 bytes);
 * 256 bytes leaves headroom for future commands.
 */
#define PROTOCOL_MAX_PAYLOAD 256U
#define PROTOCOL_HEADER_SIZE ((uint16_t)sizeof(protocol_header_t))
#define PROTOCOL_CRC_SIZE 2U
#define PROTOCOL_BUF_SIZE (PROTOCOL_HEADER_SIZE + PROTOCOL_MAX_PAYLOAD + PROTOCOL_CRC_SIZE)

static uint8_t rx_buf[PROTOCOL_BUF_SIZE];
static uint16_t rx_pos = 0;

/**
 * @brief Dispatch a fully-validated frame to its registered handler.
 *        Generated from the X-macro table in protocol.h.
 */
static void dispatch_handler(uint16_t cmd, void *payload, uint16_t length)
{
	switch (cmd) {
#define X(Enum, Cmd_id, Handler)                                                                                       \
	case Cmd_id:                                                                                                   \
		Handler(payload, length);                                                                              \
		break;
		X_PROTOCOL_handlerS
#undef X
		    default : protocol_send_error();
		break;
	}
}

/**
 * @brief Drain the serial RX buffer and assemble/dispatch any complete frames.
 *        On bad CRC, oversized length, or buffer overflow, an ERR is emitted
 *        and the receive state is reset.
 */
static void protocol_receive(void)
{
	while (Serial.available()) {
		uint8_t byte = (uint8_t)Serial.read();

		if (rx_pos < PROTOCOL_BUF_SIZE) {
			rx_buf[rx_pos++] = byte;
		} else {
			/* Buffer overflow — discard and reset. */
			protocol_send_error();
			rx_pos = 0;
			continue;
		}

		/* Need a full header before total frame length is known. */
		if (rx_pos < PROTOCOL_HEADER_SIZE) {
			continue;
		}

		protocol_header_t hdr;
		memcpy(&hdr, rx_buf, sizeof(hdr));

		if (hdr.length > PROTOCOL_MAX_PAYLOAD) {
			protocol_send_error();
			rx_pos = 0;
			continue;
		}

		uint16_t total = PROTOCOL_HEADER_SIZE + hdr.length + PROTOCOL_CRC_SIZE;
		if (rx_pos < total) {
			continue; /* not enough bytes yet */
		}

		uint16_t rx_crc = (uint16_t)rx_buf[total - 2] | ((uint16_t)rx_buf[total - 1] << 8);
		uint16_t calc_crc = protocol_crc16_xmodem(rx_buf, total - PROTOCOL_CRC_SIZE);

		if (calc_crc != rx_crc) {
			protocol_send_error();
		} else {
			dispatch_handler(hdr.cmd, &rx_buf[PROTOCOL_HEADER_SIZE], hdr.length);
		}

		/* Shift any back-to-back trailing bytes to the front of the buffer. */
		uint16_t extra = rx_pos - total;
		if (extra > 0) {
			memmove(rx_buf, rx_buf + total, extra);
		}
		rx_pos = extra;
	}
}

static uint32_t status_led_last_blink = 0;

/**
 * @brief Toggle the status LED at #STATUS_LED_BLINK_PERIOD ms intervals.
 */
static inline void update_status_led(void)
{
	if (millis() - status_led_last_blink > STATUS_LED_BLINK_PERIOD) {
		digitalWrite(STATUS_LED, !digitalRead(STATUS_LED));
		status_led_last_blink = millis();
	}
}

/**
 * @brief Bring up the SPI bus shared by the AD5160 digital potentiometers.
 */
static void initialize_spi(void)
{
	SPI.begin();
	SPI.setClockDivider(SPI_CLOCK_DIV128); /* 125 kHz @ 16 MHz */
	SPI.setDataMode(SPI_MODE0);	       /* AD5160 (Rev C.) */
	SPI.setBitOrder(MSBFIRST);	       /* AD5160 (Rev C.), p. 13, fig. 37 */
}

void setup()
{
	pinMode(STATUS_LED, OUTPUT);

	Serial.pins(0, 1);
	Serial.begin(SERIAL_BAUD_RATE);

	initialize_spi();

	eeprom_init();

	rsense_init();
}

void loop()
{
	protocol_receive();
	update_status_led();
	rsense_periodic();
}