#include "Chassis.h"
#include "gpio.h"
#include "user_math.h"
#include "bsp_buzzer.h"
#include "bsp_delay.h"
#include "nuc.h"

/*
机器人坐标系，向前x，右y，上yaw
不同于nuc (前x，左y，上yaw)
            x
            |
              --y
*/
static osThreadId_t thread_alert;
static int8_t Chassis_SetCtrl(Chassis_t *c, CMD_t *ctrl) {
    if (c == NULL) return CHASSIS_ERR_NULL;
    if (ctrl->CMD_CtrlType == c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode)
        return CHASSIS_OK;
    c->chassis_ctrl.ctrl = ctrl->CMD_CtrlType;
    c->chassis_ctrl.mode = ctrl->CMD_mode;
    return CHASSIS_OK;
}

int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
    if (c == NULL) return CHASSIS_ERR_NULL;
    if (can == NULL) return CHASSIS_ERR_NULL;
	
    for (uint8_t i = 0; i < 4; i++) {
        c->motorfeedback.rotor_rpm3508[i] = can->motor.chassis_motor3508.as_array[i].rotor_speed;
        c->motorfeedback.rotor_current3508[i] = can->motor.chassis_motor3508.as_array[i].torque_current;
    }
//    for (uint8_t i = 0; i < 4; i++) {
//		
//		c->sick_cali.sick_dis[i]=can->sickfed.raw_dis[i] / 10; //没使用原值
//    }
		c->sick_cali.sick_dis[0]=(fp32)(can->sickfed.raw_dis[0]) ;
		c->sick_cali.sick_dis[1]=(fp32)(can->sickfed.raw_dis[1]+30) ;  //有点误差，手动补偿
		c->sick_cali.sick_dis[2]=(fp32)(can->sickfed.raw_dis[2] );
		
		c->vofa_send[7] =c->sick_cali.sick_dis[1]-c->sick_cali.sick_dis[2];
		c->vofa_send[6] = c->sick_cali.sick_dis[0];
		c->vofa_send[5] =c->sick_cali.sick_dis[1];
		c->vofa_send[4] =c->sick_cali.sick_dis[2];
    return CHASSIS_OK;
}

int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_freq) {
    if (c == NULL) return CHASSIS_ERR_NULL;
		  if ((thread_alert = osThreadGetId()) == NULL)
    return CHASSIS_ERR_NULL;
    c->param = param;

    for (int i = 0; i < 4; i++) {
        PID_init(&(c->pid.chassis_3508VecPID[i]), PID_POSITION_D, &(c->param->M3508_param));
    }
    PID_init(&(c->pid.chassis_PICKWzPID_HIGN), PID_POSITION, &(c->param->chassis_PICKWzPID_HIGN_param));
    PID_init(&(c->pid.chassis_PICKWzPID_LOW), PID_POSITION, &(c->param->chassis_PICKWzPID_LOW_param));
    /*sick*/
    PID_init(&(c->pid.SickCaliWzInPID), PID_POSITION, &(c->param->SickCali_WInparam));
    PID_init(&(c->pid.SickCaliWzOutPID), PID_POSITION, &(c->param->SickCali_WOutparam));
    PID_init(&(c->pid.SickCaliVxInPID), PID_POSITION, &(c->param->SickCali_XInparam));
    PID_init(&(c->pid.SickCaliVxOutPID), PID_POSITION, &(c->param->SickCali_XOutparam));
    PID_init(&(c->pid.SickCaliVyInPID), PID_POSITION, &(c->param->SickCali_YInparam));
    PID_init(&(c->pid.SickCaliVyOutPID), PID_POSITION, &(c->param->SickCali_YOutparam));
    /*修正 */
    PID_init(&(c->pid.Chassis_AngleAdjust), PID_POSITION, &(c->param->Chassis_AngleAdjust_param));	

    LowPassFilter2p_Init(&(c->filled[0]), target_freq, 80.0f); // 角加速度滤波
    LowPassFilter2p_Init(&(c->filled[1]), target_freq, 50.0f); 
    LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f); 
    LowPassFilter2p_Init(&(c->filled[3]), target_freq, 80.0f); 
    
		KalmanCreate(&c->extKalman[0],10,1); 
		KalmanCreate(&c->extKalman[1],10,1); 
		KalmanCreate(&c->extKalman[2],10,1); 
		
		
		c->sick_cali .sickparam=c->param ->sickparam ;
		
    c->ang_cail.is_open=1;
    return CHASSIS_OK;
}

