#include "dcdc.h"

#include "app_config.h"
#include "board.h"
#include "stm32g474xx.h"

#define HRTIM_TIMER_C_INDEX          2U
#define ADC_SAMPLE_TIME_47CYCLES     5U
#define HRTIM_DLL_READY_TIMEOUT      1000000UL

static DCDC_State s_state = DCDC_STATE_STOPPED;
static DCDC_Fault s_fault = DCDC_FAULT_NONE;
static uint32_t s_duty_ticks = DCDC_START_DUTY_TICKS;
static uint32_t s_period_ticks = DCDC_HRTIM_PERIOD_TICKS;
static int32_t s_integrator_ticks;
static bool s_hrtim_ready;

static void gpio_init_for_dcdc(void);
static void adc1_init(void);
static uint16_t adc1_read_channel(uint32_t channel);
static bool hrtim1_timer_c_init(void);
static bool hrtim1_wait_dll_ready(void);
static void hrtim1_outputs_enable(bool enable);
static void hrtim1_set_duty(uint32_t duty_ticks);
static uint32_t hrtim_period_from_clock(void);
static uint32_t hrtim_max_duty_ticks(void);
static uint32_t adc_raw_to_mv(uint16_t raw);
static uint32_t sense_mv_to_voltage_mv(uint32_t sense_mv, uint32_t scale_ppm);
static uint32_t sense_mv_to_current_ma(uint32_t sense_mv);
static void set_usbpd_input_switch(bool enable);
static void set_loads_off(void);
static void latch_fault(DCDC_Fault fault);

void DCDC_Init(void)
{
    gpio_init_for_dcdc();
    set_usbpd_input_switch(false);
    set_loads_off();

    adc1_init();
    s_hrtim_ready = hrtim1_timer_c_init();
    if (s_hrtim_ready)
    {
        hrtim1_outputs_enable(false);
    }

    s_state = DCDC_STATE_READY;
    s_fault = DCDC_FAULT_NONE;
}

void DCDC_Start(void)
{
    if (s_fault != DCDC_FAULT_NONE)
    {
        return;
    }

    if (!s_hrtim_ready)
    {
        latch_fault(DCDC_FAULT_HRTIM);
        return;
    }

#if DCDC_CONNECT_USBPD_INPUT
    set_usbpd_input_switch(true);
#endif

    s_integrator_ticks = 0;
    hrtim1_set_duty(DCDC_START_DUTY_TICKS);
    HRTIM1->sMasterRegs.MCR |= HRTIM_MCR_TCCEN;
    hrtim1_outputs_enable(true);
    s_state = DCDC_STATE_RUNNING;
}

void DCDC_Stop(void)
{
    hrtim1_outputs_enable(false);
    hrtim1_set_duty(DCDC_MIN_DUTY_TICKS);
    HRTIM1->sMasterRegs.MCR &= ~HRTIM_MCR_TCCEN;
    set_usbpd_input_switch(false);
    s_state = DCDC_STATE_STOPPED;
}

void DCDC_ControlStep(void)
{
    DCDC_Measurements m;
    int32_t error_mv;
    int32_t feed_forward;
    int32_t duty;

    DCDC_ReadMeasurements(&m);

    if (s_state != DCDC_STATE_RUNNING)
    {
        return;
    }

    if (m.vin_mv < DCDC_MIN_VIN_MV)
    {
        latch_fault(DCDC_FAULT_UNDERVOLTAGE);
        return;
    }

    if (m.vout_mv > DCDC_MAX_VOUT_MV)
    {
        latch_fault(DCDC_FAULT_OVERVOLTAGE);
        return;
    }

    if (m.iin_ma > DCDC_HARD_INPUT_CURRENT_MA)
    {
        latch_fault(DCDC_FAULT_OVERCURRENT);
        return;
    }

    /*
     * Starter buck controller:
     *   1. feed-forward estimates duty from Vout/Vin,
     *   2. PI term removes static voltage error,
     *   3. current term pulls duty down before hard over-current trips.
     *
     * This is deliberately readable, not a final compensated SMPS loop.
     */
    feed_forward = (int32_t)(((uint64_t)DCDC_TARGET_VOUT_MV * s_period_ticks) / m.vin_mv);
    error_mv = (int32_t)DCDC_TARGET_VOUT_MV - (int32_t)m.vout_mv;

    s_integrator_ticks += (error_mv * DCDC_KI_TICKS_PER_100MV) / 100;
    if (s_integrator_ticks > 3000)
    {
        s_integrator_ticks = 3000;
    }
    else if (s_integrator_ticks < -3000)
    {
        s_integrator_ticks = -3000;
    }

    duty = feed_forward +
           ((error_mv * DCDC_KP_TICKS_PER_100MV) / 100) +
           s_integrator_ticks;

    if (m.iin_ma > DCDC_MAX_INPUT_CURRENT_MA)
    {
        duty -= (int32_t)((m.iin_ma - DCDC_MAX_INPUT_CURRENT_MA) * DCDC_CURRENT_LIMIT_KP);
    }

    if (duty < (int32_t)DCDC_MIN_DUTY_TICKS)
    {
        duty = (int32_t)DCDC_MIN_DUTY_TICKS;
    }
    else if (duty > (int32_t)hrtim_max_duty_ticks())
    {
        duty = (int32_t)hrtim_max_duty_ticks();
    }

    hrtim1_set_duty((uint32_t)duty);
}

