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

/* Demo includes. */
#include "currentMeas.h"
#include "automation.h"
#include "utils.h"
#include "signals.h"
#include <math.h>
#include <limits.h>

#define ACS724

#define MVOLT_PER_AMP_05AB		400	// ACS724LLCTR-05AB-T   +/- 5A
#define MVOLT_PER_AMP_20AB		100	// ACS724LLCTR-20AB-T   +/- 20A
#define ADC1_DMABufferSize	   	1	// counts for measuring system voltage reference
#define ADC1_CHANNELS			3
#define ADC2_DMABufferSize	   	30	// counts for measuring TRMS current
#define ADC2_CHANNELS			2

extern TIM_HandleTypeDef htim6;
extern ADC_HandleTypeDef hadc1, hadc2;
extern osSemaphoreId ADC1SemHandle;
extern osSemaphoreId ADC2SemHandle;
extern osMutexId measMutexHandle;

uint16_t adc1_buffer[ADC1_DMABufferSize][ADC1_CHANNELS] = {0};
uint16_t raw1_buffer[ADC1_DMABufferSize][ADC1_CHANNELS] = {0};

uint16_t adc2_buffer[ADC2_DMABufferSize][ADC2_CHANNELS] = {0};
uint16_t raw2_buffer[ADC2_DMABufferSize][ADC2_CHANNELS] = {0};

CURRENT_THRESHOLD_STRUCT current_thresholds[] = {
		{ CURRENT_VALVE_REF, CURRENT_VALVE_UPPER_LIMIT, CURRENT_VALVE_LOWER_LIMIT },	// Valve
		{ CURRENT_ACTUATOR_REF, CURRENT_ACTUATOR_UPPER_LIMIT, CURRENT_ACTUATOR_LOWER_LIMIT }	// Actuator
};

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

bool isShortedPosition( uint16_t instant_current, uint16_t current_reference )
{
	return instant_current > current_reference;
}

bool isNotConnectedPosition( uint16_t instant_current_ma )
{
	return instant_current_ma <= CURRENT_AT_REST_THRESHOLD_MA;
}

bool isMalfunctionPosition( uint16_t instant_current_ma, uint16_t current_reference_ma )
{
	return ( instant_current_ma < current_reference_ma ) &&
			!isNotConnectedPosition( instant_current_ma );
}

void current_consumption_checking(uint16_t current_ma,
		CURRENT_DEVICE_TYPE device_type, uint16_t current_output_state )
{
	uint16_t v = 0, v_count=0;
	float current_mA = (float) current_ma / 1000.0f; // Convert to mA

	if( device_type == VALVE_DEVICE ) {
		current_output_state &= ALL_VALVES_MASK; // Mask to valve outputs only
	}
	else {
		current_output_state &= ACTUATORS_MASK; // Mask to actuator outputs only
	}

    for (v = 0; v < (sizeof(uint16_t) * CHAR_BIT); v++)
    {
        if ((current_output_state >> v) & 1) {
        	v_count++;
        }
    }

	// --- Logic for monitoring idle power ---
	if (current_output_state == 0) { // If outputs are assumed to be at low state
		if (current_mA > CURRENT_AT_REST_THRESHOLD_MA) {
			// ALERT: Abnormal consumption
			APP_LOG(TS_OFF, VLEVEL_M,"ALERT: abnormal current consumption: %.2f mA (Threshold: %.2f mA)\r\n",
					current_mA, CURRENT_AT_REST_THRESHOLD_MA);
			addErrorToList( 0x220 );
		} else {
			APP_LOG(TS_OFF, VLEVEL_M,"Outputs are low, current consumption: %.2f mA (OK)\r\n", current_mA);
		}
	} else { // If outputs are enabled (HIGH), check consumption
		if( v_count == 1 ) {	// mean one valve or one actuator is enabled
			// read value from adc measurement task
			APP_LOG(TS_ON, VLEVEL_M, "Device current: %.2f mA\r\n",current_mA);
			if( isShortedPosition( current_mA, current_thresholds[device_type].currentUpperLimit ) ) {
				APP_LOG(TS_ON, VLEVEL_M, "Warning: short circuit detected on position %d\r\n",v);
				addErrorToList( 0x208 + v );
			}
			else if( isMalfunctionPosition( current_mA, current_thresholds[device_type].currentUpperLimit ) ) {
				APP_LOG(TS_ON, VLEVEL_M, "Warning: current out-of-range on position %d\r\n",v);
				addErrorToList( 0x208 + v );
			}
			else if( isNotConnectedPosition( current_mA ) ) {
				APP_LOG(TS_ON, VLEVEL_M, "Warning: no device on position %d\r\n",v);
				addErrorToList( 0x202 );
			}
		} else {
			// Multiple outputs are enabled, we cannot determine the current for each output
			APP_LOG(TS_ON, VLEVEL_M, "Warning: multiple outputs enabled (%d), current cannot be determined\r\n", v_count);
		}
	}
}

