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

/* Demo includes. */
#include "waterTank.h"
#include "fdcan_bus.h"
#include "flash_page.h"
#include "vt100.h"

//#define DATA_LOGGER

#define CAPTURE_BUFFER_SIZE 	255
#define CALIB_COUNT_MAX			5
#define SAMPLE_MAX				50
#define MAX_SERIES_COUNT		10
#define CALIBRATION_STEP_COMPLETE_FLAG 0x8

#define BASELINE	22

/* Reference Capacitance Value is 100 pF 1%. This capacitance is used in calibration phase */
#define REFCAP 100e-12

/* Resistance Value. The reference capacitance and humidity sensor are
   charged/discharged through the resistance R159 available on STM32303C-EVAL.
   It's value is 68K 1%
*/
#define RES 68000

/* TIM3 PWM frequency value in Hertz. TIM3 is used to charge/discharge the level sensor.
   Min TIM3 PWM frequency is SystemCoreClock/65535 and max TIM3 PWM frequency  = 1/(RES * CAP)
   with RES is the resistance and CAP is the minimum sensor capacitance.
   For level sensor, CAP is equal to 244 pF + 100 pF.
   when SystemCoreClock is 120MHz, the min frequency is 1831 Hz and the max frequency is 147KHz
*/

extern COMP_HandleTypeDef hcomp1;
extern DAC_HandleTypeDef hdac1;
extern TIM_HandleTypeDef htim1, htim3, htim4;

extern SYS_MEM_STRUCT sys_mem;

extern osSemaphoreId ICsemHandle;

__IO uint16_t probeLevelCapacitance;
uint16_t lastProbeLevelCapacitance = 0;
uint32_t flowFrequency = 0;
uint16_t lastFlowRate = 0;
__IO uint16_t capture_buffer[CAPTURE_BUFFER_SIZE];
const int meas_offset = 1;
double meas;
uint16_t levelp;
CommandOpcode currentMachineState = OP_NORMAL;
volatile uint16_t calibrationStep = 0;
uint16_t last_calibrationStep = 0;
#ifdef DATA_LOGGER
	uint16_t samples = 0;
#endif

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

#ifdef DATA_LOGGER
void waitForButtonPress(void) {
	while (HAL_GPIO_ReadPin(WATER_FLOW_GPIO_Port, WATER_FLOW_Pin) == GPIO_PIN_RESET) {
		osDelay(10); // Anti-rebond
	}
	osDelay(100); // Délai supplémentaire pour éviter les multiples détections
}
#endif


/**
  * @brief  Input Capture callback in non blocking mode
  * @param  htim : TIM4 IC handle
  * @retval None
  */
void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
	uint32_t SumICReadValue;

	if (htim->Instance == TIM4 && htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1) {

		SumICReadValue = 0;
		for (int i = 0; i < CAPTURE_BUFFER_SIZE; i++) {
			SumICReadValue += capture_buffer[i];
		}
		double triggerTime_us = (double)(SumICReadValue/CAPTURE_BUFFER_SIZE)/(SystemCoreClock/1000000);
		probeLevelCapacitance = triggerTime_us*1000000/RES;

		BaseType_t  xHigherPriorityTaskWoken = pdFALSE;
		xSemaphoreGiveFromISR(ICsemHandle, &xHigherPriorityTaskWoken);
		portYIELD_FROM_ISR( xHigherPriorityTaskWoken );

    }
}

/**
 * @brief  Save settings to flash memory
 * @retval None
 */
void saveSettings()
{
	Flash_Write_Data (FLASH_USER_START_ADDR, (__IO uint64_t *)&sys_mem, sizeof( sys_mem ));
}

// Lagrange interpolation
double LagrangeInterpolation( double xp )
{
	double nr, dr, yp=0;

	for(int i = 0; i < LAGRANGE_SIZE; i++)
	{
		nr = dr = 1;
		for(int j = 0; j < LAGRANGE_SIZE; j++)
		{
			if( i != j )
	        {
				nr = nr * ( xp - sys_mem.LagrangeX[j] );
				dr = dr * ( sys_mem.LagrangeX[i] - sys_mem.LagrangeX[j] );
	        }
		}
        if (dr != 0) { // check if division by zero
            yp += ((nr / dr) * sys_mem.LagrangeY[i]);
        } else {
            return NAN;
        }
	}
	return yp;
}

void update_waterTank_stats( dev_watertank *device )
{
	if( lastFlowRate != device->MCU_WaterFlowRate )
	{
		saveSettings();
		lastFlowRate = device->MCU_WaterFlowRate;
	}

	if( lastProbeLevelCapacitance != probeLevelCapacitance )
	{
	  if( probeLevelCapacitance < sys_mem.LagrangeX[0] ) {
		  levelp = 0;
		  meas = 0;
	  }
	  else {
		  meas = LagrangeInterpolation(probeLevelCapacitance);  // probe measure in cm
		  meas += meas_offset;
		  if( meas > 100 )
			  meas = 100;
		  // In real, 90 cm on probe measure means 91 cm inside tank
		  levelp = meas * 100 / 100; // level in %
	  }
	  lastProbeLevelCapacitance = probeLevelCapacitance;
	  device->MCU_WaterTankLevel = levelp;
	}

	if( device->MCU_WaterConsumption == 0 ) {
		device->MCU_WaterConsumption = sys_mem.waterConsumption * 100; // in centiliter
	}
}