void DCDC_ReadMeasurements(DCDC_Measurements *out)
{
    uint32_t vin_sense_mv;
    uint32_t iin_sense_mv;
    uint32_t vout_sense_mv;

    out->vin_raw = adc1_read_channel(2U);
    out->iin_raw = adc1_read_channel(3U);
    out->vout_raw = adc1_read_channel(4U);

    vin_sense_mv = adc_raw_to_mv(out->vin_raw);
    iin_sense_mv = adc_raw_to_mv(out->iin_raw);
    vout_sense_mv = adc_raw_to_mv(out->vout_raw);

    out->vin_mv = sense_mv_to_voltage_mv(vin_sense_mv, DCDC_VIN_SCALE_PPM);
    out->iin_ma = sense_mv_to_current_ma(iin_sense_mv);
    out->vout_mv = sense_mv_to_voltage_mv(vout_sense_mv, DCDC_VOUT_SCALE_PPM);
}

DCDC_State DCDC_GetState(void)
{
    return s_state;
}

DCDC_Fault DCDC_GetFault(void)
{
    return s_fault;
}

bool DCDC_IsHrtimReady(void)
{
    return s_hrtim_ready;
}

uint32_t DCDC_GetDutyTicks(void)
{
    return s_duty_ticks;
}

uint32_t DCDC_GetPeriodTicks(void)
{
    return s_period_ticks;
}

const char *DCDC_StateText(DCDC_State state)
{
    switch (state)
    {
        case DCDC_STATE_STOPPED: return "stopped";
        case DCDC_STATE_READY:   return "ready";
        case DCDC_STATE_RUNNING: return "running";
        case DCDC_STATE_FAULT:   return "fault";
        default:                 return "unknown";
    }
}

const char *DCDC_FaultText(DCDC_Fault fault)
{
    switch (fault)
    {
        case DCDC_FAULT_NONE:         return "none";
        case DCDC_FAULT_UNDERVOLTAGE: return "vin undervoltage";
        case DCDC_FAULT_OVERVOLTAGE:  return "vout overvoltage";
        case DCDC_FAULT_OVERCURRENT:  return "input overcurrent";
        case DCDC_FAULT_HRTIM:        return "hrtim dll timeout";
        default:                      return "unknown";
    }
}

static void gpio_init_for_dcdc(void)
{
    RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN | RCC_AHB2ENR_GPIOBEN | RCC_AHB2ENR_GPIOCEN;
    (void)RCC->AHB2ENR;

    /* PA1/PA2/PA3 are analog feedback signals: VIN, input current, VOUT. */
    GPIOA->MODER |= (3UL << (1U * 2U)) | (3UL << (2U * 2U)) | (3UL << (3U * 2U));
    GPIOA->PUPDR &= ~((3UL << (1U * 2U)) | (3UL << (2U * 2U)) | (3UL << (3U * 2U)));

    /* PB12/PB13 are HRTIM1 Timer C outputs for the synchronous buck leg. */
    GPIOB->MODER &= ~((3UL << (12U * 2U)) | (3UL << (13U * 2U)));
    GPIOB->MODER |=  ((2UL << (12U * 2U)) | (2UL << (13U * 2U)));
    GPIOB->OTYPER &= ~((1UL << 12U) | (1UL << 13U));
    GPIOB->OSPEEDR |= ((3UL << (12U * 2U)) | (3UL << (13U * 2U)));
    GPIOB->PUPDR &= ~((3UL << (12U * 2U)) | (3UL << (13U * 2U)));
    GPIOB->AFR[1] &= ~((0xFUL << ((12U - 8U) * 4U)) | (0xFUL << ((13U - 8U) * 4U)));
    GPIOB->AFR[1] |=  ((13UL  << ((12U - 8U) * 4U)) | (13UL  << ((13U - 8U) * 4U)));

    /* PC3 controls USBPD_VBUS to VIN switch. PC14/PC15 switch onboard loads. */
    GPIOC->MODER &= ~((3UL << (3U * 2U)) | (3UL << (14U * 2U)) | (3UL << (15U * 2U)));
    GPIOC->MODER |=  ((1UL << (3U * 2U)) | (1UL << (14U * 2U)) | (1UL << (15U * 2U)));
    GPIOC->OTYPER &= ~((1UL << 3U) | (1UL << 14U) | (1UL << 15U));
    GPIOC->OSPEEDR |= ((2UL << (3U * 2U)) | (2UL << (14U * 2U)) | (2UL << (15U * 2U)));
    GPIOC->PUPDR &= ~((3UL << (3U * 2U)) | (3UL << (14U * 2U)) | (3UL << (15U * 2U)));
}

