#include "ad_inverter.h"

#include "ad_parameter_identification.h"
#include "main.h"

extern TIM_HandleTypeDef htim1;

#ifndef AD_ID_DUTY_MAX
#define AD_ID_DUTY_MAX                    (0.20f)
#endif

#ifndef AD_ID_DUTY_MIN
#define AD_ID_DUTY_MIN                    (0.005f)
#endif

#ifndef AD_INVERTER_PWM_PERIOD_UP_TICKS
#define AD_INVERTER_PWM_PERIOD_UP_TICKS   (8499UL)
#endif

#ifndef AD_INVERTER_PWM_PERIOD_CENTER_TICKS
#define AD_INVERTER_PWM_PERIOD_CENTER_TICKS (4249UL)
#endif

#define AD_ID_PI                          (3.14159265358979323846f)
#define AD_ID_TWO_PI                      (6.28318530717958647692f)
#define AD_ID_TWO_PI_OVER_THREE           (2.09439510239319549231f)
#define AD_ID_INV_SQRT2                   (0.70710678118654752440f)
#define AD_ID_RS_SETTLE_US                (200000UL)
#define AD_ID_RS_MEASURE_US               (800000UL)
#define AD_ID_LS_SETTLE_US                (20000UL)
#define AD_ID_LS_PULSE_US                 (50000UL)
#define AD_ID_LOCKED_RR_SETTLE_US         (300000UL)
#define AD_ID_LOCKED_RR_MEASURE_US        (1000000UL)
#define AD_ID_MAG_SETTLE_US               (1000000UL)
#define AD_ID_MAG_MEASURE_US              (1500000UL)
#define AD_ID_INERTIA_ACCEL_US            (2000000UL)
#define AD_ID_INERTIA_COAST_US            (2500000UL)
#define AD_ID_MIN_CURRENT_A               (0.05f)
#define AD_ID_MIN_AC_CURRENT_A            (0.05f)
#define AD_ID_MIN_DELTA_CURRENT_A         (0.05f)
#define AD_ID_MIN_IMPEDANCE_OHM           (0.001f)
#define AD_ID_MIN_SPEED_RPM               (20.0f)
#define AD_ID_MIN_SPEED_DELTA_RPM         (10.0f)
#define AD_ID_MIN_ACCEL_RAD_S2            (0.1f)
#define AD_ID_LOCKED_RR_FREQ_HZ           (5.0f)
#define AD_ID_NO_LOAD_FREQ_MAX_HZ         (20.0f)
#define AD_ID_INERTIA_FREQ_MAX_HZ         (20.0f)
#ifndef AD_ID_ROTATION_TEST_START_FREQ_HZ
#define AD_ID_ROTATION_TEST_START_FREQ_HZ (0.5f)
#endif

#ifndef AD_ID_ROTATION_TEST_ALIGN_US
#define AD_ID_ROTATION_TEST_ALIGN_US      (500000UL)
#endif

#define AD_ID_DEFAULT_NOMINAL_FREQ_HZ     (50.0f)
#define AD_ID_ROTATION_MIN_UPDATE_DT_S    (0.000001f)
#define AD_ID_ROTATION_MAX_UPDATE_DT_S    (0.010000f)

#define AD_ID_RESULT_RUNNING              (0U)
#define AD_ID_RESULT_COMPLETE             (1U)
#define AD_ID_RESULT_FAILED               (2U)

#define AD_ID_STAGE_IDLE                  (0U)
#define AD_ID_STAGE_SETTLE                (1U)
#define AD_ID_STAGE_MEASURE               (2U)
#define AD_ID_STAGE_PULSE                 (3U)
#define AD_ID_STAGE_ACCEL                 (4U)
#define AD_ID_STAGE_COAST                 (5U)

#define AD_ID_KEY_AUTO_RS                 (100U)
#define AD_ID_KEY_AUTO_LS                 (101U)
#define AD_ID_KEY_AUTO_RR                 (102U)
#define AD_ID_KEY_AUTO_LM                 (103U)
#define AD_ID_KEY_AUTO_JB                 (104U)
#define AD_ID_KEY_LOCKED_LS               (110U)
#define AD_ID_KEY_LOCKED_RR               (111U)

static AD_InverterState_t g_inverter_state;

typedef struct
{
    uint8_t key;
    uint8_t stage;
    uint8_t auto_stage;
    uint32_t stage_start_us;
    uint32_t sample_count;
    float current_sum_A;
    float current_sq_sum_A2;
    float vdc_sum_V;
    float current_start_A;
    float vdc_start_V;
    float angle_rad;
    uint32_t pwm_timestamp_us;
    float speed_start_rpm;
    float speed_end_rpm;
    float accel_torque_Nm;
    float accel_alpha_rad_s2;
} AD_IdRuntime_t;

static AD_IdRuntime_t g_id_runtime;

typedef struct
{
    uint8_t active;
    uint8_t motor_control_type;
    float target_duty;
    float target_frequency_hz;
    float current_duty;
    float current_frequency_hz;
    float duty_slew_per_s;
    float frequency_slew_hz_per_s;
    float angle_rad;
    float update_dt_s;
    uint16_t target_ramp_time_ms;
} AD_RotationPwmRuntime_t;

static volatile AD_RotationPwmRuntime_t g_rotation_pwm;

static void rotation_pwm_stop(void)
{
    g_rotation_pwm.active = 0U;
    g_inverter_state.electrical_frequency_Hz = 0.0f;
    g_inverter_state.electrical_angle_rad = 0.0f;
}

static float slew_towards(float current, float target, float max_delta)
{
    if (max_delta <= 0.0f) {
        return target;
    }
    if (current < target) {
        current += max_delta;
        if (current > target) {
            current = target;
        }
    } else if (current > target) {
        current -= max_delta;
        if (current < target) {
            current = target;
        }
    }
    return current;
}

static uint32_t inverter_get_tim1_clock_hz(void)
{
    uint32_t clock_hz = HAL_RCC_GetPCLK2Freq();

#if defined(RCC_CFGR_PPRE2)
    if ((RCC->CFGR & RCC_CFGR_PPRE2) != 0UL) {
        clock_hz *= 2UL;
    }
#endif

    return (clock_hz != 0UL) ? clock_hz : SystemCoreClock;
}

static float inverter_get_pwm_update_dt_s(void)
{
    uint32_t timer_clock_hz = inverter_get_tim1_clock_hz();
    uint32_t prescaler = htim1.Instance->PSC + 1UL;
    uint32_t period_ticks = __HAL_TIM_GET_AUTORELOAD(&htim1) + 1UL;
    float dt_s;

    if ((timer_clock_hz == 0UL) || (period_ticks == 0UL)) {
        return AD_ID_ROTATION_MAX_UPDATE_DT_S;
    }

    dt_s = ((float)period_ticks * (float)prescaler) / (float)timer_clock_hz;
    if (dt_s < AD_ID_ROTATION_MIN_UPDATE_DT_S) {
        dt_s = AD_ID_ROTATION_MIN_UPDATE_DT_S;
    } else if (dt_s > AD_ID_ROTATION_MAX_UPDATE_DT_S) {
        dt_s = AD_ID_ROTATION_MAX_UPDATE_DT_S;
    }

    return dt_s;
}

static void inverter_enable_update_irq(void)
{
    __HAL_TIM_CLEAR_FLAG(&htim1, TIM_FLAG_UPDATE);
    __HAL_TIM_ENABLE_IT(&htim1, TIM_IT_UPDATE);
}

static void inverter_disable_update_irq(void)
{
    __HAL_TIM_DISABLE_IT(&htim1, TIM_IT_UPDATE);
    __HAL_TIM_CLEAR_FLAG(&htim1, TIM_FLAG_UPDATE);
}

static void inverter_clear_break_flags(void)
{
#if defined(TIM_FLAG_BREAK)
    __HAL_TIM_CLEAR_FLAG(&htim1, TIM_FLAG_BREAK);
#endif
#if defined(TIM_FLAG_BREAK2)
    __HAL_TIM_CLEAR_FLAG(&htim1, TIM_FLAG_BREAK2);
#endif
#if defined(TIM_FLAG_SYSTEM_BREAK)
    __HAL_TIM_CLEAR_FLAG(&htim1, TIM_FLAG_SYSTEM_BREAK);
#endif
}

