/*
 * toothedwheel.c
 *
 *  Created on: Aug 4, 2025
 *      Author: herli
 */

#include "toothed_wheel.h"
#include "timeouts.h"
#include "injection.h"
#include "FBKW.h"
#include "can_port.h"
#include "timing_manager.h"
#include "tein_detection.h"


volatile uint8_t currentTooth = 0;

volatile uint32_t IC_RPM_Val1 = 0;
volatile uint32_t IC_RPM_Val2 = 0;
volatile uint32_t IC_MT = 0;
volatile uint32_t IC_EOI = 0;
volatile uint32_t IC_TEIN = 0;
volatile uint32_t IC_NOM_BIP = 0;

volatile uint32_t prev_Difference = 0xFFFFFFFF;
volatile uint32_t Tooth_Difference = 0;

volatile uint8_t TW_current_max_teeth = 0;
volatile uint8_t TW_RPM_SENSOR_STATE = 0;

volatile float RPM;

volatile float MT_frequency = 0;
volatile float MT_RPM = 0;
//volatile float next_MT_RPM = 0;

volatile float last_MT_RPM = 0;

float actual_phi1;


volatile float TEETH_frequency = 0;
volatile float TEETH_RPM = 0;
volatile float last_TEETHRPM = 0;
volatile uint8_t SYNCOUT_clear = 0;

volatile uint32_t RPM_Difference;

volatile uint8_t startedEngine = 0;
volatile uint8_t count_CKP = 0;


extern float MEPI;

uint8_t real_boi_pending = 0;
uint8_t real_eoi_pending = 0;
uint8_t phi1_eval_pending = 0;

uint8_t hasCapturedTeeth = 0;

uint8_t real_eoi_updated = 0;
uint8_t real_bip_updated = 0;
uint8_t eoi_eval_pending = 0;

void TW_RESET_SENSOR(){
	TW_RPM_SENSOR_STATE = 0;
	TW_current_max_teeth = 0;
	prev_Difference = 0xFFFFFFFF;
	hasCapturedTeeth = 0;
}
void TW_EVAL_MAX_TEETH(){
	TW_RPM_SENSOR_STATE = (TW_current_max_teeth == TW_PERCYL_TEETH) ? 1 : 0 ;
}
uint32_t sensorfail;

