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

#include <id.h>
#include "injection.h"
#include "main.h"
#include "toothed_wheel.h"
#include "tein_detection.h"
#include "fuel_map.h"
#include "temperature.h"
#include "timeouts.h"
#include "timing_manager.h"

#if HAS_PREINJECTION
#include "pre_injection.h"
#endif

static inline void TIM1_OC4_SetSafe(uint16_t ccr4_new);
float INJ_UPDATE_TARGET_EOI();

float PHI_AD = 1.875;

uint8_t angleMode = 0;

volatile uint8_t isInjecting;
volatile uint8_t hasInjected = 0;

volatile uint8_t hasInjectionEnded = 1;
volatile uint8_t hasInjectionEndedFlag = 0;

uint8_t inj_mode = 0;


uint8_t InjectionPrescaler = 16;
float dFi = 0;

uint8_t INJ_EOI_COMPENSATION = 0;

uint32_t T_on = 0; 		//Time on in microseconds
uint16_t T_peak = 600;

uint32_t T_delayFI = 150;
uint32_t T_delay = 0;

uint32_t T_hold = 0;
uint32_t T_injecting = 0;

float target_eoi = 0;

float boostmult = 0;

uint8_t triggerTeeth;

uint8_t blankInj = 0;
uint8_t cilCount;
uint8_t retardteeth = 1;

float current_Alpha = 0;	//Alpha is the angle corresponding to time-on
float current_Beta = 0.0; 	//Beta is the angle corresponding to FI delay
float corrected_Beta = 0.0;

float forceTemp = 35.6;

extern float next_MT_RPM;

int missed = 0;
void INJ_MT_TASK(){
	INJ_END();
	if(!hasInjected && !blankInj){
		missed++;
	}
	hasInjected = 0;
	hasInjectionEnded = 0;
	hasInjectionEndedFlag = 0;
	triggerTeeth = 0;
#if HAS_PREINJECTION
	PI_EVAL(RPM, ME, MEPI);
#endif
}

extern float nominal_bip_angle;

float lastEOI = 0;
extern float last_accel;
uint8_t awaitingInj = 0;

float INJ_GET_NOMINAL_EOI();
void INJ_UPDATE_ALPHA(){
    /*float rp_sum = MT_RPM + last_MT_RPM;
    float dt = (PHI1 + dFi) / 90 * (1.0f / CYLINDERS) * (120.0f /  rp_sum);
	// B) Predict the RPM at that firing time, if you need it
	float rpm_at_phiad = MT_RPM + last_accel*dt;*/

	//FM_GET_PHIAD(MT_RPM, ME, forceTemp);

	//correction_eoi_accel = TM_GET_ACCEL_CORRECTION(last_MT_RPM, MT_RPM, TEETH_RPM);
	FM_GET_PHIAD(MT_RPM, ME, forceTemp);

	float target = INJ_UPDATE_TARGET_EOI() + correction_eoi;

	//current_Alpha = PHI_AD + nominal_bip_angle - corrected_Beta;
	current_Alpha = target - (PHI1 + dFi ); //+ correction_beta

	//current_Alpha = GetAlpha(RPM, ME, T_ein, forceTemp); //PHIAD map
	if (INJ_CLOSING_MARGIN){
		current_Alpha += TIMETODEG(INJ_CLOSING_MARGIN, TEETH_RPM);
	}
	current_Alpha = current_Alpha < 0.1 ? 0 : current_Alpha;
	boostmult = BoostMultiplier(inj_mode, RPM, ME);
	current_Alpha *= boostmult;

}

void INJ_EVAL_EOI_COMPENSATION(){
	/*if(!isInjecting){
		compensatingEOI = (current_Alpha > 3 ) ? 1 : 0; //antes estaba con ton, pero si el me va bajando poco a poco, de el anterior pulso tiene ton,
	}*/
	//esto la verdad no se muy bien para que esta....
}

uint8_t margin_curr_us = 70;