static void inverter_write_pwm_polarity(uint16_t flags)
{
    uint32_t main_mask = TIM_CCER_CC1P | TIM_CCER_CC2P | TIM_CCER_CC3P;
    uint32_t comp_mask = TIM_CCER_CC1NP | TIM_CCER_CC2NP | TIM_CCER_CC3NP;
    uint32_t ccer = htim1.Instance->CCER;

    ccer &= ~(main_mask | comp_mask);
    if ((flags & AD_INVERTER_PWM_POLARITY_MAIN_INVERTED) != 0U) {
        ccer |= main_mask;
    }
    if ((flags & AD_INVERTER_PWM_POLARITY_COMP_INVERTED) != 0U) {
        ccer |= comp_mask;
    }

    htim1.Instance->CCER = ccer;
}

static void inverter_write_pwm_timing(uint16_t mode)
{
    uint32_t cr1 = htim1.Instance->CR1;
    uint32_t period;

    cr1 &= ~(TIM_CR1_CMS | TIM_CR1_DIR);
    if (mode == (uint16_t)AD_INVERTER_PWM_TIMING_CENTER) {
        cr1 |= TIM_COUNTERMODE_CENTERALIGNED1;
        period = AD_INVERTER_PWM_PERIOD_CENTER_TICKS;
    } else {
        period = AD_INVERTER_PWM_PERIOD_UP_TICKS;
    }

    htim1.Instance->CR1 = cr1;
    __HAL_TIM_SET_AUTORELOAD(&htim1, period);
    __HAL_TIM_SET_COUNTER(&htim1, 0UL);
}

static float clamp01(float value)
{
    if (value < 0.0f) {
        return 0.0f;
    }
    if (value > 1.0f) {
        return 1.0f;
    }
    return value;
}

static uint32_t duty_to_compare(float duty)
{
    uint32_t period = __HAL_TIM_GET_AUTORELOAD(&htim1);
    float compare_f = clamp01(duty) * (float)period;

    if (compare_f < 0.0f) {
        return 0UL;
    }
    if (compare_f > (float)period) {
        return period;
    }
    return (uint32_t)(compare_f + 0.5f);
}

static uint32_t duty_to_compare_inverted(float duty)
{
    return duty_to_compare(1.0f - clamp01(duty));
}

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