volatile uint8_t isMT = 0;
uint8_t SYNCOUT_TEETH = 0;
volatile float accel_rpm = 0;
// en un timeout tendria que resetear el prevdiff, y hascapturedteet.
void TW_TEETH_CAPTURE(){
	if(hasCapturedTeeth){
		Tooth_Difference = IC_RPM_Val2 >= IC_RPM_Val1 ? IC_RPM_Val2-IC_RPM_Val1 : IC_RPM_Val2-(IC_RPM_Val1-0xffffffff);
	}
	if(Tooth_Difference > prev_Difference * TW_MT_THRESHOLD && TEETH_RPM > 2 && !isInjecting){
		//esto es para que la realentizacion no nos afecte y podamos bajar el treshold
		isMT = 1;
		//si tiene TEETH RPM y no está inyectando.. es MT 100%
		//si no hay algun diente capturado antes: esta claro que no es missing tooth
	}else{
		//el original, ademas checkea si esta en el ultimo diente y sabe que viene el missing tooth.
		//Sigue leyendo rpm pero da error de sensor y no inyecta... habra que ver si es necesario implementarlo.
		isMT = 0;
	}

	if(isMT){

		RPM_Difference = IC_RPM_Val2 >= IC_MT ? IC_RPM_Val2-IC_MT : IC_RPM_Val2-(IC_MT-0xffffffff);
		IC_MT = IC_RPM_Val2;

		TW_current_max_teeth = currentTooth;
		TW_RPM_SENSOR_STATE = (TW_current_max_teeth == TW_PERCYL_TEETH) ? 1 : 0 ;

		currentTooth = 0;

		INJ_MT_TASK();

		if(!TW_current_max_teeth){
			/*correction_beta = 0.0;
			correction_eoi = 0.0;*/
			//quitao para q no de error
			//Timeout_ResetByIndex(4, TIM16->CNT);  // Reset captor defect timeout
		}
		if(!TW_RPM_SENSOR_STATE){
			ClearEdgeBuffer();
			//TODO ENABLE CLEAREDGEBUFFER
			//TW_RESET_SENSOR();
			//TW_Reset_Pending_Tasks();
			//IC_MT = 0;
			sensorfail++;
			hasCapturedTeeth = 0;
			TEETH_RPM = 0;
			return;
		}

		FBKW_RESET_FEEDBACK();

		last_MT_RPM = MT_RPM;
		MT_frequency = refClock/RPM_Difference;
		MT_RPM = fclamp(60.0 * MT_frequency / CYLINDERS, MIN_RPM, 4000);
		RPM=MT_RPM;

		TM_UPDATE_ACCELERATION(last_MT_RPM, MT_RPM, RPM_Difference);//next_MT_RPM =

		MakeEdgeBufferOld();
		UpdateEdgeBuffer(IC_RPM_Val2, MT_RPM, 0, isInjecting);


		TW_PREPARE_BOI_ACCELCORRECTION();
		if(startup && RPM > 200){
			can_port_send_msg_def(&MSG_ID_EMPF3); // TX from template (+ symbols if any)
	        Timeout_ResetByIndex(18, TIM16->CNT);  // re-arm for another 40ms
		}
		INJ_PREPARE_ONCE();
		//TW_CALCULATE_INSTANT_EOI_CORRECTION();
		real_bip_updated = 0;
		real_eoi_updated = 0;
		if(currentTooth == SYNCOUT_TEETH){
			if(SYNC_Pulse_IsSyncoutMode()){
				SYNCOUT_clear = 1;
				SYNC_Pulse_GPIO_set(1);
			}
		}
	}else{ //normal teeth
		currentTooth++;

		if(!hasInjected && TW_RPM_SENSOR_STATE){ //TODO
			EvaluateInjection();
			/*if(currentTooth > 13){
				SYNCOUT_clear = 0;

			}*/
		}
		if(currentTooth == SYNCOUT_TEETH){
			if(SYNC_Pulse_IsSyncoutMode()){
				SYNCOUT_clear = 1;
				SYNC_Pulse_GPIO_set(1);
			}
		}
		if(currentTooth == SYNCOUT_TEETH + 1 && SYNCOUT_clear && SYNC_Pulse_IsSyncoutMode()){
			SYNC_Pulse_GPIO_set(0);
			SYNCOUT_clear = 0;
		}

		if(hasCapturedTeeth){
			TEETH_frequency = refClock/Tooth_Difference;
			TEETH_RPM = fclamp(60.0 * TEETH_frequency / TW_THEETHS, 1, 4000);
		}
		//UpdateEdgeBuffer(IC_RPM_Val2, TEETH_RPM, currentTooth);

		if(TW_RPM_SENSOR_STATE){

			if(!hasInjected){
				if(startedEngine && ME >  0.031){ //hay que ver, igual hace que resetee el BOI
					//TW_CALCULATE_INSTANT_BOI_CORRECTION(); //deactivated for now

				}
				//EvaluateInjection();


				INJ_EVAL_EOI_COMPENSATION();
			}
			INJ_EVAL_END();

			UpdateEdgeBuffer(IC_RPM_Val2, TEETH_RPM, currentTooth, isInjecting);

			if(!hasInjected){
				INJ_PREPARE_BIP(currentTooth);
				if(TW_RPM_SENSOR_STATE){ //TODO
					EvaluateInjection();
				}
				TW_CALCULATE_INSTANT_BOI_CORRECTION(); //deactivated for now
			}

		}


		if(currentTooth == TW_PERCYL_TEETH){
			if(RPM > 200) {
				can_port_send_msg_def(&MSG_ID_EMPF1);
			}

			if (s_empf2_pending) {
				can_port_send_msg_def(&MSG_ID_EMPF2);       // packs from EMPF2_WORDS[]
				s_empf2_pending = 0;
				Timeout_StopByIndex(17);               // stop the 60ms gate
			}
			if(HAL_GPIO_ReadPin(SHUT_OFF_GPIO_Port, SHUT_OFF_Pin) != GPIO_PIN_RESET){  	//si esta en alto

			}else{
				Timeout_ResetByIndex(11, TIM16->CNT);  // Reset turnoff timeout
			}
		}
		if(currentTooth == TW_PERCYL_TEETH - 1){
			INJ_END();
		}


		hasCapturedTeeth = 1;

		if(hasInjectionEndedFlag == 1){
			hasInjectionEndedFlag++;
		}else if(hasInjectionEndedFlag > 1){
			hasInjectionEnded = 1;

			real_boi_pending = 1;
			real_eoi_pending = 1;
		}
		if(hasTeinDetEndedFlag == 1){
			hasTeinDetEndedFlag = 0;
			hasTeinDetEnded = 1;
		}

		/*}else if(hasInjectionEndedFlag > 1){
			hasTeinDetEnded = 1;
		}*/

	}
	prev_Difference = Tooth_Difference;
	IC_RPM_Val1 = IC_RPM_Val2;

	if(TEETH_RPM > 15){
		Timeout_ResetByIndex(3, TIM16->CNT);  // Reset RPM timeout
	}
	if(RPM > TW_STARTED_RPM){
		//startedEngine = 1;
		Timeout_StartIfStopped(19, TIM16->CNT);
	}

}