void sendWaterTankDataToCANbus()
{
CAN_Frame_t xFrame;
dev_watertank *device = (dev_watertank*)xFrame.data;

	update_waterTank_stats(device);

	// flow frequency (Hz) = 7.5Q, Q is flow rate in L/min.
	device->MCU_WaterFlowRate = (flowFrequency * 60 / 7.5); // (flow frequency x 60 min) / 7.5Q = flowrate in L/hour
	flowFrequency = 0; // Reset Counter
	sys_mem.waterConsumption += (float)device->MCU_WaterFlowRate / 60; // in liter
	device->MCU_WaterConsumption = sys_mem.waterConsumption * 100; // in centiliter
	device->MCU_Rain = 0; // deprecated
	device->MCU_State = calibrationStep;
	xFrame.dataSize = sizeof(dev_watertank);
	xFrame.identifier = MCU_S_WATERTANK;
	xFrame.dataLength = FDCAN_DLC_BYTES_5;
	CAN_EnqueueFrame(&xFrame);

#ifdef DATA_LOGGER
	double triggerTime_us = ((double)probeLevelCapacitance)*RES/1000000;
	APP_LOG(TS_OFF, VLEVEL_M, "%d,%4d,%d,%5.1f\r\n",samples++,probeLevelCapacitance,levelp,triggerTime_us);
#else
	move_cursor(BASELINE, 1);
	APP_LOG(TS_OFF, VLEVEL_M, "Flow rate: %2d l/min., Water consumption: %.3f m3", device->MCU_WaterFlowRate, sys_mem.waterConsumption);
	move_cursor(BASELINE+1, 1);
	APP_LOG(TS_OFF, VLEVEL_M, "Sensor capacitance: %4d pF, Level: %.1f cm(%d%%), Trigger time: %4.1f us",probeLevelCapacitance, meas, levelp,
			(double)probeLevelCapacitance/1000000*RES);
#endif
}

void waterTank_processMsg(MSG_STRUCT *p_msg)
{
	CommandOpcode opcode = p_msg->opcode;

	switch( opcode ) {
	case OP_CALIBRATION:
		currentMachineState = OP_CALIBRATION;
		break;
	default:
		break;
	};

	osDelay( 20 );
}

void vWaterTankTask( void *pvParameters )
{
/* Remove warning for unused parameter */
    (void)pvParameters ;

	osMessageQId myQueueId = getQueueId(WATER_TANK);

    // Start DAC1 used for water level sensor
    uint32_t dac_value = 2588; // 2.086 / 3.3 * 4095
    HAL_DAC_Start(&hdac1, DAC_CHANNEL_1); // Start DAC
    HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, dac_value);

    // Start COMP1 used for water level sensor, trigger TIM4
    HAL_COMP_Start(&hcomp1);

    // *** Water level sensor
    // Start TIM4 triggered by TIM3 sets at 1831.5 HZ duty 50 and interrupt on input capture on COMP3.
    HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_2);
    HAL_TIM_IC_Start_DMA(&htim4, TIM_CHANNEL_1, (uint32_t *)capture_buffer, CAPTURE_BUFFER_SIZE);

    while(1) {
		xSemaphoreTake(ICsemHandle, portMAX_DELAY);
		// Wait for DMA transfer
		sendWaterTankDataToCANbus();

		if( last_calibrationStep != calibrationStep )
		{
		  if ( calibrationStep > 4 )
			  calibrationStep = 0;	// End of calibration
		  else
			  sys_mem.LagrangeX[calibrationStep-1] = 0;
		  last_calibrationStep = calibrationStep;
		}

		/*if (currentMachineState == OP_CALIBRATION) {
			OsDelay(2000);
			if ( !(device.MCU_State & CALIBRATION_STEP_COMPLETE_FLAG) ) {
				// Read probe level capacitance
				sys_mem.LagrangeX[calibrationStep-1] += probeLevelCapacitance;
				if( ++averageCalibCount > (CALIB_COUNT_MAX-1) )
				{
				  sys_mem.LagrangeX[calibrationStep-1] /= CALIB_COUNT_MAX;
				  averageCalibCount = 0;
				  device.MCU_State |= CALIBRATION_STEP_COMPLETE_FLAG;
				}
			}
			else {
				calibrationStep++;
			}
			continue;
		}*/
		osEvent event = osMessageGet(myQueueId, 100);
	    if (event.status == osEventMessage) {
	    	MSG_STRUCT* p_recvMsg = event.value.p;
	    	waterTank_processMsg(p_recvMsg);
	    	freeMemoryPoolMessage(p_recvMsg);
	    }
	    vTaskDelay(pdMS_TO_TICKS(500));
	}
}