void Chassis_speed_calculate(Chassis_t *c, fp32 Vx, fp32 Vy, fp32 Vw) {
    fp64 Nor_Vx, Nor_Vy;
    normalize_vector(Vx, Vy, &Nor_Vx, &Nor_Vy);
    ChassisHeadlessMode(&Nor_Vx,&Nor_Vy,c->pos088.imu_eulr.yaw);
    c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx + Nor_Vy + Vw+c->ang_cail.out; // 右前
    c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx - Nor_Vy + Vw+c->ang_cail.out; // 右后
    c->hopemotorout.OmniSpeedOut[2] = Nor_Vx - Nor_Vy + Vw +c->ang_cail.out;  // 左后
    c->hopemotorout.OmniSpeedOut[3] = Nor_Vx + Nor_Vy + Vw +c->ang_cail.out;  // 左前
	
		Chassis_AngleCompensate(c);

}
int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
    if (c == NULL) return CHASSIS_ERR_NULL;
    if (ctrl == NULL) return CHASSIS_ERR_NULL;

    Chassis_SetCtrl(c, ctrl);
    // IMU加滤波修正
    c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z);

    /*初始数据*/
    
    
    c->sick_cali.is_reach = 0;
	
	  c->vofa_send[0]= KalmanFilter(&c->extKalman[0] , c->sick_cali.sick_dis[0]);
		c->vofa_send[1]= KalmanFilter(&c->extKalman[1] , c->sick_cali.sick_dis[1]);
	  c->vofa_send[2]= KalmanFilter(&c->extKalman[2] , c->sick_cali.sick_dis[2]);

	
	
    switch (c->chassis_ctrl.ctrl) {
			case ShootGame_Mode:  //投球赛模式
				c->to_nuc.send=0;
				switch(c->chassis_ctrl.mode){
					
					case ShootRst:
						c->move_vec.Vw = ctrl->Vw * 8000;
						c->move_vec.Vx = ctrl->Vy * 8000;
						c->move_vec.Vy = ctrl->Vx * 8000;			

					break;
					
					default:
		if (ctrl->Vw != 0 || ctrl->Vy != 0 || ctrl->Vx != 0) {
        // 有遥控器输出，设置标志位1
        osThreadFlagsSet(thread_alert, SHOOT_GAME_FLAG);
			
    } else {
        // 无输出，清除标志位1
        osThreadFlagsClear( SHOOT_GAME_FLAG);
    }
			
		if(osThreadFlagsWait(SHOOT_GAME_FLAG, osFlagsWaitAny, 0)==SHOOT_GAME_FLAG) 
		{
				c->move_vec.Vw = ctrl->Vw * 8000;
				c->move_vec.Vx = ctrl->Vy * 8000;
				c->move_vec.Vy = ctrl->Vx * 8000;			
	  }else {
				c->move_vec.Vw = -ctrl->cmd_MID360.posw  * 1000;
				c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000;
				c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000;
				abs_limit_fp(&c->move_vec.Vx, 5000.0f);
				abs_limit_fp(&c->move_vec.Vy, 5000.0f);
			  abs_limit_fp(&c->move_vec.Vw, 5000.0f);
			
		}	
		
	}
     break;
 
					case REPOSITION:
          c->to_nuc.send=1;
           break;					
        
										case RELAXED:
                   c->move_vec.Vw = 0;
                   c->move_vec.Vx = 0;
                   c->move_vec.Vy = 0;											
                    break		;									
								
								
								
								
									break;
            break;

        default:

            break;
    }
		

    // 电机速度限幅
    abs_limit_fp(&c->move_vec.Vx, 6000.0f);
    abs_limit_fp(&c->move_vec.Vy, 6000.0f);
    abs_limit_fp(&c->move_vec.Vw, 6000.0f);

    Chassis_speed_calculate(c, c->move_vec.Vx, c->move_vec.Vy, c->move_vec.Vw);

    for (uint8_t i = 0; i < 4; i++) {
        c->final_out.final_3508out[i] = PID_calc(&(c->pid.chassis_3508VecPID[i]),
                                                 c->motorfeedback.rotor_rpm3508[i],
                                                 c->hopemotorout.OmniSpeedOut[i]);
        out->motor_CHASSIS3508.as_array[i] = c->final_out.final_3508out[i];
    }


		
    return CHASSIS_OK;
		
		
		
}