uint8_t ckp_process_pending = 0;
uint8_t teethCKP;

void TW_CKP_CAPTURE(){
	if(TW_RPM_SENSOR_STATE){
		count_CKP++;

		ckp_process_pending=1;

		//meter timeout cuando !count_ckp (significa que solo recibe un pulso);
		CKP_PULSE_AVAILABLE = 1;
		Timeout_ResetByIndex(13, TIM16->CNT);  // Reset CKP timeout
		FBKW_CKP_ISR();
	}
}
float fb_1;
float fb_2;
uint8_t fb_lock;
float new_fb = 0.0;
float lpf_fb = 1.0;//0.4
uint8_t feedbackMT = 0;
uint8_t valid = 0;

float ckp_rpm = 0.0;
float fbkw_offset= 0;
void TW_CALC_FBKW_FEEDBACK(){
	if(!TW_RPM_SENSOR_STATE){
		return;
	}
	int index = -1;
	uint32_t IC_CKP_Real = IC_CKP2 - FBKW_FEEDBACK_IC_DT * 16;
	for(uint8_t i = 1; i < TW_PERCYL_TEETH; i++){
		if(edgeBuf[i].ic < IC_CKP_Real && IC_CKP_Real < edgeBuf[i+1].ic){
			index = i;
			break;
		}
	}
	if(index < 0){
		if(edgeBuf[TW_PERCYL_TEETH].ic < IC_CKP_Real && IC_CKP_Real < edgeBuf[0].ic){//probar si lo que le pasa es que es el ultimo
			index = TW_PERCYL_TEETH;
		}else{
			return;
		}
	}
	uint8_t index0 = FBKW_FEEDBACK_ZERO / TW_TOOTH_ALPHA;

	teethCKP = index;

	uint8_t nteeths = TW_PERCYL_TEETH - index0 + 1;
	uint32_t diffCKP_e = IC_CKP_Real >= edgeBuf[index0-1].ic ? IC_CKP_Real - edgeBuf[index0-1].ic :  IC_CKP_Real - (-edgeBuf[index0-1].ic - 0xffffffff);

	/*float b_a = 1.0*FBKW_FEEDBACK_ZERO / TW_TOOTH_ALPHA - index0; //remaining angle
	float rpm_a = edgeBuf[index0].rpm;
	float rpm_2 = edgeBuf[index+1].rpm;
	float rpm_b = (rpm_2 - rpm_a)/TW_TOOTH_ALPHA * b_a + rpm_a;*/

	float avgrpm = 0;
	for(int i = index0; i < TW_PERCYL_TEETH + 1; i ++){ //poner security por overfkiw
		avgrpm += edgeBuf[i].rpm;
	}
	avgrpm = avgrpm / nteeths;
	//checkear si restando el tiempo de adquisicion al ic_ckp2

	uint32_t diffCKP_1 = IC_CKP_Real >= edgeBuf[index].ic ? IC_CKP_Real - edgeBuf[index].ic :  IC_CKP_Real - (-edgeBuf[index].ic - 0xffffffff);
	uint32_t diffCKP_2 = edgeBuf[index+1].ic >= IC_CKP_Real  ? edgeBuf[index+1].ic - IC_CKP_Real : edgeBuf[index+1].ic - (-IC_CKP_Real - 0xffffffff);

	uint8_t toNext = diffCKP_1 > diffCKP_2 ? 1 : 0;
	//uint32_t diffCKP = toNext ? diffCKP_2 : diffCKP_1;
	uint32_t diffCKP = IC_CKP_Real >= edgeBuf[index0].ic ? IC_CKP_Real - edgeBuf[index0].ic :  IC_CKP_Real - (-edgeBuf[index0].ic - 0xffffffff);

	if(feedbackMT){
		diffCKP = IC_CKP2 >= IC_MT ? IC_CKP2 - IC_MT :  IC_CKP2 - (IC_MT - 0xffffffff);
	}
	//uint32_t diffCKP = IC_CKP2 >= IC_MT ? IC_CKP2 - IC_MT :  IC_CKP2 - (IC_MT - 0xffffffff);
	float diffbot = 0;
	float difftop = 0;
	if(safetySHUTOFF){
		if(RPM < 150){
			Timeout_StopByIndex(12);  // Reset CKP timeout
		}
	}else{
		if(count_CKP > 1){
			if(feedbackMT){
				fb_2 = MT_RPM * 6.0 * diffCKP / refClock - dFi;
			}else{
				fb_2 = avgrpm * 6.0 * diffCKP_e / refClock + (index0-1) * TW_TOOTH_ALPHA - dFi;
			}

			fb_2 -= FBKW_FEEDBACK_ZERO;

			if(fb_2 > FBKW_FEEDBACK_MIN && fb_2 < FBKW_FEEDBACK_MAX){ //si esta entre margenes
				new_fb = fb_2;
				fb_lock = 0;
			}else{
				diffbot = FBKW_FEEDBACK_MIN - fb_2;
				difftop = fb_2 - FBKW_FEEDBACK_MAX;
				valid = 1;
			}
		}else{
			//feedbackfirst = RPM * 6.0 * diffCKP / refClock;
			if(feedbackMT){
				fb_1 = ckp_rpm * 6.0 * diffCKP_e / refClock + (index0-1) * TW_TOOTH_ALPHA - dFi;
			}else{
				//fb_1 = (toNext ? -1 : 1) * edgeBuf[index + toNext].rpm * 6.0 * diffCKP / refClock + (teethCKP + toNext) * TW_TOOTH_ALPHA - dFi;
				fb_1 = avgrpm * 6.0 * diffCKP_e / refClock + (index0-1) * TW_TOOTH_ALPHA - dFi;
			}

			fb_1 -= FBKW_FEEDBACK_ZERO;

			if(fb_1 > FBKW_FEEDBACK_MIN && fb_1 < FBKW_FEEDBACK_MAX){
				new_fb = fb_1;
				fb_lock = 0;
				valid = 1;
			}else{
				diffbot = FBKW_FEEDBACK_MIN - fb_1;
				difftop = fb_1 - FBKW_FEEDBACK_MAX;
			}
		}
	}

	if(!valid && count_CKP > 1){ //si no encontro ningun otro nuevo valor dentro de rango, checkear boundaries
		//estas diferencias son positivas cuando hay algun punto fuera.
		if(difftop > 0 && diffbot > 0){
			fb_lock = (difftop > diffbot) > 0 ? 2 : 1;
		}else if(difftop > 0){
			fb_lock = 2;
		}else if(diffbot > 0){
			fb_lock = 1;
		}
	}
	if(fb_lock){ //si se ha encontrado bloqueo, set the value now
		if(fb_lock > 1){
			new_fb = FBKW_FEEDBACK_MAX;
		}else{
			new_fb = FBKW_FEEDBACK_MIN;
		}
		valid = 0;
	}else{
		Timeout_ResetByIndex(12, TIM16->CNT);  // Reset CKP timeout
		Timeout_ResetByIndex(13, TIM16->CNT);
		//si estan fuera de bounds da error.
	}

	FBKW_FEEDBACK -= lpf_fb*(FBKW_FEEDBACK - new_fb - fbkw_offset);

	ckp_process_pending=0;
}

