/*
 * timing_manager.c
 *
 *  Created on: Oct 3, 2025
 *      Author: herli
 */
#include "timing_manager.h"
#include "toothed_wheel.h"
#include "tein_detection.h"
#include "injection.h"

#include "main.h"

volatile EdgeSample edgeBuf[TW_PERCYL_TEETH+1];

/*#define MT_BUF_SZ  16

typedef struct { float rpm; float acc; } MTSample;
static MTSample mtBuf[MT_BUF_SZ];
static uint8_t  mtHead = 0; // points to newest*/


float last_INJECTING_RPM = 0.0;
float last_non_Inj_RPM = 0.0;

float last_INJECTING_ALPHA = 0.0;

float last_accel = 0.0;

//static inline void MT_Push(float rpm_now, float sector_deg);
//static inline float rpm_acc_from_sector(float rpm_prev, float rpm_now, float sector_deg);
static inline float MT_AccelUpdate(float rpm_now);
static inline void MT_AccelReset();

static inline float PredictRPM(float rpm0, float accel_rpms, float dt_s);
static inline float TimeToAngleDeg(float rpm0, float accel_rpms, float dtheta_deg);
static inline float _rpm_from_dt(float deg, uint32_t dt_ticks);

static inline uint32_t ticks_diff(uint32_t newer, uint32_t older);
static inline uint32_t ticks_diff(uint32_t newer, uint32_t older)
{
    return (newer >= older) ? (newer - older) : (newer + (0xFFFFFFFFu - older) + 1u);
}

static inline uint8_t TM_GET_IN_BUFFER_ID(uint32_t C){
	uint8_t index = 0;
	for(uint8_t i = 1; i < TW_PERCYL_TEETH; i++){
		if(edgeBuf[i].ic < C && C < edgeBuf[i+1].ic){
			index = i;
			break;
		}
	}
	return index;
}

void UpdateEdgeBuffer(uint32_t tickNew, float rpm_est, uint8_t tooth, uint8_t isInj){
	if(tooth > TW_PERCYL_TEETH){
		return;
	}
	EdgeSample s;
	s.ic   	= tickNew;
	s.rpm   = rpm_est;
	s.isInj   = isInj;

	//uint32_t accel_Difference = tickNew >= edgeBuf[tooth].ic ? tickNew-edgeBuf[tooth].ic : tickNew-(edgeBuf[tooth].ic-0xffffffff);

	//float time = accel_Difference / refClock;
	float time = ticks_diff(tickNew, edgeBuf[tooth].ic) / refClock;

	//s.accel = USTODEG*(rpm_est - edgeBuf[tooth].rpm)/time; //in º/us^^2
	s.accel = (rpm_est - edgeBuf[tooth].rpm)/time; //in º/º^^2

	s.new   = 1;

	edgeBuf[tooth] = s;
}
void ClearEdgeBuffer(){
	EdgeSample s;
	s.ic   	= 0;
	s.rpm   = 0;
	s.accel = 0;
	s.isInj   = 0;
	s.new   = 0;

	for(int i = 0; i<TW_PERCYL_TEETH; i++){
		edgeBuf[i] = s;
	}
}
void MakeEdgeBufferOld(){
	for(int i = 0; i<TW_PERCYL_TEETH; i++){
		edgeBuf[i].new = 0;
	}
}
float TM_GET_RPM_FROM_NEAREST_TOOTH(uint32_t C_Ticks, uint8_t fw){
	uint8_t idx = TM_GET_IN_BUFFER_ID(C_Ticks);
	if(!idx){return 0;}
	return edgeBuf[idx + !fw].rpm;
}

float TM_TIME_FROM_NEAREST_TOOTH(uint32_t C_Ticks, uint8_t fw){
	uint8_t idx = TM_GET_IN_BUFFER_ID(C_Ticks);
	if(!idx){return 0;}
	idx += 1 - fw;

	uint32_t C_Difference = C_Ticks >= edgeBuf[idx].ic ? C_Ticks-edgeBuf[idx].ic : C_Ticks-(edgeBuf[idx].ic-0xffffffff);
	return C_Difference / 16;
}

static inline float _angle_from_dt(float rpm, uint32_t dt_ticks)
{
    // deg = 6 * rpm * seconds ; seconds = dt_ticks / refClock
    return 6.0f * rpm * ((float)dt_ticks / (float)refClock);
}
static inline float _rpm_from_dt(float deg, uint32_t dt_ticks)
{
    // deg = 6 * rpm * seconds ; seconds = dt_ticks / refClock
    return deg / (6.0f * ((float)dt_ticks / (float)refClock));

}

float TM_INTEGRATE_ANGLE_FROM_NEAREST_TOOTH(uint32_t C_Ticks, uint8_t fw)
{
    float int_angle = 0.0f;

    uint8_t idx = TM_GET_IN_BUFFER_ID(C_Ticks);
    if (!idx) return 0.0f;

    else if (fw == 1) {
        // from this tooth edge up to C_Ticks
        uint32_t dt = (uint32_t)(C_Ticks - edgeBuf[idx].ic);  // wrap-safe
        float    rpm = edgeBuf[idx].rpm;
        float    ddeg = _angle_from_dt(rpm, dt);

        // angle = tooth base + fraction forward in this tooth
        int_angle = (float)idx * TW_TOOTH_ALPHA + ddeg;
    } else {
        // from next tooth edge down to C_Ticks
        uint8_t  nx  = (uint8_t)(idx + 1);
        if (nx > TW_PERCYL_TEETH) return 0.0f;

        uint32_t dt  = (uint32_t)(edgeBuf[nx].ic - C_Ticks);  // wrap-safe
        float    rpm = edgeBuf[nx].rpm;                       // rpm of the tooth we are "in"
        float    ddeg = _angle_from_dt(rpm, dt);

        // angle = next tooth base minus the fraction we rewound
        int_angle = (float)nx * TW_TOOTH_ALPHA - ddeg;
    }

    return int_angle;
}