static float sqrtf_local(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 wrap_angle(float angle_rad)
{
    while (angle_rad >= AD_ID_TWO_PI) {
        angle_rad -= AD_ID_TWO_PI;
    }
    while (angle_rad < 0.0f) {
        angle_rad += AD_ID_TWO_PI;
    }

    return angle_rad;
}

static float cos_poly(float angle_rad)
{
    float x2 = angle_rad * angle_rad;
    float x4 = x2 * x2;
    float x6 = x4 * x2;

    return 1.0f - (0.5f * x2) + (0.0416666667f * x4) - (0.0013888889f * x6);
}

static float cos_approx(float angle_rad)
{
    float x = wrap_angle(angle_rad);
    float value;

    if (x > AD_ID_PI) {
        x -= AD_ID_TWO_PI;
    }

    if (x > (0.5f * AD_ID_PI)) {
        value = -cos_poly(AD_ID_PI - x);
    } else if (x < (-0.5f * AD_ID_PI)) {
        value = -cos_poly(AD_ID_PI + x);
    } else {
        value = cos_poly(x);
    }

    if (value > 1.0f) {
        return 1.0f;
    }
    if (value < -1.0f) {
        return -1.0f;
    }
    return value;
}

static float rpm_to_rad_s(float rpm)
{
    return rpm * (AD_ID_TWO_PI / 60.0f);
}

static float id_get_test_duty(void)
{
    float duty = AD_ParamID_GetPwmDutyLimit();

    g_inverter_state.requested_test_duty = duty;

    if (duty < AD_ID_DUTY_MIN) {
        duty = AD_ID_DUTY_MIN;
    } else if (duty > AD_ID_DUTY_MAX) {
        duty = AD_ID_DUTY_MAX;
    }

    g_inverter_state.applied_test_duty = duty;
    return duty;
}

static float id_get_rotation_test_duty(void)
{
    float duty = AD_ParamID_GetRotationModulation();

    g_inverter_state.requested_test_duty = duty;
    g_inverter_state.applied_test_duty = duty;
    return duty;
}

static void rotation_pwm_apply_six_step(float duty, uint8_t sector)
{
    float d = clamp01(duty);
    float high = 0.5f + d;
    float low = 0.5f - d;
    float mid = 0.5f;

    switch (sector % 6U) {
    case 0U:
        AD_Inverter_SetDuty(high, low, mid);
        break;
    case 1U:
        AD_Inverter_SetDuty(high, mid, low);
        break;
    case 2U:
        AD_Inverter_SetDuty(mid, high, low);
        break;
    case 3U:
        AD_Inverter_SetDuty(low, high, mid);
        break;
    case 4U:
        AD_Inverter_SetDuty(low, mid, high);
        break;
    default:
        AD_Inverter_SetDuty(mid, low, high);
        break;
    }
}

static void rotation_pwm_service_update(void)
{
    float duty;
    float frequency_hz;
    float angle_rad;
    float dt_s;
    uint8_t sector;

    if (g_rotation_pwm.active == 0U) {
        return;
    }
    if ((g_inverter_state.pwm_running == 0U) ||
        (g_inverter_state.service_pwm_running != 0U)) {
        rotation_pwm_stop();
        return;
    }

    dt_s = g_rotation_pwm.update_dt_s;
    if (dt_s < AD_ID_ROTATION_MIN_UPDATE_DT_S) {
        dt_s = AD_ID_ROTATION_MIN_UPDATE_DT_S;
    } else if (dt_s > AD_ID_ROTATION_MAX_UPDATE_DT_S) {
        dt_s = AD_ID_ROTATION_MAX_UPDATE_DT_S;
    }

    duty = slew_towards(g_rotation_pwm.current_duty,
                        g_rotation_pwm.target_duty,
                        g_rotation_pwm.duty_slew_per_s * dt_s);
    frequency_hz = slew_towards(g_rotation_pwm.current_frequency_hz,
                                g_rotation_pwm.target_frequency_hz,
                                g_rotation_pwm.frequency_slew_hz_per_s * dt_s);
    if (frequency_hz < 0.1f) {
        frequency_hz = 0.1f;
    }

    angle_rad = wrap_angle(g_rotation_pwm.angle_rad +
                           (AD_ID_TWO_PI * frequency_hz * dt_s));

    g_rotation_pwm.current_duty = duty;
    g_rotation_pwm.current_frequency_hz = frequency_hz;
    g_rotation_pwm.angle_rad = angle_rad;
    g_inverter_state.applied_test_duty = duty;
    g_inverter_state.electrical_frequency_Hz = frequency_hz;
    g_inverter_state.electrical_angle_rad = angle_rad;

    if (g_rotation_pwm.motor_control_type ==
        (uint8_t)AD_PARAM_ID_MOTOR_CONTROL_BLDC) {
        sector = (uint8_t)(angle_rad * (6.0f / AD_ID_TWO_PI));
        if (sector > 5U) {
            sector = 5U;
        }
        rotation_pwm_apply_six_step(duty, sector);
        return;
    }

    AD_Inverter_SetDuty(0.5f + (duty * cos_approx(angle_rad)),
                        0.5f + (duty * cos_approx(angle_rad -
                                                   AD_ID_TWO_PI_OVER_THREE)),
                        0.5f + (duty * cos_approx(angle_rad +
                                                   AD_ID_TWO_PI_OVER_THREE)));
}

static uint8_t id_configure_rotation_pwm(float duty,
                                         float frequency_hz,
                                         uint16_t motor_control_type)
{
    uint32_t primask;
    float start_frequency_hz = AD_ID_ROTATION_TEST_START_FREQ_HZ;
    float update_dt_s = inverter_get_pwm_update_dt_s();
    uint16_t ramp_time_ms = AD_ParamID_GetRotationRampTimeMs();
    float ramp_time_s = (float)ramp_time_ms * 0.001f;
    uint8_t reset_runtime;
    uint8_t target_changed;

    duty = clamp01(duty);
    if (frequency_hz < 0.1f) {
        frequency_hz = 0.1f;
    }
    if (start_frequency_hz > frequency_hz) {
        start_frequency_hz = frequency_hz;
    }

    if (AD_Inverter_Enable() == 0U) {
        rotation_pwm_stop();
        return 0U;
    }

    primask = __get_PRIMASK();
    __disable_irq();
    reset_runtime = (uint8_t)((g_rotation_pwm.active == 0U) ||
                              (g_rotation_pwm.motor_control_type !=
                               (uint8_t)motor_control_type));
    target_changed = (uint8_t)((reset_runtime != 0U) ||
                               (g_rotation_pwm.target_duty != duty) ||
                               (g_rotation_pwm.target_frequency_hz != frequency_hz) ||
                               (g_rotation_pwm.target_ramp_time_ms != ramp_time_ms));
    if (reset_runtime != 0U) {
        g_rotation_pwm.current_duty =
            (motor_control_type == (uint16_t)AD_PARAM_ID_MOTOR_CONTROL_BLDC) ?
            duty :
            0.0f;
        g_rotation_pwm.current_frequency_hz = start_frequency_hz;
        g_rotation_pwm.angle_rad = 0.0f;
    }
    if (ramp_time_s <= 0.0f) {
        ramp_time_s = 0.001f;
    }
    if (target_changed != 0U) {
        g_rotation_pwm.duty_slew_per_s =
            absf_local(duty - g_rotation_pwm.current_duty) / ramp_time_s;
        g_rotation_pwm.frequency_slew_hz_per_s =
            absf_local(frequency_hz - g_rotation_pwm.current_frequency_hz) / ramp_time_s;
    }
    g_rotation_pwm.target_duty = duty;
    g_rotation_pwm.target_frequency_hz = frequency_hz;
    g_rotation_pwm.motor_control_type = (uint8_t)motor_control_type;
    g_rotation_pwm.update_dt_s = update_dt_s;
    g_rotation_pwm.target_ramp_time_ms = ramp_time_ms;
    g_rotation_pwm.active = 1U;
    g_inverter_state.electrical_frequency_Hz = g_rotation_pwm.current_frequency_hz;
    g_inverter_state.electrical_angle_rad = g_rotation_pwm.angle_rad;
    if (primask == 0UL) {
        __enable_irq();
    }

    return 1U;
}

static float id_get_nominal_frequency_hz(void)
{
    const AD_MotorParameters_t *params = AD_ParamID_GetParameters();

    if ((params != 0) && (params->nominal_frequency_Hz > 1.0f)) {
        return params->nominal_frequency_Hz;
    }

    return AD_ID_DEFAULT_NOMINAL_FREQ_HZ;
}

static float id_get_no_load_frequency_hz(void)
{
    float frequency_hz = id_get_nominal_frequency_hz();

    if (frequency_hz > AD_ID_NO_LOAD_FREQ_MAX_HZ) {
        frequency_hz = AD_ID_NO_LOAD_FREQ_MAX_HZ;
    }
    if (frequency_hz < 1.0f) {
        frequency_hz = 1.0f;
    }

    return frequency_hz;
}

static float id_get_inertia_frequency_hz(void)
{
    float frequency_hz = id_get_nominal_frequency_hz() * 0.4f;

    if (frequency_hz > AD_ID_INERTIA_FREQ_MAX_HZ) {
        frequency_hz = AD_ID_INERTIA_FREQ_MAX_HZ;
    }
    if (frequency_hz < 1.0f) {
        frequency_hz = 1.0f;
    }

    return frequency_hz;
}

static float id_phase_a_current(const AD_Measurements_t *meas)
{
    return absf_local(meas->ia_A - (0.5f * (meas->ib_A + meas->ic_A)));
}

static float id_phase_current_sq(const AD_Measurements_t *meas)
{
    return ((meas->ia_A * meas->ia_A) +
            (meas->ib_A * meas->ib_A) +
            (meas->ic_A * meas->ic_A)) / 3.0f;
}

static float id_phase_voltage_rms(float duty, float vdc_V)
{
    return absf_local(duty) * vdc_V * AD_ID_INV_SQRT2;
}

static void id_accumulate_ac_sample(const AD_Measurements_t *meas)
{
    g_id_runtime.current_sq_sum_A2 += id_phase_current_sq(meas);
    g_id_runtime.vdc_sum_V += meas->vdc_V;
    g_id_runtime.sample_count++;
}

static float id_average_ac_current_rms(void)
{
    if (g_id_runtime.sample_count == 0UL) {
        return 0.0f;
    }

    return sqrtf_local(g_id_runtime.current_sq_sum_A2 / (float)g_id_runtime.sample_count);
}

static float id_average_vdc(void)
{
    if (g_id_runtime.sample_count == 0UL) {
        return 0.0f;
    }

    return g_id_runtime.vdc_sum_V / (float)g_id_runtime.sample_count;
}

static uint32_t elapsed_us(uint32_t now_us, uint32_t start_us)
{
    return now_us - start_us;
}

static void id_runtime_reset(uint8_t key, uint8_t stage, uint32_t now_us)
{
    rotation_pwm_stop();
    g_id_runtime.key = key;
    g_id_runtime.stage = stage;
    g_id_runtime.stage_start_us = now_us;
    g_id_runtime.sample_count = 0UL;
    g_id_runtime.current_sum_A = 0.0f;
    g_id_runtime.current_sq_sum_A2 = 0.0f;
    g_id_runtime.vdc_sum_V = 0.0f;
    g_id_runtime.current_start_A = 0.0f;
    g_id_runtime.vdc_start_V = 0.0f;
    g_id_runtime.angle_rad = 0.0f;
    g_id_runtime.pwm_timestamp_us = now_us;
    g_id_runtime.speed_start_rpm = 0.0f;
    g_id_runtime.speed_end_rpm = 0.0f;
    g_id_runtime.accel_torque_Nm = 0.0f;
    g_id_runtime.accel_alpha_rad_s2 = 0.0f;
    g_inverter_state.id_stage = stage;
}

static uint8_t id_apply_dc_vector(float duty)
{
    AD_InverterCommand_t command;
    float d = clamp01(duty);

    g_inverter_state.applied_test_duty = d;
    g_inverter_state.electrical_frequency_Hz = 0.0f;
    g_inverter_state.electrical_angle_rad = 0.0f;
    command.enable = 1U;
    command.duty_a = 0.5f + d;
    command.duty_b = 0.5f - d;
    command.duty_c = 0.5f - d;

    return AD_Inverter_ApplyCommand(&command);
}

static uint8_t id_apply_rotating_vector(float duty,
                                        float frequency_hz,
                                        const AD_Measurements_t *meas)
{
    AD_InverterCommand_t command;
    float d = clamp01(duty);
    float dt_s;
    uint32_t dt_us;

    if (frequency_hz < 0.1f) {
        frequency_hz = 0.1f;
    }

    dt_us = elapsed_us(meas->timestamp_us, g_id_runtime.pwm_timestamp_us);
    dt_s = (float)dt_us * 0.000001f;
    if (dt_s > 0.1f) {
        dt_s = 0.1f;
    }

    g_id_runtime.angle_rad = wrap_angle(g_id_runtime.angle_rad +
                                        (AD_ID_TWO_PI * frequency_hz * dt_s));
    g_id_runtime.pwm_timestamp_us = meas->timestamp_us;

    g_inverter_state.applied_test_duty = d;
    g_inverter_state.electrical_frequency_Hz = frequency_hz;
    g_inverter_state.electrical_angle_rad = g_id_runtime.angle_rad;
    command.enable = 1U;
    command.duty_a = 0.5f + (d * cos_approx(g_id_runtime.angle_rad));
    command.duty_b = 0.5f + (d * cos_approx(g_id_runtime.angle_rad -
                                             AD_ID_TWO_PI_OVER_THREE));
    command.duty_c = 0.5f + (d * cos_approx(g_id_runtime.angle_rad +
                                             AD_ID_TWO_PI_OVER_THREE));

    return AD_Inverter_ApplyCommand(&command);
}

static uint8_t id_apply_six_step_sector(float duty, uint8_t sector)
{
    AD_InverterCommand_t command;
    float d = clamp01(duty);
    float high = 0.5f + d;
    float low = 0.5f - d;
    float mid = 0.5f;

    command.enable = 1U;
    g_inverter_state.electrical_frequency_Hz = 0.0f;
    g_inverter_state.electrical_angle_rad = 0.0f;
    switch (sector % 6U) {
    case 0U:
        command.duty_a = high;
        command.duty_b = low;
        command.duty_c = mid;
        break;
    case 1U:
        command.duty_a = high;
        command.duty_b = mid;
        command.duty_c = low;
        break;
    case 2U:
        command.duty_a = mid;
        command.duty_b = high;
        command.duty_c = low;
        break;
    case 3U:
        command.duty_a = low;
        command.duty_b = high;
        command.duty_c = mid;
        break;
    case 4U:
        command.duty_a = low;
        command.duty_b = mid;
        command.duty_c = high;
        break;
    default:
        command.duty_a = mid;
        command.duty_b = low;
        command.duty_c = high;
        break;
    }

    return AD_Inverter_ApplyCommand(&command);
}

static void id_store_rs(float rs_ohm)
{
    AD_MotorParameters_t params = *AD_ParamID_GetParameters();

    params.Rs_ohm = rs_ohm;
    params.valid_mask |= AD_MOTOR_PARAM_VALID_RS;
    AD_ParamID_SetParameters(&params);
}

static void id_update_inductance_model(AD_MotorParameters_t *params)
{
    float lls_H;
    float llr_H;

    if (params == 0) {
        return;
    }

    if (((params->valid_mask & AD_MOTOR_PARAM_VALID_LL) != 0UL) &&
        ((params->valid_mask & AD_MOTOR_PARAM_VALID_LM) != 0UL) &&
        (params->Ll_H > 0.0f) &&
        (params->Lm_H > 0.0f)) {
        lls_H = 0.5f * params->Ll_H;
        llr_H = 0.5f * params->Ll_H;
        params->Ls_H = params->Lm_H + lls_H;
        params->Lr_H = params->Lm_H + llr_H;
        params->valid_mask |= (AD_MOTOR_PARAM_VALID_LS | AD_MOTOR_PARAM_VALID_LR);
    } else if (((params->valid_mask & AD_MOTOR_PARAM_VALID_LL) != 0UL) &&
               (params->Ll_H > 0.0f)) {
        params->Ls_H = params->Ll_H;
        params->Lr_H = params->Ll_H;
        params->valid_mask |= (AD_MOTOR_PARAM_VALID_LS | AD_MOTOR_PARAM_VALID_LR);
    }
}

static void id_store_ls(float ls_H)
{
    AD_MotorParameters_t params = *AD_ParamID_GetParameters();

    params.Ll_H = ls_H;
    params.valid_mask |= AD_MOTOR_PARAM_VALID_LL;
    id_update_inductance_model(&params);
    AD_ParamID_SetParameters(&params);
}

static void id_store_rr(float rr_ohm)
{
    AD_MotorParameters_t params = *AD_ParamID_GetParameters();

    params.Rr_ohm = rr_ohm;
    params.valid_mask |= AD_MOTOR_PARAM_VALID_RR;
    AD_ParamID_SetParameters(&params);
}

static void id_store_lm(float lm_H)
{
    AD_MotorParameters_t params = *AD_ParamID_GetParameters();

    params.Lm_H = lm_H;
    params.valid_mask |= AD_MOTOR_PARAM_VALID_LM;
    id_update_inductance_model(&params);
    AD_ParamID_SetParameters(&params);
}

static void id_store_inertia_friction(float j_kg_m2, float b_Nm_s)
{
    AD_MotorParameters_t params = *AD_ParamID_GetParameters();

    params.J_kg_m2 = j_kg_m2;
    params.B_Nm_s = b_Nm_s;
    params.valid_mask |= (AD_MOTOR_PARAM_VALID_J | AD_MOTOR_PARAM_VALID_B);
    AD_ParamID_SetParameters(&params);
}

static uint8_t id_has_valid_parameters(void)
{
    const AD_MotorParameters_t *params = AD_ParamID_GetParameters();

    return (uint8_t)(((params != 0) && (params->valid_mask != 0UL)) ? 1U : 0U);
}

static uint8_t id_finish_auto_partial(uint32_t now_us)
{
    if (id_has_valid_parameters() == 0U) {
        return AD_ID_RESULT_FAILED;
    }

    AD_ParamID_MarkPartialComplete();
    AD_ParamID_MarkStepFailed();
    id_runtime_reset(AD_PARAM_ID_MODE_AUTO_IDENTIFICATION, AD_ID_STAGE_IDLE, now_us);
    g_id_runtime.auto_stage = 0U;
    return AD_ID_RESULT_COMPLETE;
}

static void set_service_debug_duty(uint8_t output, float duty)
{
    g_inverter_state.duty_a = 0.0f;
    g_inverter_state.duty_b = 0.0f;
    g_inverter_state.duty_c = 0.0f;
    g_inverter_state.electrical_frequency_Hz = 0.0f;
    g_inverter_state.electrical_angle_rad = 0.0f;

    switch (output) {
    case AD_INVERTER_PWM_OUTPUT_UH:
    case AD_INVERTER_PWM_OUTPUT_UL:
        g_inverter_state.duty_a = duty;
        break;
    case AD_INVERTER_PWM_OUTPUT_VH:
    case AD_INVERTER_PWM_OUTPUT_VL:
        g_inverter_state.duty_b = duty;
        break;
    case AD_INVERTER_PWM_OUTPUT_WH:
    case AD_INVERTER_PWM_OUTPUT_WL:
        g_inverter_state.duty_c = duty;
        break;
    case AD_INVERTER_PWM_OUTPUT_ALL:
        g_inverter_state.duty_a = 0.5f;
        g_inverter_state.duty_b = 0.5f;
        g_inverter_state.duty_c = 0.5f;
        break;
    default:
        break;
    }
}

static uint8_t id_step_rs(uint8_t key, const AD_Measurements_t *meas, float duty)
{
    float i_avg_A;
    float vdc_avg_V;
    float v_phase_V;
    float rs_ohm;

    if ((g_id_runtime.key != key) || (g_id_runtime.stage == AD_ID_STAGE_IDLE)) {
        id_runtime_reset(key, AD_ID_STAGE_SETTLE, meas->timestamp_us);
    }

    if (id_apply_dc_vector(duty) == 0U) {
        return AD_ID_RESULT_FAILED;
    }

    if (g_id_runtime.stage == AD_ID_STAGE_SETTLE) {
        if (elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) >= AD_ID_RS_SETTLE_US) {
            id_runtime_reset(key, AD_ID_STAGE_MEASURE, meas->timestamp_us);
        }
        return AD_ID_RESULT_RUNNING;
    }

    g_id_runtime.current_sum_A += id_phase_a_current(meas);
    g_id_runtime.vdc_sum_V += meas->vdc_V;
    g_id_runtime.sample_count++;

    if (elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) < AD_ID_RS_MEASURE_US) {
        return AD_ID_RESULT_RUNNING;
    }

    AD_Inverter_Disable();

    if (g_id_runtime.sample_count == 0UL) {
        return AD_ID_RESULT_FAILED;
    }

    i_avg_A = g_id_runtime.current_sum_A / (float)g_id_runtime.sample_count;
    vdc_avg_V = g_id_runtime.vdc_sum_V / (float)g_id_runtime.sample_count;

    if (i_avg_A < AD_ID_MIN_CURRENT_A) {
        return AD_ID_RESULT_FAILED;
    }

    v_phase_V = (4.0f / 3.0f) * duty * vdc_avg_V;
    rs_ohm = v_phase_V / i_avg_A;
    id_store_rs(rs_ohm);
    id_runtime_reset(key, AD_ID_STAGE_IDLE, meas->timestamp_us);
    return AD_ID_RESULT_COMPLETE;
}

