/**
 * bldc_fsm.c — 状态机主逻辑实现
 *
 * 状态流转:
 *   STOP → (FSM_Start) → ALIGN → (定时到) → ACCEL → (检测到6次稳定过零) → RUN → (故障) → FAULT
 *   FAULT → (FSM_ClearFault) → STOP
 *
 * 调度架构:
 *   中断级 (20kHz, TIM1_UP_IRQHandler):
 *     - 读 ADC → 滑动平均滤波
 *     - 电流环 PI → 更新 PWM 占空比
 *     - 过零检测 + 30° 延迟计数 → 换相
 *     - 保护监测 (过流计数)
 *
 *   主循环级 (while(1)):
 *     - 速度环 500Hz (每40个PWM周期)
 *     - 位置环 50Hz (每400个PWM周期)
 *     - 堵转检测
 *     - 状态机流转
 */

#include "bldc_fsm.h"

/* ==================== 全局变量 ==================== */
volatile FsmState  g_state         = FSM_STOP;
volatile uint8_t   g_fault         = FAULT_NONE;
volatile uint8_t   g_run_enable    = 0;
volatile uint32_t  g_pwm_cycle_count = 0;
volatile uint32_t  g_comm_step     = 0;
volatile uint32_t  g_comm_count    = 0;
volatile uint16_t  g_comm_interval_us   = 0;
volatile uint16_t  g_comm_interval_pwm  = 0;
volatile int16_t   g_speed_rpm     = 0;       /* Q15 */
volatile int16_t   g_current_ref_ma = 0;      /* Q15 */
volatile int16_t   g_speed_ref_rpm = Q15_FLOAT(1500.0f);  /* 默认 1500 RPM */
volatile int16_t   g_position_ref  = 0;
volatile uint16_t  g_pwm_duty      = 0;
volatile uint16_t  g_accel_count   = 0;
volatile int16_t   g_i_bus_ma      = 0;   /* 母线电流 mA (Q15) */

/* ==================== 内部状态变量 ==================== */
static uint32_t s_align_timer      = 0;    /* 预定位计时器 (PWM 周期计数) */
static uint8_t  s_zc_waiting       = 0;    /* 等待过零标志 */
static uint32_t s_stall_timer      = 0;    /* 堵转检测计时器 */
static uint32_t s_oc_counter       = 0;    /* 过流持续计数 */
static uint32_t s_speed_loop_div   = 0;    /* 速度环分频计数 */
static uint32_t s_pos_loop_div     = 0;    /* 位置环分频计数 */
static uint32_t s_zc_delay_counter = 0;    /* 过零后延迟计数 */
static uint8_t  s_zc_pending       = 0;    /* 有过零等待换相 */
static uint32_t s_prev_comm_cycle  = 0;    /* 上次换相时的 PWM 周期计数 */

/* 加速阶段内部参数 */
static uint16_t s_accel_freq;              /* 当前加速换相频率 (0.1Hz 单位) */
static uint16_t s_accel_interval_pwm;      /* 当前加速换相间隔 (PWM 周期数) */
static uint16_t s_accel_step_counter;      /* 加速换相计数 */
static int16_t  s_accel_duty;              /* 当前加速占空比 */

/* ==================== 状态名表 (调试用) ==================== */
static const char *g_state_names[] = {
    "STOP", "ALIGN", "ACCEL", "RUN", "FAULT"
};

const char* FSM_StateStr(void)
{
    return g_state_names[g_state];
}

/* ==================== 初始化 ==================== */
void FSM_Init(void)
{
    g_state = FSM_STOP;
    g_fault = FAULT_NONE;
    g_run_enable = 0;
    g_pwm_cycle_count = 0;
    g_comm_step = 0;
    g_comm_count = 0;
    g_pwm_duty = 0;

    PID_Init();
    BEMF_Init();

    /* 状态机各计数器清零 */
    s_align_timer = 0;
    s_zc_waiting = 0;
    s_stall_timer = 0;
    s_oc_counter = 0;
    s_speed_loop_div = 0;
    s_pos_loop_div = 0;
    s_zc_pending = 0;
}

/* ==================== 启动/停止 ==================== */
void FSM_Start(void)
{
    if (g_state == FSM_STOP || g_state == FSM_FAULT) {
        g_run_enable = 1;
        g_fault = FAULT_NONE;
        g_state = FSM_ALIGN;

        /* 清理计数 */
        s_align_timer = 0;
        g_accel_count = 0;
        BEMF_ClearZcCount();

        /* 使能 PWM 主输出 */
        TIM1->BDTR |= TIM_BDTR_MOE;
    }
}