void FBKW_RESET_FEEDBACK(){
	//fb_1 = 0;
	//fb_2 = 0;
	//valid = 0;
	count_CKP = 0;

	uint8_t index0 = FBKW_FEEDBACK_ZERO / TW_TOOTH_ALPHA-1;
	uint8_t nteeths = TW_PERCYL_TEETH - index0;
	uint32_t diff_rpm_to_end = edgeBuf[TW_PERCYL_TEETH].ic >= edgeBuf[index0].ic ? edgeBuf[TW_PERCYL_TEETH].ic - edgeBuf[index0].ic :  edgeBuf[TW_PERCYL_TEETH].ic - (-edgeBuf[index0].ic - 0xffffffff);
	ckp_rpm = 60.0 * refClock / diff_rpm_to_end * (nteeths)/TW_THEETHS;
}



float real_Beta = 0.0;
extern float dPHI1_bip_nom;

void TW_CALC_REAL_BETA(){
	uint8_t index = 0;
	for(uint8_t i = 1; i < TW_PERCYL_TEETH; i++){
		if(edgeBuf[i].ic < IC_INJ && IC_INJ < edgeBuf[i+1].ic){
			index = i;
			break;
		}
	}
	if(!index){
		return;
	}
	uint32_t Inj_Difference = edgeBuf[index + 1].ic >= IC_INJ ? edgeBuf[index+1].ic-IC_INJ : edgeBuf[index+1].ic-(IC_INJ-0xffffffff);
	//uint32_t Inj_Difference = IC_INJ >= edgeBuf[index].ic ? IC_INJ-edgeBuf[index].ic : IC_INJ-(edgeBuf[index].ic-0xffffffff);

	//uint32_t Tooth_Difference = edgeBuf[index+1].ic >= edgeBuf[index].ic ? edgeBuf[index+1].ic-edgeBuf[index].ic : edgeBuf[index+1].ic-(edgeBuf[index].ic-0xffffffff);

	real_Beta = (index+1)*TW_TOOTH_ALPHA - 6.0*edgeBuf[index+1].rpm*Inj_Difference/refClock;
	//real_Beta = index*TW_TOOTH_ALPHA + 6.0*edgeBuf[index].rpm*Inj_Difference/refClock;

	/*float rpm_1 = edgeBuf[index].rpm;
	float rpm_2 = edgeBuf[index+1].rpm;
	float rpm_b = (rpm_2 - rpm_1)/(Tooth_Difference) * Inj_Difference + rpm_1;

	real_Beta = index*TW_TOOTH_ALPHA + 6.0*rpm_b*Inj_Difference/refClock;*/

	real_boi_pending = 0;
}

