#include "turnDetection.h"

//--------------------------define
// 定义转弯阈值
#define TURNING_THRESHOLD 1.0  // 以角度为单位，这个值需要根据实际情况调整

// 低通滤波参数
#define ALPHA 0.9  // 这个值需要根据实际情况调整

//--------------------------param
// 初始化姿态角度和角速度
float previousPitch = 0.0;
float previousRoll = 0.0;
float previousYaw = 0.0;
float previousAngularVelocity = 0.0;
// 转弯标志
uint8_t turnFlag = 0;
// 陀螺仪数据异常标志
uint8_t gyroscopeDataAnomaly = 0; // 1:数据异常 0；数据正常

/**
 * @brief       将陀螺仪值转换为角速度值
 * @param       none
 * @note       
 * @retval      angle：角速度值
 */
float getAngularVelocity(void)
{
	int16_t gyroscopeData[3] = {0};

	//  // 读取陀螺仪数据
	atk_ms6050_get_gyroscope(&gyroscopeData[0], &gyroscopeData[1], &gyroscopeData[2]);
	
    // 计算陀螺仪数据的模长（即角速度）
    float gyroMagnitude = sqrt(pow(gyroscopeData[0], 2) + pow(gyroscopeData[1], 2) + pow(gyroscopeData[2], 2));
    return gyroMagnitude;
}

// 低通滤波函数
float lowPassFilter(float previousValue, float rawValue) {
    return ALPHA * previousValue + (1 - ALPHA) * rawValue;
}

// 判断车辆是否正在转弯的函数
int isTurning(void) {
    // 获取当前的 pitch、roll 和 yaw 角度
    float roll = 0.0, pitch = 0.0, yaw = 0.0; 
	// 错误计数
	static uint8_t errcnt = 0; // 次数10次为故障
	
//	atk_ms6050_dmp_get_data(&pitch, &roll, &yaw);
	
	while(atk_ms6050_dmp_get_data(&pitch,&roll,&yaw)!=0){}
	printf("pitch:%.1f\n", pitch);
    printf("roll:%.1f\n", roll);
    printf("yaw:%.1f\n", yaw);

	// 故障判断
	if((pitch == 0) && (roll == 0) && (yaw == 0)){
		if(errcnt < 10){
			errcnt++;
		}else{
			errcnt = errcnt;
			BIT_SET(s_comData.Malfunction, gyroscope); // 置位陀螺仪数据异常故障
			return 0;
		}
	}else{
		errcnt = 0;
		BIT_CLEAR(s_comData.Malfunction, gyroscope);
	}

    // 获取当前的角速度
    float angularVelocity = getAngularVelocity();

    // 应用低通滤波
    pitch = lowPassFilter(previousPitch, pitch);
    roll = lowPassFilter(previousRoll, roll);
    yaw = lowPassFilter(previousYaw, yaw);
    angularVelocity = lowPassFilter(previousAngularVelocity, angularVelocity);

    // 计算姿态角度的变化速度
    float pitchChangeRate = fabs(pitch - previousPitch);
    float rollChangeRate = fabs(roll - previousRoll);
    float yawChangeRate = fabs(yaw - previousYaw);
	printf("pitchChangeRate:%.1f\n", pitchChangeRate);
    printf("rollChangeRate:%.1f\n", rollChangeRate);
    printf("yawChangeRate:%.1f\n", yawChangeRate);

    // 判断车辆是否正在转弯
	if (pitchChangeRate > TURNING_THRESHOLD || rollChangeRate > TURNING_THRESHOLD || yawChangeRate > TURNING_THRESHOLD) {	
        turnFlag = 1;  // 表示车辆正在转弯
    } else {
        turnFlag = 0;  // 表示车辆未转弯
    }

    // 更新上一次的姿态角度和角速度
    previousPitch = pitch;
    previousRoll = roll;
    previousYaw = yaw;
    previousAngularVelocity = angularVelocity;

	if(turnFlag == 1){
		return 1;
	}else{
		return 0;
	}
}