float INTEGRATE_ANGLE_FROM_REFERENCE(float time, float reference, uint8_t fw){
    float int_angle = 0.0f;
    float rem_time = time;

    uint8_t idx = reference / TW_TOOTH_ALPHA;
    float off_angle = reference - idx * TW_TOOTH_ALPHA;

    if (!idx) return 0.0f;

    /*else if (fw == 1) {
    	for(int i = idx; i < TW_TOOTH_ALPHA; i--){
			float dt_tooth = ticks_diff(edgeBuf[i].ic, edgeBuf[i-1].ic)/16;
    		if(i == nx){ //si es el primer valor (va a ser parcial)
                float alpha_dt = off_angle / TW_TOOTH_ALPHA * dt_tooth;  // wrap-safe
                if(alpha_dt > rem_time){
                	float diff = alpha_dt - rem_time;
                	int_angle += off_angle - TW_TOOTH_ALPHA * diff/dt_tooth;
					rem_time = 0;
                }else{
                	int_angle += off_angle;
                	rem_time -= alpha_dt;
                }
    		}else if(rem_time > dt_tooth){
            	int_angle += TW_TOOTH_ALPHA;
            	rem_time -= dt_tooth;
    		}else{
    			int_angle += TW_TOOTH_ALPHA * rem_time/dt_tooth;
    			rem_time = 0;
    		}

    		if(rem_time < 0.5f){
    			break;
    		}
    	}*/

    //} else {
        // from next tooth edge down to C_Ticks
        uint8_t  nx  = (uint8_t)(idx + 1);
        if (nx > TW_PERCYL_TEETH) return 0.0f;

    	for(int i = nx; i > 1; i--){
			float dt_tooth = ticks_diff(edgeBuf[i].ic, edgeBuf[i-1].ic)/16;
    		if(i == nx){ //si es el primer valor (va a ser parcial)
                float alpha_dt = off_angle / TW_TOOTH_ALPHA * dt_tooth;  // wrap-safe
                if(alpha_dt > rem_time){
                	float diff = alpha_dt - rem_time;
                	int_angle += off_angle - TW_TOOTH_ALPHA * diff/dt_tooth;
					rem_time = 0;
                }else{
                	int_angle += off_angle;
                	rem_time -= alpha_dt;
                }
    		}else if(rem_time > dt_tooth){
            	int_angle += TW_TOOTH_ALPHA;
            	rem_time -= dt_tooth;
    		}else{
    			int_angle += TW_TOOTH_ALPHA * rem_time/dt_tooth;
    			rem_time = 0;
    		}

    		if(rem_time < 0.5f){
    			break;
    		}
    	}

    //}

    return int_angle;
}
float TM_GET_REAL_BIP_ANGLE(uint32_t C_Ticks)
{
    float int_angle = 0.0f;

    uint8_t idx = TM_GET_IN_BUFFER_ID(C_Ticks);
    if (!idx) return 0.0f;

    uint8_t nx = (uint8_t)(idx + 1);
    if (nx > TW_PERCYL_TEETH) return 0.0f;

    uint32_t t0_ticks = edgeBuf[idx].ic;
    uint32_t t1_ticks = edgeBuf[nx].ic;
    uint32_t Dt_ticks = ticks_diff(t1_ticks, t0_ticks);

	float Dt = Dt_ticks/refClock;
	// Base angular velocities (deg/s)
	float w0 = edgeBuf[idx].rpm*6.0f;
	float w1 = edgeBuf[nx].rpm*6.0f;

	// Scale both w0,w1 so the *area* over the full tooth equals TW_TOOTH_ALPHA
	// Integral of linear ω over [0,Dt] is 0.5*(w0 + w1)*Dt
	float denom = 0.5f * (w0 + w1) * Dt;
	float s = (denom != 0.0f) ? (TW_TOOTH_ALPHA / denom) : 0.0f;
	float w0s = w0 * s;
	float w1s = w1 * s;

	float a_s = (w1s - w0s) / Dt;

	//if (fw == 1) {
		// Forward from t0 to C_Ticks
		uint32_t dt_ticks = ticks_diff(C_Ticks, t0_ticks);
		if (dt_ticks > Dt_ticks) dt_ticks = Dt_ticks; // clamp inside tooth
		float dt = (dt_ticks)/refClock;

		float theta_local = w0s * dt + 0.5f * a_s * dt * dt;  // deg
		int_angle = (float)idx * TW_TOOTH_ALPHA + theta_local;
	/*} else {
		// Backward from t1 down to C_Ticks
		uint32_t dt_ticks = u32_diff(t1_ticks, C_Ticks);
		if (dt_ticks > Dt_ticks) dt_ticks = Dt_ticks; // clamp
		float dt = TICKS_TO_SECONDS(dt_ticks);

		// Integrate backward: area from (t1 - dt) to t1 equals using end-velocity w1s
		// Symmetry gives: θ_local = w1s*dt - 0.5*a_s*dt^2
		float theta_local = w1s * dt - 0.5f * a_s * dt * dt;  // deg
		int_angle = (float)nx * TW_TOOTH_ALPHA - theta_local;
	}*/

	// Optional: keep in [0, total_per_cycle) if you need it
	return int_angle;
}

float TM_INTEGRATE_ANGLE_FROM_REFERENCE(uint32_t C_time, float refAngle, uint8_t fw){
	/*float int_angle = 0.0;
	uint8_t idx = TM_GET_IN_BUFFER_ID(C_Ticks);
	if(!idx){
		return int_angle;
	}
	idx = fw ? idx : idx + 1;
	if(idx > TW_PERCYL_TEETH){
		return int_angle;
	}*/

	float int_angle = refAngle;
	uint8_t id_ref = refAngle / TW_TOOTH_ALPHA;
	id_ref = fw ? id_ref : id_ref + 1;
	while(C_time){
		id_ref = int_angle / TW_TOOTH_ALPHA + !fw*1;
		int_angle += -1*!fw*edgeBuf[id_ref].rpm*USTODEG;

		C_time--;
	}

	/*if(fw){
		uint32_t C_Difference = C_Ticks >= edgeBuf[idx].ic ? C_Ticks-edgeBuf[idx].ic : C_Ticks-(edgeBuf[idx].ic-0xffffffff);
		int_angle = idx*TW_TOOTH_ALPHA + 6.0*edgeBuf[idx].rpm*C_Difference/refClock;
	}else{
		uint32_t C_Difference = edgeBuf[idx].ic >= C_Ticks ? edgeBuf[idx].ic-C_Ticks : edgeBuf[idx].ic-(C_Ticks-0xffffffff);
		int_angle = idx*TW_TOOTH_ALPHA - 6.0*edgeBuf[idx].rpm*C_Difference/refClock;
	}*/

	//int_angle = fw ? int_angle - refAngle : int_angle - refAngle;
	return int_angle;
}