float correction_beta = 0.0;
volatile float  s_boi_corr_deg = 0.0f;
float correction_boi_accel = 0.0;
float correction_boi_accel_2 = 0.0;

uint8_t boi_eval_pending = 0;
void TW_DEFER_BOI_EVAL(uint32_t ic){
	IC_INJ = ic;
	boi_eval_pending = 1;
}
uint8_t par = 0;
uint8_t filter = 4;

void TW_CALC_INJ_BOI(){
	if(TW_RPM_SENSOR_STATE){
		TW_UPDATE_BOI_COMP();
		TW_UPDATE_INJ_DELAY();
	}
	correction_beta = s_boi_corr_deg + correction_boi_accel_2;
	boi_eval_pending = 0;
}
extern uint32_t T_delay;
extern uint32_t T_delayFI;
float val;

void TW_UPDATE_INJ_DELAY(){
	uint32_t t = TM_TIME_FROM_NEAREST_TOOTH(IC_INJ, 1);

	if(t - (T_delayFI - T_delay) < 100){
		T_delay = t - (T_delayFI - T_delay);
	}else{
		T_delay = 20;
	}

	float inj_rpm = TM_GET_RPM_FROM_NEAREST_TOOTH(IC_INJ, 0);
	INJ_UPDATE_BOI_MARGIN(inj_rpm);
}
// Tunables (per update/event)
static float s_boi_Kp = 0.15f;      // proportional gain
static float s_boi_Ki = 0.00f;      // integral gain (per event)
static float s_boi_max_step_deg = 0.4f; // max per-update change

