/*
 * ford_immo.c
 *
 *  Created on: Nov 21, 2025
 *      Author: herli
 */
#include "ford_immo.h"
#include "timeouts.h"


uint16_t FIEONA_FIRSTWORD = 0x2001;
uint16_t FIEONA_SECONDWORD = 0x2000;

static uint16_t _immo_next2  = 0x4801;  // predicted next rx = f(0x2001)
static uint16_t _rx_received = 0;
static uint8_t  _rx_fresh    = 0;

// --- Core step function: f(x) = u16(u16(x*x) + (x >> 2)) ---

#define IMMO_FIXUP 0x2001

static uint16_t immo_f(uint16_t x)
{
	uint16_t sq = (uint16_t)(x * x);
	return (uint16_t)(sq + (x >> 2));
}

// --- Advance: compute next tx and prediction ---

uint16_t FIEONA_advance()
{
    uint16_t rx = _rx_fresh ? _rx_received : _immo_next2;
	_rx_fresh = 0;

	// Phase 1: tx = f(rx), fixup if < 2
	uint16_t tx = immo_f(rx);
	if (tx < 2) tx = IMMO_FIXUP;

	// Phase 2: next2 = f(tx), blacklist check
	uint16_t next2 = immo_f(tx);
	if (next2 < 2 || next2 == 0x054B || next2 == 0x4656 || next2 == 0x88FC)
		next2 = IMMO_FIXUP;

	FIEONA_FIRSTWORD  = tx;
	FIEONA_SECONDWORD = tx & 0xFF00;
	_immo_next2 = next2;
	return tx;
}

// --- RX Handler ---

static uint16_t last_send4_tick = 0;   // last tick for this timeout
extern uint8_t count;

void Fieona_SEND4_Handler(
    const struct CanMessageDef *msg,
    const uint8_t in_data[8],
    CanTxFn tx
)
{
	if(count > 1){
		last_send4_tick = TIM16->CNT;
	}
    // Extract first word (little endian)
    uint16_t rx_word = (uint16_t)in_data[0] | ((uint16_t)in_data[1] << 8);

    // Desync check (firmware validate_rx_word_and_aux behavior)
	if (rx_word != _immo_next2) {
		if (rx_word != IMMO_FIXUP) {
			rx_word = 0;
		}
	}

	_rx_received = rx_word;
	_rx_fresh = 1;
}

void FIEONA_Poll(){
	if(last_send4_tick){
		uint16_t newtick = last_send4_tick - 250;
		Timeout_ResetByIndex(20, newtick);  // deberia mejorarlo bastante esto, en vez de
		last_send4_tick = 0;
	}
}