static void adc1_init(void)
{
    RCC->AHB2ENR |= RCC_AHB2ENR_ADC12EN;
    (void)RCC->AHB2ENR;

    /* Synchronous ADC clock HCLK/4 = 42.5 MHz. */
    ADC12_COMMON->CCR = (ADC12_COMMON->CCR & ~ADC_CCR_CKMODE) |
                        (ADC_CCR_CKMODE_1 | ADC_CCR_CKMODE_0);

    ADC1->CR &= ~ADC_CR_DEEPPWD;
    ADC1->CR |= ADC_CR_ADVREGEN;
    Board_DelayMs(1U);

    ADC1->CR |= ADC_CR_ADCAL;
    while ((ADC1->CR & ADC_CR_ADCAL) != 0U)
    {
        __NOP();
    }

    ADC1->CFGR = ADC_CFGR_OVRMOD;
    ADC1->SMPR1 =
        (ADC_SAMPLE_TIME_47CYCLES << ADC_SMPR1_SMP2_Pos) |
        (ADC_SAMPLE_TIME_47CYCLES << ADC_SMPR1_SMP3_Pos) |
        (ADC_SAMPLE_TIME_47CYCLES << ADC_SMPR1_SMP4_Pos);

    ADC1->ISR = ADC_ISR_ADRDY;
    ADC1->CR |= ADC_CR_ADEN;
    while ((ADC1->ISR & ADC_ISR_ADRDY) == 0U)
    {
        __NOP();
    }
}

static uint16_t adc1_read_channel(uint32_t channel)
{
    ADC1->SQR1 = (channel << ADC_SQR1_SQ1_Pos);
    ADC1->ISR = ADC_ISR_EOC | ADC_ISR_EOS;
    ADC1->CR |= ADC_CR_ADSTART;

    while ((ADC1->ISR & ADC_ISR_EOC) == 0U)
    {
        __NOP();
    }

    return (uint16_t)(ADC1->DR & 0x0FFFU);
}

static bool hrtim1_timer_c_init(void)
{
    HRTIM_Timerx_TypeDef *timer = &HRTIM1->sTimerxRegs[HRTIM_TIMER_C_INDEX];

    RCC->APB2ENR |= RCC_APB2ENR_HRTIM1EN;
    (void)RCC->APB2ENR;

    HRTIM1->sCommonRegs.ODISR = HRTIM_ODISR_TC1ODIS | HRTIM_ODISR_TC2ODIS;
    s_period_ticks = hrtim_period_from_clock();

    HRTIM1->sCommonRegs.ICR = HRTIM_ICR_DLLRDYC;
    HRTIM1->sCommonRegs.DLLCR = HRTIM_DLLCR_CALEN |
                                HRTIM_DLLCR_CALRTE_1 |
                                HRTIM_DLLCR_CAL;
    /* In rescue clock mode DLLRDY may never rise; keep diagnostics alive. */
    if (!hrtim1_wait_dll_ready())
    {
        HRTIM1->sCommonRegs.ODISR = HRTIM_ODISR_TC1ODIS | HRTIM_ODISR_TC2ODIS;
        HRTIM1->sMasterRegs.MCR &= ~HRTIM_MCR_TCCEN;
        return false;
    }

    timer->TIMxCR = HRTIM_TIMCR_CONT;
    timer->PERxR = s_period_ticks;
    timer->REPxR = 0U;
    timer->CMP1xR = DCDC_MIN_DUTY_TICKS;
    timer->CMP2xR = hrtim_max_duty_ticks();
    timer->CMP3xR = 1000U;

    timer->DTxR =
        ((DCDC_DEADTIME_RISING_TICKS & 0x1FFUL) << HRTIM_DTR_DTR_Pos) |
        ((DCDC_DEADTIME_FALLING_TICKS & 0x1FFUL) << HRTIM_DTR_DTF_Pos) |
        (HRTIM_DTR_DTPRSC_1 | HRTIM_DTR_DTPRSC_0);

    /*
     * Complementary buck PWM:
     *   CHC1 goes active at period event and inactive at CMP1.
     *   CHC2 goes active at CMP1 and inactive at period event.
     * HRTIM dead-time block delays transitions to avoid shoot-through.
     */
    timer->SETx1R = HRTIM_SET1R_PER;
    timer->RSTx1R = HRTIM_RST1R_CMP1;
    timer->SETx2R = HRTIM_SET2R_CMP1;
    timer->RSTx2R = HRTIM_RST2R_PER;
    timer->OUTxR = HRTIM_OUTR_DTEN | HRTIM_OUTR_FAULT1_1 | HRTIM_OUTR_FAULT2_1;

    /* Trigger point for future synchronized ADC sampling. */
    HRTIM1->sCommonRegs.ADC1R = HRTIM_ADC1R_AD1TCC3;

    HRTIM1->sMasterRegs.MCR &= ~HRTIM_MCR_TCCEN;
    return true;
}

