/*
 * pre_injection.c
 *
 *  Created on: Sep 16, 2025
 *      Author: herli
 */
#include <id.h>
#include "pre_injection.h"
#include "main.h"
#include "toothed_wheel.h"
#include "tein_detection.h"
#include "fuel_map.h"
#include "temperature.h"
#include "timeouts.h"


uint8_t active_PI = 0;
float dT_Tein = 90;

float PI_PHIAD = 0.0;

void PI_EVAL(float RPM, float ME, float MEPI){
	active_PI = RPM < 1500 && RPM > 3 && ME > 0.031 && MEPI > 0.031;

}

void PI_UPDATE_PHIAD(){
	if(active_PI){
		PI_PHIAD = FM_GET_PHIAD(RPM, MEPI, Temp); //PHIAD map
		PI_PHIAD = PI_PHIAD < 0.1 ? 0 : PI_PHIAD;
	}else{
		PI_PHIAD = 0;
	}
}

extern uint8_t InjectionPrescaler;
//should be called just as the injection is ending
void PI_Start(){
	TIM1->EGR = TIM_EGR_UG;
	TIM1->CR1 = TIM_CR1_CEN|TIM_CR1_OPM; 	//COUNTER ENABLE (CEN) AND ONEPULSEMODE (OPM)

	float T_on = DEGTOTIME(PI_PHIAD, TEETH_RPM) - T_ein + dT_Tein;
	float T_delayFI = 300; //antes estaba con las mt_rpm
	float T_hold = 0;
	float T_peak = 0;

	if(!T_on){
		T_peak = 174;
		T_hold = 0;
		blankInj = 1;
		compensatingEOI = 0;
		T_ein_filtered = 800;
		//Send error solenoid CAN
	}else if(T_on < 860){
		T_peak = 600;
		T_hold = 270;
		T_ein_filtered = 800;
		blankInj = 0;
		compensatingEOI = 0;
	}else{
		blankInj = 0;
		if(CompensateTein){
			T_peak = (T_ein > 800) ? T_ein - 200 : PH_PEAK_DEF;
		}else{
			T_peak = PH_PEAK_DEF;
			T_ein_filtered = 800;
		}
		T_hold = T_on > T_peak ? T_on - T_peak : 0;
	}
	uint32_t t1  = T_delayFI * InjectionPrescaler;//+1
	t1 = !t1 ? 1 : t1;
	if(blankInj){
		TIM1->CCR1 = t1;
		TIM1->CCR4 = t1 + T_peak * InjectionPrescaler;
	}else{
		uint32_t t2  = t1 + T_peak * InjectionPrescaler;
		uint32_t t4  = t2 + (T_hold + INJ_CLOSING_MARGIN) * InjectionPrescaler; //los 25 es por el cambio de topologia, para que cierre al mismo tiempo
		if(t1 && t2 && t4){
			if(t1 < t2 && t2 <= t4){
				TIM1->CCR1 = t1;
				TIM1->CCR2 = t2;
				//TIM1->CCR4 = compensatingEOI ? 65535 : t4;
				TIM1->CCR4 = t4;
				TIM1->CCR3 = t1 + AcquisitionTime * InjectionPrescaler;  // End time (sampling end)
			}
		}
	}

	TIM1->CNT = 0;
	TIM1->EGR = TIM_EGR_UG;
	TIM1->CR1 = TIM_CR1_OPM | TIM_CR1_CEN;

}
