#include <stdio.h>
#include "main.h"
#include <string.h>
#include <stdbool.h>
#include "cmsis_os.h"
#include "commonMsg.h"
#include "vt100.h"

/* Demo includes. */
#include "disdrometer.h"
#include "fdcan_bus.h"

#define ADC2_DMABufferSize	   	500
#define SAMPLING_FREQUENCY		10000.0 // Hz
#define SAMPLING_PERIOD 		(1.0 / SAMPLING_FREQUENCY) // Secondes

#define BASELINE	25
extern ADC_HandleTypeDef hadc2;
extern TIM_HandleTypeDef htim1;
extern DAC_HandleTypeDef hdac3;
extern OPAMP_HandleTypeDef hopamp2;
extern COMP_HandleTypeDef hcomp3;


extern osSemaphoreId ADC2semHandle;

uint16_t adc2_buffer[ADC2_DMABufferSize] = {0};
volatile bool rainSignalSampling = false; // Flag to indicate rain signal sampling
uint16_t rainThreshold = 2234; // Threshold for rain detection, 1.8 / 3.3 * 4095
__IO uint16_t rainDropCount = 0;
__IO uint16_t rainDropSum = 0;

/*-----------------------------------------------------------*/

void HAL_COMP_TriggerCallback(COMP_HandleTypeDef *hcomp)
 {
	if (hcomp->Instance == COMP3 ) {
		__HAL_COMP_COMP3_EXTI_DISABLE_IT();
		__HAL_COMP_COMP3_EXTI_CLEAR_FLAG();
	}
}

void ADC2_ConvCplt()
{
	BaseType_t  xHigherPriorityTaskWoken = pdFALSE;
	xSemaphoreGiveFromISR(ADC2semHandle, &xHigherPriorityTaskWoken);
	portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
}

void HAL_RTCEx_WakeUpTimerEventCallback(RTC_HandleTypeDef *hrtc)
{
	rainDropSum += rainDropCount; // Add rain drop count to sum
	// Reset rain drop counter
	rainDropCount = 0; // Reset rain drop counter
}

void initMovingAverage(MovingAverage *ma)
{
    for (int i = 0; i < WINDOW_SIZE; i++) {
        ma->samples[i] = NOMINAL_CENTER_VOLTAGE; // Initialiser avec la tension nominale
    }
    ma->head = 0;
    ma->count = WINDOW_SIZE;
    ma->sum = NOMINAL_CENTER_VOLTAGE * WINDOW_SIZE; // Initialiser la somme
}

// Ajoute un nouvel échantillon et met à jour la moyenne
float updateMovingAverage(MovingAverage *ma, float newSample)
{
    // Soustraire l'échantillon le plus ancien de la somme
    ma->sum -= ma->samples[ma->head];
    // Remplacer l'échantillon le plus ancien par le nouvel échantillon
    ma->samples[ma->head] = newSample;
    // Ajouter le nouvel échantillon à la somme
    ma->sum += newSample;

    // Déplacer la tête (indice circulaire)
    ma->head = (ma->head + 1) % WINDOW_SIZE;

    // Calculer et retourner la moyenne actuelle
    return ma->sum / WINDOW_SIZE;
}

float movingAverage(MovingAverage *ma) {
	if (ma->count == 0) {
		return (NOMINAL_CENTER_VOLTAGE+THRESHOLD_OFFSET); // Eviter la division par zéro
	}
	return ma->sum / ma->count;
}

void sendDisdrometerToCANbus()
{
CAN_Frame_t xFrame;
dev_rain_sens *device = (dev_rain_sens*)xFrame.data;

    device->MCU_RainCondition = rainDropSum;
	xFrame.dataSize = sizeof(dev_rain_sens);
	xFrame.identifier = MCU_S_RAINGAUGE;
	xFrame.dataLength = FDCAN_DLC_BYTES_5;
	CAN_EnqueueFrame(&xFrame);

#ifndef DATA_LOGGER
	move_cursor(BASELINE, 1);
	APP_LOG(TS_OFF, VLEVEL_M, "Rain drop counter: %d, Sum: %d",rainDropCount,rainDropSum);
#endif
}