static uint8_t id_step_ls(uint8_t key, const AD_Measurements_t *meas, float duty)
{
    float i_now_A;
    float delta_i_A;
    float v_phase_V;
    float dt_s;
    float ls_H;

    if ((g_id_runtime.key != key) || (g_id_runtime.stage == AD_ID_STAGE_IDLE)) {
        id_runtime_reset(key, AD_ID_STAGE_SETTLE, meas->timestamp_us);
        g_id_runtime.current_start_A = id_phase_a_current(meas);
        g_id_runtime.vdc_start_V = meas->vdc_V;
    }

    if (id_apply_dc_vector(duty) == 0U) {
        return AD_ID_RESULT_FAILED;
    }

    if (g_id_runtime.stage == AD_ID_STAGE_SETTLE) {
        if (elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) >= AD_ID_LS_SETTLE_US) {
            id_runtime_reset(key, AD_ID_STAGE_PULSE, meas->timestamp_us);
            g_id_runtime.current_start_A = id_phase_a_current(meas);
            g_id_runtime.vdc_start_V = meas->vdc_V;
        }
        return AD_ID_RESULT_RUNNING;
    }

    if (elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) < AD_ID_LS_PULSE_US) {
        return AD_ID_RESULT_RUNNING;
    }

    AD_Inverter_Disable();
    i_now_A = id_phase_a_current(meas);
    delta_i_A = absf_local(i_now_A - g_id_runtime.current_start_A);

    if (delta_i_A < AD_ID_MIN_DELTA_CURRENT_A) {
        return AD_ID_RESULT_FAILED;
    }

    v_phase_V = (4.0f / 3.0f) * duty * g_id_runtime.vdc_start_V;
    dt_s = (float)elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) * 0.000001f;
    ls_H = (v_phase_V * dt_s) / delta_i_A;
    id_store_ls(ls_H);
    id_runtime_reset(key, AD_ID_STAGE_IDLE, meas->timestamp_us);
    return AD_ID_RESULT_COMPLETE;
}

