/*
 * tein_detection.c
 *
 *  Created on: May 17, 2025
 *      Author: herli
 */

#include <id.h>
#include "tein_detection.h"
#include "injection.h"

#include "timeouts.h"

#include <stdint.h>
#include <stdlib.h>

#define MAX_MINIMA      1
#define VARIATION_TOL   5     // tighter margin since index range is smaller

typedef struct {
	int16_t index;
	float dv;   // 1st derivative
	float d2v;  // 2nd derivative
} MinimaPoint;

uint8_t dma_buffer_tein[DMA_BUFFER_SIZE_TEIN];
static int16_t signal[DMA_BUFFER_SIZE_TEIN];         // Centered signal
static float smooth[DMA_BUFFER_SIZE_TEIN];         // 3-point moving average
static float dv_dx[DMA_BUFFER_SIZE_TEIN];          // 1st derivative
static float d2v_dx2[DMA_BUFFER_SIZE_TEIN];        // 2nd derivative
static MinimaPoint last_minima[MAX_MINIMA];          // For extrapolation


float LPF_Beta = 0.01; // 0<ß<1
float LPF_Beta_acc = 0.1;
float T_ein_in;
float T_ein_filtered = 800;
float T_ein = TEIN_FAULT;
uint8_t T_ein_status = 0;

int CompensateTein = 0;
uint32_t TeinProcessedTime;
uint32_t lastTeinProcessedTime;

int ProcessTein;
uint32_t diff_ConvCallback;

volatile uint8_t hasTeinDetEnded = 1;
volatile uint8_t hasTeinDetEndedFlag = 0;

void smooth_signal() {
    for (int i = 1; i < DMA_BUFFER_SIZE_TEIN - 1; i++) {
        smooth[i] = (signal[i - 1] + signal[i] + signal[i + 1]) / 3.0f;
    }
    smooth[0] = smooth[1];
    smooth[DMA_BUFFER_SIZE_TEIN - 1] = smooth[DMA_BUFFER_SIZE_TEIN - 2];
}

void compute_derivatives() {
    for (int i = 1; i < DMA_BUFFER_SIZE_TEIN - 1; i++) {
        dv_dx[i] = (smooth[i + 1] - smooth[i - 1]) / 2.0f;
        d2v_dx2[i] = smooth[i + 1] - 2.0f * smooth[i] + smooth[i - 1];
    }
    dv_dx[0] = dv_dx[1];
    dv_dx[DMA_BUFFER_SIZE_TEIN - 1] = dv_dx[DMA_BUFFER_SIZE_TEIN - 2];
    d2v_dx2[0] = d2v_dx2[1];
    d2v_dx2[DMA_BUFFER_SIZE_TEIN - 1] = d2v_dx2[DMA_BUFFER_SIZE_TEIN - 2];
}

int detect_minima(MinimaPoint* output, uint16_t PeakEndIndex) {
    int count = 0;
    for (int i = PeakEndIndex; i < DMA_BUFFER_SIZE_TEIN - 1 && count < MAX_MINIMA; i++) {
        if (dv_dx[i - 1] < 0 && dv_dx[i] >= 0 && d2v_dx2[i] > 0) {
            output[count].index = (int16_t)i;
            output[count].dv = dv_dx[i];
            output[count].d2v = d2v_dx2[i];
            count++;
        }
    }
    return count;
}