void process_adc2_buffer(MovingAverage* ma)
{
    // Process rain signal
	uint16_t line=BASELINE+1;
	// --- 1. First rising edge detection ---
	double seuil_detection_front = rainThreshold;
	int index_debut_front = -1;

	for (int i = 0; i < ADC2_DMABufferSize; i++) {
		uint16_t currentValue = adc2_buffer[i];
		if ( currentValue > seuil_detection_front && index_debut_front == -1) {
			index_debut_front = i;
			//break; // First sample over threshold
		}
		updateMovingAverage(ma, currentValue);
	}
	// Adaptive threshold for DAC output
	float upperThreshold = movingAverage(ma) + THRESHOLD_OFFSET;
	HAL_DAC_SetValue(&hdac3, DAC_CHANNEL_1, DAC_ALIGN_12B_R, (uint32_t)upperThreshold);

	if (index_debut_front == -1) {
		move_cursor(line+1, 1);
		APP_LOG(TS_OFF, VLEVEL_M, "No rising edge detected at the threshold."); // End of the function if no rising edge is found
		clear_line_from_cursor(line+2,1);
		clear_line_from_cursor(line+3,1);
	}
	else {
		move_cursor(line+1, 1);
		APP_LOG(TS_OFF, VLEVEL_M, "First rising edge detected at sample: %3d", index_debut_front);

		// --- 2. First pulse amplitude calculation ---
		double amplitude_impulsion = 0.0;
		int index_pic = -1;

		// Look after peak in a window after first rising edge
		// This window has to be wide enough to include the entire pulse.
		// Sampling frequency is 10 kHz, so 100 (samples) is 10 ms.
		int fin_recherche_pic = index_debut_front + 100; // Ajust this value if pulses are wider
		if (fin_recherche_pic > ADC2_DMABufferSize) {
			fin_recherche_pic = ADC2_DMABufferSize;
		}

		for (int i = index_debut_front; i < fin_recherche_pic; i++) {
			if (adc2_buffer[i] > amplitude_impulsion) {
				amplitude_impulsion = adc2_buffer[i];
				index_pic = i;
			}
		}

		if (index_pic == -1) {
			move_cursor(line+4, 1);
			APP_LOG(TS_OFF, VLEVEL_M, "Error : Pulse peak not found.");
		}
		else {
			move_cursor(line+2, 1);
			APP_LOG(TS_OFF, VLEVEL_M, "Peak pulse amplitude: %.f at sample: %3d", amplitude_impulsion, index_pic);

			// --- 3. Full Width at Half Maximum (FWHM) calculation ---

			double seuil_mi_hauteur = seuil_detection_front;	//amplitude_impulsion / 2.0;
			int index_t1 = -1; // First sample before peak reaching half maximum peak
			int index_t2 = -1; // First sample after reaching half maximum peak

			// Looking for t1 : Start at index 0 and go on to the peak
			// The fist sample which is just over the mid-amplitude is t1.
			// Or, the first sample which reach or pass the mid-amplitude threshold after the start of pulse.
			// If pulse starts at 0, t1 is the first sample where signal goes up the mid-amplitude threshold.
			for (int i = index_debut_front; i < index_pic; i++) {
				if (adc2_buffer[i] >= seuil_mi_hauteur) {
					index_t1 = i;
					break;
				}
			}

			// Looking for t2 : Start from the peak and go down until the signal goes under the mid-amplitude threshold.
			for (int i = index_pic; i < ADC2_DMABufferSize; i++) {
				if (adc2_buffer[i] <= seuil_mi_hauteur) {
					index_t2 = i;
					break;
				}
			}

			// If t1 not found, this means pulse is already over the threshold (not realistic if pulse starts from 0)
			// or mid-amplitude threshold is too high.
			// Si t2 not found, this means pulse do not go under the threshold in the searching window.
			if (index_t1 != -1 && index_t2 != -1) {
				int largeur_echantillons = index_t2 - index_t1;
				double largeur_secondes = (double)largeur_echantillons * SAMPLING_PERIOD;
				move_cursor(line+3, 1);
				APP_LOG(TS_OFF, VLEVEL_M, "Pulse width (FWHM): %d samples (%.2fms)", largeur_echantillons, largeur_secondes*1000);
				rainDropCount++; // Increment rain drop count
				clear_line_from_cursor(line+4,1);
				clear_line_from_cursor(line+5,1);
			} else {
				move_cursor(line+4, 1);
				APP_LOG(TS_OFF, VLEVEL_M, "Error: Impossible to calculate the width FWHM (uncompleted pulse or half-height threshold not reached on both sides).");
				move_cursor(line+5, 1);
				APP_LOG(TS_OFF, VLEVEL_M, "index_t1 : %d, index_t2 : %d", index_t1, index_t2);
			}
		}

	}
}

void HAL_ADC_ErrorCallback(ADC_HandleTypeDef *hadc)
{
	if( hadc->Instance == ADC2 ) {
		// Manage ADC error
		rainSignalSampling = 0; // reinit state for retries
		// ...
	}
}

void vDisdrometerTask( void *pvParameters )
{
/* Remove warning for unused parameter */
    (void)pvParameters ;
    MovingAverage ma;

    initMovingAverage(&ma);

    // Start DAC3 used for rain sensor
    uint32_t dac_value = rainThreshold;
    HAL_DAC_Start(&hdac3, DAC_CHANNEL_1); // Start DAC
    HAL_DAC_SetValue(&hdac3, DAC_CHANNEL_1, DAC_ALIGN_12B_R, dac_value);
    // Calibrate The ADC On Power-Up For Better Accuracy
    HAL_ADCEx_Calibration_Start(&hadc2,ADC_SINGLE_ENDED);

    // Start OPAMP2 in PGA mode with gain = 8 for rain sensor - piezo
    HAL_OPAMP_Start(&hopamp2);
    // Start COMP3
    HAL_COMP_Start(&hcomp3);

	while(1) {
		//HAL_TIM_Base_Start(&htim1);
		HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); // Restart TIM1 PWM for DAC
		HAL_ADC_Start_DMA(&hadc2, (uint32_t*)adc2_buffer, (uint32_t)ADC2_DMABufferSize);
		__HAL_COMP_COMP3_EXTI_ENABLE_IT();

		// Wait for DMA transfer
	    xSemaphoreTake(ADC2semHandle, portMAX_DELAY);
		HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1); // Stop TIM1 PWM for ADC
	    process_adc2_buffer(&ma);
	    sendDisdrometerToCANbus();
	}
}
