#include "cp.h"
#include "adc.h"
#include "board.h"
#include "tim.h"
#include "debug.h"
#include <stdint.h>

#define MAX_DUTY 450
#define CP_EMA_ALPHA_Q8 38
#define CP_DEBOUNCE_MS_DEFAULT 10
#define CP_DEBOUNCE_MS_F 60
#define CP_DEBOUNCE_MS_F_LOW_DUTY 100
#define CP_LOW_DUTY_THRESHOLD_PERCENT 10

#define CP_A_ENTER_MV 11000
#define CP_A_EXIT_MV 10000

#define CP_B_ENTER_LOW_MV 8000
#define CP_B_ENTER_HIGH_MV 10000
#define CP_B_EXIT_LOW_MV 7500
#define CP_B_EXIT_HIGH_MV 10500

#define CP_C_ENTER_LOW_MV 5000
#define CP_C_ENTER_HIGH_MV 7000
#define CP_C_EXIT_LOW_MV 4500
#define CP_C_EXIT_HIGH_MV 7500

#define CP_D_ENTER_LOW_MV 2000
#define CP_D_ENTER_HIGH_MV 4000
#define CP_D_EXIT_LOW_MV 1500
#define CP_D_EXIT_HIGH_MV 4500

#define CP_E_ENTER_LOW_MV -1000
#define CP_E_ENTER_HIGH_MV 2000
#define CP_E_EXIT_LOW_MV -1500
#define CP_E_EXIT_HIGH_MV 2500

#define CP_F_ENTER_MV -11500
#define CP_F_EXIT_MV -10500

static int32_t cp_voltage_mv = 0;
static int32_t cp_voltage_filt_mv = 0;
static uint8_t cp_filter_initialized = 0;
static uint8_t cp_duty = 0;
CP_State_t fake_cp_state = EV_STATE_ACQUIRING;
static CP_State_t cp_stable_state = EV_STATE_ACQUIRING;
static CP_State_t cp_candidate_state = EV_STATE_ACQUIRING;
static uint32_t cp_candidate_since_ms = 0;

static uint32_t CP_ReadAdcChannel(uint32_t ch) {
    uint32_t adc = 0;

    ADC_Select_Channel(ch);
    HAL_ADC_Start(&hadc1);
    HAL_ADC_PollForConversion(&hadc1, 10);
    adc = HAL_ADC_GetValue(&hadc1);
    HAL_ADC_Stop(&hadc1);

    return adc;
}
#define VREFINT_CAL_ADDR ((uint16_t*)0x1FFFF7BA) // для STM32F1!

static uint8_t CP_IsInRange(int32_t v, int32_t lo, int32_t hi) {
    return (v >= lo && v <= hi) ? 1u : 0u;
}

static int32_t CP_ApplyEma(int32_t raw_mv) {
    if (!cp_filter_initialized) {
        cp_voltage_filt_mv = raw_mv;
        cp_filter_initialized = 1;
        return cp_voltage_filt_mv;
    }

    cp_voltage_filt_mv += ((raw_mv - cp_voltage_filt_mv) * CP_EMA_ALPHA_Q8) / 256;
    return cp_voltage_filt_mv;
}

static CP_State_t CP_ClassifyWithHysteresis(int32_t v, CP_State_t prev) {
    switch (prev) {
    case EV_STATE_A_IDLE:
        if (v >= CP_A_EXIT_MV) return EV_STATE_A_IDLE;
        break;
    case EV_STATE_B_CONN_PREP:
        if (CP_IsInRange(v, CP_B_EXIT_LOW_MV, CP_B_EXIT_HIGH_MV)) return EV_STATE_B_CONN_PREP;
        break;
    case EV_STATE_C_CONN_ACTIVE:
        if (CP_IsInRange(v, CP_C_EXIT_LOW_MV, CP_C_EXIT_HIGH_MV)) return EV_STATE_C_CONN_ACTIVE;
        break;
    case EV_STATE_D_CONN_ACT_VENT:
        if (CP_IsInRange(v, CP_D_EXIT_LOW_MV, CP_D_EXIT_HIGH_MV)) return EV_STATE_D_CONN_ACT_VENT;
        break;
    case EV_STATE_E_NO_POWER:
        if (CP_IsInRange(v, CP_E_EXIT_LOW_MV, CP_E_EXIT_HIGH_MV)) return EV_STATE_E_NO_POWER;
        break;
    case EV_STATE_F_ERROR:
        if (v <= CP_F_EXIT_MV) return EV_STATE_F_ERROR;
        break;
    default:
        break;
    }

    if (v >= CP_A_ENTER_MV) return EV_STATE_A_IDLE;
    if (CP_IsInRange(v, CP_B_ENTER_LOW_MV, CP_B_ENTER_HIGH_MV)) return EV_STATE_B_CONN_PREP;
    if (CP_IsInRange(v, CP_C_ENTER_LOW_MV, CP_C_ENTER_HIGH_MV)) return EV_STATE_C_CONN_ACTIVE;
    if (CP_IsInRange(v, CP_D_ENTER_LOW_MV, CP_D_ENTER_HIGH_MV)) return EV_STATE_D_CONN_ACT_VENT;
    if (CP_IsInRange(v, CP_E_ENTER_LOW_MV, CP_E_ENTER_HIGH_MV)) return EV_STATE_E_NO_POWER;
    if (v <= CP_F_ENTER_MV) return EV_STATE_F_ERROR;
    return EV_STATE_ACQUIRING;
}

