/*
 * ford_immo.c
 *
 *  Created on: Nov 21, 2025
 *      Author: herli
 */
#include "ford_immo.h"
#include "timeouts.h"
#include "can_port.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;
}



uint16_t FIEONA_TRANS0 = 0x0507;
uint16_t FIEONA_TRANS1 = 0x381C;
uint16_t FIEONA_TRANS2 = 0xFC1C;
uint16_t FIEONA_TRANS3 = 0x375B;//0x71F5

uint8_t eps_fieona_override = 0;

#define FIEONA_UNLOCK_MS 600000
uint32_t fieona_unlock_end = 0;
uint8_t fieona_unlocked = 0;
void Fieona_SEND3_Handler(const uint8_t in_data[8]){
    uint16_t rx_word = (uint16_t)in_data[0] | ((uint16_t)in_data[1] << 8);

    if(fieona_unlocked){ //si recibo un 700, despues de que este bloqueada, y no es 23 o 24, no contestamos
    	if(rx_word == 0x0023){
			FIEONA_TRANS0 = 0xAA63;
			FIEONA_TRANS2 = 0x0001;
			FIEONA_TRANS2 = 0x2100;
			eps_fieona_override = 1;
    	}else if(rx_word == 0x0024){
			FIEONA_TRANS0 = 0x0164;
			FIEONA_TRANS2 = 0x0001;
			FIEONA_TRANS2 = 0;
			eps_fieona_override = 1;
    	}else{
            Timeout_StopByIndex(18);
    	}
    	Timeout_ResetByIndex(1, TIM16->CNT);

    }else{	//si no esta bloqueado
    	if(rx_word == 0x0024){ //y es 24 -> envio como desinmovilizando pero sin 2100
			FIEONA_TRANS0 = 0x0164;
			FIEONA_TRANS1 = 0x0001;
			FIEONA_TRANS2 = 0;

			if(!fieona_unlocked){
				fieona_unlock_end = 0; //si estaba desbloqueando fuck you (creo que deberia ir a si...
			}
			eps_fieona_override = 1;

		}else if(rx_word == 0x00B2){ //y es b2 -> envio desinmovilizando con 2100
			FIEONA_TRANS0 = 0x0164;
			FIEONA_TRANS1 = 0x0001;
			FIEONA_TRANS2 = 0x2100;
			eps_fieona_override = 1;

			if(!fieona_unlock_end){ //si no habia empezado a desinmovilizar empiezo
		    	Timeout_StartIfStopped(1, TIM16->CNT);

				fieona_unlock_end = HAL_GetTick() + FIEONA_UNLOCK_MS; //roll over is 50 days with 32bit hal tick
			}else{	//si habia empezado, chekiamos si ya acabo.
				if(HAL_GetTick() > fieona_unlock_end){
					fieona_unlocked = 1;
					fieona_unlock_end = 0;
				}
			}
		}else{ //si envio cualquier otra cosa dejo de desinmovilizar y no overrideo na.
			//fieona_unlocked = 0; //aqui creo que deberia volver a bloquearla, pero no voy a tocar na por ahora.
			fieona_unlock_end = 0;
			eps_fieona_override = 0;
		}


    }


}
void Fieona_SEND3_set_init(uint8_t i){
	if(i){
    	FIEONA_TRANS0 = 0x0507;
	}else{
    	FIEONA_TRANS0 = 0x7F81;
	}
	FIEONA_TRANS1 = 0x381C;
	FIEONA_TRANS2 = 0xFC1C;
	FIEONA_TRANS3 = 0x71F5;
}

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;
	}
}