#include "systick.h"
#include "sensor.h"
#include "user_ctrl.h"
#include "nclink.h"
#include "bsp_usart.h"
#include "pid.h"
#include "subtask_demo.h"
#include "bsp_eeprom.h"
#include "developer_mode.h"

int8_t SDK1_Mode_Setup=0;					 //SDK任务控制计数器，同时也可以用做底部视觉工作模式配置
uint8_t Forward_Vision_Mode_Setup=0x00;//前向视觉传感器的工作模式
int16_t task_select_cnt=1;//任务选择计数器,在完成自动起飞后,会在当前case的基础上
													//自加或者自减task_select_cnt,从而实现自动起飞后,执行不同的飞行任务
//特别提示，任务1~9中并没有自动起飞的程序，需要自己手动起飞后，再去切sdk通道执行，
//也可以在任务10中自动起飞函数执行完毕后,修改SDK1_Mode_Setup的值后再执行1~9
uint8_t offboard_start_flag=0;//外部控制模式启动标志位
															//当飞控Pilot端切入SDK后会将此标志位发送给上位机planner端



void SDK_Init(void)
{
  float sdk_mode_default1 = 0, task_select = 0;
  ReadFlashParameterOne(SDK1_MODE_DEFAULT, &sdk_mode_default1);

  if(isnan(sdk_mode_default1) == 0)
  {
    SDK1_Mode_Setup = (uint8_t)(sdk_mode_default1);//俯视
  }

  ReadFlashParameterOne(TASK_SELECT_AFTER_TAKEOFF, &task_select);

  if(isnan(task_select) == 0) task_select_cnt = (int16_t)(task_select);//自动起飞后任务选择
  else task_select = 1;

  Reserved_Params_Init();
}



void Auto_Flight_Ctrl(int8_t *mode)
{
	if(offboard_start_flag==0)//飞控端进入SDK后，运行标志位会置1 
	{
		//控制器和任务相关变量复位
		OpticalFlow_Ctrl_Reset();
		flight_subtask_reset();
		Total_Controller.Height_Position_Control.Expect=uav_ins.position[_UP];//非SDK时更新高度期望
		return;
	}
	
	switch(*mode)
	{
   case 0:
		{
			flight_subtask_1();//顺时针转动90度，完成后降落
		}
		break;
    case 1:
		{
			flight_subtask_3();//以10deg/s的角速度顺时针转动10000ms，完成后降落
		}
		break;
    case 2:
		{
		  flight_subtask_5();//机体坐标系下相对位移,正方形轨迹
		}
		break;
    case 3:
		{
			flight_subtask_7();//导航坐标系下，基于初始点的绝对坐标位移,正方形轨迹
		}
		break;
    case 4:
		{
			flight_subtask_8();//航点飞行轨迹圆，半径、航点数量可设置
		}
		break;
		case 5://自动起飞到某一高度
		{
			if(Auto_Takeoff(150)==1)
			{
				*mode+=1;//到达目标高度后，切换到下一SDK任务
			}
		}
		break;		
		case 6://用户自定义航点飞行-无需二次编程，就可以实现3维空间内的若干航点遍历飞行
		{
			//用户通过地面站自定义或者按键手动输入三维的航点位置，无人机依次遍历各个航点，当前最多支持28个航点，可以自由扩展
			Navigation_User_Setpoint();
		}
		break;
		case 7://自动起飞到180cm高度，完成后根据task_select_cnt值来决策执行基础任务还是发挥任务
		{
			if(Auto_Takeoff(180)==1)//期望高度180cm
			{
				*mode+=task_select_cnt;//task_select_cnt设置为1:到达目标高度后，切换到基础部分任务
															 //task_select_cnt设置为2:到达目标高度后，切换到发挥部分任务
			}
		}	
		break;
		case 8://2023年TI国赛G题——基础部分
		{
			Air_Ground_Extinguish_Fire_System_Basic();//无人机航点遍历后返航降落
		}
		break;
		case 9://基本飞行保障必备子函数
		{
			basic_auto_flight_support();
		}
		break;
		case 10:{}//用户预留任务，编写后注意加上break跳出
		case 11:{}//用户预留任务，编写后注意加上break跳出
		case 12:{}//用户预留任务，编写后注意加上break跳出
		case 13:{}//用户预留任务，编写后注意加上break跳出
		case 14:{}//用户预留任务，编写后注意加上break跳出
		case 15:{}//用户预留任务，编写后注意加上break跳出
		case 16:{}//用户预留任务，编写后注意加上break跳出
		case 17:{}//用户预留任务，编写后注意加上break跳出
		case 18:{}//用户预留任务，编写后注意加上break跳出
		case 19:{}//用户预留任务，编写后注意加上break跳出
		case 20:{}//用户预留任务，编写后注意加上break跳出
		case 21:{}//用户预留任务，编写后注意加上break跳出
		case 22:{}//用户预留任务，编写后注意加上break跳出
		case 23:{}//用户预留任务，编写后注意加上break跳出
		case 24:{}//用户预留任务，编写后注意加上break跳出
		case 25:{}//用户预留任务，编写后注意加上break跳出
		case 26:{}//用户预留任务，编写后注意加上break跳出
		case 27:{}//用户预留任务，编写后注意加上break跳出
		case 28:{}//用户预留任务，编写后注意加上break跳出
		case 29:{}//用户预留任务，编写后注意加上break跳出
		case 30:{}//用户预留任务，编写后注意加上break跳出
    case 31://前面预留case不满足情况下执行此情形
		{
			Flight.roll_outer_control_output =RC_ROLL;
			Flight.pitch_outer_control_output=RC_PITCH;			
			Flight.yaw_ctrl_mode=ROTATE;
			Flight.yaw_outer_control_output  =RC_YAW;
			Flight_Alt_Hold_Control(ALTHOLD_MANUAL_CTRL,NUL,NUL);//高度控制
		}
		break;
		case 32://SDK模式中原地降落至地面怠速后停桨,用于任务执行完成后降落
		{
			Horizontal_Control_VIO(0);		
			Flight.yaw_ctrl_mode=ROTATE;
			Flight.yaw_outer_control_output  =RC_YAW;
		  Flight_Alt_Hold_Control(ALTHOLD_AUTO_VEL_CTRL,NUL,-30);//高度控制
		}
    break;		
	  default:
		{
			Flight.roll_outer_control_output =RC_ROLL;
			Flight.pitch_outer_control_output=RC_PITCH;			
			Flight.yaw_ctrl_mode=ROTATE;
			Flight.yaw_outer_control_output  =RC_YAW;
			Flight_Alt_Hold_Control(ALTHOLD_MANUAL_CTRL,NUL,NUL);//高度控制
		}		
	}
}