static uint8_t id_step_locked_rr(uint8_t key, const AD_Measurements_t *meas, float duty)
{
    const AD_MotorParameters_t *params;
    float i_rms_A;
    float vdc_avg_V;
    float v_phase_rms_V;
    float z_abs_ohm;
    float x_ohm;
    float r_total_ohm;
    float rr_ohm;
    float ll_H;
    float rs_ohm;
    float omega_rad_s = AD_ID_TWO_PI * AD_ID_LOCKED_RR_FREQ_HZ;

    if ((g_id_runtime.key != key) || (g_id_runtime.stage == AD_ID_STAGE_IDLE)) {
        id_runtime_reset(key, AD_ID_STAGE_SETTLE, meas->timestamp_us);
    }

    if (id_apply_rotating_vector(duty, AD_ID_LOCKED_RR_FREQ_HZ, meas) == 0U) {
        return AD_ID_RESULT_FAILED;
    }

    if (g_id_runtime.stage == AD_ID_STAGE_SETTLE) {
        if (elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) >=
            AD_ID_LOCKED_RR_SETTLE_US) {
            id_runtime_reset(key, AD_ID_STAGE_MEASURE, meas->timestamp_us);
        }
        return AD_ID_RESULT_RUNNING;
    }

    id_accumulate_ac_sample(meas);
    if (elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) <
        AD_ID_LOCKED_RR_MEASURE_US) {
        return AD_ID_RESULT_RUNNING;
    }

    AD_Inverter_Disable();

    i_rms_A = id_average_ac_current_rms();
    vdc_avg_V = id_average_vdc();
    if ((i_rms_A < AD_ID_MIN_AC_CURRENT_A) || (vdc_avg_V <= 0.0f)) {
        return AD_ID_RESULT_FAILED;
    }

    params = AD_ParamID_GetParameters();
    if (params == 0) {
        return AD_ID_RESULT_FAILED;
    }

    ll_H = ((params->valid_mask & AD_MOTOR_PARAM_VALID_LL) != 0UL) ?
           params->Ll_H :
           params->Ls_H;
    if (ll_H <= 0.0f) {
        return AD_ID_RESULT_FAILED;
    }

    rs_ohm = ((params->valid_mask & AD_MOTOR_PARAM_VALID_RS) != 0UL) ?
             params->Rs_ohm :
             0.0f;

    v_phase_rms_V = id_phase_voltage_rms(duty, vdc_avg_V);
    z_abs_ohm = v_phase_rms_V / i_rms_A;
    x_ohm = omega_rad_s * ll_H;

    if ((z_abs_ohm <= AD_ID_MIN_IMPEDANCE_OHM) ||
        ((z_abs_ohm * z_abs_ohm) <= (x_ohm * x_ohm))) {
        return AD_ID_RESULT_FAILED;
    }

    r_total_ohm = sqrtf_local((z_abs_ohm * z_abs_ohm) - (x_ohm * x_ohm));
    rr_ohm = r_total_ohm - rs_ohm;
    if (rr_ohm <= AD_ID_MIN_IMPEDANCE_OHM) {
        return AD_ID_RESULT_FAILED;
    }

    id_store_rr(rr_ohm);
    id_runtime_reset(key, AD_ID_STAGE_IDLE, meas->timestamp_us);
    return AD_ID_RESULT_COMPLETE;
}

static uint8_t id_step_locked_rotor(uint8_t key, const AD_Measurements_t *meas, float duty)
{
    uint8_t result;

    if ((g_id_runtime.key != key) &&
        (g_id_runtime.key != AD_ID_KEY_LOCKED_LS) &&
        (g_id_runtime.key != AD_ID_KEY_LOCKED_RR)) {
        id_runtime_reset(key, AD_ID_STAGE_IDLE, meas->timestamp_us);
        g_id_runtime.auto_stage = 0U;
    }

    if (g_id_runtime.auto_stage == 0U) {
        result = id_step_ls(AD_ID_KEY_LOCKED_LS, meas, duty);
        if (result == AD_ID_RESULT_COMPLETE) {
            g_id_runtime.key = key;
            g_id_runtime.auto_stage = 1U;
            return AD_ID_RESULT_RUNNING;
        }
        return result;
    }

    result = id_step_locked_rr(AD_ID_KEY_LOCKED_RR, meas, duty);
    if (result == AD_ID_RESULT_COMPLETE) {
        g_id_runtime.key = key;
        g_id_runtime.auto_stage = 0U;
        return AD_ID_RESULT_COMPLETE;
    }

    return result;
}

static uint8_t id_step_magnetizing(uint8_t key, const AD_Measurements_t *meas, float duty)
{
    const AD_MotorParameters_t *params;
    float frequency_hz = id_get_no_load_frequency_hz();
    float i_rms_A;
    float vdc_avg_V;
    float v_phase_rms_V;
    float l_total_H;
    float lls_H = 0.0f;
    float lm_H;

    if ((g_id_runtime.key != key) || (g_id_runtime.stage == AD_ID_STAGE_IDLE)) {
        id_runtime_reset(key, AD_ID_STAGE_SETTLE, meas->timestamp_us);
    }

    if (id_apply_rotating_vector(duty, frequency_hz, meas) == 0U) {
        return AD_ID_RESULT_FAILED;
    }

    if (g_id_runtime.stage == AD_ID_STAGE_SETTLE) {
        if (elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) >=
            AD_ID_MAG_SETTLE_US) {
            id_runtime_reset(key, AD_ID_STAGE_MEASURE, meas->timestamp_us);
        }
        return AD_ID_RESULT_RUNNING;
    }

    id_accumulate_ac_sample(meas);
    if (elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) <
        AD_ID_MAG_MEASURE_US) {
        return AD_ID_RESULT_RUNNING;
    }

    AD_Inverter_Disable();

    i_rms_A = id_average_ac_current_rms();
    vdc_avg_V = id_average_vdc();
    if ((i_rms_A < AD_ID_MIN_AC_CURRENT_A) || (vdc_avg_V <= 0.0f)) {
        return AD_ID_RESULT_FAILED;
    }

    params = AD_ParamID_GetParameters();
    if ((params != 0) &&
        ((params->valid_mask & AD_MOTOR_PARAM_VALID_LL) != 0UL) &&
        (params->Ll_H > 0.0f)) {
        lls_H = 0.5f * params->Ll_H;
    }

    v_phase_rms_V = id_phase_voltage_rms(duty, vdc_avg_V);
    l_total_H = v_phase_rms_V / ((AD_ID_TWO_PI * frequency_hz) * i_rms_A);
    lm_H = l_total_H - lls_H;
    if (lm_H <= 0.0f) {
        return AD_ID_RESULT_FAILED;
    }

    id_store_lm(lm_H);
    id_runtime_reset(key, AD_ID_STAGE_IDLE, meas->timestamp_us);
    return AD_ID_RESULT_COMPLETE;
}

