#include "ad_board.h"

#include <string.h>

#include "ad_inverter.h"
#include "main.h"
#include "simulink_interface.h"
#include "stm32g4xx_ll_adc.h"

#define AD_BOARD_SLOW_PERIOD_MS             (10UL)
#define AD_BOARD_IDLE_BLINK_MS              (500UL)
#define AD_BOARD_FAULT_BLINK_MS             (100UL)
#define AD_BOARD_BUTTON_DEBOUNCE_MS         (200UL)

#define AD_BOARD_DEFAULT_CURRENT_LIMIT_A    (10.0f)
#define AD_BOARD_DEFAULT_OVERVOLTAGE_V      (60.0f)
#define AD_BOARD_DEFAULT_UNDERVOLTAGE_V     (0.0f)
#define AD_BOARD_DEFAULT_SPEED_LIMIT_RPM    (1000.0f)
#define AD_BOARD_DEFAULT_TEMPERATURE_C      (85.0f)
#define AD_BOARD_DEFAULT_PWM_DUTY_LIMIT     (AD_PARAM_ID_DEFAULT_PWM_DUTY_LIMIT)

#ifndef AD_BOARD_ADC_ENABLE
#define AD_BOARD_ADC_ENABLE                 1
#endif

#ifndef AD_BOARD_LED_ENABLE
#define AD_BOARD_LED_ENABLE                 0
#endif

#ifndef AD_BOARD_ADC_VREF_V
#define AD_BOARD_ADC_VREF_V                 (3.3f)
#endif

#ifndef AD_BOARD_ADC_RAW_FULL_SCALE
#define AD_BOARD_ADC_RAW_FULL_SCALE         (4095.0f)
#endif

#ifndef AD_BOARD_ADC_CURRENT_ZERO_V
#define AD_BOARD_ADC_CURRENT_ZERO_V         (AD_BOARD_ADC_VREF_V * 0.5f)
#endif

#ifndef AD_BOARD_ADC_DEFAULT_PHASE_SHUNT_OHM
#define AD_BOARD_ADC_DEFAULT_PHASE_SHUNT_OHM (0.1f)
#endif

#ifndef AD_BOARD_ADC_MIN_PHASE_SHUNT_OHM
#define AD_BOARD_ADC_MIN_PHASE_SHUNT_OHM    (0.001f)
#endif

#ifndef AD_BOARD_ADC_MAX_PHASE_SHUNT_OHM
#define AD_BOARD_ADC_MAX_PHASE_SHUNT_OHM    (65.535f)
#endif

#ifndef AD_BOARD_ADC_CURRENT_A_PER_V
#define AD_BOARD_ADC_CURRENT_A_PER_V        (1.0f / AD_BOARD_ADC_DEFAULT_PHASE_SHUNT_OHM)
#endif

#ifndef AD_BOARD_ADC_VDC_BUS_V_PER_ADC_V
/* High range calibration. Above about 14.5 V this scale matches the bench supply. */
#define AD_BOARD_ADC_VDC_BUS_V_PER_ADC_V    (12.1770f)
#endif

#ifndef AD_BOARD_ADC_VDC_LOW_CAL_ADC_V
#define AD_BOARD_ADC_VDC_LOW_CAL_ADC_V      (0.618f)
#endif

#ifndef AD_BOARD_ADC_VDC_LOW_CAL_BUS_V
#define AD_BOARD_ADC_VDC_LOW_CAL_BUS_V      (10.0f)
#endif

#ifndef AD_BOARD_ADC_VDC_HIGH_CAL_BUS_V
#define AD_BOARD_ADC_VDC_HIGH_CAL_BUS_V     (14.5f)
#endif

#ifndef AD_BOARD_ADC_VDC_AVG_SAMPLES
#define AD_BOARD_ADC_VDC_AVG_SAMPLES        (16UL)
#endif

#ifndef AD_BOARD_ADC_VDC_FILTER_ALPHA
#define AD_BOARD_ADC_VDC_FILTER_ALPHA       (0.125f)
#endif

#ifndef AD_BOARD_ADC_CURRENT_RMS_FILTER_ALPHA
#define AD_BOARD_ADC_CURRENT_RMS_FILTER_ALPHA (0.02f)
#endif

#ifndef AD_BOARD_ADC_MCU_TEMP_FILTER_ALPHA
#define AD_BOARD_ADC_MCU_TEMP_FILTER_ALPHA  (0.05f)
#endif

#ifndef AD_BOARD_TORQUE_FILTER_ALPHA
#define AD_BOARD_TORQUE_FILTER_ALPHA        (0.05f)
#endif

#ifndef AD_BOARD_TORQUE_MIN_SPEED_RAD_S
#define AD_BOARD_TORQUE_MIN_SPEED_RAD_S     (1.0f)
#endif

#ifndef AD_BOARD_TORQUE_MIN_ACCEL_RAD_S2
#define AD_BOARD_TORQUE_MIN_ACCEL_RAD_S2    (0.1f)
#endif

#ifndef AD_BOARD_TORQUE_MIN_POWER_W
#define AD_BOARD_TORQUE_MIN_POWER_W         (0.01f)
#endif

#ifndef AD_BOARD_TORQUE_MIN_CURRENT_RMS_A
#define AD_BOARD_TORQUE_MIN_CURRENT_RMS_A   (0.01f)
#endif

#ifndef AD_BOARD_TORQUE_RMS_POWER_FACTOR
#define AD_BOARD_TORQUE_RMS_POWER_FACTOR    (1.0f)
#endif

#ifndef AD_BOARD_TORQUE_DEFAULT_POLE_PAIRS
#define AD_BOARD_TORQUE_DEFAULT_POLE_PAIRS  ((float)AD_PARAM_ID_DEFAULT_POLE_PAIRS)
#endif

#ifndef AD_BOARD_TORQUE_MAX_ABS_NM
#define AD_BOARD_TORQUE_MAX_ABS_NM          (1000.0f)
#endif

#ifndef AD_BOARD_SLIP_MIN_SYNC_RPM
#define AD_BOARD_SLIP_MIN_SYNC_RPM          (1.0f)
#endif

#ifndef AD_BOARD_SLIP_MAX_ABS_PERCENT
#define AD_BOARD_SLIP_MAX_ABS_PERCENT       (327.67f)
#endif