/**********************************************************************/
nav_planner_cmd planner_cmd =
{
  ._yaw_ctrl_mode = 0,
  ._yaw_ctrl_start = 0,
  ._execution_time_ms = 0,
  ._roll_outer_control_output = 0,
  ._pitch_outer_control_output = 0,
  ._yaw_outer_control_output = 0,
};


void planner_send_to_pilot(void)
{
	//俯仰横滚期望
	planner_cmd._roll_outer_control_output=Flight.roll_outer_control_output;
	planner_cmd._pitch_outer_control_output=Flight.pitch_outer_control_output;	
	//偏航期望
	planner_cmd._yaw_outer_control_output=Flight.yaw_outer_control_output;
	planner_cmd._yaw_ctrl_mode=Flight.yaw_ctrl_mode;
	planner_cmd._yaw_ctrl_start=Flight.yaw_ctrl_start;
	planner_cmd._execution_time_ms=Flight.execution_time_ms;
	//速度期望
	planner_cmd._nav_target_vel.x=nav_target_vel.x;
	planner_cmd._nav_target_vel.y=nav_target_vel.y;
	planner_cmd._nav_target_vel.z=nav_target_vel.z;

  static uint8_t buf[100];
  uint8_t sum = 0, _cnt = 0, i = 0;

  buf[_cnt++] = NCLink_Head[0];
  buf[_cnt++] = NCLink_Head[1];
  buf[_cnt++] = 0x18;
  buf[_cnt++] = 0;

  buf[_cnt++] = BYTE0(planner_cmd._yaw_ctrl_mode); //4
  buf[_cnt++] = BYTE0(planner_cmd._yaw_ctrl_start); //5

  Float2Byte(&planner_cmd._roll_outer_control_output, buf, _cnt); //6
  _cnt += 4;
  Float2Byte(&planner_cmd._pitch_outer_control_output, buf, _cnt); //10
  _cnt += 4;
  Float2Byte(&planner_cmd._yaw_outer_control_output, buf, _cnt); //14
  _cnt += 4;
  Float2Byte(&planner_cmd._nav_target_vel.x, buf, _cnt); //18
  _cnt += 4;
  Float2Byte(&planner_cmd._nav_target_vel.y, buf, _cnt); //22
  _cnt += 4;
  Float2Byte(&planner_cmd._nav_target_vel.z, buf, _cnt); //26
  _cnt += 4;

  buf[_cnt++] = BYTE3(planner_cmd._execution_time_ms);
  buf[_cnt++] = BYTE2(planner_cmd._execution_time_ms);
  buf[_cnt++] = BYTE1(planner_cmd._execution_time_ms);
  buf[_cnt++] = BYTE0(planner_cmd._execution_time_ms);

  buf[3] = _cnt - 4;

  for(i = 0; i < _cnt; i++) sum ^= buf[i];

  buf[_cnt++] = sum;

  buf[_cnt++] = NCLink_End[0];
  buf[_cnt++] = NCLink_End[1];
  usart0_send_bytes(buf, _cnt);
}