void FSM_Stop(void)
{
    g_run_enable = 0;

    /* 关 PWM 所有输出 */
    COMM_Brake();

    /* 清理 */
    g_pwm_duty = 0;
    s_zc_pending = 0;
    g_comm_step = 0;

    g_state = FSM_STOP;
}

/* ==================== 设置速度参考值 ==================== */
void FSM_SetSpeedRef(int16_t rpm)
{
    /* 限幅到合理范围 */
    if (rpm < 100) rpm = 100;
    if (rpm > 8000) rpm = 8000;

    g_speed_ref_rpm = Q15_FLOAT((float)rpm);  /* 转 Q15 */
}

/* ==================== 故障清除 ==================== */
void FSM_ClearFault(void)
{
    if (g_state == FSM_FAULT) {
        g_fault = FAULT_NONE;
        g_state = FSM_STOP;
        s_oc_counter = 0;
    }
}

/* ==================== ALIGN 阶段 ==================== */
/* 预定位: 给 U+V- 通电 500ms, 转子锁到 0° 电角度 */
static void FSM_Align(uint32_t pwm_duty_q15)
{
    /* 首次进入: 设置 U+V- 通电 */
    if (s_align_timer == 0) {
        /* U高 100% + V低 PWM */
        TIM1->CCER &= ~(CC1NE|CC2E|CC3E|CC3NE);
        TIM1->CCER |= (CC1E | CC2NE);
        TIM1->CCR1 = PWM_ARR_VALUE;          /* U高 100% */
        TIM1->CCR2 = pwm_duty_q15;           /* V低 duty */
        TIM1->CCR3 = 0;

        g_pwm_duty = (uint16_t)pwm_duty_q15;
    }

    s_align_timer++;

    /* 预定位持续 ALIGN_TIME_MS (500ms = 10000 PWM 周期 @ 20kHz) */
    uint32_t align_timeout = (ALIGN_TIME_MS * PWM_FREQ_HZ) / 1000;

    if (s_align_timer >= align_timeout) {
        /* 预定位完成 → 进入 ACCEL */
        s_align_timer = 0;
        g_state = FSM_ACCEL;
        g_accel_count = 0;

        /* 初始化加速参数 */
        s_accel_freq = ACCEL_START_FREQ_HZ * 10;  /* 300 (代表 30.0Hz) */
        s_accel_interval_pwm = (PWM_FREQ_HZ * 10) / s_accel_freq;  /* = 20000*10/300 = 666 PWM 周期 */
        s_accel_step_counter = 0;
        s_accel_duty = ACCEL_DUTY_MIN;

        /* 切到步0 (U+V-) 作为加速起点 */
        g_comm_step = COMM_STEP0_U_P_V_N;
        COMM_SetStep(COMM_STEP0_U_P_V_N, (uint16_t)s_accel_duty);

        BEMF_ClearZcCount();
    }
}

/* ==================== ACCEL 阶段 ==================== */
/* 外同步开环加速: 按预定间隔强制换相, 逐步加速 */
static void FSM_Accel(void)
{
    s_accel_step_counter++;

    /* 1. 到达换相间隔 → 强制换相 */
    if (s_accel_step_counter >= s_accel_interval_pwm) {
        s_accel_step_counter = 0;
        g_accel_count++;

        /* 前进到下一步 */
        g_comm_step = (g_comm_step + 1) % COMM_STEP_COUNT;

        /* 执行换相 (使用当前加速占空比) */
        const CommEntry *e = &g_comm_table[g_comm_step];
        TIM1->CCER = (TIM1->CCER & ~CCER_POLARITY_MASK) | e->ccer_bits_set;
        TIM1->CCR1 = e->ch1_ccr;
        TIM1->CCR2 = (uint16_t)s_accel_duty;   /* 低侧用加速占空比 */
        TIM1->CCR3 = e->ch3_ccr;

        /* 2. 每 ACCEL_STEP_COUNT 次换相后提速 */
        if ((g_accel_count % ACCEL_STEP_COUNT) == 0 && g_accel_count < ACCEL_TOTAL_STEPS) {
            /* 换相频率提高 ACCEL_RATE % */
            s_accel_freq = (s_accel_freq * ACCEL_RATE) / 100;
            s_accel_interval_pwm = (PWM_FREQ_HZ * 10) / s_accel_freq;
            if (s_accel_interval_pwm < 3) s_accel_interval_pwm = 3;  /* 最小 3 个 PWM 周期 */

            /* 占空比线性爬升 */
            s_accel_duty += (ACCEL_DUTY_MAX - ACCEL_DUTY_MIN) * ACCEL_STEP_COUNT / ACCEL_TOTAL_STEPS;
            if (s_accel_duty > ACCEL_DUTY_MAX) s_accel_duty = ACCEL_DUTY_MAX;
        }

        /* 3. 检查是否可切入闭环 */
        if (BEMF_GetValidZcCount() >= ACCEL_SETTLE_COUNT || g_accel_count >= ACCEL_TOTAL_STEPS) {
            g_state = FSM_RUN;
            g_comm_interval_pwm = s_accel_interval_pwm;  /* 用于速度环初始值 */

            /* 重置 PID 积分, 准备闭环 */
            PID_ResetIntegral(&g_pid_speed);
        }
    }
}