#ifndef AD_BOARD_ADC_DEFAULT_MCU_TEMP_C
#define AD_BOARD_ADC_DEFAULT_MCU_TEMP_C     (25.0f)
#endif

#ifndef AD_BOARD_ADC_ZERO_CAL_ENABLE
#define AD_BOARD_ADC_ZERO_CAL_ENABLE        1
#endif

#ifndef AD_BOARD_ADC_ZERO_CAL_SAMPLES
#define AD_BOARD_ADC_ZERO_CAL_SAMPLES       (64UL)
#endif

#ifndef AD_BOARD_ADC_VDC_ZERO_MAX_ADC_V
#define AD_BOARD_ADC_VDC_ZERO_MAX_ADC_V     (0.05f)
#endif

#ifndef AD_BOARD_ADC_TIMEOUT
#define AD_BOARD_ADC_TIMEOUT                (1000000UL)
#endif

#ifndef AD_BOARD_ADC_TIMEOUT_FAULT_THRESHOLD
#define AD_BOARD_ADC_TIMEOUT_FAULT_THRESHOLD (3U)
#endif

/* X-NUCLEO-IHM08M1 schematic, Figure 6: PA0-PhA, PC1-PhB, PC0-PhC, PA1-BUSV. */
#define AD_BOARD_ADC_CH_IA                  (1UL)
#define AD_BOARD_ADC_CH_IB                  (7UL)
#define AD_BOARD_ADC_CH_IC                  (6UL)
#define AD_BOARD_ADC_CH_VDC                 (2UL)
#define AD_BOARD_ADC_CH_MCU_TEMP            (16UL)
#define AD_BOARD_ADC_GPIOA_PIN_MASK         (GPIO_PIN_0 | GPIO_PIN_1)
#define AD_BOARD_ADC_GPIOC_PIN_MASK         (GPIO_PIN_0 | GPIO_PIN_1)
#define AD_BOARD_ADC_FAULT_INIT             (1UL << 0)
#define AD_BOARD_ADC_FAULT_TIMEOUT          (1UL << 1)
#define AD_BOARD_TWO_PI                     (6.28318530717958647692f)

static volatile uint8_t g_button_toggle_request;
static uint8_t g_logging_enabled;
static uint8_t g_led_state;
static uint32_t g_last_button_tick_ms;
static uint32_t g_last_slow_tick_ms;
static uint32_t g_last_led_tick_ms;
static AD_BoardAdcSnapshot_t g_adc_snapshot;
static uint8_t g_vdc_filter_ready;
static float g_vdc_adc_filtered_V;
static uint8_t g_current_rms_filter_ready;
static float g_ia_rms_sq_A2;
static float g_ib_rms_sq_A2;
static float g_ic_rms_sq_A2;
static uint8_t g_mcu_temp_filter_ready;
static float g_mcu_temperature_C;
static uint8_t g_adc_timeout_count;
static uint8_t g_torque_speed_ready;
static uint8_t g_torque_filter_ready;
static uint32_t g_torque_timestamp_us;
static float g_torque_speed_rad_s;
static float g_torque_filtered_Nm;
static float g_phase_shunt_resistance_ohm;

static uint8_t wait_bits_set(volatile uint32_t *reg, uint32_t mask, uint32_t timeout)
{
    while (((*reg) & mask) == 0UL) {
        if (timeout == 0UL) {
            return 0U;
        }
        timeout--;
    }
    return 1U;
}

static uint8_t wait_bits_clear(volatile uint32_t *reg, uint32_t mask, uint32_t timeout)
{
    while (((*reg) & mask) != 0UL) {
        if (timeout == 0UL) {
            return 0U;
        }
        timeout--;
    }
    return 1U;
}

static uint8_t board_adc_stop_regular(void)
{
#if AD_BOARD_ADC_ENABLE != 0
    if ((ADC1->CR & ADC_CR_ADSTART) == 0UL) {
        return 1U;
    }

    ADC1->CR |= ADC_CR_ADSTP;
    if (wait_bits_clear(&ADC1->CR, ADC_CR_ADSTART, AD_BOARD_ADC_TIMEOUT) == 0U) {
        return 0U;
    }
    if (wait_bits_clear(&ADC1->CR, ADC_CR_ADSTP, AD_BOARD_ADC_TIMEOUT) == 0U) {
        return 0U;
    }
#endif

    return 1U;
}

static void board_adc_note_timeout(void)
{
    if (g_adc_timeout_count < 0xFFU) {
        g_adc_timeout_count++;
    }

    if (g_adc_timeout_count >= AD_BOARD_ADC_TIMEOUT_FAULT_THRESHOLD) {
        g_adc_snapshot.faults |= AD_BOARD_ADC_FAULT_TIMEOUT;
    }
}

static void board_adc_note_success(void)
{
    g_adc_timeout_count = 0U;
    g_adc_snapshot.faults &= ~AD_BOARD_ADC_FAULT_TIMEOUT;
}

static float adc_raw_to_voltage(uint16_t raw)
{
    return ((float)raw * AD_BOARD_ADC_VREF_V) / AD_BOARD_ADC_RAW_FULL_SCALE;
}

static float board_normalize_phase_shunt_resistance(float resistance_ohm)
{
    if (resistance_ohm <= 0.0f) {
        return AD_BOARD_ADC_DEFAULT_PHASE_SHUNT_OHM;
    }
    if (resistance_ohm < AD_BOARD_ADC_MIN_PHASE_SHUNT_OHM) {
        return AD_BOARD_ADC_MIN_PHASE_SHUNT_OHM;
    }
    if (resistance_ohm > AD_BOARD_ADC_MAX_PHASE_SHUNT_OHM) {
        return AD_BOARD_ADC_MAX_PHASE_SHUNT_OHM;
    }

    return resistance_ohm;
}

static float board_phase_shunt_resistance_ohm(void)
{
    return board_normalize_phase_shunt_resistance(g_phase_shunt_resistance_ohm);
}

static float board_current_a_per_v(void)
{
    return AD_BOARD_ADC_CURRENT_A_PER_V *
           (AD_BOARD_ADC_DEFAULT_PHASE_SHUNT_OHM / board_phase_shunt_resistance_ohm());
}