float dalpha;
float TM_INTEGRATE_TIME_FROM_REFERENCE(float C_Angle, uint8_t teeth_f){
	float int_time = 0.0;

	uint8_t idx = C_Angle/TW_TOOTH_ALPHA; //empiezo diente despues de phi1

	if(idx < teeth_f){
		return 0.0f;
	}
	float C_Difference = C_Angle - idx*TW_TOOTH_ALPHA;


	int_time = C_Difference/(edgeBuf[idx+1].rpm*USTODEG);

	for(int i = idx; i > teeth_f; i--){
		if(edgeBuf[i].rpm < 20){
			break;
		}
		int_time += TW_TOOTH_ALPHA / (edgeBuf[i].rpm*USTODEG);
	}

	return int_time;
}
uint8_t accelbipenabled = 0;
float TM_INTEGRATE_TIME_BIP_PREDICTIVE(float C_Angle, uint8_t teeth_i){
	float int_time = 0.0;

	uint8_t idx = C_Angle/TW_TOOTH_ALPHA; //empiezo diente despues de phi1
	if(idx < teeth_i){
		return 0.0f;
	}
	float Remaining_Angle = C_Angle - idx*TW_TOOTH_ALPHA;

	uint8_t lastUpdatedTeeth = 0;
	for(int i = 1; i < idx + 1; i++){
		if(edgeBuf[i].new){
			lastUpdatedTeeth = i;
			break;
		}
	}
	if(!lastUpdatedTeeth || lastUpdatedTeeth > TW_PERCYL_TEETH){
		return TM_INTEGRATE_TIME_BIP(C_Angle, teeth_i);
	}
	float acc_factor_f = edgeBuf[lastUpdatedTeeth].rpm / edgeBuf[idx-1].rpm; //cuanto cambio desde la ultima
	float acc_factor_s = edgeBuf[idx+1].rpm / edgeBuf[idx-1].rpm; //cuanto se desacelera en la inyeccion

	acc_factor_f = accelbipenabled ? acc_factor_f : 1.0;
	acc_factor_s = accelbipenabled ? acc_factor_s : 1.0;

	float rpm = edgeBuf[lastUpdatedTeeth].rpm * acc_factor_s > MIN_RPM ? edgeBuf[lastUpdatedTeeth].rpm * acc_factor_s : TEETH_RPM;
	int_time = Remaining_Angle / (rpm*USTODEG);

	for(int i = teeth_i; i < idx; i++){
		if(edgeBuf[i+1].rpm < MIN_RPM){
			rpm = TEETH_RPM;
		}else{
			rpm = edgeBuf[i+1].isInj ? edgeBuf[lastUpdatedTeeth].rpm * acc_factor_s : edgeBuf[i+1].rpm * acc_factor_f;
		}
		int_time += TW_TOOTH_ALPHA / (rpm*USTODEG);
	}
	return int_time;
}
float correction_factor;
float bip_acc_K = 0;
float TM_INTEGRATE_TIME_BIP(float C_Angle, uint8_t teeth_i){
	float int_time = 0.0;

	uint8_t idx = C_Angle/TW_TOOTH_ALPHA; //empiezo diente despues de phi1
	if(idx < teeth_i){
		return 0.0f;
	}
	float Remaining_Angle = C_Angle - idx*TW_TOOTH_ALPHA;

	//float rpm = edgeBuf[idx+1].rpm > MIN_RPM ? edgeBuf[idx+1].rpm : TEETH_RPM;
	//int_time = Remaining_Angle / (rpm*USTODEG);
	int_time = Remaining_Angle/TW_TOOTH_ALPHA * ticks_diff(edgeBuf[idx+1].ic, edgeBuf[idx].ic)/16;

	for(int i = teeth_i; i < idx; i++){
		int_time += ticks_diff(edgeBuf[i+1].ic, edgeBuf[i].ic)/16;
	}

	/* acceleration factor */
	float dt_to_bip = TimeToAngleDeg(MT_RPM, last_accel, C_Angle);
	float rpm_at_bip = PredictRPM(MT_RPM, last_accel, dt_to_bip);

	correction_factor = fclamp(MT_RPM/rpm_at_bip,1,1.5);


	int_time *= 1 + (correction_factor-1)*bip_acc_K;

	return int_time;
}
/*float TM_INTEGRATE_TIME_BIP(float C_Angle, uint8_t teeth_i){
	float int_time = 0.0;

	uint8_t idx = C_Angle/TW_TOOTH_ALPHA; //empiezo diente despues de phi1
	if(idx < teeth_i){
		return 0.0f;
	}
	float Remaining_Angle = C_Angle - idx*TW_TOOTH_ALPHA;

	float rpm = edgeBuf[idx+1].rpm > MIN_RPM ? edgeBuf[idx+1].rpm : TEETH_RPM;
	int_time = Remaining_Angle / (rpm*USTODEG);

	for(int i = teeth_i; i < idx; i++){
		rpm = edgeBuf[i+1].rpm > MIN_RPM ? edgeBuf[i+1].rpm : TEETH_RPM;
		int_time += TW_TOOTH_ALPHA / (rpm*USTODEG);
	}

	return int_time;
}*/