// State
static float s_boi_I = 0.0f;        // integral accumulator
static float max_boi = 1.8;
void TW_UPDATE_BOI_COMP(void)
{
    float err = PHI1 + dFi - actual_phi1;// + correction_boi_accel_2

    // reject outliers, also clear integral so we don't carry stale windup
    if (err > 4.0f || err < -4.0f) {
    	s_boi_I = 0.0f; return;
    }

    // Unsaturated control
    float u_unsat = s_boi_Kp * err + s_boi_I;

    // Saturate to per-step limit
    float delta = u_unsat;
    if (delta >  s_boi_max_step_deg) delta =  s_boi_max_step_deg;
    if (delta < -s_boi_max_step_deg) delta = -s_boi_max_step_deg;

    // Simple anti-windup: only integrate when not saturated
    if (delta == u_unsat) {
        s_boi_I += s_boi_Ki * err;
        // optional clamp for the integral itself (keeps it reasonable)
        /*float I_lim = s_boi_max_step_deg;
        if (s_boi_I >  I_lim) s_boi_I =  I_lim;
        if (s_boi_I < -I_lim) s_boi_I = -I_lim;*/
    }

    // Apply correction
    s_boi_corr_deg += delta;
    if (s_boi_corr_deg >  max_boi) s_boi_corr_deg =  max_boi;
	if (s_boi_corr_deg < -max_boi) s_boi_corr_deg = -max_boi;
}


//static float accel_m = 0.23; //160 //comparing to last probamos con 5 en el coche si se reduce el error intentar dejarlo fino
//float accel_m_2=4; //antes

//ahora
static float accel_m = 0.35;
float accel_m_2=0;


float lastrpmteeth = 0.0;
float accel= 0;

float drpm_=0;
//float acc = 0;
//float lpf_accboi = 0.1;
uint8_t roundingmode;
extern float last_accel;
int tooth = 0;
void TW_CALCULATE_INSTANT_BOI_CORRECTION(){

	//uint8_t lookuptooth = currentTooth < TW_PERCYL_TEETH ? currentTooth + 1 : 0;
	//uint8_t dtooth = currentTooth >= lookuptooth ? currentTooth + 3 - lookuptooth : lookuptooth + TW_PERCYL_TEETH + 2 - (currentTooth);
	uint8_t dtooth = 30;
	//drpm_ = (TEETH_RPM - edgeBuf[lookuptooth].rpm)/(TEETH_RPM*dtooth);
	/*if(MT_RPM > lastrpmteeth && MT_RPM > TEETH_RPM){
		drpm_ = (TEETH_RPM - MT_RPM)/(TEETH_RPM*currentTooth);
	}else if(MT_RPM < lastrpmteeth && MT_RPM < TEETH_RPM){
		drpm_ = (TEETH_RPM - MT_RPM)/(TEETH_RPM*currentTooth);
	}else{
		//drpm_ = (TEETH_RPM - lastrpmteeth)/(TEETH_RPM*dtooth);
	}*/
	drpm_ = (TEETH_RPM - lastrpmteeth)/(dtooth);
	/*if(drpm_ * last_accel < 20 ){
		drpm_ = 0;
	}*/
	float drpm_2 = (MT_RPM - last_MT_RPM) / MT_RPM;
	//acc -= lpf_accboi*(acc-drpm_);
	//float drpm_ = (TEETH_RPM - lastrpmteeth)/TEETH_RPM;

	float comp = drpm_ * accel_m + drpm_2 * accel_m_2;
	//float comp = last_accel * accel_m;

	//float comp = accel_m * TM_GET_LOCAL_ACC_TO_ANGLE(PHI1 + dFi, roundingmode);
	//float comp = edgeBuf[currentTooth].accel * accel_m;

	//correction_boi_accel_2 = (comp > 0.5 || comp < -0.5) ? 0 : comp ;
	correction_boi_accel_2 = fclamp(comp, -1, 1);
	/*if(correction_boi_accel_2 < -0.4){
		tooth = currentTooth;
		correction_boi_accel_2 = 0;
	}*/
	correction_beta = s_boi_corr_deg + correction_boi_accel_2;
}
void TW_PREPARE_BOI_ACCELCORRECTION(){
	uint8_t index = 0;
	for(uint8_t i = 1; i < TW_PERCYL_TEETH; i++){
		if(edgeBuf[i].ic < IC_INJ && IC_INJ < edgeBuf[i+1].ic){
			index = i;
			break;
		}
	}
	if(!index){
		return;
	}
	lastrpmteeth = edgeBuf[index - 1].rpm;
}