static float adc_current_from_voltage(float voltage, float zero_voltage)
{
    if (zero_voltage <= 0.0f) {
        zero_voltage = AD_BOARD_ADC_CURRENT_ZERO_V;
    }

    return (voltage - zero_voltage) * board_current_a_per_v();
}

static float adc_vdc_from_voltage(float vdc_adc_V)
{
    float high_adc_V;
    float low_slope;
    float mid_slope;
    float vdc_V;

    if (vdc_adc_V <= 0.0f) {
        return 0.0f;
    }

    high_adc_V = AD_BOARD_ADC_VDC_HIGH_CAL_BUS_V / AD_BOARD_ADC_VDC_BUS_V_PER_ADC_V;
    if ((AD_BOARD_ADC_VDC_LOW_CAL_ADC_V <= 0.0f) ||
        (AD_BOARD_ADC_VDC_LOW_CAL_BUS_V <= 0.0f) ||
        (high_adc_V <= AD_BOARD_ADC_VDC_LOW_CAL_ADC_V)) {
        return vdc_adc_V * AD_BOARD_ADC_VDC_BUS_V_PER_ADC_V;
    }

    if (vdc_adc_V <= AD_BOARD_ADC_VDC_LOW_CAL_ADC_V) {
        low_slope = AD_BOARD_ADC_VDC_LOW_CAL_BUS_V / AD_BOARD_ADC_VDC_LOW_CAL_ADC_V;
        return vdc_adc_V * low_slope;
    }

    if (vdc_adc_V >= high_adc_V) {
        return vdc_adc_V * AD_BOARD_ADC_VDC_BUS_V_PER_ADC_V;
    }

    mid_slope = (AD_BOARD_ADC_VDC_HIGH_CAL_BUS_V - AD_BOARD_ADC_VDC_LOW_CAL_BUS_V) /
                (high_adc_V - AD_BOARD_ADC_VDC_LOW_CAL_ADC_V);
    vdc_V = AD_BOARD_ADC_VDC_LOW_CAL_BUS_V +
            ((vdc_adc_V - AD_BOARD_ADC_VDC_LOW_CAL_ADC_V) * mid_slope);

    return (vdc_V > 0.0f) ? vdc_V : 0.0f;
}

static float adc_mcu_temperature_from_raw(uint16_t raw)
{
    float cal1;
    float cal2;
    float raw_at_cal_vref;
    float temp_C;

    cal1 = (float)(*TEMPSENSOR_CAL1_ADDR);
    cal2 = (float)(*TEMPSENSOR_CAL2_ADDR);
    if ((raw == 0U) || (cal2 <= cal1)) {
        return AD_BOARD_ADC_DEFAULT_MCU_TEMP_C;
    }

    raw_at_cal_vref = ((float)raw * (AD_BOARD_ADC_VREF_V * 1000.0f)) /
                      (float)TEMPSENSOR_CAL_VREFANALOG;
    temp_C = (((raw_at_cal_vref - cal1) *
               (float)(TEMPSENSOR_CAL2_TEMP - TEMPSENSOR_CAL1_TEMP)) /
              (cal2 - cal1)) +
             (float)TEMPSENSOR_CAL1_TEMP;

    if (temp_C < -40.0f) {
        temp_C = -40.0f;
    } else if (temp_C > 150.0f) {
        temp_C = 150.0f;
    }

    return temp_C;
}

static float board_mcu_temperature_filter(float temp_C)
{
    float alpha = AD_BOARD_ADC_MCU_TEMP_FILTER_ALPHA;

    if (alpha < 0.0f) {
        alpha = 0.0f;
    } else if (alpha > 1.0f) {
        alpha = 1.0f;
    }

    if (g_mcu_temp_filter_ready == 0U) {
        g_mcu_temperature_C = temp_C;
        g_mcu_temp_filter_ready = 1U;
    } else {
        g_mcu_temperature_C += alpha * (temp_C - g_mcu_temperature_C);
    }

    return g_mcu_temperature_C;
}

static uint16_t board_adc_read_channel(uint32_t channel)
{
#if AD_BOARD_ADC_ENABLE != 0
    if (board_adc_stop_regular() == 0U) {
        board_adc_note_timeout();
        return 0U;
    }

    ADC1->SQR1 = ((channel & 0x1FUL) << 6);
    ADC1->ISR = ADC_ISR_EOSMP | ADC_ISR_EOC | ADC_ISR_EOS | ADC_ISR_OVR;
    ADC1->CR |= ADC_CR_ADSTART;

    if (wait_bits_set(&ADC1->ISR, ADC_ISR_EOC, AD_BOARD_ADC_TIMEOUT) == 0U) {
        (void)board_adc_stop_regular();
        board_adc_note_timeout();
        return 0U;
    }

    board_adc_note_success();
    return (uint16_t)(ADC1->DR & 0xFFFFUL);
#else
    (void)channel;
    return 0U;
#endif
}

static uint16_t board_adc_read_channel_average(uint32_t channel, uint32_t samples, uint16_t *last_raw)
{
#if AD_BOARD_ADC_ENABLE != 0
    uint32_t index;
    uint32_t sum = 0UL;
    uint16_t raw = 0U;

    if (samples == 0UL) {
        samples = 1UL;
    }

    for (index = 0UL; index < samples; index++) {
        raw = board_adc_read_channel(channel);
        sum += raw;
    }

    if (last_raw != 0) {
        *last_raw = raw;
    }

    return (uint16_t)((sum + (samples / 2UL)) / samples);
#else
    (void)channel;
    (void)samples;
    if (last_raw != 0) {
        *last_raw = 0U;
    }
    return 0U;
#endif
}

static float adc_voltage_minus_offset(float voltage, float offset)
{
    voltage -= offset;
    return (voltage > 0.0f) ? voltage : 0.0f;
}

static float board_absf(float value)
{
    return (value < 0.0f) ? -value : value;
}

static float board_rpm_to_rad_s(float rpm)
{
    return rpm * (AD_BOARD_TWO_PI / 60.0f);
}

static float board_clamp_abs(float value, float limit)
{
    if (limit <= 0.0f) {
        return value;
    }
    if (value > limit) {
        return limit;
    }
    if (value < -limit) {
        return -limit;
    }
    return value;
}