static uint8_t id_step_inertia_friction(uint8_t key, const AD_Measurements_t *meas, float duty)
{
    const AD_MotorParameters_t *params;
    float frequency_hz = id_get_inertia_frequency_hz();
    float speed_now_rpm = absf_local(meas->speed_rpm);
    float speed_delta_rpm;
    float omega_start_rad_s;
    float omega_end_rad_s;
    float omega_avg_rad_s;
    float accel_dt_s;
    float coast_dt_s;
    float i_rms_A;
    float vdc_avg_V;
    float v_phase_rms_V;
    float apparent_power_W;
    float copper_loss_W = 0.0f;
    float mech_power_W;
    float torque_Nm;
    float accel_rad_s2;
    float decel_rad_s2;
    float j_kg_m2;
    float b_Nm_s;

    if ((g_id_runtime.key != key) || (g_id_runtime.stage == AD_ID_STAGE_IDLE)) {
        id_runtime_reset(key, AD_ID_STAGE_ACCEL, meas->timestamp_us);
        g_id_runtime.speed_start_rpm = speed_now_rpm;
    }

    if (g_id_runtime.stage == AD_ID_STAGE_ACCEL) {
        if (id_apply_rotating_vector(duty, frequency_hz, meas) == 0U) {
            return AD_ID_RESULT_FAILED;
        }

        id_accumulate_ac_sample(meas);
        if (elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) <
            AD_ID_INERTIA_ACCEL_US) {
            return AD_ID_RESULT_RUNNING;
        }

        AD_Inverter_Disable();
        g_id_runtime.speed_end_rpm = speed_now_rpm;
        speed_delta_rpm = g_id_runtime.speed_end_rpm - g_id_runtime.speed_start_rpm;
        if (speed_delta_rpm < AD_ID_MIN_SPEED_DELTA_RPM) {
            return AD_ID_RESULT_FAILED;
        }

        accel_dt_s = (float)elapsed_us(meas->timestamp_us,
                                       g_id_runtime.stage_start_us) * 0.000001f;
        omega_start_rad_s = rpm_to_rad_s(g_id_runtime.speed_start_rpm);
        omega_end_rad_s = rpm_to_rad_s(g_id_runtime.speed_end_rpm);
        omega_avg_rad_s = 0.5f * (omega_start_rad_s + omega_end_rad_s);
        accel_rad_s2 = (omega_end_rad_s - omega_start_rad_s) / accel_dt_s;
        if ((omega_avg_rad_s <= 0.1f) || (accel_rad_s2 <= AD_ID_MIN_ACCEL_RAD_S2)) {
            return AD_ID_RESULT_FAILED;
        }

        i_rms_A = id_average_ac_current_rms();
        vdc_avg_V = id_average_vdc();
        if ((i_rms_A < AD_ID_MIN_AC_CURRENT_A) || (vdc_avg_V <= 0.0f)) {
            return AD_ID_RESULT_FAILED;
        }

        params = AD_ParamID_GetParameters();
        if ((params != 0) &&
            ((params->valid_mask & AD_MOTOR_PARAM_VALID_RS) != 0UL)) {
            copper_loss_W = 3.0f * i_rms_A * i_rms_A * params->Rs_ohm;
        }

        v_phase_rms_V = id_phase_voltage_rms(duty, vdc_avg_V);
        apparent_power_W = 3.0f * v_phase_rms_V * i_rms_A;
        mech_power_W = apparent_power_W - copper_loss_W;
        if (mech_power_W <= 0.0f) {
            return AD_ID_RESULT_FAILED;
        }

        torque_Nm = mech_power_W / omega_avg_rad_s;
        g_id_runtime.accel_torque_Nm = torque_Nm;
        g_id_runtime.accel_alpha_rad_s2 = accel_rad_s2;
        id_runtime_reset(key, AD_ID_STAGE_COAST, meas->timestamp_us);
        g_id_runtime.speed_start_rpm = speed_now_rpm;
        g_id_runtime.accel_torque_Nm = torque_Nm;
        g_id_runtime.accel_alpha_rad_s2 = accel_rad_s2;
        return AD_ID_RESULT_RUNNING;
    }

    AD_Inverter_Disable();
    if ((elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) <
         AD_ID_INERTIA_COAST_US) &&
        (speed_now_rpm > AD_ID_MIN_SPEED_RPM)) {
        return AD_ID_RESULT_RUNNING;
    }

    g_id_runtime.speed_end_rpm = speed_now_rpm;
    omega_start_rad_s = rpm_to_rad_s(g_id_runtime.speed_start_rpm);
    omega_end_rad_s = rpm_to_rad_s(g_id_runtime.speed_end_rpm);
    if ((omega_start_rad_s <= rpm_to_rad_s(AD_ID_MIN_SPEED_RPM)) ||
        (omega_end_rad_s >= omega_start_rad_s)) {
        return AD_ID_RESULT_FAILED;
    }

    coast_dt_s = (float)elapsed_us(meas->timestamp_us,
                                   g_id_runtime.stage_start_us) * 0.000001f;
    omega_avg_rad_s = 0.5f * (omega_start_rad_s + omega_end_rad_s);
    decel_rad_s2 = (omega_start_rad_s - omega_end_rad_s) / coast_dt_s;

    if ((coast_dt_s <= 0.0f) ||
        (omega_avg_rad_s <= 0.1f) ||
        (decel_rad_s2 <= 0.0f) ||
        (g_id_runtime.accel_torque_Nm <= 0.0f) ||
        (g_id_runtime.accel_alpha_rad_s2 <= AD_ID_MIN_ACCEL_RAD_S2)) {
        return AD_ID_RESULT_FAILED;
    }

    j_kg_m2 = g_id_runtime.accel_torque_Nm / g_id_runtime.accel_alpha_rad_s2;
    b_Nm_s = (j_kg_m2 * decel_rad_s2) / omega_avg_rad_s;
    if ((j_kg_m2 <= 0.0f) || (b_Nm_s <= 0.0f)) {
        return AD_ID_RESULT_FAILED;
    }

    id_store_inertia_friction(j_kg_m2, b_Nm_s);
    id_runtime_reset(key, AD_ID_STAGE_IDLE, meas->timestamp_us);
    return AD_ID_RESULT_COMPLETE;
}

static uint8_t id_step_rotation_3hz(uint8_t key, const AD_Measurements_t *meas, float duty)
{
    uint32_t ramp_us;
    float target_frequency_hz;
    uint16_t motor_control_type;

    (void)duty;

    if ((g_id_runtime.key != key) || (g_id_runtime.stage == AD_ID_STAGE_IDLE)) {
        id_runtime_reset(key, AD_ID_STAGE_MEASURE, meas->timestamp_us);
    }

    duty = id_get_rotation_test_duty();
    ramp_us = elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us);
    motor_control_type = AD_ParamID_GetMotorControlType();

    if ((motor_control_type == (uint16_t)AD_PARAM_ID_MOTOR_CONTROL_BLDC) &&
        (ramp_us < AD_ID_ROTATION_TEST_ALIGN_US)) {
        rotation_pwm_stop();
        g_id_runtime.pwm_timestamp_us = meas->timestamp_us;
        if (id_apply_six_step_sector(duty, 0U) == 0U) {
            return AD_ID_RESULT_FAILED;
        }
        return AD_ID_RESULT_RUNNING;
    }

    target_frequency_hz = AD_ParamID_GetRotationFrequencyHz();
    if (id_configure_rotation_pwm(duty, target_frequency_hz, motor_control_type) == 0U) {
        return AD_ID_RESULT_FAILED;
    }

    return AD_ID_RESULT_RUNNING;
}

