#include "navi.h"


//导航初始化一系列参数
int8_t Action_init(ops_t *o , const ops_param_t *param , Action_POS_t *pos)
{
	if(o ==NULL)  return DEVICE_ERR;
	if(param == NULL) return DEVICE_ERR;
//	while(!Action_ready)
//	{		
//     TIM4->CCR3 = 9999;//等待全场定位初始化
//	}
	
//    TIM4->CCR3 = 0;
//	HAL_GPIO_WritePin(LED_G_GPIO_Port,LED_G_Pin,GPIO_PIN_SET);//接收到码盘数据后亮灯指示
//	
	
//			osDelay(5);

	o->param = param;
	/*全场定位pid初始化*/ 
	//全场定位跑路径的xy方向速度环pid
	PID_init(&o->pid_PosSpeed_x, PID_POSITION,(&o->param->ops_pid.pid_PosSpeed_x_param));
	PID_init(&o->pid_PosSpeed_y, PID_POSITION,(&o->param->ops_pid.pid_PosSpeed_y_param));
	 /* 初始化角度纠正PID */
	PID_init(&o->pid_OutAngle,PID_POSITION,(&o->param->ops_pid.pid_OutAngle_param));//外环
	PID_init(&o->pid_InnerAngle,PID_POSITION,(&o->param->ops_pid.pid_InnerAngle_param));//内环
	//底盘xy方向位置环pid初始化
	//内环
	PID_init(&o->pid_pos_x_inner,PID_POSITION,(&o->param->ops_pid.pid_pos_x_inner_param));
	PID_init(&o->pid_pos_y_inner,PID_POSITION,(&o->param->ops_pid.pid_pos_y_inner_param));
	//外环
	PID_init(&o->pid_pos_x_out,PID_POSITION,(&o->param->ops_pid.pid_pos_x_out_param));
	PID_init(&o->pid_pos_y_out,PID_POSITION,(&o->param->ops_pid.pid_pos_y_out_param));


	
	//底盘路径速度pid初始化
	PID_init(&o->path_speed_pid,PID_POSITION,(&o->param->ops_pid.path_speed_pid_param));
	//全场定位底盘坐标初始化
	o->chassis_pos->pos_x=0;
	o->chassis_pos->pos_y=0;	
	o->chassis_pos->pos_lastX=0;
	o->chassis_pos->pos_lastY=0;
	o->chassis_map =param->path ;	
	o->chassis_pos = pos;
	
	
	//设置全场定位允许的误差范围
	o->state.mistake = POS_ALLOW_MISTAKE;
	o->state .angle_mistake =POS_ALLOW_ANGLE_MISTAKE ;
	o->state.moveState = START;
	o->state.points_num = param->path_num;
	//标志位初始化
	o->POS_IS_CPT = NO;
  



	return DEVICE_OK;
}
//利用C板imu纠正角度
fp32 ops9_AngleCorr(ops_t *o,fp32 hope_angle)
{
	fp32 delta_angle,delta_w;
	//外环角度纠正
	delta_angle = PID_calc(&o->pid_OutAngle,o->ops_imu_pos.yaw,hope_angle);
	//内环速度纠正
	delta_w = -PID_calc(&o->pid_InnerAngle,o->ops_gyro.z,delta_angle);
	return delta_w;
}

//底盘xy速度计算(全场定位)
void POS_chassis_set(ops_t *o,fp32 vx_set, fp32 vy_set, fp32 yaw_angle_set){
		o->final_out.vx = (PID_calc(&o->pid_PosSpeed_x, o->chassis_pos->pos_Vx,vx_set));//根据遥控器的方向选择正负
		o->final_out.vy = PID_calc(&o->pid_PosSpeed_y, o->chassis_pos->pos_Vy,vy_set);
	  o->final_out.yaw_angle = ops9_AngleCorr(o,yaw_angle_set);
}


//定点控制
void hold_point(ops_t *o,point_t p,fp32 yaw_angle_set)
{
	fp32 delta_x,delta_y;
	fp32 delta_vx,delta_vy;
	fp32 vx_set,vy_set;//x,y,w目标速度
	
	//x
	delta_x = PID_calc(&o->pid_pos_x_out,o->chassis_pos->pos_x,p.x);
	delta_vx = PID_calc(&o->pid_pos_x_inner,o->chassis_pos->pos_Vx,delta_x);
	
	//y
	delta_y = PID_calc(&o->pid_pos_y_out,o->chassis_pos->pos_y,p.y);
	delta_vy = PID_calc(&o->pid_pos_y_inner,o->chassis_pos->pos_Vy,delta_y);
	

	
	
	vx_set = delta_vx;
	vy_set = delta_vy;
	
	POS_chassis_set(o,vx_set,vy_set,yaw_angle_set);
}


  