// --- Fonction pour calculer la valeur RMS ---
float calculate_current_rms( CURRENT_DEVICE_TYPE device_type, int acs_sensivity )
{
    float sum_of_squares = 0.0f;
    float voltage_measured;
    float current_instantaneous;

    // --- step 1 : Calculate DC offset of the signal (samples average) ---
    long sum_adc_raw = 0;
    for (uint16_t i = 0; i < ADC2_DMABufferSize; i++)
    {
        sum_adc_raw += raw2_buffer[i][device_type];
    }
    float average_adc_raw = (float)sum_adc_raw / ADC2_DMABufferSize;
    float dc_offset_voltage = (average_adc_raw / 4095.0) * VREF;

    // --- step 2 : for each sample ---
    for (uint16_t i = 0; i < ADC2_DMABufferSize; i++)
    {
        // Convert raw ADC value to a voltage (0 à VREF)
        voltage_measured = ((float)raw2_buffer[i][device_type] / 4095.0) * VREF;

        // Minus DC offset for getting the AC signal
        float ac_voltage = voltage_measured - dc_offset_voltage;

        // Convert AC voltage in instant current
        current_instantaneous = ac_voltage / acs_sensivity;

        // Add the square of the instantaneous current to the sum
        sum_of_squares += (current_instantaneous * current_instantaneous);
    }

    // --- setp 3 : Calculate the mean square
    float mean_square = sum_of_squares / ADC2_DMABufferSize;

    // --- step 4 : Calculate the square root (RMS) ---
    float rms_current = sqrtf(mean_square);

    return rms_current;
}


void external_temp_calculation( float* meas )
{
	float mVoltage = raw1_buffer[0][1] * VREF / 4095;
	// On-board MCP9701 device
	*meas = (mVoltage - 400) / 19.5;
}

bool VREF_calculation()
{
	bool ret = false;
	if( raw1_buffer[0][1] > 0 ) {
		VREF = __HAL_ADC_CALC_VREFANALOG_VOLTAGE(raw1_buffer[0][0], ADC_RESOLUTION_12B);
		ret = true;
	}
	return ret;
}

void battery_voltage_calculation( float* meas )
{
   	float mVoltage = raw1_buffer[0][2] * VREF / 4095;
	// On-board MCP9701 device
	*meas = (mVoltage * 3);
}

void Reference_Meas_processFrame()
{
	if( VREF_calculation() ) {
		external_temp_calculation(&meas_case_temperature);
		battery_voltage_calculation(&meas_battery_voltage);
	}
}

void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc)
{
	BaseType_t  xHigherPriorityTaskWoken = pdFALSE;
	if(hadc->Instance == ADC1)
	{
		memcpy(raw1_buffer,adc1_buffer,sizeof(adc1_buffer));
		xSemaphoreGiveFromISR(ADC1SemHandle, &xHigherPriorityTaskWoken);
	}
	else if(hadc->Instance == ADC2)
	{
		//HAL_GPIO_WritePin(DBG1_GPIO_Port,DBG1_Pin,GPIO_PIN_SET);
		//HAL_GPIO_TogglePin(DBG1_GPIO_Port,DBG1_Pin);
		memcpy(raw2_buffer,adc2_buffer,sizeof(adc2_buffer));
		xSemaphoreGiveFromISR(ADC2SemHandle, &xHigherPriorityTaskWoken);
	}

	portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
	//HAL_GPIO_WritePin(DBG1_GPIO_Port,DBG1_Pin,GPIO_PIN_RESET);
}

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

    TickType_t xLastWakeTime;
    const TickType_t xFrequency = pdMS_TO_TICKS(100); // Mesurer toutes les 100 ms (10 Hz)
    uint16_t current_output_state = 0; // Outputs initial state

	osMessageQId myQueueId = getQueueId(ADC_MEAS);

    // Start ADC Calibration
    while(HAL_ADCEx_Calibration_Start(&hadc1,ADC_SINGLE_ENDED) != HAL_OK )
    	;
    while(HAL_ADCEx_Calibration_Start(&hadc2,ADC_SINGLE_ENDED) != HAL_OK )
    	;

    // ADC1 Clk = 64MMz / 2 (synchronous prescaler) = 32MHz
    // ADC VREF sampling time: 4*µs
    // Minimum sampling time calculation: 4µ / (1 / 32M) = 128 cycles mini. select: 247.5

    HAL_ADC_Start_DMA(&hadc1, (uint32_t*)&adc1_buffer, ((uint32_t)(ADC1_DMABufferSize*ADC1_CHANNELS)));
    HAL_ADC_Start_DMA(&hadc2, (uint32_t*)&adc2_buffer, ((uint32_t)(ADC2_DMABufferSize*ADC2_CHANNELS)));
    HAL_TIM_Base_Start(&htim6);

    osDelay(1000);	// Wait for stabilization

	xLastWakeTime = xTaskGetTickCount();

	while(1) {
		// Convert ADC1 values
	    xSemaphoreTake(ADC1SemHandle, portMAX_DELAY);

	    // Attempt to receive the output state of the output task in a non-blocking manner.
		// If a new state is available, it is updated. Otherwise, the last known state is retained.
		xQueueReceive(myQueueId, &current_output_state, 0);

		// Convert ADC2 values
	    xSemaphoreTake(ADC2SemHandle, portMAX_DELAY);

	    //buggy lines commented out
	    //meas_valves_current = calculate_current_rms( VALVE_DEVICE, MVOLT_PER_AMP_05AB );
		//current_consumption_checking(meas_valves_current*1000, VALVE_DEVICE, current_output_state );

		//meas_drives_current = calculate_current_rms( ACTUATOR_DEVICE, MVOLT_PER_AMP_20AB );
        //current_consumption_checking(meas_drives_current*1000, ACTUATOR_DEVICE, current_output_state );

        Reference_Meas_processFrame();

		// Wait for the next measurement period
		vTaskDelayUntil(&xLastWakeTime, xFrequency);
	}
}
