#include "cp.h"
#include "tim.h"

#define CP_CAPTURE_TARGET_CLK_HZ 1000000U
#define CP_INPUT_SIGNAL_INVERTED 1U

typedef enum {
    CP_WAIT_RISING_1 = 0,
    CP_WAIT_FALLING,
    CP_WAIT_RISING_2
} CP_CaptureState_t;

static volatile CP_CaptureState_t cp_capture_state = CP_WAIT_RISING_1;
static volatile uint32_t cp_rise_1 = 0;
static volatile uint32_t cp_fall = 0;
static volatile uint32_t cp_rise_2 = 0;
static volatile uint32_t cp_last_update_ms = 0;
static volatile CP_Measurement_t cp_measurement = {0, 0, 0};

static uint32_t CP_TicksDiff(uint32_t now, uint32_t prev, uint32_t arr);
static uint32_t CP_GetTimerClockHz(void);
static void CP_ApplyStateOutputs(void);
static uint32_t CP_GetTim3InputClockHz(void);

void CP_Init(void) {
    TIM_IC_InitTypeDef sConfigIC = {0};
    uint32_t tim3_input_clk = CP_GetTim3InputClockHz();
    uint32_t target_prescaler = 0U;

    if (tim3_input_clk > CP_CAPTURE_TARGET_CLK_HZ) {
        target_prescaler = (tim3_input_clk / CP_CAPTURE_TARGET_CLK_HZ) - 1U;
    }
    if (target_prescaler > 0xFFFFU) {
        target_prescaler = 0xFFFFU;
    }

    __HAL_TIM_SET_PRESCALER(&htim3, (uint16_t)target_prescaler);
    __HAL_TIM_SET_COUNTER(&htim3, 0U);

    sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING;
    sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI;
    sConfigIC.ICPrescaler = TIM_ICPSC_DIV1;
    sConfigIC.ICFilter = 2;
    HAL_TIM_IC_ConfigChannel(&htim3, &sConfigIC, TIM_CHANNEL_2);

    cp_capture_state = CP_WAIT_RISING_1;
    cp_measurement.valid = 0;
    cp_last_update_ms = HAL_GetTick();
    CP_ApplyStateOutputs();
    HAL_TIM_IC_Start_IT(&htim3, TIM_CHANNEL_2);
}

void CP_Task(void) {
    CP_ApplyStateOutputs();

    if ((HAL_GetTick() - cp_last_update_ms) > 200U) {
        cp_measurement.valid = 0;
        cp_measurement.frequency_hz = 0;
        cp_measurement.duty_percent = 0;
    }
}

CP_Measurement_t CP_GetMeasurement(void) {
    CP_Measurement_t snapshot;
    __disable_irq();
    snapshot = cp_measurement;
    __enable_irq();
    return snapshot;
}

void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) {
    if ((htim->Instance != TIM3) || (htim->Channel != HAL_TIM_ACTIVE_CHANNEL_2)) {
        return;
    }

    uint32_t captured = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_2);
    uint32_t arr = __HAL_TIM_GET_AUTORELOAD(htim);
    TIM_RESET_CAPTUREPOLARITY(htim, TIM_CHANNEL_2);

    if (cp_capture_state == CP_WAIT_RISING_1) {
        cp_rise_1 = captured;
        cp_capture_state = CP_WAIT_FALLING;
        TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_2, TIM_INPUTCHANNELPOLARITY_FALLING);
        return;
    }

    if (cp_capture_state == CP_WAIT_FALLING) {
        cp_fall = captured;
        cp_capture_state = CP_WAIT_RISING_2;
        TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_2, TIM_INPUTCHANNELPOLARITY_RISING);
        return;
    }

    cp_rise_2 = captured;
    cp_capture_state = CP_WAIT_RISING_1;
    TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_2, TIM_INPUTCHANNELPOLARITY_RISING);

    uint32_t period_ticks = CP_TicksDiff(cp_rise_2, cp_rise_1, arr);
    uint32_t high_ticks = CP_TicksDiff(cp_fall, cp_rise_1, arr);
    uint32_t timer_clk = CP_GetTimerClockHz();

    if ((period_ticks == 0U) || (high_ticks > period_ticks) || (timer_clk == 0U)) {
        cp_measurement.valid = 0;
        return;
    }

    cp_measurement.frequency_hz = timer_clk / period_ticks;
    uint32_t duty_high = (high_ticks * 100U + (period_ticks / 2U)) / period_ticks; /* rounded */
    if (duty_high > 100U) {
        duty_high = 100U;
    }
    uint32_t duty = duty_high;
