/*
 * sent.cpp
 *
 * SENT protocol decoder
 *
 * @date Dec 13, 2024
 * @author Andrey Gusakov <dron0gus@gmail.com>, (c) 2024
 */

#include "ch.h"
#include "hal.h"
#include "chprintf.h"

#include "sent.h"

#ifndef SENT_INPUT_COUNT
#define SENT_INPUT_COUNT		4 // Number of sent channels
#endif

static sent_channel channels[SENT_INPUT_COUNT];

/*==========================================================================*/
/* Misc helpers.															*/
/*==========================================================================*/
#define BIT(n) (UINT32_C(1) << (n))

/*==========================================================================*/
/* ICU driver.																*/
/*==========================================================================*/
/* This SENT HW driver is based on ChibiOS ICU driver */
#if (HAL_USE_ICU == TRUE)

/* TODO: do we care about scaling abstract timer ticks to some time base? */
/* TODO: get at runtime */
/* Max timer clock for most timers on STM32 is CPU clock / 2 */
#define SENT_TIMER_CLOCK_DIV	2
/* TODO: not all timers are clocked from STM32_TIMCLK1 */
#define SENT_ICU_FREQ			(STM32_TIMCLK1 / SENT_TIMER_CLOCK_DIV)

static uint16_t lastPulse[SENT_INPUT_COUNT];
static bool overcapture[SENT_INPUT_COUNT];

static void icuperiodcb(ICUDriver *icup, size_t index)
{
	uint16_t clocks;
	uint8_t flags = 0;
	const ICUConfig *icucfg = icup->config;

	if ((icucfg->channel == ICU_CHANNEL_1) || (icucfg->channel == ICU_CHANNEL_2)) {
		/* channel 1 and channel 2 supports period measurements */
		clocks = icuGetPeriodX(icup);
	} else {
		/* this is freerunnig timer and we need to calculate period using just captured timer value and previous one */
		/* TODO: support 32 bit timers too? */
		uint16_t val = icuGetWidthX(icup);

		/* can overflow */
		clocks = val - lastPulse[index];

		lastPulse[index] = val;
	}

	if (overcapture[index]) {
		flags |= SENT_FLAG_HW_OVERFLOW;
		overcapture[index] = false;
	}

	SENT_ISR_Handler(index, clocks, flags);
}

//static void icuovercapture(ICUDriver *icup, size_t index)
//{
//	overcapture[index] = true;
//}

/* ICU callbacks */
static void icuperiodcb_in1(ICUDriver *icup)
{
	icuperiodcb(icup, 0);
}

//static void icuovercapture_in1(ICUDriver *icup)
//{
//	icuovercapture(icup, 0);
//}

static void icuperiodcb_in2(ICUDriver *icup)
{
	icuperiodcb(icup, 1);
}

//static void icuovercapture_in2(ICUDriver *icup)
//{
//	icuovercapture(icup, 1);
//}

/* ICU configs */
static /* const */ ICUConfig icucfg[SENT_INPUT_COUNT] =
{
#if (STM32_ICU_USE_TIM4 == TRUE)
	/* IN_DIN4 -> DIN4 -> PB6 -> TIM4_CH1 */
	{
		.mode = ICU_INPUT_ACTIVE_LOW,
		.frequency = SENT_ICU_FREQ,
		.width_cb = NULL,
		.period_cb = icuperiodcb_in1,
		.overflow_cb = NULL,
		.channel = ICU_CHANNEL_1,
		.dier = 0U,
		.arr = 0xFFFFFFFFU,
		//.overcapture_cb = icuovercapture_in1,
	},
#endif
#if (STM32_ICU_USE_TIM2 == TRUE)
	/* IN_A0 -> A0 -> PA1 -> TIM5_CH2/TIM2_CH2 */
	{
		.mode = ICU_INPUT_ACTIVE_LOW,
		.frequency = SENT_ICU_FREQ,
		.width_cb = NULL,
		.period_cb = icuperiodcb_in2,
		.overflow_cb = NULL,
		.channel = ICU_CHANNEL_2,
		.dier = 0U,
		.arr = 0xFFFFFFFFU,
		//.overcapture_cb = icuovercapture_in2,
	}
#endif
};

