/**********************************
 * 文件名称: hall_sensor.c
 * 功能描述: 霍尔传感器模块
 * 功能: 读取霍尔传感器信号，计算电机角度和速度
 **********************************/
#include "main.h"
#include "hall_sensor.h"
#include "motor_define.h"

#define SPEED_FILTER_DEPTH 8

Hall_TypeDef Hall = {0};  // 全局实例

Hall_TypeDef* Hall_Get(void)  
{
    return &Hall;
}

static float speed_history[SPEED_FILTER_DEPTH] = {0};

static uint8_t speed_idx = 0;

float Get_Filtered_Speed(void)
{
    speed_history[speed_idx++] =(Hall.speed[0]+Hall.speed[1]+Hall.speed[2]+Hall.speed[3]+Hall.speed[4]+Hall.speed[5])/6.0f;
    if(speed_idx >= SPEED_FILTER_DEPTH) speed_idx = 0;
    
    float sum = 0;
    for(int i=0; i<SPEED_FILTER_DEPTH; i++)
        sum += speed_history[i];
    return sum / SPEED_FILTER_DEPTH;
}


/**
 * @brief   TIM5中断处理函数
 * @note    处理霍尔传感器信号中断，计算电机角度和速度
 */
void TIM5_IRQHandler(void)
{

  if(TIM_GetFlagStatus(TIM5, TIM_FLAG_CC1) == SET)
  {
		Hall.ccr = (float)TIM_GetCapture1(TIM5);
		TIM_ClearFlag(TIM5, TIM_FLAG_CC1);
		
		Hall.state_last = Hall.state; // 记录上次状态
		
    uint8_t W = GPIO_ReadInputDataBit(HALL_CH3_GPIO_PORT, HALL_CH3_PIN); // 需要根据实际进行修改
    uint8_t V = GPIO_ReadInputDataBit(HALL_CH2_GPIO_PORT, HALL_CH2_PIN);
    uint8_t U = GPIO_ReadInputDataBit(HALL_CH1_GPIO_PORT, HALL_CH1_PIN);
    Hall.state = U;		//计算扇区
		Hall.state |= V << 1;
		Hall.state |= W << 2;
		
    switch(Hall.state)
    {
			// CW 645132 CCW 623154
      case 0x06: 
				Hall.theta[0]  = 0.4506f; 
			  Hall.angle = Hall.theta[0];
				Hall.count[0] = Hall.ccr;
				Hall.speed[0]  = (Hall.speed[0]+( 2.0f * PI + Hall.theta[0]-Hall.theta[5]) / (Hall.count[0] / HALL_SPEED_FACTOR))/2.0f; // 电弧度
			  Hall.angle_add = Hall.speed[0] / HALL_SAMPLE_FREQ; // 速度乘电流环单次时间 速度*0.0001s 单次电流环插值间隔
			
			break;
      case 0x04:
				Hall.theta[1]  = 0.4506f + PI / 3;  
				Hall.angle = Hall.theta[1]  ;			
				Hall.count[1] = Hall.ccr;
				Hall.speed[1]  = (Hall.speed[1]+(Hall.theta[1]-Hall.theta[0]) / (Hall.count[1] / HALL_SPEED_FACTOR))/2.0f;  
				Hall.angle_add = Hall.speed[1] / HALL_SAMPLE_FREQ;
			break;
			case 0x05: 
				Hall.theta[2]  = 0.4506f + 2.0f * PI / 3;
			  Hall.count[2] = Hall.ccr;
				Hall.angle = Hall.theta[2]  ;
				Hall.speed[2]  =  (Hall.speed[2]+ (Hall.theta[2]-Hall.theta[1]) / (Hall.count[2] / HALL_SPEED_FACTOR))/2.0f;  
				Hall.angle_add = Hall.speed[2] / HALL_SAMPLE_FREQ;
			break;
      case 0x01: 
				Hall.theta[3]  = 0.4506f + PI;  
				Hall.angle = Hall.theta[3];			
				Hall.count[3] = Hall.ccr;
				Hall.speed[3]  = (Hall.speed[3] +  (Hall.theta[3]-Hall.theta[2]) / (Hall.count[3] / HALL_SPEED_FACTOR))/2.0f;  
				Hall.angle_add = Hall.speed[3] / HALL_SAMPLE_FREQ;
			break;
      case 0x03: 
				Hall.theta[4]  = 0.4506f + 4.0f * PI / 3; 
			  Hall.angle = Hall.theta[4] ;
				Hall.count[4] = Hall.ccr;
				Hall.speed[4]  =   (Hall.speed[4]+(Hall.theta[4]-Hall.theta[3]) / (Hall.count[4] / HALL_SPEED_FACTOR))/2.0f;  
				Hall.angle_add = Hall.speed[4] / HALL_SAMPLE_FREQ;
			break;
      case 0x02:
				Hall.theta[5]  = 0.4506f + 5.0f * PI / 3; 
			  Hall.angle = Hall.theta[5]  ;
				Hall.count[5] = Hall.ccr;
				Hall.speed[5]  =  (Hall.speed[5]+ (Hall.theta[5]-Hall.theta[4]) / (Hall.count[5] / HALL_SPEED_FACTOR))/2.0f;   
				Hall.angle_add = Hall.speed[5] / HALL_SAMPLE_FREQ;
			break;
      default: break;
    }
    
  }
}

// 电流环中调用的角度插值函数
void Hall_Update_Interpolated_Angle(void)
{
    static uint8_t last_state = 0xFF;
    
    // 检查状态是否变化
    if(Hall.state != last_state) {
        last_state = Hall.state;
    } 
    else {
        // 状态未变化，累加角度
        Hall.angle += Hall.angle_add;
    }
    
    // 角度归一化
    if(Hall.angle >= 2*PI) Hall.angle -= 2*PI;
    if(Hall.angle < 0) Hall.angle += 2*PI;
}


void Hall_Init_For_Startup(void)
{
    // 清零速度数组
    for(int i = 0; i < 6; i++)
    {
        Hall.speed[i] = 0.0f;
        Hall.theta[i] = 0.0f;
        Hall.count[i] = 0;
    }
    
    // 清零速度历史
    for(int i = 0; i < SPEED_FILTER_DEPTH; i++)
        speed_history[i] = 0.0f;
    speed_idx = 0;
    
    // 重置角度增量
    Hall.angle_add = 0.0f;
		Hall.Speed = 0.0f;
		// 重置计数值
    Hall.ccr = 0;
		
    // 强制获取当前霍尔状态
    uint8_t W = GPIO_ReadInputDataBit(HALL_CH3_GPIO_PORT, HALL_CH3_PIN);
    uint8_t V = GPIO_ReadInputDataBit(HALL_CH2_GPIO_PORT, HALL_CH2_PIN);
    uint8_t U = GPIO_ReadInputDataBit(HALL_CH1_GPIO_PORT, HALL_CH1_PIN);
    Hall.state = U | (V << 1) | (W << 2);
    Hall.state_last = Hall.state;
}