static uint8_t id_step_auto(const AD_Measurements_t *meas, float duty)
{
    uint8_t result;

    if ((g_id_runtime.key != AD_PARAM_ID_MODE_AUTO_IDENTIFICATION) &&
        (g_id_runtime.key != AD_ID_KEY_AUTO_RS) &&
        (g_id_runtime.key != AD_ID_KEY_AUTO_LS) &&
        (g_id_runtime.key != AD_ID_KEY_AUTO_RR) &&
        (g_id_runtime.key != AD_ID_KEY_AUTO_LM) &&
        (g_id_runtime.key != AD_ID_KEY_AUTO_JB)) {
        id_runtime_reset(AD_PARAM_ID_MODE_AUTO_IDENTIFICATION, AD_ID_STAGE_IDLE, meas->timestamp_us);
        g_id_runtime.auto_stage = 0U;
    }

    if (g_id_runtime.auto_stage == 0U) {
        result = id_step_rs(AD_ID_KEY_AUTO_RS, meas, duty);
        if (result == AD_ID_RESULT_COMPLETE) {
            g_id_runtime.key = AD_PARAM_ID_MODE_AUTO_IDENTIFICATION;
            g_id_runtime.auto_stage = 1U;
            return AD_ID_RESULT_RUNNING;
        }
        return result;
    }

    if (g_id_runtime.auto_stage == 1U) {
        result = id_step_ls(AD_ID_KEY_AUTO_LS, meas, duty);
        if (result == AD_ID_RESULT_COMPLETE) {
            g_id_runtime.key = AD_PARAM_ID_MODE_AUTO_IDENTIFICATION;
            g_id_runtime.auto_stage = 2U;
            return AD_ID_RESULT_RUNNING;
        }
        return result;
    }

    if (g_id_runtime.auto_stage == 2U) {
        if (AD_ParamID_IsLockedRotorAllowed() == 0U) {
            AD_ParamID_MarkLockedRotorSkipped();
            g_id_runtime.key = AD_PARAM_ID_MODE_AUTO_IDENTIFICATION;
            g_id_runtime.auto_stage = 3U;
            return AD_ID_RESULT_RUNNING;
        }

        result = id_step_locked_rr(AD_ID_KEY_AUTO_RR, meas, duty);
        if (result == AD_ID_RESULT_COMPLETE) {
            g_id_runtime.key = AD_PARAM_ID_MODE_AUTO_IDENTIFICATION;
            g_id_runtime.auto_stage = 3U;
            return AD_ID_RESULT_RUNNING;
        }
        if (result == AD_ID_RESULT_FAILED) {
            return id_finish_auto_partial(meas->timestamp_us);
        }
        return result;
    }

    if (g_id_runtime.auto_stage == 3U) {
        result = id_step_magnetizing(AD_ID_KEY_AUTO_LM, meas, duty);
        if (result == AD_ID_RESULT_COMPLETE) {
            g_id_runtime.key = AD_PARAM_ID_MODE_AUTO_IDENTIFICATION;
            g_id_runtime.auto_stage = 4U;
            return AD_ID_RESULT_RUNNING;
        }
        if (result == AD_ID_RESULT_FAILED) {
            return id_finish_auto_partial(meas->timestamp_us);
        }
        return result;
    }

    result = id_step_inertia_friction(AD_ID_KEY_AUTO_JB, meas, duty);
    if (result == AD_ID_RESULT_COMPLETE) {
        g_id_runtime.key = AD_PARAM_ID_MODE_AUTO_IDENTIFICATION;
        g_id_runtime.auto_stage = 0U;
        return AD_ID_RESULT_COMPLETE;
    }
    if (result == AD_ID_RESULT_FAILED) {
        return id_finish_auto_partial(meas->timestamp_us);
    }

    return result;
}

void AD_Inverter_SetDuty(float duty_a, float duty_b, float duty_c)
{
    g_inverter_state.duty_a = clamp01(duty_a);
    g_inverter_state.duty_b = clamp01(duty_b);
    g_inverter_state.duty_c = clamp01(duty_c);

    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, duty_to_compare(g_inverter_state.duty_a));
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, duty_to_compare(g_inverter_state.duty_b));
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, duty_to_compare(g_inverter_state.duty_c));
}

void AD_Inverter_Disable(void)
{
    rotation_pwm_stop();
    inverter_disable_update_irq();
    AD_Inverter_SetDuty(0.0f, 0.0f, 0.0f);
    (void)HAL_TIMEx_PWMN_Stop(&htim1, TIM_CHANNEL_3);
    (void)HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3);
    (void)HAL_TIMEx_PWMN_Stop(&htim1, TIM_CHANNEL_2);
    (void)HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_2);
    (void)HAL_TIMEx_PWMN_Stop(&htim1, TIM_CHANNEL_1);
    (void)HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1);
    g_inverter_state.pwm_running = 0U;
    g_inverter_state.service_pwm_running = 0U;
    g_inverter_state.service_output = AD_INVERTER_PWM_OUTPUT_NONE;
    g_inverter_state.applied_test_duty = 0.0f;
}

void AD_Inverter_SetPwmPolarityFlags(uint16_t flags)
{
    flags &= (uint16_t)AD_INVERTER_PWM_POLARITY_MASK;

    if (flags == g_inverter_state.pwm_polarity_flags) {
        return;
    }

    if (g_inverter_state.pwm_running != 0U) {
        AD_Inverter_Disable();
    }

    g_inverter_state.pwm_polarity_flags = flags;
    inverter_write_pwm_polarity(flags);
}

uint16_t AD_Inverter_GetPwmPolarityFlags(void)
{
    return g_inverter_state.pwm_polarity_flags;
}

void AD_Inverter_SetPwmTimingMode(uint16_t mode)
{
    mode = (mode == (uint16_t)AD_INVERTER_PWM_TIMING_UP) ?
           (uint16_t)AD_INVERTER_PWM_TIMING_UP :
           (uint16_t)AD_INVERTER_PWM_TIMING_CENTER;

    if (mode == g_inverter_state.pwm_timing_mode) {
        return;
    }

    if (g_inverter_state.pwm_running != 0U) {
        AD_Inverter_Disable();
    }

    g_inverter_state.pwm_timing_mode = mode;
    inverter_write_pwm_timing(mode);
    AD_Inverter_SetDuty(g_inverter_state.duty_a,
                        g_inverter_state.duty_b,
                        g_inverter_state.duty_c);
}

uint16_t AD_Inverter_GetPwmTimingMode(void)
{
    return g_inverter_state.pwm_timing_mode;
}

void AD_Inverter_Init(void)
{
    g_inverter_state.initialized = 1U;
    g_inverter_state.outputs_allowed_by_build = (AD_INVERTER_ENABLE_OUTPUTS != 0) ? 1U : 0U;
    g_inverter_state.pwm_running = 0U;
    g_inverter_state.service_pwm_running = 0U;
    g_inverter_state.service_output = AD_INVERTER_PWM_OUTPUT_NONE;
    g_inverter_state.requested_test_duty = AD_PARAM_ID_DEFAULT_PWM_DUTY_LIMIT;
    g_inverter_state.applied_test_duty = 0.0f;
    g_inverter_state.electrical_frequency_Hz = 0.0f;
    g_inverter_state.electrical_angle_rad = 0.0f;
    g_inverter_state.pwm_polarity_flags =
        (uint16_t)(AD_INVERTER_DEFAULT_PWM_POLARITY_FLAGS &
                   AD_INVERTER_PWM_POLARITY_MASK);
    g_inverter_state.pwm_timing_mode = (uint16_t)AD_INVERTER_PWM_TIMING_CENTER;
    g_inverter_state.id_stage = AD_ID_STAGE_IDLE;
    inverter_write_pwm_polarity(g_inverter_state.pwm_polarity_flags);
    inverter_write_pwm_timing(g_inverter_state.pwm_timing_mode);
    id_runtime_reset(AD_ID_STAGE_IDLE, AD_ID_STAGE_IDLE, 0UL);
    AD_Inverter_Disable();
}