float margin_deg = 0.3;
void INJ_UPDATE_BOI_MARGIN(float rpm){
	margin_deg = rpm * (T_delay + margin_curr_us)* USTODEG; //
	margin_deg = fclamp(margin_deg, 0.0001, 10);
}
void INJ_PREPARE_ONCE(){
	if(RPM < 800){
		InjectionPrescaler = 1;
		TIM1->PSC = 160-1;
	}else{
		InjectionPrescaler = 8;
		TIM1->PSC = 20-1;
	}
	INJ_PREPARE_BIP(0);
	INJ_UPDATE_ALPHA();
	INJ_UPDATE_TYPE();

}
extern float correction_boi_accel_2;
extern float s_boi_corr_deg;
void INJ_PREPARE_BIP(uint8_t teeth){
	if(teeth  < triggerTeeth || !triggerTeeth)
	{
		INJ_UPDATE_BOI_TRIGGER();
		INJ_UPDATE_ALPHA();
	}
	//correction_boi_accel_2 = TM_UPDATE_ACCEL_CORRECTION_BIP(last_MT_RPM, MT_RPM, TEETH_RPM);
	//correction_beta = s_boi_corr_deg + correction_boi_accel_2;

	INJ_UPDATE_T_Delay_phi(); //THIS should be updated more times if it is using predictive rpm

	//TM_GET_ACCEL_CORRECTION(last_MT_RPM, MT_RPM, TEETH_RPM);

}

void INJ_UPDATE_T_Delay_phi(){
	//float time = TM_INTEGRATE_TIME_BIP(PHI1 + dFi + correction_beta, triggerTeeth) + 0.5f;
	//float time = TM_INTEGRATE_TIME_BIP(PHI1 + dFi + correction_beta, triggerTeeth) + 0.5f;
	float time = (PHI1 + dFi + correction_beta - triggerTeeth * TW_TOOTH_ALPHA)/(USTODEG * TEETH_RPM)+ 0.5f;

	T_delayFI = time - T_ein + TEIN_READING_OFFSET;

}
uint16_t t1, t2, t3, t4;

extern float last_INJECTING_RPM;
float eq_rpm;
void INJ_UPDATE_TYPE(){
	if(eq_rpm < 20){ eq_rpm = TEETH_RPM;}///1.8;
	//if(eq_rpm < )

	T_injecting = DEGTOTIME(current_Alpha, eq_rpm*0.6);
	blankInj = T_injecting ? 0 : 1;

	T_peak = ((T_ein > 800 && CompensateTein) ? T_ein - 200 : PH_PEAK_DEF);
	T_hold = T_injecting ? T_ein + T_injecting - T_peak : 0;
	//T_hold = (ME > 0.04 && T_hold < 340)? 340 : T_hold; //a ver si

	T_ein_filtered = (T_hold < 260 || !CompensateTein) ? 800 : T_ein_filtered;
	compensatingEOI = T_hold < 260 ? 0 : 1; //wtf compensatingEOI

	if(!T_hold){
		T_peak = 174;
	}else if(T_hold < 260){
		T_peak = 600;
		T_hold = 270;
	}

	t1 = (T_delayFI - T_delay) * InjectionPrescaler; //
	t2 = t1 + T_peak * InjectionPrescaler;
	t3 = t1 + AcquisitionTime * InjectionPrescaler;
	t4 = blankInj ? t2 + 1 : t2 + (T_hold + INJ_CLOSING_MARGIN) * InjectionPrescaler;
}
uint8_t accel_calc=0;

