#include "Int_TMC2209.h"

/* 
 * TIM2 CH1 使用 TOGGLE 模式输出 STEP：
 * 每次中断只翻转一次电平，两个中断才构成一个完整 STEP 脉冲。
 */

// 静态变量，记录分频状态
static uint8_t double_flag = 0;

void Int_TMC2209_init(void)
{
}

static void Int_TMC2209_set_steps(Stepper_t *stepper)
{
    /* 将目标运动参数换算为总步数和梯形/三角速度曲线参数 */
    // --- 参数单位定义 ---
    // stepper->distance  -> 代表目标距离 (mm)
    // stepper->speed   -> 代表目标速度 (mm/s)
    // stepper->acc     -> 代表加速度 (mm/s^2)
    // stepper->start_speed -> 代表启动速度 (mm/s)

    // 1. 计算总步数
    // 总步数 = 距离(mm) * 每毫米步数
    stepper->total_step = (uint32_t)(stepper->distance * STEPS_PER_MM);

    // 2. 物理运动学公式计算加速距离
    // 公式: x = (v^2 - v0^2) / 2a
    // 单位全部统一为 mm, mm/s, mm/s^2
    float v_target = stepper->speed;      // mm/s
    float v_start = stepper->start_speed; // mm/s
    float accel = stepper->acc;           // mm/s^2

    // 防止除0错误
    if (accel == 0 || v_target == 0)
        return;

    // 计算加速所需的距离 (mm)
    float acc_distance_mm = (v_target * v_target - v_start * v_start) / (2.0f * accel);

    // 将加速距离转换为加速步数
    stepper->acc_step = (uint32_t)(acc_distance_mm * STEPS_PER_MM);

    // 3. 规划梯形/三角形速度曲线
    if (stepper->total_step < stepper->acc_step * 2)
    {
        // 距离太短，跑不到目标速度 -> 三角形加减速
        stepper->acc_step = stepper->total_step / 2;
        stepper->constant_step = 0;
        debug_printf("Model: Triangle, AccStep: %d", stepper->acc_step);
    }
    else
    {
        // 距离足够 -> 梯形加减速
        stepper->constant_step = stepper->total_step - stepper->acc_step * 2;
        debug_printf("Model: Trapezoid, Acc: %d, Const: %d", stepper->acc_step, stepper->constant_step);
    }

    // 4. 计算每一步的速度增量 (mm/s per step)
    // 逻辑：(目标速度 - 启动速度) / 加速步数
    // 含义：每走一步，速度增加多少 mm/s
    if (stepper->acc_step > 0)
    {
        stepper->speed_inc = (float)(v_target - v_start) / stepper->acc_step;
    }
    else
    {
        stepper->speed_inc = 0;
    }

    // 5. 初始化状态
    stepper->step_count = 0;
    stepper->current_speed = stepper->start_speed;
    stepper->state = ACCELERATE;
}