static float board_get_pole_pairs(const AD_MotorParameters_t *params)
{
    if ((params != 0) && (params->pole_pairs > 0.0f)) {
        if (params->pole_pairs > (float)AD_PARAM_ID_MAX_POLE_PAIRS) {
            return (float)AD_PARAM_ID_MAX_POLE_PAIRS;
        }
        return params->pole_pairs;
    }

    return AD_BOARD_TORQUE_DEFAULT_POLE_PAIRS;
}

static float board_torque_omega_from_pwm(const AD_MotorParameters_t *params)
{
    const AD_InverterState_t *inverter = AD_Inverter_GetState();
    float pole_pairs;

    if ((inverter == 0) ||
        (inverter->pwm_running == 0U) ||
        (inverter->electrical_frequency_Hz <= 0.0f)) {
        return 0.0f;
    }

    pole_pairs = board_get_pole_pairs(params);
    if (pole_pairs <= 0.0f) {
        pole_pairs = AD_BOARD_TORQUE_DEFAULT_POLE_PAIRS;
    }

    return (AD_BOARD_TWO_PI * inverter->electrical_frequency_Hz) / pole_pairs;
}

static float board_torque_omega_rad_s(const AD_Measurements_t *meas,
                                      const AD_MotorParameters_t *params)
{
    float omega_rad_s;

    if (meas == 0) {
        return 0.0f;
    }

    omega_rad_s = board_rpm_to_rad_s(meas->speed_rpm);
    if (board_absf(omega_rad_s) >= AD_BOARD_TORQUE_MIN_SPEED_RAD_S) {
        return omega_rad_s;
    }

    omega_rad_s = board_torque_omega_from_pwm(params);
    if (board_absf(omega_rad_s) >= AD_BOARD_TORQUE_MIN_SPEED_RAD_S) {
        return omega_rad_s;
    }

    return 0.0f;
}

static void board_adc_zero_calibrate(void)
{
#if (AD_BOARD_ADC_ENABLE != 0) && (AD_BOARD_ADC_ZERO_CAL_ENABLE != 0)
    g_adc_snapshot.ia_zero_raw = board_adc_read_channel_average(AD_BOARD_ADC_CH_IA,
                                                                AD_BOARD_ADC_ZERO_CAL_SAMPLES,
                                                                0);
    g_adc_snapshot.ib_zero_raw = board_adc_read_channel_average(AD_BOARD_ADC_CH_IB,
                                                                AD_BOARD_ADC_ZERO_CAL_SAMPLES,
                                                                0);
    g_adc_snapshot.ic_zero_raw = board_adc_read_channel_average(AD_BOARD_ADC_CH_IC,
                                                                AD_BOARD_ADC_ZERO_CAL_SAMPLES,
                                                                0);
    g_adc_snapshot.vdc_zero_raw = board_adc_read_channel_average(AD_BOARD_ADC_CH_VDC,
                                                                 AD_BOARD_ADC_ZERO_CAL_SAMPLES,
                                                                 0);

    g_adc_snapshot.ia_zero_adc_V = adc_raw_to_voltage(g_adc_snapshot.ia_zero_raw);
    g_adc_snapshot.ib_zero_adc_V = adc_raw_to_voltage(g_adc_snapshot.ib_zero_raw);
    g_adc_snapshot.ic_zero_adc_V = adc_raw_to_voltage(g_adc_snapshot.ic_zero_raw);
    g_adc_snapshot.vdc_zero_adc_V = adc_raw_to_voltage(g_adc_snapshot.vdc_zero_raw);

    if (g_adc_snapshot.vdc_zero_adc_V > AD_BOARD_ADC_VDC_ZERO_MAX_ADC_V) {
        g_adc_snapshot.vdc_zero_raw = 0U;
        g_adc_snapshot.vdc_zero_adc_V = 0.0f;
    }

    g_adc_snapshot.zero_calibrated = 1U;
#endif
}

static float board_vdc_filter(float vdc_adc_V)
{
    float alpha = AD_BOARD_ADC_VDC_FILTER_ALPHA;

    if (alpha < 0.0f) {
        alpha = 0.0f;
    } else if (alpha > 1.0f) {
        alpha = 1.0f;
    }

    if (g_vdc_filter_ready == 0U) {
        g_vdc_adc_filtered_V = vdc_adc_V;
        g_vdc_filter_ready = 1U;
    } else {
        g_vdc_adc_filtered_V += alpha * (vdc_adc_V - g_vdc_adc_filtered_V);
    }

    return g_vdc_adc_filtered_V;
}

static float board_sqrtf(float value)
{
    float x;
    uint8_t i;

    if (value <= 0.0f) {
        return 0.0f;
    }

    x = (value > 1.0f) ? value : 1.0f;
    for (i = 0U; i < 8U; i++) {
        x = 0.5f * (x + (value / x));
    }

    return x;
}

static float board_average_phase_current_rms(const AD_Measurements_t *meas)
{
    float sum_A = 0.0f;
    float count = 0.0f;

    if (meas == 0) {
        return 0.0f;
    }

    if (meas->ia_rms_A > AD_BOARD_TORQUE_MIN_CURRENT_RMS_A) {
        sum_A += meas->ia_rms_A;
        count += 1.0f;
    }
    if (meas->ib_rms_A > AD_BOARD_TORQUE_MIN_CURRENT_RMS_A) {
        sum_A += meas->ib_rms_A;
        count += 1.0f;
    }
    if (meas->ic_rms_A > AD_BOARD_TORQUE_MIN_CURRENT_RMS_A) {
        sum_A += meas->ic_rms_A;
        count += 1.0f;
    }

    return (count > 0.0f) ? (sum_A / count) : 0.0f;
}

static float board_phase_voltage_rms_estimate(const AD_Measurements_t *meas)
{
    float sum_sq_V2;

    if (meas == 0) {
        return 0.0f;
    }

    sum_sq_V2 = (meas->va_V * meas->va_V) +
                (meas->vb_V * meas->vb_V) +
                (meas->vc_V * meas->vc_V);

    return board_sqrtf(sum_sq_V2 / 3.0f);
}