void EvaluateInjection(){
	//AJUSTADO PARA 40mV histeresis de comparador, mayor histeresis mayor retraso de la inyeccion.

	if(currentTooth == triggerTeeth && !safetySHUTOFF){

		INJ_UPDATE_T_Delay_phi(); //THIS should be updated more times if it is using predictive rpm
		t1 = (T_delayFI - T_delay) * InjectionPrescaler; //

		t2 = t1 + T_peak * InjectionPrescaler;
		t3 = t1 + AcquisitionTime * InjectionPrescaler;
		t4 = blankInj ? t2 + 1 : t2 + (T_hold + INJ_CLOSING_MARGIN) * InjectionPrescaler;

		if(t1 && t2 && t4){
			if(t1 < t2 && t2 <= t4){
				TIM1->CCR1 = t1;
				TIM1->CCR2 = t2;
				TIM1->CCR3 = t3;  // End time (sampling end)
				TIM1->CCR4 = t4;
			}
		}

		TIM1->CNT = 0;
		TIM1->EGR = TIM_EGR_UG;

		TIM1->CR1 = TIM_CR1_OPM | TIM_CR1_CEN;

		cilCount++;
		cilCount%=CYLINDERS;

		hasInjected = 1;
		awaitingInj = 1;

		accel_calc = 0;

		eq_rpm = TEETH_RPM;
	}
}
void INJ_UPDATE_BOI_TRIGGER(){
	//current_Beta = GetBeta(TEETH_RPM, T_ein) + dFi;

	/*current_Beta = PHI1 + dFi - T_ein*USTODEG*TEETH_RPM;
	current_Beta = fclamp(current_Beta, 0, 90);
	corrected_Beta = current_Beta + correction_beta;*/

	//triggerTeeth = (corrected_Beta - margin_deg) / TW_TOOTH_ALPHA; //margen a ver si mejora
	uint8_t id = TM_GET_TRIGGER_TEETH_FROM_REFERENCE_AND_TIME(PHI1 + dFi + correction_beta, T_ein + T_delay + margin_curr_us);
	if(id){
		uint8_t idd = id-retardteeth;
		if(RPM < 1000){
			idd--;
		}
		triggerTeeth = idd > TW_PERCYL_TEETH ? TW_PERCYL_TEETH : idd;
		triggerTeeth = idd < 0 ? 0 : idd;

	}
	//triggerTeeth = retardteeth;
}

int mode;
float dalpha_teinstatus = 0;
extern float correction_eoi_accel;
extern float actual_phi1;
extern uint8_t real_bip_updated;

uint8_t using_real_phi1 = 0;
float dPHIAD_Tein=0;
float min_dPHIAD_Tein=-21;
float min_alpha=0.3; //1

float lpf_target = 1;
uint8_t closingTooth = 0;
float margin_eoi_tooth = 3;


float INJ_UPDATE_TARGET_EOI(){
	//INJ_UPDATE_ALPHA();
	//FM_GET_PHIAD(MT_RPM, ME, forceTemp);

	dPHIAD_Tein = fclamp(TM_GET_PHIAD_dTEIN(PHI1 + dFi + PHI_AD, MT_RPM), min_dPHIAD_Tein, 0); //+ dPHIAD_Tein + correction_beta
	//+ correction_beta
	TM_GET_ACCEL_CORRECTION(last_MT_RPM, MT_RPM, TEETH_RPM, triggerTeeth);

	//float new_target_eoi = PHI1 + dFi + dPHIAD_Tein + PHI_AD + correction_eoi_accel;//(using_real_phi1 ? actual_phi1 : PHI1 + dFi + correction_beta)
	float new_target_eoi = PHI1 + dFi  + correction_eoi_accel;//PHI_AD > 1 ?

	if(ME > 0.04){
		new_target_eoi += fclamp(dPHIAD_Tein + PHI_AD, RPM < 2100 ? min_alpha : 5.5, 90);
	}else{
		new_target_eoi += dPHIAD_Tein + PHI_AD;
	}

	//target_eoi = PHI1 + dFi + correction_beta + dPHIAD_Tein + PHI_AD + correction_eoi_accel;//(using_real_phi1 ? actual_phi1 : PHI1 + dFi + correction_beta)

    /*if(!T_ein_status){
    	float d_tein_angle = target_eoi - TM_INTEGRATE_ANGLE_FROM_NEAREST_TOOTH(IC_EOI + 16*PH_PEAK_DEF,1);
    	new_target_eoi += d_tein_angle ;
    }*/
	target_eoi += lpf_target*(new_target_eoi-target_eoi);
	//target_eoi = new_target_eoi;
	//target_eoi = new_target_eoi;
	closingTooth = (uint8_t)((target_eoi - margin_eoi_tooth)/TW_TOOTH_ALPHA);
	return target_eoi;
}
float INJ_GET_TARGET_EOI(){
	return target_eoi;
}