/* ==================== PWM 中断处理 (20kHz) ==================== */
/* 这是最关键的函数, 每个 PWM 周期执行一次 */
void FSM_PwmIsr(void)
{
    uint32_t cycle = g_pwm_cycle_count;

    /* ====== 1. 读取 ADC 结果 ====== */
    /* ADC 注入组转换完成 → 结果已在 g_adc_raw 中 */
    /* (ADC 在 TIM1 TRGO 触发下自动转换, 结果直接读取) */
    if (ADC1->SR & ADC_SR_EOC) {
        /* 注入组通道顺序采样, 结果在 ADC1->JDR1~JDR4 */
        g_adc_raw[ADC_INJ_CH_U] = (uint16_t)ADC1->JDR1;
        g_adc_raw[ADC_INJ_CH_V] = (uint16_t)ADC1->JDR2;
        g_adc_raw[ADC_INJ_CH_W] = (uint16_t)ADC1->JDR3;
        g_adc_raw[ADC_INJ_CH_I] = (uint16_t)ADC1->JDR4;

        /* 滑动平均滤波 */
        BEMF_UpdateFilter();
    }

    /* ====== 2. 电流环 (每个 PWM 周期都跑) ====== */
    /* 母线电流: ADC 值转 mA, 归一化到 Q15 */
    uint32_t i_bus_ma = (uint32_t)g_adc_raw[ADC_INJ_CH_I]
                        * CURRENT_FS_MA / ADC_RESOLUTION;
    g_i_bus_ma = (int16_t)i_bus_ma;

    /* 电流参考归一化 */
    int16_t i_ref_q15 = (int16_t)((int32_t)g_current_ref_ma * Q15_ONE
                                  / MOTOR_CURRENT_MAX);
    int16_t i_fb_q15  = (int16_t)((int32_t)i_bus_ma * Q15_ONE
                                  / MOTOR_CURRENT_MAX);

    int16_t duty_q15 = PID_CurrentRun(i_ref_q15, i_fb_q15);

    /* 更新 PWM 占空比 (给低侧管) */
    /* 电流环输出 duty_q15 范围 [PWM_DUTY_MIN, PWM_DUTY_MAX] */
    g_pwm_duty = (uint16_t)duty_q15;

    /* ====== 3. 根据状态机执行不同逻辑 ====== */
    switch (g_state) {

    case FSM_STOP:
        /* 停止状态: 无动作 */
        break;

    case FSM_ALIGN:
        FSM_Align(duty_q15);
        break;

    case FSM_ACCEL:
        FSM_Accel();
        break;

    case FSM_RUN:
        /* a) 过零检测 */
        if (BEMF_DetectZeroCross(g_comm_step)) {
            s_zc_pending = 1;      /* 标记等待换相 */
            s_zc_delay_counter = 0;

            /* 更新换相间隔 (用于 30° 延迟) */
            uint32_t now = cycle;
            uint32_t delta = now - s_prev_comm_cycle;
            s_prev_comm_cycle = now;
            g_comm_interval_pwm = (uint16_t)delta;
            g_comm_interval_us = (uint16_t)((uint64_t)delta * 1000000UL / PWM_FREQ_HZ);
        }

        /* b) 30° 延迟计数 → 执行换相 */
        if (s_zc_pending) {
            s_zc_delay_counter++;

            /* 30° 延迟 = 换相间隔 / 2 (以 PWM 周期数计算) */
            uint16_t delay_target = g_comm_interval_pwm >> 1;
            if (delay_target < 2) delay_target = 2;  /* 最小 2 个周期 */

            if (s_zc_delay_counter >= delay_target) {
                s_zc_pending = 0;

                /* 执行换相 */
                g_comm_step = (g_comm_step + 1) % COMM_STEP_COUNT;
                g_comm_count++;

                const CommEntry *e = &g_comm_table[g_comm_step];
                TIM1->CCER = (TIM1->CCER & ~CCER_POLARITY_MASK) | e->ccer_bits_set;
                TIM1->CCR1 = e->ch1_ccr;
                TIM1->CCR2 = (e->ccer_bits_set & CC2NE) ? g_pwm_duty : e->ch2_ccr;
                TIM1->CCR3 = (e->ccer_bits_set & CC3NE) ? g_pwm_duty : e->ch3_ccr;

                /* 更新速度 (由换相间隔计算) */
                /* 转速 RPM = 60 × PWM_FREQ / (极对数 × 6 × comm_interval_pwm) */
                if (g_comm_interval_pwm > 0) {
                    uint32_t numerator = 60UL * PWM_FREQ_HZ;
                    uint32_t denominator = MOTOR_POLE_PAIRS * 6UL * (uint32_t)g_comm_interval_pwm;
                    uint32_t rpm = 0;
                    if (denominator > 0) {
                        rpm = numerator / denominator;
                    }
                    /* 转速归一化到 Q15 (基准=3000RPM) */
                    g_speed_rpm = (int16_t)((uint32_t)rpm * Q15_ONE / MOTOR_SPEED_NOM);
                }
            }
        }
        break;

    case FSM_FAULT:
        /* 故障状态: PWM 由外部刹车, 此处不作操作 */
        break;
    }

    /* ====== 4. 软件过流检测 ====== */
    if (i_bus_ma > PROTECT_OC_TRIP_MA) {
        s_oc_counter++;
        if (s_oc_counter > PROTECT_OC_DURATION) {
            /* 持续过流 → 故障 */
            g_fault = FAULT_OC_SOFTWARE;
            g_state = FSM_FAULT;
            COMM_Brake();
        }
    } else {
        s_oc_counter = 0;
    }

    /* ====== 5. PWM 周期计数递增 ====== */
    g_pwm_cycle_count++;
}