static uint32_t CP_GetDebounceMs(CP_State_t next_state) {
    if (next_state == EV_STATE_F_ERROR) {
        if (cp_duty <= CP_LOW_DUTY_THRESHOLD_PERCENT) {
            return CP_DEBOUNCE_MS_F_LOW_DUTY;
        }
        return CP_DEBOUNCE_MS_F;
    }
    return CP_DEBOUNCE_MS_DEFAULT;
}

static int32_t CP_ReadVoltageMv(void)
{
    uint32_t adc = 0;
    int32_t v_adc_mv = 0;
    int32_t v_out_mv = 0;

    adc = CP_ReadAdcChannel((uint32_t)4u);
    v_adc_mv = (int32_t)((adc * 3300u) / 4095u);
    v_out_mv = ((v_adc_mv - 1723) * 1000) / 130;

    return v_out_mv;
}

void CP_Init(void) {
    /* TIM3_CH2 (PA7): set 1kHz PWM like original CCS logic. */
    htim3.Instance->PSC = 160 - 1;
    htim3.Instance->ARR = MAX_DUTY - 1;

#if DUTY_INVERT == 0
    htim3.Instance->CCR2 = MAX_DUTY;
    htim3.Instance->CCR1 = MAX_DUTY + 5;
#else
    htim3.Instance->CCR2 = 0;
    htim3.Instance->CCR1 = 0;
#endif

    HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2);
    HAL_TIM_OC_Start_IT(&htim3, TIM_CHANNEL_1);
}

void CP_SetDuty(uint8_t percentage) {
    uint32_t pwmduty = MAX_DUTY * percentage / 100;
    cp_duty = percentage;

#if DUTY_INVERT == 0
    htim3.Instance->CCR2 = pwmduty;
    htim3.Instance->CCR1 = 0 + 1;
#else
    htim3.Instance->CCR2 = MAX_DUTY - pwmduty;
    htim3.Instance->CCR1 = MAX_DUTY - pwmduty + 5;
#endif
}

uint8_t CP_GetDuty(void) {
    return cp_duty;
}

int32_t CP_GetVoltage(void) {
    return cp_voltage_mv;
}

CP_State_t CP_GetState(void) {
    int32_t voltage_real = cp_voltage_filt_mv;
    uint32_t now = HAL_GetTick();

    if(fake_cp_state != EV_STATE_ACQUIRING) {
        return fake_cp_state;
    }

    CP_State_t instant_state = CP_ClassifyWithHysteresis(voltage_real, cp_stable_state);

    if (instant_state == cp_stable_state) {
        cp_candidate_state = cp_stable_state;
        cp_candidate_since_ms = now;
    } else {
        if (cp_candidate_state != instant_state) {
            cp_candidate_state = instant_state;
            cp_candidate_since_ms = now;
        } else if ((now - cp_candidate_since_ms) >= CP_GetDebounceMs(cp_candidate_state)) {
            cp_stable_state = cp_candidate_state;
        }
    }

    return cp_stable_state;
}

void CP_Loop(void) {
    (void)CP_GetState();
}

void HAL_TIM_OC_DelayElapsedCallback(TIM_HandleTypeDef *htim)
{
    if (htim->Instance == TIM3 && htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1) {
        if (ADC_TryLock() == 0u) {
            return;
        }
        cp_voltage_mv = CP_ReadVoltageMv();
        (void)CP_ApplyEma(cp_voltage_mv);
        ADC_Unlock();
    }
}