uint8_t TM_GET_TRIGGER_TEETH_FROM_REFERENCE_AND_TIME(float C_Angle, float time){
	float int_time = 0.0;

	uint8_t idx = C_Angle/TW_TOOTH_ALPHA; //empiezo diente despues de phi1

	if(idx > TW_PERCYL_TEETH){
		return 0;
	}
	float C_Difference = C_Angle - idx*TW_TOOTH_ALPHA;


	int_time = C_Difference/(edgeBuf[idx+1].rpm*USTODEG);

	uint8_t idx_trigger = 0;
	for(int i = idx; i > 0; i--){
		if(edgeBuf[i].rpm < 20){
			break;
		}
		int_time += TW_TOOTH_ALPHA / (edgeBuf[i].rpm*USTODEG);
		if(int_time > time){
			return i; //seria -1 pero luego le restamos, asi puede encontrar el 1 tmb;
			break;
		}
	}
	return 0;
}
uint8_t dtein_mode = 13;
float dtein_rpm = 0;
float extra = 0;
float extra2 = 00;

extern float eq_rpm;
float rpmdif = 100;
int negative = 0;
float TM_GET_PHIAD_dTEIN(float EOI_Angle, float MT_RPM){
	float d_alpha = 0.0f;

	uint8_t idx = EOI_Angle/TW_TOOTH_ALPHA; //empiezo diente despues de phi1
	float Remaining_Angle = EOI_Angle - idx*TW_TOOTH_ALPHA;


	float rpm0   = MT_RPM;   // your base RPM
	float accel  = last_accel;           // RPM/s from your filter


	if(accel > -200 && accel < 200){
		// A) Adjust a time computed from an angle (e.g., 31° to EOI)
		float dt_to_eoi = TimeToAngleDeg(rpm0, accel, EOI_Angle);
		// B) Predict the RPM at that firing time, if you need it
		float rpm_at_eoi = PredictRPM(rpm0, accel, dt_to_eoi);

		//d_alpha = - (TEIN_NOMINAL+!T_ein_status*PH_PEAK_DEF)*USTODEG*rpm_at_eoi;
		d_alpha = - (TEIN_NOMINAL+!T_ein_status*PH_PEAK_DEF)*USTODEG*MT_RPM;

	}else{
		d_alpha = - (TEIN_NOMINAL+!T_ein_status*PH_PEAK_DEF)*USTODEG*MT_RPM;
	}
	//no compensations
	float rpm = 0;

    float rpm_inj_sum = 0.0f;
    uint8_t n_inj = 0;

	switch(dtein_mode){ //something between 1 and 2, but non are working properly
		case 0:
			rpm = MT_RPM;
			break;
		case 1:
			rpm = edgeBuf[idx].rpm;
			break;
		case 2:
			rpm = TEETH_RPM;
			break;
		case 3:
		    // --- 2) Build avg RPM pools (slowed vs free) ---
		    for (int i = 0; i < TW_PERCYL_TEETH; i++) {
		        float r = edgeBuf[i].rpm;
		        if (r < 20.0f) continue;
		        if (edgeBuf[i].isInj) { rpm_inj_sum += r; n_inj++; }
		    }
		    rpm  = n_inj ? (rpm_inj_sum  / n_inj)  : TEETH_RPM;
			break;
		case 4:
		    for (int i = 1; i < TW_PERCYL_TEETH+1; i++) {
		        float r = edgeBuf[i].rpm;
		        if (r < 20.0f) continue;
		        rpm_inj_sum += r;
		        n_inj++;
		    }
		    //rpm  = n_inj ? (rpm_inj_sum  / n_inj)  : TEETH_RPM;
		    rpm  = n_inj ? ((rpm_inj_sum + extra * edgeBuf[TW_PERCYL_TEETH].rpm)  / (n_inj + extra))  : TEETH_RPM;
			break;
		case 5:
		    for (int i = 1; i < TW_PERCYL_TEETH; i++) {
		        float r = edgeBuf[i].rpm;
		        if (r < 20.0f) continue;
		        rpm_inj_sum += r; n_inj++;
		        if (edgeBuf[i].isInj) { break; }
		    }
		    rpm  = n_inj ? (rpm_inj_sum  / n_inj)  : TEETH_RPM;
			break;
		case 6:
		    for (int i = 1; i < TW_PERCYL_TEETH; i++) {
		        float r = edgeBuf[i].rpm;
		        if (r < 20.0f) continue;
		        rpm_inj_sum += r; n_inj++;
		        if (edgeBuf[i].isInj && !edgeBuf[i+1].isInj) { break; }
		    }
		    rpm  = n_inj ? (rpm_inj_sum  / n_inj)  : TEETH_RPM;
			break;
		case 7:
		    // --- 2) Build avg RPM pools (slowed vs free) ---
		    uint8_t flag = 0;
		    for (int i = 1; i < TW_PERCYL_TEETH; i++) {
		        float r = edgeBuf[i].rpm;
		        if (r < 20.0f) continue;
		        if (edgeBuf[i].isInj) { flag = 1; }
		        if(flag){
		        	rpm_inj_sum += r; n_inj++;
		        }
		    }
		    rpm  = n_inj ? (rpm_inj_sum  / n_inj)  : TEETH_RPM;
			break;
		case 8:
		    // --- 2) Build avg extended RPM pools (slowed vs free) ---
		    for (int i = 0; i < TW_PERCYL_TEETH; i++) {
		        float r = edgeBuf[i].rpm;
		        if (r < 20.0f) continue;
		        if (edgeBuf[i].isInj){
		        	rpm_inj_sum += r; n_inj++;
		        }else if(edgeBuf[i-1].isInj) { rpm_inj_sum += r; n_inj++; }
		    }
		    rpm  = n_inj ? (rpm_inj_sum  / n_inj)  : TEETH_RPM;
			break;
		case 9:
		    for (int i = 1; i < idx + 2; i++) {
		        float r = edgeBuf[i].rpm;
		        if (r < 20.0f) continue;
		        if (edgeBuf[i].isInj){
		        	rpm_inj_sum += r; n_inj++;
		        }else if(edgeBuf[i-1].isInj) { rpm_inj_sum += r; n_inj++; }
		    }
		    rpm  = n_inj ? (rpm_inj_sum  / n_inj)  : TEETH_RPM;
			break;
		case 10:
			uint32_t ticks_eoi = edgeBuf[idx].ic + Remaining_Angle/TW_TOOTH_ALPHA * ticks_diff(edgeBuf[idx+1].ic, edgeBuf[idx].ic) - TEIN_NOMINAL*16;
			uint8_t id_end = TM_GET_IN_BUFFER_ID(ticks_eoi);
			if(id_end){
				return TM_INTEGRATE_ANGLE_FROM_NEAREST_TOOTH(ticks_eoi, 0)- EOI_Angle;
				//float angle = 1.0f * TW_TOOTH_ALPHA * ticks_diff(ticks_eoi, edgeBuf[id_end].ic) / ticks_diff(edgeBuf[id_end+1].ic, edgeBuf[id_end].ic);
				//return -(angle + TW_TOOTH_ALPHA*(idx - id_end - 1) + Remaining_Angle);
			}
			//uint32_t ic_nom_eoi = IC_EOI + TEIN_NOMINAL*16;
			break;
		case 11:
		    for (int i = 1; i < TW_PERCYL_TEETH+1; i++) {
		        float r = edgeBuf[i].rpm;
		        if (r < 20.0f) continue;
		        rpm_inj_sum += r;
		        n_inj++;
		    }
		    rpm  = n_inj ? (rpm_inj_sum  / n_inj)  : TEETH_RPM;

			float dt_to_eoi = TimeToAngleDeg(MT_RPM, last_accel, EOI_Angle); //tendria q ser mt rpm no las last injecting rpm.
			// B) Predict the RPM at that firing time, if you need it
			float rpm_next = PredictRPM(rpm0, last_accel, dt_to_eoi);

			rpm *= rpm_next / MT_RPM;
			break;
		case 12:
			// A) Adjust a time computed from an angle (e.g., 31° to EOI)
			float dt_to_eoi2 = TimeToAngleDeg(MT_RPM, accel, 360/CYLINDERS);//EOI_Angle
			// B) Predict the RPM at that firing time, if you need it
			float rpm_at_eoi = fclamp(PredictRPM(MT_RPM, last_accel, dt_to_eoi2),MT_RPM-rpmdif,MT_RPM+rpmdif);
			//if((rpm_at_eoi - MT_RPM)*last_accel > 0){
				rpm = rpm_at_eoi;
			break;

			//}else{
			//	rpm = MT_RPM;
			//}
		case 13:
		    for (int i = 1; i < TW_PERCYL_TEETH+1; i++) {
		        float r = edgeBuf[i].rpm;
		        if (r < 20.0f) continue;
		        rpm_inj_sum += r;
		        n_inj++;
		    }
		    //rpm  = n_inj ? (rpm_inj_sum  / n_inj)  : TEETH_RPM;
		    rpm  = n_inj ? ((rpm_inj_sum + extra * edgeBuf[TW_PERCYL_TEETH].rpm)  / (n_inj + extra))  : TEETH_RPM;
			// A) Adjust a time computed from an angle (e.g., 31° to EOI)
			float dt_to_eoi3 = TimeToAngleDeg(rpm, accel, extra2);//EOI_Angle //360/CYLINDERS
			if (negative){ dt_to_eoi3 *= -1;}
			// B) Predict the RPM at that firing time, if you need it
			float rpm_at_eoi2 = fclamp(PredictRPM(rpm, last_accel, dt_to_eoi3),rpm-rpmdif,rpm+rpmdif);
			//if((rpm_at_eoi - MT_RPM)*last_accel > 0){
			rpm = rpm_at_eoi2;
			//}else{
			//	rpm = MT_RPM;
			//}
			break;

		default:
			break;
	}
	dtein_rpm = rpm;
	d_alpha = - (TEIN_NOMINAL)*USTODEG*dtein_rpm; //bien, usa las slowed -!T_ein_status*PH_PEAK_DEF*USTODEG*TEETH_RPM
	if(!T_ein_status){
		//d_alpha -= INTEGRATE_ANGLE_FROM_REFERENCE(PH_PEAK_DEF, 	EOI_Angle + d_alpha, 0);
		d_alpha -= PH_PEAK_DEF*USTODEG*dtein_rpm ;

	}



	return d_alpha;
}