float INJ_GET_NOMINAL_EOI(){
	return PHI1 + dFi + PHI_AD + dPHIAD_Tein;
}
uint32_t T_integrated;

extern float real_eoi;

float lpf_fastamount = 1;
float min = -100;
float max = 100;

float K_rpm = .1;
float max_slow = 0.8;
float min_slow = 0.0;

float fastAmount = 0.0;
float fastAmount_lpf = 0.0;
float new_fastAmount = 1.0;

float fA_m = 0.5;
float fA_n = 0;

float margineq = 6;
float lastTooth = 0.0;

void INJ_EVAL_END(){
	if(isInjecting && compensatingEOI){ //esto solo lo hace en el ultimo diente, vamos a probar updatearlo en cada diente
		uint16_t now = TIM1->CNT;

		//float target = INJ_GET_TARGET_EOI() + correction_eoi;
		/*if(0){
			FM_GET_PHIAD(MT_RPM, ME, forceTemp);
		}*/
		float target = INJ_UPDATE_TARGET_EOI() + correction_eoi;
		//float b_a = target +using_real_phi1*(actual_phi1 -PHI1 - dFi)- TW_TOOTH_ALPHA*currentTooth; //remaining angle + correction_eoi
		float b_a = target - TW_TOOTH_ALPHA*currentTooth; //remaining angle + correction_eoi
		if(real_bip_updated){
			float dif = (actual_phi1 - PHI1 - dFi);
			b_a += dif*fA_m;
		}

		//eq_rpm = fclamp(TEETH_RPM, edgeBuf[triggerTeeth].rpm *fA_m ,edgeBuf[triggerTeeth].rpm);

		//if(TIM1->CNT > TIM1->CCR1 + (T_ein + 200) * InjectionPrescaler){
		//if(b_a < margineq ){//b_a < 6margin_eoi_tooth
		if(currentTooth >= closingTooth ){//b_a < 6margin_eoi_tooth

			T_injecting = DEGTOTIME(b_a, TEETH_RPM); //eq_rpm
			//T_integrated = T_injecting;
			//uint32_t t4  = (T_integrated - TM_GET_T_DELAY_END()) * InjectionPrescaler; //los 25 es por el cambio de topologia, para que cierre al mismo tiempo
			TIM1_OC4_SetSafe((uint16_t)(now +(T_injecting - TM_GET_T_DELAY_END()) * InjectionPrescaler));
			lastTooth = currentTooth;
			compensatingEOI = 0;
			return;
			//to not compensate nothing more
		}else{
			T_injecting = DEGTOTIME(b_a, TEETH_RPM*0.9); //retard ass line
			TIM1_OC4_SetSafe((uint16_t)(now +(T_injecting - TM_GET_T_DELAY_END()) * InjectionPrescaler));

			return;
		}

		//uint32_t T_current = TIM1->CCR4 - TIM1->CNT;

		//float T_remaining_nom = 1.0f * T_current / InjectionPrescaler;
		//float T_remaining_comp = T_remaining_nom - lpf_eoi*(T_remaining_nom - b_a/(TEETH_RPM)/USTODEG);
		//if(!T_integrated){ T_integrated = T_current;}
		//T_integrated = TM_INTEGRATE_TIME_FROM_REFERENCE_Forward(target, currentTooth, PHI1 + dFi) + 0.5f;
		T_integrated = TM_INTEGRATE_TIME_TO_EOI(target, currentTooth) + 0.5f ;// - TEIN_NOMINAL
		//-!T_ein_status*PH_PEAK_DEF
		//T_integrated -= 0.5*(1.0f * T_current  - b_a/(TEETH_RPM)/USTODEG);
		//T_integrated = T_remaining_comp;
		/*if(b_a > 4.5){ //schedule for next teeth + 50%
			T_integrated = DEGTOTIME(b_a*1.5, TEETH_RPM);
		}else{
			T_integrated = TM_INTEGRATE_TIME_FROM_REFERENCE_Forward(target, currentTooth, PHI1 + dFi);
			lastEOI = target;
		}*/

		uint32_t t4  = (T_integrated - TM_GET_T_DELAY_END()) * InjectionPrescaler; //los 25 es por el cambio de topologia, para que cierre al mismo tiempo
		TIM1_OC4_SetSafe((uint16_t)(now + t4));

	}
}