static bool hrtim1_wait_dll_ready(void)
{
    uint32_t timeout = HRTIM_DLL_READY_TIMEOUT;

    while (((HRTIM1->sCommonRegs.ISR & HRTIM_ISR_DLLRDY) == 0U) && (timeout > 0U))
    {
        timeout--;
    }

    return timeout > 0U;
}

static void hrtim1_outputs_enable(bool enable)
{
    if (enable)
    {
        HRTIM1->sCommonRegs.OENR = HRTIM_OENR_TC1OEN | HRTIM_OENR_TC2OEN;
    }
    else
    {
        HRTIM1->sCommonRegs.ODISR = HRTIM_ODISR_TC1ODIS | HRTIM_ODISR_TC2ODIS;
    }
}

static void hrtim1_set_duty(uint32_t duty_ticks)
{
    if (duty_ticks < DCDC_MIN_DUTY_TICKS)
    {
        duty_ticks = DCDC_MIN_DUTY_TICKS;
    }
    else if (duty_ticks > hrtim_max_duty_ticks())
    {
        duty_ticks = hrtim_max_duty_ticks();
    }

    HRTIM1->sTimerxRegs[HRTIM_TIMER_C_INDEX].CMP1xR = duty_ticks;
    s_duty_ticks = duty_ticks;
}

static uint32_t hrtim_period_from_clock(void)
{
    uint64_t ticks = (((uint64_t)SystemCoreClock * 32ULL) + (DCDC_PWM_FREQUENCY_HZ / 2ULL)) /
                     DCDC_PWM_FREQUENCY_HZ;

    if (ticks < 100ULL)
    {
        ticks = 100ULL;
    }
    else if (ticks > 0xFFFFULL)
    {
        ticks = 0xFFFFULL;
    }

    return (uint32_t)ticks;
}

static uint32_t hrtim_max_duty_ticks(void)
{
    uint32_t max_ticks = DCDC_MAX_DUTY_TICKS;

    if (s_period_ticks > 10U && max_ticks >= (s_period_ticks - 1U))
    {
        max_ticks = s_period_ticks - 1U;
    }

    return max_ticks;
}

static uint32_t adc_raw_to_mv(uint16_t raw)
{
    return ((uint32_t)raw * ADC_REFERENCE_MV) / ADC_FULL_SCALE_COUNTS;
}

static uint32_t sense_mv_to_voltage_mv(uint32_t sense_mv, uint32_t scale_ppm)
{
    return (uint32_t)(((uint64_t)sense_mv * 1000000ULL) / scale_ppm);
}

static uint32_t sense_mv_to_current_ma(uint32_t sense_mv)
{
    return (sense_mv * 1000UL) / DCDC_IIN_UV_PER_MA;
}

static void set_usbpd_input_switch(bool enable)
{
    if (enable)
    {
        GPIOC->BSRR = (1UL << 3U);
    }
    else
    {
        GPIOC->BRR = (1UL << 3U);
    }
}

static void set_loads_off(void)
{
    GPIOC->BRR = (1UL << 14U) | (1UL << 15U);
}

static void latch_fault(DCDC_Fault fault)
{
    hrtim1_outputs_enable(false);
    hrtim1_set_duty(DCDC_MIN_DUTY_TICKS);
    HRTIM1->sMasterRegs.MCR &= ~HRTIM_MCR_TCCEN;
    set_usbpd_input_switch(false);

    s_fault = fault;
    s_state = DCDC_STATE_FAULT;
}