uint8_t current_inj_rpm_available = 0;
float inj_accel;

//UNUSED
float TM_INTEGRATE_TIME_FROM_REFERENCE_Forward(float C_Angle, uint8_t teeth_i, float InitInj){

	float int_time = 0.0;

	uint8_t idx = C_Angle/TW_TOOTH_ALPHA; //empiezo diente despues de phi1
	if(idx < teeth_i){
		return 0.0f;
	}
	float Remaining_Angle = C_Angle - idx*TW_TOOTH_ALPHA;

	/* calcular la current accel con MT_RPM hasta teeth rpm (solo vale antes de empezar a inyectar)  y comparar signo*/

    /*uint32_t dt = (uint32_t)(edgeBuf[trigger_teeth].ic - edgeBuf[1].ic);  // wrap-safe

	float rpm_now = _rpm_from_dt(trigger_teeth * TW_TOOTH_ALPHA, dt);

	float seconds = dt/refClock;
    float acc_raw = (rpm_now - edgeBuf[1].rpm) / (seconds);*/

	/*if(acc_raw * accel < 0){
		accel = acc_raw;
	}*/
	float calc_rpm = 0;
	/*if(!current_inj_rpm_available || current_inj_rpm_available < teeth_i){
		for(int i = 1; i < TW_PERCYL_TEETH; i++){
			if(!edgeBuf[i].isInj){
				continue;
			}
			if(edgeBuf[i].rpm < 20 || !edgeBuf[i].new){
				break;
			}
			current_inj_rpm_available = i;
			calc_rpm = edgeBuf[i].rpm;
		}
		if(!current_inj_rpm_available){
			float rpm0   = last_INJECTING_RPM;   // your base RPM
			float accel  = last_accel;           // RPM/s from your filter

			// A) Adjust a time computed from an angle (e.g., 31° to EOI)
			float dt_to_eoi = TimeToAngleDeg(MT_RPM, inj_accel, C_Angle); //tendria q ser mt rpm no las last injecting rpm.
			// con last inj rpm, que es mas lento, da dt mayor, y tambien unas rpm menor y dt todavia mayor...

			// B) Predict the RPM at that firing time, if you need it
			float rpm_at_eoi = PredictRPM(rpm0, inj_accel, dt_to_eoi);

			calc_rpm = last_INJECTING_RPM;
		}
	}else{
		calc_rpm = edgeBuf[current_inj_rpm_available].rpm;
	}*/
	calc_rpm = TEETH_RPM;

	int_time += Remaining_Angle / (calc_rpm*USTODEG);
	//int_time += Remaining_Angle / (edgeBuf[idx].rpm*USTODEG);

	for(int i = teeth_i; i < idx; i++){
		if(edgeBuf[i].rpm < 20){
			break;
		}
		if(edgeBuf[i].isInj){

		}
		int_time += TW_TOOTH_ALPHA / (calc_rpm*USTODEG);
		//int_time += TW_TOOTH_ALPHA / (edgeBuf[i].rpm*USTODEG);

	}
	return int_time;
}
extern float target_eoi;
extern float real_eoi;