static ICUDriver *icudrivers[SENT_INPUT_COUNT] =
{
#if (STM32_ICU_USE_TIM4 == TRUE)
	&ICUD4,
#else
	nullptr,
#endif

#if (STM32_ICU_USE_TIM2 == TRUE)
	&ICUD2,
#else
	nullptr,
#endif
};

void startSent() {
#if (STM32_ICU_USE_TIM4 == TRUE)
	palSetPadMode(GPIOB, 6, PAL_MODE_INPUT);
#endif
#if (STM32_ICU_USE_TIM2 == TRUE)
	palSetPadMode(GPIOA, 1, PAL_MODE_INPUT);
#endif

	for (int i = 0; i < SENT_INPUT_COUNT; i++) {
		ICUConfig *cfg = &icucfg[i];
		ICUDriver *icu = icudrivers[i];

		if (icu == nullptr) {
			continue;
		}

		icuStart(icu, cfg);
		icuStartCapture(icu);
		icuEnableNotifications(icu);
	}
}
#endif //HAL_USE_ICU

/*==========================================================================*/
/* Forward declarations.													*/
/*==========================================================================*/

static int GmPressureDecode(size_t index);
static void GmPressureDebug(void);

/*==========================================================================*/
/* Debug.																	*/
/*==========================================================================*/
extern BaseSequentialStream *chp;

void sent_channel::Info() {
	uint8_t stat;
	uint16_t sig0, sig1;

	chprintf(chp, "Unit time %lu timer ticks\r\n", tickPerUnit);
	chprintf(chp, "Pause pulse detected %s\r\n", pausePulseReceived ? "Yes" : "No");
	chprintf(chp, "Total pulses %lu\r\n", pulseCounter);

	if (GetSignals(&stat, &sig0, &sig1) == 0) {
		chprintf(chp, "Last valid fast msg Status 0x%01x Sig0 0x%03x Sig1 0x%03x\r\n", stat, sig0, sig1);
	}

	chprintf(chp, "Slow channels:\r\n");
	for (int i = 0; i < SENT_SLOW_CHANNELS_MAX; i++) {
		if (scMsg[i].valid) {
			chprintf(chp, " %d: ID %d: %d (0x%x)\r\n", i, scMsg[i].id, scMsg[i].data, scMsg[i].data);
		}
	}

	#if SENT_STATISTIC_COUNTERS
		chprintf(chp, "HW overflows %lu\r\n", statistic.hwOverflowCnt);

		chprintf(chp, "Pause pulses %lu\r\n", statistic.PauseCnt);
		chprintf(chp, "Restarts %lu\r\n", statistic.RestartCnt);
		chprintf(chp, "Interval errors %lu short, %lu long\r\n", statistic.ShortIntervalErr, statistic.LongIntervalErr);
		chprintf(chp, "Total frames %lu with CRC error %lu (%d%%)\r\n", statistic.FrameCnt, statistic.CrcErrCnt, 100 * statistic.CrcErrCnt / statistic.FrameCnt);
		chprintf(chp, "Total slow channel messages %lu + %lu with crc6 errors %lu (%d%%)\r\n", statistic.sc12, statistic.sc16, statistic.scCrcErr, 100 * statistic.scCrcErr / (statistic.sc12 + statistic.sc16));
		chprintf(chp, "Sync errors %lu\r\n", statistic.SyncErr);
	#endif
	GmPressureDebug();
}

void sentDebug(void)
{
	for (int i = 0; i < SENT_INPUT_COUNT; i++) {
		if (icudrivers[i] == nullptr)
			continue;

		sent_channel &channel = channels[i];

		chprintf(chp, "---- SENT input %d ----\r\n", i);
		channel.Info();
		chprintf(chp, "--------------------\r\n");
	}
}

/*==========================================================================*/
/* Decoder feed handler and mailbox											*/
/*==========================================================================*/
/* 4 per channel should be enough */
#define SENT_MB_SIZE		(4 * SENT_INPUT_COUNT)

static msg_t sent_mb_buffer[SENT_MB_SIZE];
static MAILBOX_DECL(sent_mb, sent_mb_buffer, SENT_MB_SIZE);

