#include "cp.h"

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

#define MAX_DUTY 450
#define FILTER_ORDER 100

static int32_t cp_voltage_mv = 0;
static uint8_t cp_duty = 0;
CP_State_t fake_cp_state = EV_STATE_ACQUIRING;
CP_State_t cp_state_buffer = EV_STATE_ACQUIRING;

#define VREFINT_CAL_ADDR ((uint16_t*)0x1FFFF7BA) // для STM32F1!

static int32_t CP_ReadVoltageMv(void)
{
    uint32_t adc_cp = adc_data.cp_raw;
    uint32_t adc_vref = adc_data.vrefint_raw; // нужно измерять!

    // VREFINT в мВ (берётся из даташита или калибровки MCU)
    const int32_t VREFINT_MV = 1210;

    // напряжение на входе АЦП
    int32_t v_adc_mv = (adc_cp * VREFINT_MV) / adc_vref;

    // дальше твоя формула
    int32_t v_out_mv = ((v_adc_mv - 1723) * 7704) / 1000;

    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(&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) {
    cp_voltage_mv = CP_ReadVoltageMv();
    return cp_voltage_mv;
}

CP_State_t CP_GetState(void) {
    int32_t voltage_real = CP_GetVoltage();

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

    if (voltage_real >= (12000-1000)) {
        return EV_STATE_A_IDLE;
    } else if (voltage_real >= (9000-1000) && voltage_real <= (9000+1000)) {
        return EV_STATE_B_CONN_PREP;
    } else if (voltage_real >= (6000-1000) && voltage_real <= (6000+1000)) {
        return EV_STATE_C_CONN_ACTIVE;
    } else if (voltage_real >= (3000-1000) && voltage_real <= (3000 + 1000)) {
        return EV_STATE_D_CONN_ACT_VENT;
    } else if (voltage_real >= (0-1000) && voltage_real <= (0+2000)){
        return EV_STATE_E_NO_POWER;
    } else if (voltage_real <= (-12000+1000)) {
        return EV_STATE_F_ERROR;
    } else {
        return EV_STATE_ACQUIRING;
    }
}

CP_State_t CP_GetFilteredState(void) {
    return cp_state_buffer;
}

void CP_FilterState(void) {

    static CP_State_t pending_state = EV_STATE_ACQUIRING;
    static uint8_t stable_count = 0u;
    CP_State_t current_state = CP_GetState();

    /* Keep last accepted state while CP is still acquiring. */
    if (current_state == EV_STATE_ACQUIRING) {
        pending_state = EV_STATE_ACQUIRING;
        stable_count = 0u;
        return;
    }

    if (current_state != pending_state) {
        pending_state = current_state;
        stable_count = 1u;
        return;
    }

    if (stable_count < FILTER_ORDER) {
        stable_count++;
    }

    if (stable_count >= FILTER_ORDER) {
        cp_state_buffer = pending_state;
    }
}

void CP_Loop(void) {
    static uint32_t tick;
    if ((int32_t)(HAL_GetTick() - tick) < 1) return;
    tick = HAL_GetTick();
    static uint8_t initialized = 0;
    static CP_State_t prev_state = EV_STATE_ACQUIRING;
    static uint8_t prev_duty = 0;

    CP_FilterState();

    CP_State_t current_state = CP_GetFilteredState();
    uint8_t current_duty = cp_duty;

    if (!initialized) {
        prev_state = current_state;
        prev_duty = current_duty;
        initialized = 1;
        return;
    }

    if (current_state != prev_state) {
        log_printf(LOG_INFO, "CP state changed: %d -> %d\n", prev_state, current_state);
        prev_state = current_state;
    }

    if (current_duty != prev_duty) {
        log_printf(LOG_INFO, "CP duty changed: %u -> %u\n", prev_duty, current_duty);
        prev_duty = current_duty;
    }
}