float TM_INTEGRATE_TIME_TO_EOI(float C_Angle, uint8_t teeth_i){

	float int_time = 0.0;

	uint8_t idx = C_Angle/TW_TOOTH_ALPHA; //empiezo diente despues de phi1
	if(idx < teeth_i){
		return 0.0f;
	}
	float Remaining_Angle = C_Angle - idx*TW_TOOTH_ALPHA;


	/*uint8_t id = 0;
    for (int i = 1; i < TW_PERCYL_TEETH; i++) {
        if (edgeBuf[i].rpm < 20.0f) break;
        if (!edgeBuf[i].isInj) continue;
        if(!edgeBuf[i+1].isInj){
        	id = i;
        }
    }
    if(!id){id = teeth_i;}
	uint32_t t_diff_fast = ticks_diff(edgeBuf[2].ic, edgeBuf[1].ic);
	uint32_t t_diff_slow = ticks_diff(edgeBuf[id].ic, edgeBuf[id-1].ic);*/

	uint32_t t_diff_fast = ticks_diff(edgeBuf[idx+2].ic, edgeBuf[idx+1].ic);
	int_time = Remaining_Angle/TW_TOOTH_ALPHA * ticks_diff(edgeBuf[idx+1].ic, edgeBuf[idx].ic)/16;
	//int_time = Remaining_Angle/TW_TOOTH_ALPHA * t_diff_fast/16;

	for(int i = teeth_i; i < idx; i++){
		int_time += ticks_diff(edgeBuf[i+1].ic, edgeBuf[i].ic)/16;
		//int_time += t_diff_fast/16;
	}
	/*float dt_to_eoi = TimeToAngleDeg(MT_RPM, last_accel, C_Angle); //tendria q ser mt rpm no las last injecting rpm.
	// B) Predict the RPM at that firing time, if you need it
	float rpm_next = PredictRPM(rpm0, last_accel, dt_to_eoi);

	int_time *= rpm_next / MT_RPM;*/

	return int_time;
}
float lpf_rpm = 1;
void TM_UPDATE_INJECTION_RPM_AND_ALPHA(float EOI_angle, float BIP_angle, uint32_t IC_EOI, uint32_t IC_BIP){
    uint8_t idx2 = BIP_angle / TW_TOOTH_ALPHA + 1; // target tooth index

    float eq_rpm = 0.0f;
    uint8_t count = 0;

    for (int i = idx2; i < TW_PERCYL_TEETH; i++) {
        if (edgeBuf[i].rpm < 20.0f) break;
        if (!edgeBuf[i].isInj) continue;
        eq_rpm += edgeBuf[i].rpm;
        count++;
        if (!edgeBuf[i+1].isInj) {
        	eq_rpm += edgeBuf[i+1].rpm;
            count++;
        }
    }
    if (count) {
        eq_rpm /= count; // averaged slowed rpm
    } else {
        eq_rpm = edgeBuf[idx2].rpm;
    }
    //last_INJECTING_RPM = eq_rpm;

    uint8_t idx = TM_GET_IN_BUFFER_ID(IC_BIP);

	uint32_t inj_Difference = IC_EOI >= IC_BIP ? IC_EOI-IC_BIP : IC_EOI-(IC_BIP-0xffffffff);
	float alpha = fclamp(EOI_angle - BIP_angle, 0, 90);

	eq_rpm = fclamp(60.0 * refClock/inj_Difference * alpha / 360, MIN_RPM, 4000);
	//Otra forma de calcular las rpm
	//seria calcular donde esta bip real y eoi real
	//(en angulos, que ya lo tenemos, sacar el periodo en angulo y dividir entre el periodo en tiempo...

	//por ahora calculamos rpm promedio y ya

    uint8_t idx_eoi = TM_GET_IN_BUFFER_ID(IC_EOI);

    last_INJECTING_RPM = edgeBuf[idx_eoi].rpm;
	//last_INJECTING_RPM = alpha < 0.2 ? edgeBuf[idx-1].rpm : eq_rpm; voy a usarla para la aceleracion

	//last_INJECTING_RPM += lpf_rpm*(edgeBuf[idx_eoi].rpm - last_INJECTING_RPM);
	/*if(last_INJECTING_RPM < 50){
		last_INJECTING_RPM++;
	}*/
	last_INJECTING_ALPHA = alpha;

	last_non_Inj_RPM = edgeBuf[idx-2].rpm;

	current_inj_rpm_available = 0;

	//MT_Push(edgeBuf[0].rpm, TW_THEETHS * TW_TOOTH_ALPHA / CYLINDERS);

	//uint32_t acc_Difference = edgeBuf[idx-1].ic >= edgeBuf[0].ic ? edgeBuf[idx-1].ic-edgeBuf[0].ic : edgeBuf[idx-1].ic-(edgeBuf[0].ic-0xffffffff);

}
float rpm_error_accel = 0.0;
float TM_UPDATE_ACCELERATION(float last_MT_RPM, float now_MT_RPM, uint32_t dt_mt){
	//esto se da con la MT_RPM en su periodo, si usamos el siguiente enviemos last_rpm (despues de actualizar)


	// A) Adjust a time computed from an angle (e.g., 31° to EOI)
	float dt_seconds = dt_mt / refClock;
	// con last inj rpm, que es mas lento, da dt mayor, y tambien unas rpm menor y dt todavia mayor...

	// B) Predict the RPM at that firing time, if you need it
	float theory_new_rpm = PredictRPM(last_MT_RPM, last_accel, dt_seconds);

	rpm_error_accel = theory_new_rpm - now_MT_RPM;


	if(rpm_error_accel > 50 || rpm_error_accel < -50){
		MT_AccelReset();
		//last_accel = MT_AccelUpdate(now_MT_RPM); //la aceleracion
	}
	last_accel = MT_AccelUpdate(now_MT_RPM); //la aceleracion

	//last_accel = (now_MT_RPM - last_MT_RPM)/dt_seconds; //la aceleracion
	float next_RPM = last_accel * dt_seconds + now_MT_RPM;

	return next_RPM;
}