static uint8_t board_rms_torque_power_estimate(const AD_Measurements_t *meas,
                                               float *power_W)
{
    float v_phase_rms_V;
    float i_phase_rms_A;
    float power_factor = AD_BOARD_TORQUE_RMS_POWER_FACTOR;

    if ((meas == 0) || (power_W == 0)) {
        return 0U;
    }

    if (power_factor < 0.0f) {
        power_factor = 0.0f;
    } else if (power_factor > 1.0f) {
        power_factor = 1.0f;
    }
    if (power_factor <= 0.0f) {
        return 0U;
    }

    v_phase_rms_V = board_phase_voltage_rms_estimate(meas);
    i_phase_rms_A = board_average_phase_current_rms(meas);
    if ((v_phase_rms_V <= 0.0f) ||
        (i_phase_rms_A <= AD_BOARD_TORQUE_MIN_CURRENT_RMS_A)) {
        return 0U;
    }

    *power_W = 3.0f * v_phase_rms_V * i_phase_rms_A * power_factor;
    return (board_absf(*power_W) >= AD_BOARD_TORQUE_MIN_POWER_W) ? 1U : 0U;
}

static void board_update_current_rms(AD_Measurements_t *meas)
{
    float alpha = AD_BOARD_ADC_CURRENT_RMS_FILTER_ALPHA;
    float ia_sq;
    float ib_sq;
    float ic_sq;

    if (meas == 0) {
        return;
    }

    if (alpha < 0.0f) {
        alpha = 0.0f;
    } else if (alpha > 1.0f) {
        alpha = 1.0f;
    }

    ia_sq = meas->ia_A * meas->ia_A;
    ib_sq = meas->ib_A * meas->ib_A;
    ic_sq = meas->ic_A * meas->ic_A;

    if (g_current_rms_filter_ready == 0U) {
        g_ia_rms_sq_A2 = ia_sq;
        g_ib_rms_sq_A2 = ib_sq;
        g_ic_rms_sq_A2 = ic_sq;
        g_current_rms_filter_ready = 1U;
    } else {
        g_ia_rms_sq_A2 += alpha * (ia_sq - g_ia_rms_sq_A2);
        g_ib_rms_sq_A2 += alpha * (ib_sq - g_ib_rms_sq_A2);
        g_ic_rms_sq_A2 += alpha * (ic_sq - g_ic_rms_sq_A2);
    }

    meas->ia_rms_A = board_sqrtf(g_ia_rms_sq_A2);
    meas->ib_rms_A = board_sqrtf(g_ib_rms_sq_A2);
    meas->ic_rms_A = board_sqrtf(g_ic_rms_sq_A2);
}

static void board_update_phase_voltage_estimate(AD_Measurements_t *meas)
{
    const AD_InverterState_t *inverter = AD_Inverter_GetState();
    float duty_common;

    if ((meas == 0) || (inverter == 0) || (meas->vdc_V <= 0.0f)) {
        return;
    }

    duty_common = (inverter->duty_a + inverter->duty_b + inverter->duty_c) / 3.0f;
    meas->va_V = (inverter->duty_a - duty_common) * meas->vdc_V;
    meas->vb_V = (inverter->duty_b - duty_common) * meas->vdc_V;
    meas->vc_V = (inverter->duty_c - duty_common) * meas->vdc_V;
}

static uint8_t board_mechanical_torque_estimate(const AD_Measurements_t *meas,
                                                float *torque_Nm)
{
    const AD_MotorParameters_t *params = AD_ParamID_GetParameters();
    float omega_rad_s;
    float alpha_rad_s2 = 0.0f;
    float dt_s;
    uint32_t dt_us;
    uint8_t valid = 0U;

    if ((meas == 0) || (torque_Nm == 0) || (params == 0)) {
        return 0U;
    }

    omega_rad_s = board_rpm_to_rad_s(meas->speed_rpm);
    if (g_torque_speed_ready != 0U) {
        dt_us = meas->timestamp_us - g_torque_timestamp_us;
        if (dt_us > 0UL) {
            dt_s = (float)dt_us * 0.000001f;
            alpha_rad_s2 = (omega_rad_s - g_torque_speed_rad_s) / dt_s;
            valid = 1U;
        }
    }

    g_torque_speed_rad_s = omega_rad_s;
    g_torque_timestamp_us = meas->timestamp_us;
    g_torque_speed_ready = 1U;

    if (valid == 0U) {
        return 0U;
    }

    if ((board_absf(omega_rad_s) < AD_BOARD_TORQUE_MIN_SPEED_RAD_S) &&
        (board_absf(alpha_rad_s2) < AD_BOARD_TORQUE_MIN_ACCEL_RAD_S2)) {
        return 0U;
    }

    *torque_Nm = 0.0f;
    valid = 0U;
    if (((params->valid_mask & AD_MOTOR_PARAM_VALID_J) != 0UL) &&
        (params->J_kg_m2 > 0.0f)) {
        *torque_Nm += params->J_kg_m2 * alpha_rad_s2;
        valid = 1U;
    }
    if (((params->valid_mask & AD_MOTOR_PARAM_VALID_B) != 0UL) &&
        (params->B_Nm_s > 0.0f)) {
        *torque_Nm += params->B_Nm_s * omega_rad_s;
        valid = 1U;
    }

    return valid;
}

static uint8_t board_electrical_torque_estimate(const AD_Measurements_t *meas,
                                                float *torque_Nm)
{
    const AD_MotorParameters_t *params = AD_ParamID_GetParameters();
    float omega_rad_s;
    float phase_current_sq_A2;
    float power_W;
    float copper_loss_W = 0.0f;

    if ((meas == 0) || (torque_Nm == 0)) {
        return 0U;
    }

    omega_rad_s = board_torque_omega_rad_s(meas, params);
    if (board_absf(omega_rad_s) < AD_BOARD_TORQUE_MIN_SPEED_RAD_S) {
        return 0U;
    }

    if (board_rms_torque_power_estimate(meas, &power_W) != 0U) {
        if (omega_rad_s < 0.0f) {
            power_W = -power_W;
        }
    } else {
        power_W = (meas->va_V * meas->ia_A) +
                  (meas->vb_V * meas->ib_A) +
                  (meas->vc_V * meas->ic_A);

        if ((params != 0) &&
            ((params->valid_mask & AD_MOTOR_PARAM_VALID_RS) != 0UL) &&
            (params->Rs_ohm > 0.0f)) {
            phase_current_sq_A2 = ((meas->ia_A * meas->ia_A) +
                                   (meas->ib_A * meas->ib_A) +
                                   (meas->ic_A * meas->ic_A)) / 3.0f;
            copper_loss_W = 3.0f * phase_current_sq_A2 * params->Rs_ohm;
            if (power_W > copper_loss_W) {
                power_W -= copper_loss_W;
            } else if (power_W < -copper_loss_W) {
                power_W += copper_loss_W;
            } else {
                power_W = 0.0f;
            }
        }

        if (board_absf(power_W) < AD_BOARD_TORQUE_MIN_POWER_W) {
            return 0U;
        }
    }

    *torque_Nm = power_W / omega_rad_s;
    return 1U;
}