float real_eoi = 0.0;
float correction_eoi = 0.0;
static volatile float  s_eoi_corr_deg = 0.0f;

void TW_DEFER_EOI_EVAL(uint32_t ic){
	IC_EOI = ic;
	eoi_eval_pending = 1;
}
uint8_t inj_rpm_pending = 0;

void TW_CALC_REAL_EOI(){

	real_eoi = TM_INTEGRATE_ANGLE_FROM_NEAREST_TOOTH(IC_EOI, 0); //friggin backwards integration

	real_eoi_pending = 0;
	real_eoi_updated = 1;
	inj_rpm_pending = 1;

}


//float drpm_eoi = 0.0;
//float correction_eoi_accel = 0.0;
void TW_CALCULATE_INSTANT_EOI_CORRECTION(){
	//float drpm_ = (TEETH_RPM - lastrpmteeth)/TEETH_RPM;
	float valid_accel = last_accel;//fclamp(last_accel, -100, 50);
	if(RPM < MIN_RPM){
		return;
	}
	//float comp = fclamp(-valid_accel * eoi_acc_K / RPM, -10, 10);
	//correction_eoi_accel += lpf_eoi_acc_k*(comp-correction_eoi_accel);

	/*if(last_accel < 5 && last_accel > -5){
		//correction_eoi_accel = 0;
		//correction_eoi_accel += lpf_eoi_acc_k*(0-correction_eoi_accel);
	}else{
		correction_eoi_accel += lpf_eoi_acc_k*(comp-correction_eoi_accel);
	}*/
	//correction_eoi_accel = (comp > 2 || comp < -2) ? 0 : comp ;
	//correction_eoi_accel = eoi_acc_K*correction_boi_accel_2;
	//correction_eoi = correction_eoi_accel + s_eoi_corr_deg;
}

static inline float angle_diff_deg(float a, float b)
{
    float d = a - b;
    while (d >  180.0f) d -= 360.0f;
    while (d < -180.0f) d += 360.0f;
    return d;
}
static float s_eoi_Kp = 0.45f;      // proportional gain
static float s_eoi_Ki = 0.00f;      // integral gain (per event)
static float s_eoi_max_step_deg = 0.4f; // max per-update change

// State
static float s_eoi_I = 0.0f;        // integral accumulator

extern float correction_eoi_accel;
float eoi_clamp = 1.67;//sacado del osciloscopio
float eoi_kslow = 0.45;

float cummulative_eoi_error = 0.0;
void TW_UPDATE_EOI_COMP(void)
{
    float target = INJ_GET_TARGET_EOI();

    float err = angle_diff_deg(target, real_eoi);  // wrap-aware
    /*if(!T_ein_status){
    	err = angle_diff_deg(target, TM_INTEGRATE_ANGLE_FROM_NEAREST_TOOTH(IC_EOI + 16*PH_PEAK_DEF,1));
    }*/
    // reject outliers, also clear integral so we don't carry stale windup
    if (err > 30.0f || err < -30.0f) {
    	s_boi_I = 0.0f; return;
    }

    //estas dos lineas son nuevas, la idea es que se suma el nuevo error y se calcula en base a eso
    err += cummulative_eoi_error;
    err /= filter;

    // Unsaturated control
    /*if(err < -0.15 ){
    	err *= eoi_kslow;
    }*/
    float u_unsat = ((err < -0.15) ? eoi_kslow : s_eoi_Kp) * err + s_eoi_I;

    // Saturate to per-step limit
    float delta = u_unsat;
    if (delta >  s_eoi_max_step_deg) delta =  s_eoi_max_step_deg;
    if (delta < -s_eoi_max_step_deg) delta = -s_eoi_max_step_deg;

    // Simple anti-windup: only integrate when not saturated
    if (delta == u_unsat) {
        s_eoi_I += s_eoi_Ki * err;
        // optional clamp for the integral itself (keeps it reasonable)
        /*float I_lim = s_boi_max_step_deg;
        if (s_boi_I >  I_lim) s_boi_I =  I_lim;
        if (s_boi_I < -I_lim) s_boi_I = -I_lim;*/
    }

    // Apply correction
    s_eoi_corr_deg += delta;
    s_eoi_corr_deg = fclamp (s_eoi_corr_deg, -eoi_clamp, eoi_clamp);

    cummulative_eoi_error = 0;
}
void TW_UPDATE_EOI_ERROR(void)
{
    float target = INJ_GET_TARGET_EOI();

    float err = angle_diff_deg(target, real_eoi);  // wrap-aware

    // reject outliers, also clear integral so we don't carry stale windup
    if (err > 30.0f || err < -30.0f) {
    	return;
    }
    cummulative_eoi_error += err;
}
void TW_CALC_INJ_EOI(){
	if(TW_RPM_SENSOR_STATE && !blankInj){
		if(!(par % filter)){
			TW_UPDATE_EOI_COMP(); //igual haciendo promedios se subsana algo

		}else{//este else es nuevo
			TW_UPDATE_EOI_ERROR();
		}
		par++;
		TM_UPDATE_END_DELAY_CORRECTION();
	}
	correction_eoi = s_eoi_corr_deg;
	eoi_eval_pending = 0;
}


