#include "MT6813CT.h"
#include "cmsis_os.h"
#include "i2c.h"
#include "uType.h"
#include "math.h"
#include "DRV8837D.h"

float diff_speed = 0.0f;
uint8_t no_mag_flag = 0;    // 磁铁状态标志位
uint16_t angle = 0;					// 读取到的磁角度寄存器数据
float cur_angle_deg = 0.0f; // 读取到的磁角度转成单位度deg

float prev_angle_deg = 0.0f;       // 上一次角度（单位：度）
uint32_t prev_time = 0;            // 上一次采样的时间（单位：ms）
extern float Motor_spe;		//  电机转速
float delta_time = 0.0f; 	//	读取角度间隔时间（毫秒）
static float last_angle_deg = 0;

extern uint32_t period_buffer[CAPTURE_DEPTH];    // PWM周期
extern uint32_t high_buffer[CAPTURE_DEPTH];      // PWM高电平时间

#define SPEED_NOISE_THRESHOLD 40.0f  // 速度突变阈值
static float speed_buffer[3] = {0}; // 三点速度缓存：前一帧、中间、后一帧
static int speed_index = 0;

void Scanning_I2C_ID(void)	
{	
    HAL_StatusTypeDef result;
    uint8_t found = 0;

    while (!found) // 直到发现 0x06 地址
    {
			result = HAL_I2C_IsDeviceReady(&hi2c2, 0x06 << 1, 2, 20);
			if (result == HAL_OK)
			{
					found = 1;
					break;					
			 }else{
					printf("I2C Slave ID incorrect\n");
			}
				osDelay(100);
				printf("HAL_I2C_IsDeviceReady fail\n");
				HAL_I2C_DeInit(&hi2c2);  // 停止并释放 I2C 硬件资源
				osDelay(100);
				MX_I2C2_Init();
        osDelay(100);
		}
}

void read_angle(void) 	// 硬件iic读取角度
{
    uint8_t angle_data[2];    

		Scanning_I2C_ID();
		HAL_I2C_Mem_Read(&hi2c2, 0xc, 0x03, I2C_MEMADD_SIZE_8BIT, angle_data, 1, 1000);
		osDelay(10);
		HAL_I2C_Mem_Read(&hi2c2, 0xc, 0x04, I2C_MEMADD_SIZE_8BIT, &angle_data[1], 1, 1000);

    angle = ((angle_data[0]&0xfe) << 5) | ((angle_data[1] & 0xFC) >> 2);
		no_mag_flag = (angle_data[1] & 0x02) >> 1;  // 提取磁铁状态标志位
    cur_angle_deg = (angle * 360.0f)*2 / 16384.0f;
		if(no_mag_flag == 1)
		{
			angle = 0;
			diff_speed = 0.0f;
			Motor_spe = 0.0f;
			return;
		}	
		// 转速计算部分
    uint32_t now_time = HAL_GetTick();  // 当前时间
    delta_time = now_time - prev_time;  // 毫秒

    float delta_angle = cur_angle_deg - prev_angle_deg;

    // 处理角度跳变（例如从359°跳到0°）
    if (delta_angle > 180.0f)
        delta_angle -= 360.0f;
    else if (delta_angle < -180.0f)
        delta_angle += 360.0f;

    if (delta_time > 0)
    {
        Motor_spe = (delta_angle / delta_time) * 1000.0f;  // 单位：度/秒
    }

    // 更新上一帧数据
    prev_angle_deg = cur_angle_deg;
    prev_time = now_time;
}
// 输入捕获+DMA
void Process_One_Capture(void)
{
    static uint32_t last_angle_tick = 0;
    uint32_t now_tick = HAL_GetTick();  // ms
		static float last_motor_spe = 0;
//	  static float last_delta_angle = 0;
	
    // 读取当前 DMA 写入位置
    uint16_t pos = (CAPTURE_DEPTH - __HAL_DMA_GET_COUNTER(htim1.hdma[TIM_DMA_ID_CC1])) % CAPTURE_DEPTH;

    uint32_t high = high_buffer[pos];
    uint32_t period = period_buffer[pos];
    if (period == 0) return;

    float duty = (float)high / period;
    float angle_now = duty * 360.0f;
		// 简单一阶滤波
		angle_now = 0.2f * angle_now + (1.0f - 0.2f) * last_angle_deg;
    cur_angle_deg = angle_now;

    // 计算速度（角度差 / 时间差）
    float dt = (now_tick - last_angle_tick) / 1000.0f; // 秒
    

    if (dt > 0.001f) // > 1ms
		{
				delta_time = dt;
				
				float delta_angle = angle_now - last_angle_deg; // 角度差
				if (delta_angle > 180.0f) delta_angle -= 360.0f;
				else if (delta_angle < -180.0f) delta_angle += 360.0f;

				delta_angle = roundf(delta_angle * 10.0f) / 10.0f; // 保留1位小数
				
				float new_speed = 0;
				if (fabsf(delta_angle) < 60.0f)  // 忽略大跳变
						new_speed = delta_angle / dt; // deg/s
				else
						new_speed = last_motor_spe; // 保持上一次值

				// 抑制突变（速度差大于 800）
				if (fabsf(new_speed - last_motor_spe) > 400.0f)
						new_speed = last_motor_spe;

				// 方向修正（防止正转/反转状态下出现反方向速度）
				static float last_valid_positive_speed = 0;
				static float last_valid_negative_speed = 0;

				if (Motormode == MOTOR_FORWARD) // 电机正转
				{
						if (new_speed < 0)
								new_speed = last_valid_positive_speed;  // 替换为上一次正向速度
						else
								last_valid_positive_speed = new_speed;  // 更新正向速度
				}
				else if (Motormode == MOTOR_REVERSE) // 电机反转
				{
						if (new_speed > 0)
								new_speed = last_valid_negative_speed;  // 替换为上一次负向速度
						else
								last_valid_negative_speed = new_speed;  // 更新反向速度
				}

				if((angle_now > 15.0f && angle_now < 55.0f)||
					 (angle_now > 350.0f && angle_now < 359.9f)) // 死区
				{
					new_speed = last_motor_spe;  // 替换为上一次速度
				}
				
				UpdateMotorSpeed(new_speed);

				// 一阶滤波
//				Motor_spe = 0.3f * new_speed + 0.7f * last_motor_spe;
				last_motor_spe = new_speed;
				
//				last_delta_angle  = delta_angle;
				last_angle_tick = now_tick;
				last_angle_deg = angle_now;
		}

}