static void board_update_torque_estimate(AD_Measurements_t *meas)
{
    float alpha = AD_BOARD_TORQUE_FILTER_ALPHA;
    float torque_Nm = 0.0f;
    uint8_t valid;

    if (meas == 0) {
        return;
    }

    board_update_phase_voltage_estimate(meas);
    valid = board_mechanical_torque_estimate(meas, &torque_Nm);
    if (valid == 0U) {
        valid = board_electrical_torque_estimate(meas, &torque_Nm);
    }

    if (valid == 0U) {
        g_torque_filter_ready = 0U;
        g_torque_filtered_Nm = 0.0f;
        meas->torque_Nm = 0.0f;
        return;
    }

    torque_Nm = board_clamp_abs(torque_Nm, AD_BOARD_TORQUE_MAX_ABS_NM);
    if (alpha < 0.0f) {
        alpha = 0.0f;
    } else if (alpha > 1.0f) {
        alpha = 1.0f;
    }

    if (g_torque_filter_ready == 0U) {
        g_torque_filtered_Nm = torque_Nm;
        g_torque_filter_ready = 1U;
    } else {
        g_torque_filtered_Nm += alpha * (torque_Nm - g_torque_filtered_Nm);
    }

    meas->torque_Nm = g_torque_filtered_Nm;
}

static void board_update_slip_measurement(AD_Measurements_t *meas)
{
    const AD_InverterState_t *inverter = AD_Inverter_GetState();
    const AD_MotorParameters_t *params = AD_ParamID_GetParameters();
    float pole_pairs;
    float synchronous_rpm;
    float slip_percent;

    if ((meas == 0) ||
        (inverter == 0) ||
        (inverter->pwm_running == 0U) ||
        (inverter->electrical_frequency_Hz == 0.0f)) {
        return;
    }

    pole_pairs = board_get_pole_pairs(params);
    if (pole_pairs <= 0.0f) {
        return;
    }

    synchronous_rpm = (60.0f * inverter->electrical_frequency_Hz) / pole_pairs;
    if (board_absf(synchronous_rpm) < AD_BOARD_SLIP_MIN_SYNC_RPM) {
        return;
    }

    slip_percent = ((synchronous_rpm - meas->speed_rpm) * 100.0f) / synchronous_rpm;
    meas->slip_percent = board_clamp_abs(slip_percent, AD_BOARD_SLIP_MAX_ABS_PERCENT);
}

static void board_adc_gpio_init(void)
{
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_GPIOC_CLK_ENABLE();

    GPIOA->MODER |= (GPIO_MODER_MODE0 | GPIO_MODER_MODE1);
    GPIOA->PUPDR &= ~(GPIO_PUPDR_PUPD0 | GPIO_PUPDR_PUPD1);

    GPIOC->MODER |= (GPIO_MODER_MODE0 | GPIO_MODER_MODE1);
    GPIOC->PUPDR &= ~(GPIO_PUPDR_PUPD0 | GPIO_PUPDR_PUPD1);
}

static void board_adc_init(void)
{
#if AD_BOARD_ADC_ENABLE != 0
    uint32_t wait;

    memset(&g_adc_snapshot, 0, sizeof(g_adc_snapshot));
    g_adc_snapshot.phase_shunt_ohm = board_phase_shunt_resistance_ohm();
    g_vdc_filter_ready = 0U;
    g_vdc_adc_filtered_V = 0.0f;
    g_current_rms_filter_ready = 0U;
    g_ia_rms_sq_A2 = 0.0f;
    g_ib_rms_sq_A2 = 0.0f;
    g_ic_rms_sq_A2 = 0.0f;
    g_mcu_temp_filter_ready = 0U;
    g_mcu_temperature_C = AD_BOARD_ADC_DEFAULT_MCU_TEMP_C;
    g_adc_timeout_count = 0U;
    g_torque_speed_ready = 0U;
    g_torque_filter_ready = 0U;
    g_torque_timestamp_us = 0UL;
    g_torque_speed_rad_s = 0.0f;
    g_torque_filtered_Nm = 0.0f;
    board_adc_gpio_init();

    __HAL_RCC_ADC12_CLK_ENABLE();

    if (board_adc_stop_regular() == 0U) {
        g_adc_snapshot.faults |= AD_BOARD_ADC_FAULT_INIT;
        return;
    }

    if ((ADC1->CR & ADC_CR_ADEN) != 0UL) {
        ADC1->CR |= ADC_CR_ADDIS;
        if (wait_bits_clear(&ADC1->CR, ADC_CR_ADEN, AD_BOARD_ADC_TIMEOUT) == 0U) {
            g_adc_snapshot.faults |= AD_BOARD_ADC_FAULT_INIT;
            return;
        }
    }

#if defined(ADC12_COMMON) && defined(ADC_CCR_CKMODE_0)
    ADC12_COMMON->CCR &= ~ADC_CCR_CKMODE;
    ADC12_COMMON->CCR |= ADC_CCR_CKMODE_1;
#endif
#if defined(ADC12_COMMON) && defined(ADC_CCR_VSENSESEL)
    ADC12_COMMON->CCR |= ADC_CCR_VSENSESEL;
    for (wait = 0UL; wait < 10000UL; wait++) {
        __NOP();
    }
#endif

#if defined(ADC_CR_DEEPPWD)
    ADC1->CR &= ~ADC_CR_DEEPPWD;
#endif
    ADC1->CR |= ADC_CR_ADVREGEN;
    for (wait = 0UL; wait < 10000UL; wait++) {
        __NOP();
    }

    ADC1->CR |= ADC_CR_ADCAL;
    if (wait_bits_clear(&ADC1->CR, ADC_CR_ADCAL, AD_BOARD_ADC_TIMEOUT) == 0U) {
        g_adc_snapshot.faults |= AD_BOARD_ADC_FAULT_INIT;
        return;
    }

    ADC1->CFGR = 0UL;
    ADC1->SMPR1 = 0x3FFFFFFFUL;
    ADC1->SMPR2 = 0x3FFFFFFFUL;
    ADC1->SQR1 = 0UL;
    ADC1->ISR = ADC_ISR_ADRDY | ADC_ISR_EOC | ADC_ISR_EOS;
    ADC1->CR |= ADC_CR_ADEN;

    if (wait_bits_set(&ADC1->ISR, ADC_ISR_ADRDY, AD_BOARD_ADC_TIMEOUT) == 0U) {
        g_adc_snapshot.faults |= AD_BOARD_ADC_FAULT_INIT;
        return;
    }

    g_adc_snapshot.initialized = 1U;
    board_adc_zero_calibrate();
#else
    memset(&g_adc_snapshot, 0, sizeof(g_adc_snapshot));
    g_adc_snapshot.phase_shunt_ohm = board_phase_shunt_resistance_ohm();
    g_current_rms_filter_ready = 0U;
    g_ia_rms_sq_A2 = 0.0f;
    g_ib_rms_sq_A2 = 0.0f;
    g_ic_rms_sq_A2 = 0.0f;
    g_mcu_temp_filter_ready = 0U;
    g_mcu_temperature_C = AD_BOARD_ADC_DEFAULT_MCU_TEMP_C;
    g_torque_speed_ready = 0U;
    g_torque_filter_ready = 0U;
    g_torque_timestamp_us = 0UL;
    g_torque_speed_rad_s = 0.0f;
    g_torque_filtered_Nm = 0.0f;
#endif
}

