#include "encode.h"

// 全局结构体数组，对应 4 个电机
Encoder_T g_encoder[MOTOR_COUNT] = {0};

/**
  * @brief  启动所有编码器接口
  */
void Encoder_Init_All(void) {
    HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);
    HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
    HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL);
    HAL_TIM_Encoder_Start(&htim5, TIM_CHANNEL_ALL);
    
    // 初始化时记录当前的计数值，防止开机瞬间产生巨大的增量
    g_encoder[0].last_raw_cnt = (uint16_t)__HAL_TIM_GET_COUNTER(&htim2);
    g_encoder[1].last_raw_cnt = (uint16_t)__HAL_TIM_GET_COUNTER(&htim3);
    g_encoder[2].last_raw_cnt = (uint16_t)__HAL_TIM_GET_COUNTER(&htim4);
    g_encoder[3].last_raw_cnt = (uint16_t)__HAL_TIM_GET_COUNTER(&htim5);
}

/**
  * @brief  单路编码器更新逻辑（处理溢出的核心）
  */
static void _Encoder_Update_Single(Encoder_T *enc, TIM_HandleTypeDef *htim) {
    // 1. 读取硬件计数器（只取低16位）
    uint16_t current_raw = (uint16_t)__HAL_TIM_GET_COUNTER(htim);
    
    // 2. 核心算法：通过 int16_t 强转，自动处理 65535->0 或 0->65535 的跳变
    // 例如：current=5, last=65530 -> (uint16_t)(5-65530) = 11 -> (int16_t)11 = 11
    int16_t delta = (int16_t)(current_raw - enc->last_raw_cnt);
    
    // 3. 更新累计值和速度
    enc->total_count += delta;
    enc->speed = delta;
    
    // 4. 保存当前值
    enc->last_raw_cnt = current_raw;
}

/**
  * @brief  批量更新所有编码器数据（建议 10ms 调用一次）
  */
void Encoder_Update_All(void) {
    _Encoder_Update_Single(&g_encoder[0], &htim2);
    _Encoder_Update_Single(&g_encoder[1], &htim3);
    _Encoder_Update_Single(&g_encoder[2], &htim4);
    _Encoder_Update_Single(&g_encoder[3], &htim5);
}

/**
  * @brief  清零某个电机的行程
  */
void Encoder_Reset(uint8_t id) {
    if(id < MOTOR_COUNT) {
        g_encoder[id].total_count = 0;
    }
}
