#include "main.h"
#include "board.h"
#include "tim.h"
#include "sma_filter.h"

extern ADC_HandleTypeDef hadc1;

//TODO:
//TEMP READ
// Connector temperature sensors

InfoBlock_t *InfoBlock = (InfoBlock_t *)(VERSION_OFFSET);

uint8_t RELAY_State[RELAY_COUNT];
static volatile uint8_t adc_lock = 0;
static SMAFilter_t conn_temp_adc_filter[2];

void RELAY_Write(relay_t num, uint8_t state){
	switch (num) {
	case RELAY_AUX0:
		HAL_GPIO_WritePin(RELAY1_GPIO_Port, RELAY1_Pin, state);
		break;
	case RELAY_AUX1:
		HAL_GPIO_WritePin(RELAY2_GPIO_Port, RELAY2_Pin, state);
		break;
	case RELAY3:
		HAL_GPIO_WritePin(RELAY3_GPIO_Port, RELAY3_Pin, state);
		break;
	case RELAY_DC:
		HAL_GPIO_WritePin(RELAY4_GPIO_Port, RELAY4_Pin, state);
		break;
	case RELAY_AC:
		HAL_GPIO_WritePin(RELAY5_GPIO_Port, RELAY5_Pin, state);
		break;
	case RELAY_CP:
		HAL_GPIO_WritePin(RELAY_CP_GPIO_Port, RELAY_CP_Pin, state);
		break;
	case RELAY_CC:
		HAL_GPIO_WritePin(RELAY_CC_GPIO_Port, RELAY_CC_Pin, state);
		break;
	case RELAY_DC1:
		HAL_GPIO_WritePin(RELAY_DC_GPIO_Port, RELAY_DC_Pin, state);
		break;
	default:
		break;
	}

	RELAY_State[num] = state;
}

uint8_t RELAY_Read(relay_t num){
	return RELAY_State[num];
}


uint8_t IN_ReadInput(inputNum_t input_n){
	switch(input_n){
	case IN_SW0:
		return HAL_GPIO_ReadPin(IN_SW0_GPIO_Port, IN_SW0_Pin);
	case IN_SW1:
		return HAL_GPIO_ReadPin(IN_SW1_GPIO_Port, IN_SW1_Pin);
	case IN0:
		return HAL_GPIO_ReadPin(IN0_GPIO_Port, IN0_Pin);
	case IN_ESTOP:
		return HAL_GPIO_ReadPin(IN_ESTOP_GPIO_Port, IN_ESTOP_Pin);
	case IN_FB1:
		return HAL_GPIO_ReadPin(IN_FB1_GPIO_Port, IN_FB1_Pin);
	case IN_CONT_FB_DC:
		return HAL_GPIO_ReadPin(IN_FB2_GPIO_Port, IN_FB2_Pin);
	case ISO_IN:
		return HAL_GPIO_ReadPin(ISO_IN_GPIO_Port, ISO_IN_Pin);
	default:
		return 0;
	}
}

uint8_t GetBoardTemp(){
	//TODO
//	HAL_ADC_Start(&hadc1); // start the adc
//
//	HAL_ADC_PollForConversion(&hadc1, 100); // poll for conversion
//
//	adc_val = HAL_ADC_GetValue(&hadc1); // get the adc value
//
//	HAL_ADC_Stop(&hadc1); // stop adc
	return 0;
}

void Init_Peripheral(){
    HAL_ADCEx_Calibration_Start(&hadc1);

    RELAY_Write(RELAY_AUX0, 0);
    RELAY_Write(RELAY_AUX1, 0);
    RELAY_Write(RELAY3, 0);
    RELAY_Write(RELAY_DC, 0);
    RELAY_Write(RELAY_AC, 0);
    RELAY_Write(RELAY_CP, 1);
    RELAY_Write(RELAY_CC, 1);
    RELAY_Write(RELAY_DC1, 0);

    SMAFilter_Init(&conn_temp_adc_filter[0]);
    SMAFilter_Init(&conn_temp_adc_filter[1]);
}

float pt1000_to_temperature(float resistance) {
    // Константы для PT1000
    const float R0 = 1000.0;  // Сопротивление при 0 °C
    const float C_A = 3.9083E-3f;
    
    float temperature = (resistance-R0) / ( R0 * C_A);

    return temperature;
}


float calculate_NTC_resistance(int adc_value, float Vref, float Vin, float R) {
    // Преобразуем значение АЦП в выходное напряжение
    float Vout = (adc_value / 4095.0) * Vref;

    // Проверяем, чтобы Vout не было равно Vin
    if (Vout >= Vin) {
        return -1; // Ошибка: Vout не может быть больше или равно Vin
    }

    // Вычисляем сопротивление термистора
    float R_NTC = R * (Vout / (Vin - Vout));

    return R_NTC;
}

int16_t CONN_ReadTemp(uint8_t ch){
	ADC_LockBlocking();

	//TODO
	if(ch)ADC_Select_Channel(ADC_CHANNEL_8);
	else ADC_Select_Channel(ADC_CHANNEL_9);
    // Начало конверсии
    HAL_ADC_Start(&hadc1);


    // Ожидание окончания конверсии
    HAL_ADC_PollForConversion(&hadc1, HAL_MAX_DELAY);

    // Получение значения
    uint32_t adcValue = HAL_ADC_GetValue(&hadc1);

    // Остановка АЦП (по желанию)
    HAL_ADC_Stop(&hadc1);

    int32_t adc_filtered = SMAFilter_Update(&conn_temp_adc_filter[ch ? 1u : 0u], (int32_t)adcValue);
    if((uint32_t)adc_filtered > 4000u) {
    	ADC_Unlock();
    	return 20; //Термодатчик не подключен
    }

//    int adc_value = 2048; // Пример значения АЦП
    float Vref = 3.3;     // Напряжение опорное
    float Vin = 5.0;      // Входное напряжение
    float R = 1000;      // Сопротивление резистора в Омах

    float temp = pt1000_to_temperature(calculate_NTC_resistance((int)adc_filtered, Vref, Vin, R));

    ADC_Unlock();
    return (int16_t)temp;

}

int16_t GBT_ReadTemp(uint8_t ch){
    return CONN_ReadTemp(ch);
}

void ADC_Select_Channel(uint32_t ch) {
    ADC_ChannelConfTypeDef conf = {
        .Channel = ch,
        .Rank = 1,
        .SamplingTime = ADC_SAMPLETIME_28CYCLES_5,
    };
    if (HAL_ADC_ConfigChannel(&hadc1, &conf) != HAL_OK) {
        Error_Handler();
    }
}

uint8_t ADC_TryLock(void) {
    uint32_t primask = __get_PRIMASK();
    __disable_irq();
    if (adc_lock != 0u) {
        if (primask == 0u) {
            __enable_irq();
        }
        return 0u;
    }
    adc_lock = 1u;
    if (primask == 0u) {
        __enable_irq();
    }
    return 1u;
}

void ADC_LockBlocking(void) {
    while (ADC_TryLock() == 0u) {
        /* wait in main context until ADC is free */
    }
}

void ADC_Unlock(void) {
    uint32_t primask = __get_PRIMASK();
    __disable_irq();
    adc_lock = 0u;
    if (primask == 0u) {
        __enable_irq();
    }
}