static void board_adc_update_measurements(AD_Measurements_t *meas)
{
#if AD_BOARD_ADC_ENABLE != 0
    if (g_adc_snapshot.initialized == 0U) {
        meas->status_flags |= AD_MEAS_STATUS_DRIVER_FAULT;
        return;
    }

    g_adc_snapshot.ia_raw = board_adc_read_channel(AD_BOARD_ADC_CH_IA);
    g_adc_snapshot.ib_raw = board_adc_read_channel(AD_BOARD_ADC_CH_IB);
    g_adc_snapshot.ic_raw = board_adc_read_channel(AD_BOARD_ADC_CH_IC);
    g_adc_snapshot.vdc_raw_avg = board_adc_read_channel_average(AD_BOARD_ADC_CH_VDC,
                                                                 AD_BOARD_ADC_VDC_AVG_SAMPLES,
                                                                 &g_adc_snapshot.vdc_raw);
    g_adc_snapshot.mcu_temp_raw = board_adc_read_channel(AD_BOARD_ADC_CH_MCU_TEMP);

    g_adc_snapshot.ia_adc_V = adc_raw_to_voltage(g_adc_snapshot.ia_raw);
    g_adc_snapshot.ib_adc_V = adc_raw_to_voltage(g_adc_snapshot.ib_raw);
    g_adc_snapshot.ic_adc_V = adc_raw_to_voltage(g_adc_snapshot.ic_raw);
    g_adc_snapshot.vdc_adc_avg_V = adc_voltage_minus_offset(adc_raw_to_voltage(g_adc_snapshot.vdc_raw_avg),
                                                            g_adc_snapshot.vdc_zero_adc_V);
    g_adc_snapshot.vdc_adc_V = board_vdc_filter(g_adc_snapshot.vdc_adc_avg_V);
    g_adc_snapshot.mcu_temp_adc_V = adc_raw_to_voltage(g_adc_snapshot.mcu_temp_raw);
    g_adc_snapshot.mcu_temperature_C =
        board_mcu_temperature_filter(adc_mcu_temperature_from_raw(g_adc_snapshot.mcu_temp_raw));

    meas->ia_A = adc_current_from_voltage(g_adc_snapshot.ia_adc_V,
                                          g_adc_snapshot.ia_zero_adc_V);
    meas->ib_A = adc_current_from_voltage(g_adc_snapshot.ib_adc_V,
                                          g_adc_snapshot.ib_zero_adc_V);
    meas->ic_A = adc_current_from_voltage(g_adc_snapshot.ic_adc_V,
                                          g_adc_snapshot.ic_zero_adc_V);
    meas->vdc_V = adc_vdc_from_voltage(g_adc_snapshot.vdc_adc_V);
    meas->temperature_C = g_adc_snapshot.mcu_temperature_C;

    if (g_adc_snapshot.faults != 0UL) {
        meas->status_flags |= AD_MEAS_STATUS_DRIVER_FAULT;
    }
#else
    (void)meas;
#endif
}

static void board_apply_command(uint8_t enable, uint8_t mode, float pwm_duty_limit)
{
    AD_Command_t command;

    memset(&command, 0, sizeof(command));
    command.enable = enable;
    command.test_mode = mode;
    command.pwm_polarity_flags =
        (uint16_t)(AD_INVERTER_DEFAULT_PWM_POLARITY_FLAGS &
                   AD_INVERTER_PWM_POLARITY_MASK);
    command.pwm_timing_mode = (uint16_t)AD_INVERTER_PWM_TIMING_CENTER;
    command.motor_control_type = (uint16_t)AD_PARAM_ID_MOTOR_CONTROL_AD;
    command.rotation_ramp_time_ms = (uint16_t)AD_PARAM_ID_DEFAULT_ROTATION_RAMP_TIME_MS;
    command.current_limit_A = AD_BOARD_DEFAULT_CURRENT_LIMIT_A;
    command.voltage_limit_V = AD_BOARD_DEFAULT_OVERVOLTAGE_V;
    command.undervoltage_limit_V = AD_BOARD_DEFAULT_UNDERVOLTAGE_V;
    command.speed_limit_rpm = AD_BOARD_DEFAULT_SPEED_LIMIT_RPM;
    command.temperature_limit_C = AD_BOARD_DEFAULT_TEMPERATURE_C;
    command.pwm_duty_limit = (pwm_duty_limit > 0.0f) ?
                             pwm_duty_limit :
                             AD_BOARD_DEFAULT_PWM_DUTY_LIMIT;
    command.rotation_frequency_Hz = AD_PARAM_ID_DEFAULT_ROTATION_FREQUENCY_HZ;
    command.rotation_modulation = ((mode == AD_PARAM_ID_MODE_ROTATION_3HZ) &&
                                   (pwm_duty_limit > 0.0f)) ?
                                  pwm_duty_limit :
                                  AD_PARAM_ID_DEFAULT_ROTATION_MODULATION;

    SimulinkInterface_SetCommand(&command);
}