//判断是否到达函数(全场定位)，最最后判断
bool isArrive(point_t p,ops_t *o)
{
		uint16_t xArrive = 0, yArrive = 0, zArrive=0;

	xArrive = abs_float_double(p.x,o->chassis_pos->pos_x) < o->state.mistake ? 1:0;
	yArrive = abs_float_double(p.y,o->chassis_pos->pos_y) < o->state.mistake ? 1:0;
	
	if(xArrive && yArrive) return true;
	else return false;
}




//寻迹，跑点函数
int8_t go_path(ops_t *o,CMD_ACTION_t *ops_out)
{
   static  fp32 distance;//两点之间的距离差
   static fp32 chassis_speed_set;//底盘速度设置
   static fp32 SIN,COS;//方向解算
   static fp32 world_vx,world_vy;//x,y方向分解速度
   static fp32 chassis_vx,chassis_vy;//底盘xy分速度
   static int cnt =0 ;//计数标志位

	 static int flag_update_num;//记录flag更新，防止cnt一直变化

	/*数据同步*/
	o->current_x =o->chassis_pos ->pos_x ;
	o->current_y =o->chassis_pos ->pos_y ;	
	o->next_mapx = o->chassis_map[cnt].x;
	o->next_mapy =o->chassis_map[cnt].y;	
  o->Navi_Mode .Trig_flag = ops_out ->flag ;	
	
	if(o == NULL) return DEVICE_ERR;
	if (ops_out == NULL) return DEVICE_ERR;
  
// static fp32 yaw_angle_set = 0;	
	if(o->POS_IS_CPT == NO)//路径未跑完
	{
		if(o->state.moveState ==START)
		{
			//更新路径状态
			o->state.moveState =MOVING;			
		}
		
		
		distance = sqrt((o->chassis_map[cnt].x - o->chassis_pos->pos_x)*(o->chassis_map[cnt].x - o->chassis_pos->pos_x)
										+(o->chassis_map[cnt].y - o->chassis_pos->pos_y)*(o->chassis_map[cnt].y - o->chassis_pos->pos_y));
		
		chassis_speed_set = PID_calc(&o->path_speed_pid,-distance,0);//此处由pid计算距离得出相应的速度
		//速度分解
		SIN = (o->chassis_map[cnt].y - o->chassis_pos->pos_y)/distance;
		COS = (o->chassis_map[cnt].x - o->chassis_pos->pos_x)/distance;
		
		world_vx = chassis_speed_set * COS;
		world_vy = chassis_speed_set * SIN;
		
//		chassis_vx = world_vx;
//		chassis_vy = world_vy;
		//世界坐标转机器坐标系转换
		
		chassis_vx = -world_vy*sin(o->ops_imu_pos.yaw) + world_vx*cos(o->ops_imu_pos.yaw);
		chassis_vy = world_vy*cos(o->ops_imu_pos.yaw) + world_vx*sin(o->ops_imu_pos.yaw);
	
	}
	
/*下面的逻辑按需更改，根据cnt计数器自动跑点，遥控器触发跑点等等*/	
	
    		
	  if(isArrive(o->chassis_map[cnt],o))			//判断是否到达(全场定位)，只涉及xy的判断
	{ 	
         /*到达点后，这里应该增加cnt增加或减小的触发方式*/
          
	         if (o->Navi_Mode .Trig_flag != flag_update_num) { // 只有flag发生变化时才更新cnt

              if (o->Navi_Mode .Trig_flag==1){
				cnt++;	
		       }           
		    }        
            else if (o->Navi_Mode .Trig_flag == -1) {
			cnt--;
		     }          
           flag_update_num = o->Navi_Mode .Trig_flag; // 更新prev_flag为当前的flag状态
	}	
    else
	{
		 o->POS_IS_CPT = NO;
        hold_point(o,o->chassis_map[cnt],o->chassis_map[cnt].angle);	//调用该函数使临近误差值快速响应			
	}					
		if(cnt>3)//根据点数更改
		{
		  o->POS_IS_CPT =YES;
			  //到达目的地
	      o->final_out .vx =0;
	      o->final_out .vy =0;
	      o->final_out.yaw_angle=0;	
		}	
    

    
	  

	ops_out->out.Vx= o->final_out .vx ;
	ops_out->out.Vy = o->final_out .vy ;
	ops_out->out.Vw = o->final_out.yaw_angle;

	return DEVICE_OK;
}