uint8_t AD_Inverter_Enable(void)
{
    if (g_inverter_state.initialized == 0U) {
        return 0U;
    }

    if ((AD_INVERTER_ENABLE_OUTPUTS == 0) ||
        (AD_ParamID_IsPowerStageAllowed() == 0U)) {
        AD_Inverter_Disable();
        return 0U;
    }

    if ((g_inverter_state.pwm_running != 0U) &&
        (g_inverter_state.service_pwm_running == 0U)) {
        return 1U;
    }

    inverter_clear_break_flags();

    if (HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1) != HAL_OK) {
        AD_Inverter_Disable();
        return 0U;
    }

    if (HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_1) != HAL_OK) {
        AD_Inverter_Disable();
        return 0U;
    }
    if (HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2) != HAL_OK) {
        AD_Inverter_Disable();
        return 0U;
    }
    if (HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_2) != HAL_OK) {
        AD_Inverter_Disable();
        return 0U;
    }
    if (HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3) != HAL_OK) {
        AD_Inverter_Disable();
        return 0U;
    }
    if (HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_3) != HAL_OK) {
        AD_Inverter_Disable();
        return 0U;
    }

    g_inverter_state.pwm_running = 1U;
    g_inverter_state.service_pwm_running = 0U;
    g_inverter_state.service_output = AD_INVERTER_PWM_OUTPUT_NONE;
    inverter_enable_update_irq();
    return 1U;
}

uint8_t AD_Inverter_StartPwmOutput(uint8_t output, float duty)
{
    uint32_t channel;
    uint8_t use_complementary = 0U;
    uint8_t same_service;
    uint32_t compare;

    if ((AD_INVERTER_ENABLE_OUTPUTS == 0) ||
        (AD_ParamID_IsPowerStageAllowed() == 0U)) {
        AD_Inverter_Disable();
        return 0U;
    }

    duty = clamp01(duty);
    g_inverter_state.requested_test_duty = duty;
    g_inverter_state.applied_test_duty = duty;
    same_service = (uint8_t)((g_inverter_state.pwm_running != 0U) &&
                             (g_inverter_state.service_pwm_running != 0U) &&
                             (g_inverter_state.service_output == output));

    switch (output) {
    case AD_INVERTER_PWM_OUTPUT_UH:
        channel = TIM_CHANNEL_1;
        break;
    case AD_INVERTER_PWM_OUTPUT_UL:
        channel = TIM_CHANNEL_1;
        use_complementary = 1U;
        break;
    case AD_INVERTER_PWM_OUTPUT_VH:
        channel = TIM_CHANNEL_2;
        break;
    case AD_INVERTER_PWM_OUTPUT_VL:
        channel = TIM_CHANNEL_2;
        use_complementary = 1U;
        break;
    case AD_INVERTER_PWM_OUTPUT_WH:
        channel = TIM_CHANNEL_3;
        break;
    case AD_INVERTER_PWM_OUTPUT_WL:
        channel = TIM_CHANNEL_3;
        use_complementary = 1U;
        break;
    case AD_INVERTER_PWM_OUTPUT_ALL:
        if (same_service == 0U) {
            AD_Inverter_Disable();
        }
        AD_Inverter_SetDuty(0.5f, 0.5f, 0.5f);
        if ((same_service == 0U) && (AD_Inverter_Enable() == 0U)) {
            return 0U;
        }
        g_inverter_state.pwm_running = 1U;
        g_inverter_state.service_pwm_running = 1U;
        g_inverter_state.service_output = output;
        set_service_debug_duty(output, duty);
        return 1U;
    default:
        return 0U;
    }

    if (same_service == 0U) {
        AD_Inverter_Disable();
    }

    compare = (use_complementary != 0U) ?
              duty_to_compare_inverted(duty) :
              duty_to_compare(duty);
    __HAL_TIM_SET_COMPARE(&htim1, channel, compare);

    if (same_service == 0U) {
        inverter_clear_break_flags();
        if (use_complementary != 0U) {
            if (HAL_TIMEx_PWMN_Start(&htim1, channel) != HAL_OK) {
                AD_Inverter_Disable();
                return 0U;
            }
        } else {
            if (HAL_TIM_PWM_Start(&htim1, channel) != HAL_OK) {
                AD_Inverter_Disable();
                return 0U;
            }
        }
    }

    g_inverter_state.pwm_running = 1U;
    g_inverter_state.service_pwm_running = 1U;
    g_inverter_state.service_output = output;
    set_service_debug_duty(output, duty);
    return 1U;
}

uint8_t AD_Inverter_ApplyCommand(const AD_InverterCommand_t *command)
{
    if ((command == 0) || (command->enable == 0U)) {
        AD_Inverter_Disable();
        return 1U;
    }

    if ((AD_INVERTER_ENABLE_OUTPUTS == 0) ||
        (AD_ParamID_IsPowerStageAllowed() == 0U)) {
        AD_Inverter_Disable();
        return 0U;
    }

    if ((g_inverter_state.pwm_running != 0U) &&
        (g_inverter_state.service_pwm_running == 0U)) {
        AD_Inverter_SetDuty(command->duty_a, command->duty_b, command->duty_c);
        return 1U;
    }

    if (g_inverter_state.service_pwm_running != 0U) {
        AD_Inverter_Disable();
    }

    AD_Inverter_SetDuty(command->duty_a, command->duty_b, command->duty_c);
    return AD_Inverter_Enable();
}

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if ((htim != 0) && (htim->Instance == TIM1)) {
        rotation_pwm_service_update();
    }
}

const AD_InverterState_t* AD_Inverter_GetState(void)
{
    return &g_inverter_state;
}

void AD_ParamID_HardwareDisable(void)
{
    AD_Inverter_Disable();
}

uint8_t AD_ParamID_HardwareStep(uint8_t mode, const AD_Measurements_t *meas)
{
    uint8_t result;
    float duty = id_get_test_duty();

    switch (mode) {
    case AD_PARAM_ID_MODE_PWM_TEST_UH:
        return AD_Inverter_StartPwmOutput(AD_INVERTER_PWM_OUTPUT_UH, duty);
    case AD_PARAM_ID_MODE_PWM_TEST_UL:
        return AD_Inverter_StartPwmOutput(AD_INVERTER_PWM_OUTPUT_UL, duty);
    case AD_PARAM_ID_MODE_PWM_TEST_VH:
        return AD_Inverter_StartPwmOutput(AD_INVERTER_PWM_OUTPUT_VH, duty);
    case AD_PARAM_ID_MODE_PWM_TEST_VL:
        return AD_Inverter_StartPwmOutput(AD_INVERTER_PWM_OUTPUT_VL, duty);
    case AD_PARAM_ID_MODE_PWM_TEST_WH:
        return AD_Inverter_StartPwmOutput(AD_INVERTER_PWM_OUTPUT_WH, duty);
    case AD_PARAM_ID_MODE_PWM_TEST_WL:
        return AD_Inverter_StartPwmOutput(AD_INVERTER_PWM_OUTPUT_WL, duty);
    case AD_PARAM_ID_MODE_PWM_TEST_ALL:
        return AD_Inverter_StartPwmOutput(AD_INVERTER_PWM_OUTPUT_ALL, duty);

    case AD_PARAM_ID_MODE_STATOR_RESISTANCE:
        result = id_step_rs(mode, meas, duty);
        break;
    case AD_PARAM_ID_MODE_LOCKED_ROTOR_LEAKAGE:
        result = id_step_locked_rotor(mode, meas, duty);
        break;
    case AD_PARAM_ID_MODE_AUTO_IDENTIFICATION:
        result = id_step_auto(meas, duty);
        break;

    case AD_PARAM_ID_MODE_NO_LOAD_MAGNETIZING:
        result = id_step_magnetizing(mode, meas, duty);
        break;
    case AD_PARAM_ID_MODE_INERTIA_FRICTION:
        result = id_step_inertia_friction(mode, meas, duty);
        break;
    case AD_PARAM_ID_MODE_ROTATION_3HZ:
        result = id_step_rotation_3hz(mode, meas, duty);
        break;

    default:
        AD_Inverter_Disable();
        return 0U;
    }

    if (result == AD_ID_RESULT_COMPLETE) {
        AD_Inverter_Disable();
        AD_ParamID_MarkComplete();
        return 1U;
    }

    if (result == AD_ID_RESULT_RUNNING) {
        return 1U;
    }

    AD_Inverter_Disable();
    AD_ParamID_MarkStepFailed();
    return 0U;
}