static THD_WORKING_AREA(waSentDecoderThread, 256);

void SENT_ISR_Handler(uint8_t channel, uint16_t clocks, uint8_t flags) {
	/* encode to fit msg_t */
	msg_t msg = (flags << 24) | (channel << 16) | clocks;

	/* called from ISR */
	chSysLockFromISR();
	chMBPostI(&sent_mb, msg);
	chSysUnlockFromISR();
}

/*==========================================================================*/
/* Decoder thread.															*/
/*==========================================================================*/
static void SentDecoderThread(void*) {
	while (true) {
		msg_t ret;
		msg_t msg;

		ret = chMBFetchTimeout(&sent_mb, &msg, TIME_INFINITE);

		if (ret == MSG_OK) {
			uint16_t tick = msg & 0xffff;
			uint8_t n = (msg >> 16) & 0xff;
			uint8_t flags = (msg >> 24) & 0xff;

			if (n < SENT_INPUT_COUNT) {
				sent_channel &channel = channels[n];

				if (channel.Decoder(tick, flags) > 0) {
					/* Call high level decoder from here */
					GmPressureDecode(n);
				}
			}
		}
	}
}

/* Should be called once */
void initSent(void) {
	/* init interval mailbox */
	chMBObjectInit(&sent_mb, sent_mb_buffer, SENT_MB_SIZE);

	chThdCreateStatic(waSentDecoderThread, sizeof(waSentDecoderThread), NORMALPRIO, SentDecoderThread, nullptr);

	/* Start HW layer */
	startSent();
}

int getSentValues(size_t index, uint16_t *sig0, uint16_t *sig1) {
	if (index < SENT_INPUT_COUNT) {
		sent_channel &channel = channels[index];

		return channel.GetSignals(NULL, sig0, sig1);
	}

	/* invalid channel */
	return -1;
}

int getSentSlowChannelValue(size_t index, size_t id)
{
	if (index < SENT_INPUT_COUNT) {
		sent_channel &channel = channels[index];

		return channel.GetSlowChannelValue(id);
	}

	/* invalid channel */
	return -1;
}

/*==========================================================================*/
/* Sensor handler(s)														*/
/*==========================================================================*/

static bool gm_valid = false;
static float gm_pres = 0.0;
static float gm_temp = -273.0;

static int GmPressureDecode(size_t index)
{
	int t1, t2;

	/* Slow channels 16 and 22 transmit temperature */
	t1 = getSentSlowChannelValue(index, 16);
	t2 = getSentSlowChannelValue(index, 22);

	if ((t1 < 0) || (t2 < 0)) {
		/* this is not GM pressure sensor or slow channels is not received yet */
		return 0;
	}

	/* TODO: this is just a guess */
	/* average, 8 per degree C, 65 offset */
	gm_temp = (((float)t1 + (float)t2) / 2.0 - 512.0) / 8.0;

	/*
     * Sig0 shows about 197..198 at 1 Atm (open air) and 282 at 1000 KPa (9.86 Atm)
     * Sig1 shows about 202..203 at 1 Atm (open air) and 283 at 1000 KPa (9.86 Atm)
     * So for 8.86 Atm delta there are:
     * 84..85 units for sig0
     * 80..81 units for sig1
     * Measurements are not ideal, so lets ASSUME 10 units per 1 Atm
     * Offset is 187..188 for Sig0 and 192..193 for Sig1.
     * Average offset is 190 for 0 Atm.
     */
	uint16_t sig0, sig1;
	getSentValues(index, &sig0, &sig1);

	gm_pres = (((float)sig0 + (float)sig1) / 2 - 190) / 10.0;

	/* TODO: timestamp value */

	gm_valid = true;

	return 1;
}

static void GmPressureDebug(void)
{
	if (!gm_valid)
		return;

	/* We can not print float */
	int pres = gm_pres * 1000;
	int temp = gm_temp * 1000;

	chprintf(chp, "GM: press %d.%03d, temp %d.%03d\r\n", pres / 1000, pres % 1000, temp / 1000, temp % 1000);
}

float GmPressureGetPressure(void)
{
	return gm_pres;
}

float GmPressureGetTemperature(void)
{
	return gm_temp;
}