/*
sick0,下
sick1,左2
sick2,左1


*/
fp32 diff_yaw,diff_y,diff_x;
int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) 
{
    if (c == NULL) return CHASSIS_ERR_NULL;
    if (ctrl == NULL) return CHASSIS_ERR_NULL;
    fp32 delta_w,delta_x,delta_y; 
     
    fp32 sick0 = c->sick_cali.sick_dis[0];
    fp32 sick1 = c->sick_cali.sick_dis[1];
    fp32 sick2 = c->sick_cali.sick_dis[2];
	  
	
    const   sickparam_t *param = &c->sick_cali.sickparam;
    
	  
     diff_yaw =LowPassFilter2p_Apply(&(c->filled[1]), -(fp32)(sick1 +10 - sick2));
     diff_y  = LowPassFilter2p_Apply(&(c->filled[2]),-(fp32)(sick1 - c->sick_cali.sickparam.y_set));
	
     diff_x  =  LowPassFilter2p_Apply(&(c->filled[3]),(fp32)(sick0 - c->sick_cali.sickparam.x_set));

    delta_w=PID_calc(&c->pid.SickCaliWzOutPID, diff_yaw, 0);
    delta_x=PID_calc(&c->pid.SickCaliVxOutPID, diff_x, 0);
    delta_y=PID_calc(&c->pid.SickCaliVyOutPID, diff_y,0);
	
	if(fabs(diff_yaw)>param->w_error){
		
	  c->move_vec.Vw=PID_calc(&c->pid.SickCaliWzInPID,-delta_w,0);
	}
//	if(fabs(diff_y)>param->xy_error){
//		
//	  c->move_vec.Vy=PID_calc(&c->pid.SickCaliVyInPID,delta_y,0);
//	}	
//	if(fabs(diff_x)>param->xy_error){
//		
//	  c->move_vec.Vx=PID_calc(&c->pid.SickCaliVxInPID,-delta_x,0);
//	}
	
		
    static uint16_t reach_cnt = 0;
    if (fabsf(diff_yaw) <= param->w_error &&
        fabsf(diff_x)   <= param->xy_error &&
        fabsf(diff_y)   <= param->xy_error) {
        reach_cnt++;
        if (reach_cnt >= 20) {
            
            c->sick_cali.is_reach = 1;
            
        }
    } else {
        reach_cnt = 0;
			  c->sick_cali.is_reach = 0;
    }	
		
    return CHASSIS_OK;
}

int8_t Chassis_AngleCompensate(Chassis_t *c) 
{
	  if (c == NULL) return CHASSIS_ERR_NULL;
	  if(c->ang_cail.is_open==0) return 0;
	
    static fp32 pian_angel,cur_angle;
	  
	  if((c->move_vec.Vx || c->move_vec.Vy) && (c->move_vec.Vw== 0))
    {
			c->ang_cail.ang_error=cur_angle-AngleChange(DEGREE,c->pos088.imu_eulr.yaw);		
		   
		}
    
		else	{
			 c->ang_cail.ang_cur=AngleChange(DEGREE,c->pos088.imu_eulr.yaw);			
       c->ang_cail.ang_error=0;
		}
    c->ang_cail.out = PID_calc(&c->pid.Chassis_AngleAdjust,pian_angel,0);
}