void INJ_SET_DAC(uint8_t isPeak)
{
	if(!awaitingInj) return;

	if(isPeak){
		isInjecting = 1;
		uint16_t dato_dac = (uint16_t)(((isPeak - 1)? V_PEAK : V_HOLD )*4095/3.30); //4095 in 12 bit, 255 in 8 bit
		HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, dato_dac); //channel 2 es el que usamos
		if(isPeak - 1){
			HAL_GPIO_WritePin(HOLD_CONTROL_GPIO_Port, HOLD_CONTROL_Pin, GPIO_PIN_SET);
		}
	}else{
		HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, 0); //channel 2 es el que usamos
		isInjecting = 0;
		HAL_GPIO_WritePin(HOLD_CONTROL_GPIO_Port, HOLD_CONTROL_Pin, GPIO_PIN_RESET);
	}
}

void INJ_END(){
	if(isInjecting){
		INJ_SET_DAC(0);
		HAL_GPIO_WritePin(HOLD_CONTROL_GPIO_Port, HOLD_CONTROL_Pin, GPIO_PIN_RESET);
		TW_DEFER_EOI_EVAL(TIM2->CNT);
		hasInjectionEndedFlag = 1;
	}
    //TW_DEFER_TEIN_VAL(); //freaky aah angle USELESS I THINK
}
void FORCE_INJ_END(){
	/*HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, 0); //channel 2 es el que usamos
	HAL_GPIO_WritePin(HOLD_CONTROL_GPIO_Port, HOLD_CONTROL_Pin, GPIO_PIN_RESET);
	TW_DEFER_EOI_EVAL(TIM2->CNT);
	hasInjectionEnded = 1;*/
    //TW_DEFER_TEIN_VAL(); //freaky aah angle USELESS I THINK
}
float DEGTOTIME(float DEG, float revs){
	float TIME = 1000000 / (revs * 6) * DEG; //IN MICROSECONDS
	return TIME;
}
float TIMETODEG(float TIME, float revs){
	float DEG = (revs * 6) * TIME / 1000000 ; //IN MICROSECONDS
	return DEG;
}

static inline void TIM1_OC4_SetSafe(uint16_t ccr4_new)
{
    //uint16_t cnt = TIM1->CNT;
    if(ccr4_new > TIM1->CCR2){
        TIM1->CCR4 = ccr4_new;
    }
    __DSB();  // ensure the write hits the peripheral before we test

    // If the compare time is already due or effectively "now", kill the pulse now.
    // Keep the threshold at 0 or 1 tick depending on your clock/propagation.
    if (ccr4_new < TIM1->CNT) {
        /*// Force OC4 inactive (OC4M = 100)
        uint32_t ccmr2 = TIM1->CCMR2;
        ccmr2 &= ~TIM_CCMR2_OC4M;
        ccmr2 |=  (4U << TIM_CCMR2_OC4M_Pos);  // Forced inactive
        TIM1->CCMR2 = ccmr2;
        __DSB();
        // Restore back to PWM/OC mode (e.g., PWM1 = 110)
        ccmr2 &= ~TIM_CCMR2_OC4M;
        ccmr2 |=  (6U << TIM_CCMR2_OC4M_Pos);
        TIM1->CCMR2 = ccmr2;
        __DSB();*/
    	INJ_END();

    }
}
extern uint8_t startupiscar;
void SEND1_Handler(
    const struct CanMessageDef *msg,
    const uint8_t in_data[8],
    CanTxFn tx
)
{
#if defined(T06301)
	startupiscar = 1;
#endif
	if(!isInjecting && !hasInjectionEnded){
		INJ_UPDATE_ALPHA();
	}
}