int fill_missing_minima(MinimaPoint* found, int count, MinimaPoint* filled, uint16_t cutoffIndex) {
    int lostFirst = 0;

    int d1 = last_minima[1].index - last_minima[0].index;
	int d2 = last_minima[2].index - last_minima[1].index;

    MinimaPoint valid[4];
    int valid_count = 0;
    for (int i = 0; i < count; ++i) {
        if (found[i].index <= cutoffIndex) {
            valid[valid_count++] = found[i];
        }
    }

    if (valid_count >= 3) {
    	// Copy first three by default
		filled[0] = valid[0];
		filled[1] = valid[1];
		filled[2] = valid[2];

		// Check if spacing suggests first point is missing
		// Compare spacing between found[1] and found[2] against historical d1/d2
		int spacing1 = valid[1].index - valid[0].index;
		int spacing2 = valid[2].index - valid[1].index;

		// If spacing2 matches d1 and spacing1 matches d2, it's likely a shift
		if (abs(spacing1 - d2) < VARIATION_TOL ) { //&& abs(spacing2 - d1) > VARIATION_TOL //si el espaciado es del 2 al 3, es decir
			// Shift: detected[0] is actually 2nd, detected[1] is 3rd, detected[2] is old 4th
			filled[2] = valid[1];                     // 3rd ← was 2nd
			filled[1] = valid[0];                     // 2nd ← was 1st
			filled[0].index = valid[0].index - d1;    // Predict new 1st
			lostFirst = 1;
		}
	}
    if (valid_count == 2) {
        int spacing = valid[1].index - valid[0].index;

    	if (abs(spacing - d1) < VARIATION_TOL) { 	//si tengo primero y segundo
			// found[0] = 1st, found[1] = 2nd → extrapolate 3rd
			filled[0] = valid[0];
			filled[1] = valid[1];
			filled[2].index = valid[1].index + d2;

		} else if (abs(valid[0].index - last_minima[1].index) < VARIATION_TOL &&
				   abs(valid[1].index - last_minima[2].index) < VARIATION_TOL) {		//si tengo segundo y tercero
			// Possibly captured 2nd and 3rd → predict missing 1st
			filled[2] = valid[1];
			filled[1] = valid[0];
			filled[0].index = valid[0].index - d1;
			lostFirst = 1;
		} else {													//si tengo primero y tercero (imposible)
			// Too uncertain → fallback to last known
			for (int i = 0; i < MAX_MINIMA; ++i) {
				filled[i] = last_minima[i];
			}
			lostFirst = 1;
		}
    } else if (valid_count == 1) {
    	// si solo encontré un punto, lo tomamos como que es el primero
        /*filled[1] = found[0];
		filled[0].index = filled[1].index - d1;
		filled[2].index = filled[1].index + d2;
		lostFirst = 1;*/
    	filled[0] = valid[0];
		filled[1].index = filled[0].index + d1;
		filled[2].index = filled[1].index + d2;
    } else if (valid_count == 0) {
    	// como el anterior
        for (int i = 0; i < MAX_MINIMA; i++) {
            filled[i] = last_minima[i];
        }
		lostFirst = 1;
    }
    return lostFirst;
}
extern IC_TEIN;
void ProcessAdcSignal(uint32_t IC_INJ, uint16_t T_peak, uint16_t T_hold) {
    // Center signal

    for (int i = 0; i < DMA_BUFFER_SIZE_TEIN; i++) {
        signal[i] = (int16_t)dma_buffer_tein[i] - ADC_MID;
    }
	uint8_t PeakEndIndex = T_peak / (samplePeriod - 1);

    smooth_signal();
    compute_derivatives();

    MinimaPoint detected[MAX_MINIMA];
    int found = detect_minima(detected, PeakEndIndex);


    if(!found || !T_hold){
    	//T_ein = 950;
    	return;
    }

    /*MinimaPoint final[MAX_MINIMA];
    uint16_t cutoff_index = T_on / samplePeriod;

    int lostFirst = fill_missing_minima(detected, found, final, cutoff_index);

    for (int i = 0; i < MAX_MINIMA; i++) {
        last_minima[i] = final[i];
    }*/

    // final[0..2].index now holds the local minima
    int lostFirst = 0;
    uint8_t TeinIndex = detected[0].index;  // first minimum
    uint16_t timeConvCallback = (DMA_BUFFER_SIZE_TEIN - TeinIndex) * samplePeriod; // in microseconds

    diff_ConvCallback = TeinProcessedTime >= IC_INJ ? TeinProcessedTime-IC_INJ : TeinProcessedTime-(IC_INJ-0xffffffff);
    //diff_ConvCallback = TeinProcessedTime-	lastTeinProcessedTime;


    // Convert timer ticks to microseconds
    float T_ConvCallback = diff_ConvCallback / 16.0;  // us
    T_ein_in = T_ConvCallback - timeConvCallback;
    if(T_ein_in < T_peak+40){
    	/*for (int i = 0; i < MAX_MINIMA; i++) {
			last_minima[i].index = 0;
		}*/
    	return;
    }
    float Tein_diff = T_ein_filtered - T_ein_in;
	float abs_Tein_diff = Tein_diff < 0 ? -Tein_diff : Tein_diff;	//take abs
	if(!lostFirst){
		T_ein_filtered -= abs_Tein_diff < 40 ? LPF_Beta * Tein_diff : LPF_Beta_acc * Tein_diff;
		//T_ein_filtered -= abs_Tein_diff < 40 ? LPF_Beta * Tein_diff : 0;
		//if(abs_Tein_diff < 40){
		//T_ein_filtered -= LPF_Beta * Tein_diff;
		//}
		//T_ein = T_ein_filtered - 15;
		T_ein = 1.01843 * T_ein_filtered - 10.918;
		CompensateTein = T_ein_filtered > 1700 ? 0 : 1;

		Timeout_ResetByIndex(10, TIM16->CNT);  // Reset Tein timeout
		T_ein_status = CompensateTein;
		//CompensateTein = (T_ein_filtered < 400 && CompensateTein) ? 0 : 1;
		/*if(!CompensateTein){
			T_ein = 950;
		}*/
	}
	/*else{
		CompensateTein = 0;
	}*/
	if(!T_ein_filtered){
		T_ein_filtered = T_ein_in;
	}
	//T_ein = CompensateTein ? 1.0549 * T_ein_filtered - 79.5975 : 950 ;

	//T_ein = 950;
}

extern TIM_HandleTypeDef htim1;
uint8_t timer1started;
//uint8_t pending_start_tein_adc;
void TEIN_DET_Init(){
	if(!timer1started){
		HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3); // no hace falta activarlo todas las veces;
		timer1started = 1;
		StartSampling();
	}
}

void TEIN_DET_DeInit(){
	T_ein = TEIN_FAULT;
}
void TEIN_DET_Service(){
	//if()
	if(ProcessTein){//ProcessTein && startedEngine
		ProcessAdcSignal(IC_INJ, T_peak, T_hold);
		ProcessTein = 0;
		if(!T_hold){
			Timeout_StopByIndex(10);  // Reset Tein timeout
		}
		TW_DEFER_TEIN_VAL();
	}

}

void TEIN_STATUS_ONFAULT(){
	T_ein = TEIN_FAULT;
	T_ein_status = 0;
}