float eoi_acc_K = 0.18; //-0.2 - 0.08 0.18, last rover 0.25
float eoi_acc_K_1 = 0; //-0.2 - 0.08 0.18, last rover 0.25
float eoi_acc_K_2 = 0; //-0.2 - 0.08 0.18, last rover 0.25

float lpf_eoi_acc_k = 1; //probar,
float correction_eoi_accel = 0.0f;

float limit_1 = -1.5;
float limit_2 = 1; //en otro setting 0 mehor
float TM_GET_ACCEL_CORRECTION(float last_MT_RPM, float now_MT_RPM, float TEETH_RPM, uint8_t triggerTeeth){
	//esto se da con la MT_RPM en su periodo, si usamos el siguiente enviemos last_rpm (despues de actualizar)
    if(now_MT_RPM < 1){
    	correction_eoi_accel = 0;
    	return 0.0;
    }
	float rp_sum = now_MT_RPM + last_MT_RPM;
    float dt = (1.0f / CYLINDERS) * (120.0f /  rp_sum);

    inj_accel = (edgeBuf[triggerTeeth].rpm - last_non_Inj_RPM)/dt;
    ////inj_accel = (now_MT_RPM - last_MT_RPM)/dt;
    //inj_accel = (TEETH_RPM - last_INJECTING_RPM)/dt;
    //inj_accel = (TEETH_RPM - edgeBuf[currentTooth].rpm)/dt;
    //inj_accel = last_accel;

	float comp = fclamp(-inj_accel * eoi_acc_K / now_MT_RPM, limit_1, limit_2); //-10, 10
	/*if(inj_accel>0){
		comp = inj_accel * eoi_acc_K_1/ now_MT_RPM;
	}else{
		comp = inj_accel * eoi_acc_K_2/ now_MT_RPM;
	}*/
	//last accel
	correction_eoi_accel += lpf_eoi_acc_k*(comp-correction_eoi_accel);
	//correction_eoi_accel = fclamp(comp, 0, -2000);
	correction_eoi_accel = comp;
	return correction_eoi_accel;
}

float last_to_inj_degs= 0;
float now_to_inj_degs = 0;
float newaccel = 0;
float TM_GET_INJ_ACCEL(uint8_t currentTooth){
	uint16_t alpha = currentTooth * TW_TOOTH_ALPHA;
	uint32_t ticks = ticks_diff(IC_RPM_Val2, edgeBuf[0].ic);
	float dt = ticks / refClock;

	last_to_inj_degs = now_to_inj_degs;
	now_to_inj_degs = alpha / dt;

	newaccel = (now_to_inj_degs - last_to_inj_degs)/dt;

	float comp = fclamp(-newaccel * eoi_acc_K / MT_RPM, limit_1, limit_2); //-10, 10
	correction_eoi_accel += lpf_eoi_acc_k*(comp-correction_eoi_accel);

	return correction_eoi_accel;
}

//float bip_acc_K = 0.055;
float lpf_bip_acc_k = 1;
float correction_bip_accel = 0.0f;
float last_teeth_rpm_bip = 0.0;
void TM_PREPARE_ACCEL_CORRECTION_BIP(uint8_t triggerteeth){ //should be triggerteeth-1, and then evaluate comp 1 teeth before triggerteeth.
	uint8_t idx = triggerteeth - 1;
	last_teeth_rpm_bip = (idx && idx < TW_PERCYL_TEETH) ? edgeBuf[idx].rpm : -1;
}
float TM_UPDATE_ACCEL_CORRECTION_BIP(float last_MT_RPM, float now_MT_RPM, float TEETH_RPM){
    float rp_sum = now_MT_RPM + last_MT_RPM;
    float dt = (1.0f / CYLINDERS) * (120.0f /  rp_sum);

    float bip_accel = (TEETH_RPM - last_non_Inj_RPM)/dt;
	float comp = fclamp(-bip_accel * bip_acc_K / now_MT_RPM, -10, 10);
	correction_bip_accel += lpf_bip_acc_k*(comp-correction_bip_accel);

}
float TM_GET_ACCEL_CORRECTION_BIP(){
	return correction_bip_accel;
}

uint16_t T_delayEnd = 20;
//float lpf_end_delay = 0.2;
void TM_UPDATE_END_DELAY_CORRECTION(){
	uint32_t t = TM_TIME_FROM_NEAREST_TOOTH(IC_EOI, 1);

	if(t - (T_integrated - T_delayEnd) < 100){
		//T_delayEnd = t - (T_integrated - T_delayEnd);
		//we were using this but bugging on borders

		//T_delayEnd -= lpf_end_delay*(T_delayEnd - (t - T_integrated));

	}else{
		T_delayEnd = 20;
	}
}
uint16_t TM_GET_T_DELAY_END(){
	return T_delayEnd;
}