#if CP_INPUT_SIGNAL_INVERTED
    duty = 100U - duty_high;
#endif
    cp_measurement.duty_percent = (uint8_t)duty;
    cp_measurement.valid = 1;
    cp_last_update_ms = HAL_GetTick();
}

static uint32_t CP_TicksDiff(uint32_t now, uint32_t prev, uint32_t arr) {
    if (now >= prev) {
        return now - prev;
    }
    return (arr - prev + 1U) + now;
}

static uint32_t CP_GetTimerClockHz(void) {
    uint32_t timclk = CP_GetTim3InputClockHz();
    uint32_t prescaler = htim3.Instance->PSC;
    return timclk / (prescaler + 1U);
}

static uint32_t CP_GetTim3InputClockHz(void) {
    RCC_ClkInitTypeDef clk_init = {0};
    uint32_t flash_latency = 0U;
    uint32_t pclk1 = HAL_RCC_GetPCLK1Freq();
    HAL_RCC_GetClockConfig(&clk_init, &flash_latency);

    if (clk_init.APB1CLKDivider == RCC_HCLK_DIV1) {
        return pclk1;
    }
    return pclk1 * 2U;
}

static void CP_ApplyStateOutputs(void) {
    static CP_State_t last_state = (CP_State_t)0xFF;

    if (last_state == cp_state) {
        return;
    }

    switch (cp_state) {
    case EV_STATE_A_IDLE:
        HAL_GPIO_WritePin(CP_RELAY_GPIO_Port, CP_RELAY_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(CP_STATE_C_GPIO_Port, CP_STATE_C_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(CP_STATE_F_GPIO_Port, CP_STATE_F_Pin, GPIO_PIN_RESET);
        break;
    case EV_STATE_B_CONN_PREP:
        HAL_GPIO_WritePin(CP_RELAY_GPIO_Port, CP_RELAY_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(CP_STATE_C_GPIO_Port, CP_STATE_C_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(CP_STATE_F_GPIO_Port, CP_STATE_F_Pin, GPIO_PIN_RESET);
        break;
    case EV_STATE_C_CONN_ACTIVE:
        HAL_GPIO_WritePin(CP_RELAY_GPIO_Port, CP_RELAY_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(CP_STATE_C_GPIO_Port, CP_STATE_C_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(CP_STATE_F_GPIO_Port, CP_STATE_F_Pin, GPIO_PIN_RESET);
        break;
    case EV_STATE_F_ERROR:
        HAL_GPIO_WritePin(CP_RELAY_GPIO_Port, CP_RELAY_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(CP_STATE_C_GPIO_Port, CP_STATE_C_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(CP_STATE_F_GPIO_Port, CP_STATE_F_Pin, GPIO_PIN_SET);
        break;

    case EV_STATE_D_CONN_ACT_VENT:
        HAL_GPIO_WritePin(CP_RELAY_GPIO_Port, CP_RELAY_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(CP_STATE_C_GPIO_Port, CP_STATE_C_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(CP_STATE_F_GPIO_Port, CP_STATE_F_Pin, GPIO_PIN_RESET);
        break;
    case EV_STATE_E_NO_POWER:
        HAL_GPIO_WritePin(CP_RELAY_GPIO_Port, CP_RELAY_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(CP_STATE_C_GPIO_Port, CP_STATE_C_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(CP_STATE_F_GPIO_Port, CP_STATE_F_Pin, GPIO_PIN_SET);
        break;
    case EV_STATE_ACQUIRING:
    default:
        HAL_GPIO_WritePin(CP_RELAY_GPIO_Port, CP_RELAY_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(CP_STATE_F_GPIO_Port, CP_STATE_F_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(CP_STATE_C_GPIO_Port, CP_STATE_C_Pin, GPIO_PIN_RESET);
        break;
    }

    last_state = cp_state;
}