void UpdateMotorSpeed(float new_speed)
{
    // 先把新速度放入缓存，循环覆盖三个点
    speed_buffer[speed_index] = new_speed;
    speed_index = (speed_index + 1) % 3;

    // 只有当缓冲满三点时才判断滤波
    if (speed_buffer[0] != 0 && speed_buffer[1] != 0 && speed_buffer[2] != 0)
    {
        // 中间点是当前速度，比较前后点
        float prev_speed = speed_buffer[(speed_index + 2) % 3];
        float curr_speed = speed_buffer[(speed_index + 1) % 3];
        float next_speed = speed_buffer[speed_index];

        // 识别中间值为低值但前后较高，视为噪点
        if ((curr_speed < 80) &&
            (prev_speed > 80) &&
            (next_speed > 80))
        {
            Motor_spe = (prev_speed + next_speed) / 2.0f;
        }
        // 普通速度噪点跳变抑制
        else if (fabsf(curr_speed - prev_speed) > SPEED_NOISE_THRESHOLD &&
                 fabsf(curr_speed - next_speed) > SPEED_NOISE_THRESHOLD)
        {
            Motor_spe = (prev_speed + next_speed) / 2.0f;
        }
        else
        {
            Motor_spe = curr_speed;
        }
    }
    else
    {
        Motor_spe = new_speed;  // 缓存没满时直接用新速度
    }
}

//		printf("\n");
//		printf("Angle(deg): %.2f \n", diff_speed);
//		printf("Raw: %d / 0x%04X \n", angle, angle);
//		printf("Magnet_Status: %s\n", no_mag_flag ? "NO" : "YES");

/*
void read_angle(void)
{    
  uint8_t temp[2];
  int16_t angle;
  float angle_f;    
  HAL_I2C_Mem_Read(&hi2c2, 0x06 << 1, 0x03, I2C_MEMADD_SIZE_8BIT, temp, 2, 50);    
  angle = ((temp[0]&0xfe) << 5) | ((temp[1] & 0xFC) >> 2);    
  angle_f = (float)angle * 360 / 16384;
  printf("angle = %d\tangle_f: %.03f\r\n", angle, angle_f);
}
*/
/*	SlaveAddress：0x6<<1 
 * IIC 方式读取角度信息
 * 返回数据为 0 ~ 360 之间的浮点数
 */
/*
void read_angle(void)
{
    uint32_t angle = 0;
    float fangle = 0.0f;
    uint8_t ReadBuffer1,ReadBuffer2;

    HAL_I2C_Mem_Read(&hi2c2,0xc,0x3,I2C_MEMADD_SIZE_8BIT,&ReadBuffer1,1,0XFF);
    angle = ReadBuffer1;
    angle <<= 8;
    HAL_I2C_Mem_Read(&hi2c2,0xc,0x4,I2C_MEMADD_SIZE_8BIT,&ReadBuffer2,1,0XFF);
    angle += ReadBuffer2;
    angle >>= 2;            //取数据高 14 位
		printf("\n");
		printf("ReadBuffer1: 0x%2x\n",ReadBuffer1);
		printf("ReadBuffer2: 0x%2x\n",ReadBuffer2);
    fangle = (float)(angle * 360.0f) / 16384.0f;
		printf("fangle(deg): %.2f\n",fangle);

}
*/