extern float T_ein;
extern float dPHI1_bip;

void TW_DEFER_TEIN_VAL(){
	IC_NOM_BIP = IC_TEIN - TEIN_NOMINAL * 16; //16 prescaler
	hasTeinDetEndedFlag = 1;
	phi1_eval_pending = 1;
}

extern float dPHI1_bip_nom_2;
extern float dPHI1_bip_2;
void TW_CALC_BIP_ANGLE(){
	IC_TEIN = IC_INJ + T_ein*16 - TEIN_READING_OFFSET*16; //16 prescaler

	uint8_t index = 0;
	for(uint8_t i = 1; i < TW_PERCYL_TEETH; i++){
		if(edgeBuf[i].ic < IC_TEIN && IC_TEIN <= edgeBuf[i+1].ic){ //si lo llamo antes del siguiente diente nunca llega
			index = i;
			break;
		}
	}
	if(!index){
		return;
	}
	float newphi1 = TM_INTEGRATE_ANGLE_FROM_NEAREST_TOOTH(IC_TEIN, 0); //maybe it gets calculated fromtooth behing
	if(newphi1 > 1){
		actual_phi1 = newphi1;
		phi1_eval_pending = 0;
		real_bip_updated = 1;
	}
	/*if(actual_phi1 < 1){
		index = 0;
	}*/
	//+TM_INTEGRATE_ANGLE_FROM_NEAREST_TOOTH(IC_TEIN, 0)
	//actual_phi1 = TM_GET_REAL_BIP_ANGLE(IC_TEIN);

}

float fclamp(float value, float min, float max) {
    const float t = value < min ? min : value;
    return t > max ? max : t;
}
void TW_Reset_Pending_Tasks(){
	boi_eval_pending = 0;
	eoi_eval_pending = 0;
	phi1_eval_pending = 0;
	ckp_process_pending = 0;
}

void TW_Service(){

	if(real_boi_pending){
		TW_CALC_REAL_BETA();
	}
	if(real_eoi_pending){
		TW_CALC_REAL_EOI();
	}
	if(boi_eval_pending && hasInjectionEnded){ //&& startedEngine
		TW_CALC_INJ_BOI();
	}
	//antes estaba en vez de real eoi, con eoi eval, o sea apenas recibia eoi, pero sin actualizarlo??
	if(eoi_eval_pending && hasInjectionEnded & real_eoi_updated){//&& startedEngine
		TW_CALC_INJ_EOI();
	}

	if(phi1_eval_pending && hasTeinDetEnded){
		TW_CALC_BIP_ANGLE();
	}

	if(real_eoi_updated && real_bip_updated && inj_rpm_pending){
		TM_UPDATE_INJECTION_RPM_AND_ALPHA(real_eoi, actual_phi1, IC_EOI, IC_TEIN);
		inj_rpm_pending = 0;
	}
	if(ckp_process_pending){
		TW_CALC_FBKW_FEEDBACK();
	}

}