float TM_GET_LOCAL_ACC_TO_ANGLE(float C_Angle, uint8_t roundingMode){
	uint8_t idx = C_Angle/TW_TOOTH_ALPHA; //empiezo diente despues de phi1

	if(roundingMode > 1){
		idx++;
	}else if(roundingMode){
		idx = (C_Angle+1.5f)/TW_TOOTH_ALPHA;
	}

	uint32_t accel_Difference = edgeBuf[idx].ic >= edgeBuf[0].ic ? edgeBuf[idx].ic-edgeBuf[0].ic : edgeBuf[idx].ic-(edgeBuf[0].ic-0xffffffff);
	float now_rpm = idx * 60.0 * refClock/accel_Difference / TW_THEETHS;
	return (now_rpm - edgeBuf[0].rpm)/idx;
}

/*
// ---- Tunables ----
#define SECTOR_DEG   90.0f   // angle between your MT RPM samples (90° in your case)
#define EMA_ALPHA    0.010f   // accel smoothing [0..1]0.015
#define RPM_MIN      50.0f   // ignore junk below this
#define DT_MIN_S     (SECTOR_DEG/360.0f * (120.0f / (2.0f*10000.0f))) // ~safety floor

// ---- State ----
static float s_prev_rpm = -1.0f;   // negative => not seeded
static float s_acc_ema  = 0.0f;    // filtered accel (RPM/s)


// Call once per new missing-tooth RPM; returns latest accel in RPM/s
static inline float MT_AccelUpdate(float rpm_now)
{
    if (rpm_now < RPM_MIN) return s_acc_ema;        // ignore bogus
    if (s_prev_rpm < 0.0f) { s_prev_rpm = rpm_now; return s_acc_ema; } // seed

    // Δt from sector angle and average RPM between samples:
    float rp_sum = rpm_now + s_prev_rpm;
    if (rp_sum <= 1e-3f) rp_sum = 1e-3f;
    float dt = (SECTOR_DEG / 360.0f) * (120.0f / rp_sum);
    if (dt < DT_MIN_S) dt = DT_MIN_S;

    // raw acceleration and simple clamp for sanity
    float acc_raw = (rpm_now - s_prev_rpm) / dt;
    if (acc_raw >  8000.0f) acc_raw =  8000.0f;
    if (acc_raw < -8000.0f) acc_raw = -8000.0f;

    // smooth it
    s_acc_ema += EMA_ALPHA * (acc_raw - s_acc_ema);

    s_prev_rpm = rpm_now;
    return s_acc_ema;
}*/
// ---- Tunables ----
#define SECTOR_DEG   90.0f     // angle between MT samples
float TAU_S      =  0.1f;     // smoothing time-constant (seconds) -> adjust to taste
#define RPM_MIN      50.0f
#define ACC_ABS_LIM  8000.0f

// ---- State ----
static float s_prev_rpm = -1.0f;
static float s_acc_ema  = 0.0f;

// Call once per new missing-tooth RPM; returns filtered accel in RPM/s
static inline float MT_AccelUpdate(float rpm_now)
{
    if (rpm_now < RPM_MIN) return s_acc_ema;
    if (s_prev_rpm < 0.0f) { s_prev_rpm = rpm_now; return s_acc_ema; }

    // Δt from sector angle and avg RPM between samples
    float rp_sum = rpm_now + s_prev_rpm;
    if (rp_sum <= 1e-3f) rp_sum = 1e-3f;
    float dt = (SECTOR_DEG / 360.0f) * (120.0f / rp_sum);  // seconds

    // Raw accel (RPM/s)
    float acc_raw = (rpm_now - s_prev_rpm) / dt;
    if (acc_raw >  ACC_ABS_LIM) acc_raw =  ACC_ABS_LIM;
    if (acc_raw < -ACC_ABS_LIM) acc_raw = -ACC_ABS_LIM;

    // ---- Time-constant EMA ----
    // Exact-discrete 1st-order low-pass: alpha = dt / (tau + dt)
    float alpha = dt / (TAU_S + dt);

    // Optional: keep alpha within sane bounds (helps extreme RPMs)
    if (alpha < 0.01f) alpha = 0.01f;   // very high RPM -> don't freeze
    if (alpha > 0.6f)  alpha = 0.6f;    // very low RPM -> don't overshoot

    s_acc_ema += alpha * (acc_raw - s_acc_ema);

    s_prev_rpm = rpm_now;
    return s_acc_ema;
}
static inline void MT_AccelReset()
{
    s_acc_ema = 0;
}

// last_injecting_rpm: RPM0  (current)
// last_accel:         a     (RPM/s)

/* 1) Predict RPM after dt seconds */
static inline float PredictRPM(float rpm0, float accel_rpms, float dt_s)
{
    return rpm0 + accel_rpms * dt_s;
}

/* 2) Time to sweep dtheta_deg with constant accel.
      Returns seconds. Falls back to constant-speed if accel ~ 0. */
static inline float TimeToAngleDeg(float rpm0, float accel_rpms, float dtheta_deg)
{
    const float eps = 1e-6f;
    float w0 = rpm0 * 6.0f;          // deg/s
    float a  = accel_rpms * 6.0f;    // deg/s^2

    if (dtheta_deg <= 0.0f || w0 <= eps) return 0.0f;

    if (fabsf(a) < 1e-4f) {
        // ~constant speed
        return dtheta_deg / w0;
    }

    // Solve 0.5*a*t^2 + w0*t - dtheta = 0
    float disc = w0*w0 + 2.0f*a*dtheta_deg;
    if (disc <= 0.0f) {
        // acceleration too negative to reach angle; fallback to constant-speed
        return dtheta_deg / fmaxf(w0, eps);
    }

    float t = (-w0 + sqrtf(disc)) / a;   // physical root for forward motion
    if (t < 0.0f) {
        // numerical/edge-case guard
        return dtheta_deg / fmaxf(w0, eps);
    }
    return t;
}