static void board_set_led(uint8_t on)
{
#if AD_BOARD_LED_ENABLE != 0
    GPIO_PinState state = (on != 0U) ? GPIO_PIN_SET : GPIO_PIN_RESET;
    HAL_GPIO_WritePin(LD2_GPIO_Port, LD2_Pin, state);
#else
    (void)on;
#endif
}

static void board_toggle_led(void)
{
    g_led_state = (g_led_state == 0U) ? 1U : 0U;
    board_set_led(g_led_state);
}

static void board_update_led(uint32_t now_ms)
{
    uint32_t status = AD_ParamID_GetStatus();

    if ((status & AD_PARAM_ID_STATUS_FAULT_LATCHED) != 0UL) {
        if ((now_ms - g_last_led_tick_ms) >= AD_BOARD_FAULT_BLINK_MS) {
            g_last_led_tick_ms = now_ms;
            board_toggle_led();
        }
        return;
    }

    if ((status & AD_PARAM_ID_STATUS_ACTIVE) != 0UL) {
        g_led_state = 1U;
        board_set_led(1U);
        return;
    }

    if ((now_ms - g_last_led_tick_ms) >= AD_BOARD_IDLE_BLINK_MS) {
        g_last_led_tick_ms = now_ms;
        board_toggle_led();
    }
}

void AD_Board_Init(void)
{
    g_button_toggle_request = 0U;
    g_logging_enabled = 0U;
    g_led_state = 0U;
    g_last_button_tick_ms = 0UL;
    g_last_slow_tick_ms = HAL_GetTick();
    g_last_led_tick_ms = HAL_GetTick();
    g_phase_shunt_resistance_ohm = AD_BOARD_ADC_DEFAULT_PHASE_SHUNT_OHM;

    board_set_led(0U);
    board_adc_init();
    AD_Inverter_Init();
    SimulinkInterface_Init();
    board_apply_command(0U, AD_PARAM_ID_MODE_IDLE, AD_BOARD_DEFAULT_PWM_DUTY_LIMIT);
}

void AD_Board_ReadMeasurements(AD_Measurements_t *meas)
{
    if (meas == 0) {
        return;
    }

    memset(meas, 0, sizeof(*meas));
    meas->timestamp_us = HAL_GetTick() * 1000UL;
    meas->temperature_C = AD_BOARD_ADC_DEFAULT_MCU_TEMP_C;
    board_adc_update_measurements(meas);
    board_update_current_rms(meas);
    board_update_torque_estimate(meas);
    board_update_slip_measurement(meas);
}

const AD_BoardAdcSnapshot_t* AD_Board_GetAdcSnapshot(void)
{
    return &g_adc_snapshot;
}

void AD_Board_ResetAdcFaults(void)
{
    board_adc_init();
}

void AD_Board_SetPhaseShuntResistanceOhm(float resistance_ohm)
{
    g_phase_shunt_resistance_ohm = board_normalize_phase_shunt_resistance(resistance_ohm);
    g_adc_snapshot.phase_shunt_ohm = g_phase_shunt_resistance_ohm;
    g_current_rms_filter_ready = 0U;
    g_ia_rms_sq_A2 = 0.0f;
    g_ib_rms_sq_A2 = 0.0f;
    g_ic_rms_sq_A2 = 0.0f;
}

float AD_Board_GetPhaseShuntResistanceOhm(void)
{
    return board_phase_shunt_resistance_ohm();
}

void AD_Board_StartDataLogging(void)
{
    g_logging_enabled = 1U;
    board_apply_command(1U, AD_PARAM_ID_MODE_DATA_LOGGING, AD_BOARD_DEFAULT_PWM_DUTY_LIMIT);
}

void AD_Board_StartParamTest(uint8_t mode)
{
    AD_Board_StartParamTestWithDuty(mode, AD_BOARD_DEFAULT_PWM_DUTY_LIMIT);
}

void AD_Board_StartParamTestWithDuty(uint8_t mode, float pwm_duty_limit)
{
    g_logging_enabled = 1U;
    board_apply_command(1U, mode, pwm_duty_limit);
}

void AD_Board_StopParamTest(void)
{
    g_logging_enabled = 0U;
    board_apply_command(0U, AD_PARAM_ID_MODE_IDLE, AD_BOARD_DEFAULT_PWM_DUTY_LIMIT);
    AD_Inverter_Disable();
    AD_ParamID_Stop();
}

void AD_Board_ToggleDataLogging(void)
{
    if (g_logging_enabled == 0U) {
        AD_Board_StartDataLogging();
    } else {
        AD_Board_StopParamTest();
    }
}

void AD_Board_Loop(void)
{
    AD_Measurements_t measurements;
    uint32_t now_ms = HAL_GetTick();

    if (g_button_toggle_request != 0U) {
        g_button_toggle_request = 0U;
        AD_Board_ToggleDataLogging();
    }

    AD_Board_ReadMeasurements(&measurements);
    SimulinkInterface_SetMeasurements(&measurements);
    SimulinkInterface_StepFast();

    if ((now_ms - g_last_slow_tick_ms) >= AD_BOARD_SLOW_PERIOD_MS) {
        g_last_slow_tick_ms = now_ms;
        SimulinkInterface_StepSlow();
    }

    board_update_led(now_ms);
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    uint32_t now_ms;

    if (GPIO_Pin != B1_Pin) {
        return;
    }

    now_ms = HAL_GetTick();
    if ((now_ms - g_last_button_tick_ms) >= AD_BOARD_BUTTON_DEBOUNCE_MS) {
        g_last_button_tick_ms = now_ms;
        g_button_toggle_request = 1U;
    }
}