void Int_stepper_run(void)
{
    /* 仅用于调试的固定步进函数，不参与正式运行逻辑 */
    // 此函数用于测试，硬编码了步数，建议根据新的 STEPS_PER_MM 修改这里
    // 例如走 10mm:
    // uint32_t steps = 10 * STEPS_PER_MM * 2; // *2 是因为 toggle
    HAL_GPIO_WritePin(STEPPER123_EN_GPIO_Port, STEPPER123_EN_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(STEPPER1_DIR_GPIO_Port, STEPPER1_DIR_Pin, GPIO_PIN_SET);

    // 这里的3200原来是转一圈，现在如果导程是8mm，3200次toggle (1600步) = 8mm
    for (uint16_t i = 0; i < 3200; i++)
    {
        HAL_GPIO_TogglePin(STEPPER_1_STEP_GPIO_Port, STEPPER_1_STEP_Pin);
        HAL_Delay(1);
    }
}

void Int_TMC2209_start(Stepper_t *stepper, Encoder_t *encoder)
{
    /* 启动流程：
     * 1) 清零编码器基准
     * 2) 使能驱动并配置方向
     * 3) 计算速度曲线参数
     * 4) 配置并启动 TIM2 输出中断
     */
    // 清零编码器值
    encoder->pulses = 0;
    encoder->overflow = 0;
    __HAL_TIM_SET_COUNTER(&htim1, 0);
    encoder->z = 0;
    HAL_GPIO_WritePin(ENCODER_MODE_GPIO_Port, ENCODER_MODE_Pin, GPIO_PIN_RESET);
    HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_ALL);

    // 使能驱动
    HAL_GPIO_WritePin(STEPPER123_EN_GPIO_Port, STEPPER123_EN_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(STEPPER1_DIR_GPIO_Port, STEPPER1_DIR_Pin, stepper->dir);

    // 复位分频标志位
    double_flag = 0;

    // 设置步数参数
    Int_TMC2209_set_steps(stepper);

    // [修改] 计算初始ARR (基于 mm/s)
    // 最小速度保护，防止ARR计算溢出
    if (stepper->start_speed < 0.1f)
        stepper->start_speed = 0.1f;

    // 这里传入的是 mm/s
    uint32_t arr = CALC_ARR(stepper->start_speed);

    debug_printf("Start Speed: %.2f mm/s, ARR: %d", stepper->start_speed, arr);

    __HAL_TIM_SET_COUNTER(&htim2, 0);
    __HAL_TIM_SET_AUTORELOAD(&htim2, arr);
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, arr / 2);

    HAL_TIM_OC_Start_IT(&htim2, TIM_CHANNEL_1);
}

void Int_TMC2209_stop(void)
{
    /* 停止流程：停止STEP输出 -> 关闭驱动 -> 停止编码器计数 */
    HAL_TIM_OC_Stop_IT(&htim2, TIM_CHANNEL_1);
    HAL_GPIO_WritePin(STEPPER123_EN_GPIO_Port, STEPPER123_EN_Pin, GPIO_PIN_RESET);
    HAL_TIM_Encoder_Stop(&htim1, TIM_CHANNEL_ALL);
}

extern Stepper_t stepper_1;
void HAL_TIM_OC_DelayElapsedCallback(TIM_HandleTypeDef *htim)
{
    /* TIM2中断中完成步进脉冲计数、速度状态机更新和ARR重装载 */
    if (htim->Instance == TIM2)
    {
        Stepper_t *s = &stepper_1;

        double_flag++;

        // 必须满2次中断（1个完整脉冲）才进行计算
        if (double_flag < 2)
            return;

        s->step_count++;
        double_flag = 0;

        // --- 状态机管理 ---
        switch (s->state)
        {
        case ACCELERATE:
            s->current_speed += s->speed_inc; // 这里的 speed_inc 单位是 mm/s per step
            if (s->step_count >= s->acc_step || s->current_speed >= s->speed)
            {
                s->current_speed = s->speed;
                s->state = CONSTANT;
            }
            break;

        case CONSTANT:
            if (s->step_count >= (s->total_step - s->acc_step))
                s->state = DECELERATE;
            break;

        case DECELERATE:
            s->current_speed -= s->speed_inc;
            if (s->current_speed < s->start_speed)
                s->current_speed = s->start_speed;

            if (s->step_count >= s->total_step)
            {
                s->state = STOP;
                Int_TMC2209_stop(); // 内部会关闭中断，所以这里直接return即可
                return;
            }
            break;

        case STOP:
            Int_TMC2209_stop();
            return;
        }

        // --- 更新硬件 ---
        if (s->state != STOP)
        {
            // 传入 mm/s，计算新的 ARR
            uint32_t new_arr = CALC_ARR(s->current_speed);
            __HAL_TIM_SET_AUTORELOAD(&htim2, new_arr);
            __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, new_arr / 2);
        }
    }
}