/* ==================== 主循环调度 ==================== */
void FSM_Loop(void)
{
    /* === 速度环 (500Hz = 每 40 个 PWM 周期) === */
    s_speed_loop_div++;
    if (s_speed_loop_div >= SPEED_LOOP_DIV) {
        s_speed_loop_div = 0;

        if (g_state == FSM_RUN) {
            /* 目标转速 (已存为 Q15) */
            int16_t q15_output = PID_SpeedRun(g_speed_ref_rpm, g_speed_rpm);

            /* 输出映射到电流参考值 (mA) */
            g_current_ref_ma = (int16_t)((int32_t)q15_output
                                         * MOTOR_CURRENT_MAX / Q15_ONE);
            if (g_current_ref_ma < 100) g_current_ref_ma = 100;  /* 最小 100mA */
        }
    }

    /* === 位置环 (50Hz = 每 400 个 PWM 周期) === */
    s_pos_loop_div++;
    if (s_pos_loop_div >= POSITION_LOOP_DIV) {
        s_pos_loop_div = 0;

        /* 位置环为可选, 此处仅提供 P 控制接口 */
        if (0) {  /* 暂不启用位置环, 需要时打开 */
            int16_t pos_error = g_position_ref;  /* 需接入实际位置反馈 */
            int16_t speed_ref_q15 = PID_PositionP(pos_error, g_pid_pos_kp);
            if (speed_ref_q15 < 0) speed_ref_q15 = 0;
            g_speed_ref_rpm = speed_ref_q15;
        }
    }

    /* === 堵转检测 (每 40 个 PWM 周期检查一次, 即 500Hz 检查) === */
    if (g_state == FSM_RUN) {
        /* 每 100 次检查 (200ms) 看一次转速是否过低 */
        static uint8_t stall_check = 0;
        stall_check++;

        if (stall_check >= 100) {  /* 100 × 2ms = 200ms */
            stall_check = 0;

            /* 转速低于阈值, 且已经运行了足够时间 → 判定堵转 */
            if (g_speed_rpm < Q15_FLOAT(PROTECT_FAULT_SPEED / 3000.0f)
                && g_comm_count > 100)
            {
                g_fault = FAULT_STALL;
                g_state = FSM_FAULT;
                COMM_Brake();
            }
        }
    }

    /* === 故障时熄灭 LED 的简单处理 === */
    if (g_state == FSM_FAULT) {
        /* 闪烁故障指示灯 (在 main 中实现, 此处只设状态) */
    }
}