/*
    cmd Task
    
*/

/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */

#include "bsp/gpio.h"
#include "gpio.h"
#include "module/config.h"
#include "module/shoot.h"
#include "device/remote_control.h"
#include "bsp/pwm.h"

//#define DEAD_AREA 0.05f
/* USER INCLUDE END */

/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
Shoot_CMD_t cmd_for_shoot;
extern RC_ctrl_t rc_ctrl;
extern uint8_t remote_ready ;
/* USER STRUCT END */

/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_cmd(void *argument) {
  (void)argument; /* 未使用argument，消除警告 */

  
  /* 计算任务运行到指定频率需要等待的tick数 */
  const uint32_t delay_tick = osKernelGetTickFreq() / CMD_FREQ;

  osDelay(CMD_INIT_DELAY); /* 延时一段时间再开启任务 */

  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
  /* USER CODE INIT BEGIN */

  /* USER CODE INIT END */
  
  while (1) {
    tick += delay_tick; /* 计算下一个唤醒时刻 */
    /* USER CODE BEGIN */
//	osMessageQueueGet(task_runtime.msgq.cmd.rc, &at9s_from_rc, NULL, 0);
	 /* 
		sw[0]                       
		左后下                     												右后下    
		
		sw[1]&sw[2]																				sw[6]
		左后上                     												右后上    
		
		sw[3]												sw[7]		sw[4]					sw[5]	
		左1        	左2 		左旋钮  右旋钮 	右3 					右4 
		
				 ch[2]																	ch[0]		
				左拨杆 ch[3]              						 右拨杆 ch[1]
		*/	  

		switch (rc_ctrl.sw[4]) {//位置电机 开/关
    case 1800:
			 cmd_for_shoot.reverse = true;//正转
		  cmd_for_shoot.online = remote_ready;
      cmd_for_shoot.ready = true;
      cmd_for_shoot.firecmd = true;
      break;
    case 1000:
		  cmd_for_shoot.online = remote_ready;
      cmd_for_shoot.ready = true;
      cmd_for_shoot.firecmd = false;
      break;
    case 200:
			 cmd_for_shoot.reverse = false;//反转
		  cmd_for_shoot.online = remote_ready;
      cmd_for_shoot.ready = true;
      cmd_for_shoot.firecmd = true;
      break;
    default:
		  cmd_for_shoot.online = remote_ready;
      cmd_for_shoot.ready = false;
      cmd_for_shoot.firecmd = false;
      break;
    }
		static uint32_t last_time , now_time ;
		static bool relay_flag = true;
		now_time = BSP_TIME_Get_ms();
		switch (rc_ctrl.sw[5]) {//继电器 开/关
    case 200:
			if(relay_flag)
			{
				BSP_GPIO_WritePin(BSP_GPIO_RELAY, true);
				last_time = BSP_TIME_Get_ms();
				relay_flag = false;
			}
			if(now_time - last_time >=1000)
			{
				BSP_GPIO_WritePin(BSP_GPIO_RELAY, false);
			}
      break;
    case 1800:
		  BSP_GPIO_WritePin(BSP_GPIO_RELAY, false);
			relay_flag = true;
      break;
    default:
			BSP_GPIO_WritePin(BSP_GPIO_RELAY, false);
			relay_flag = true;
      break;
    }
		//两对电机位置细调
		static bool position_zero_flag = 0;
		if(position_zero_flag)
		{
				cmd_for_shoot.position = ((float)rc_ctrl.sw[7]-200)/1600;//归一化
		}
		else
		{
			cmd_for_shoot.position = 0 ;
			if(rc_ctrl.sw[7]==200) position_zero_flag = true;
		}
			
		
		switch (rc_ctrl.sw[3]) {//两对电机正/反转
    case 1800:
		  cmd_for_shoot.pair_state = SHOOT_PAIR_STOP;
      break;
		case 1000:
		  cmd_for_shoot.pair_state = SHOOT_PAIR_STOP;
      break;
    case 200:
		  cmd_for_shoot.pair_state = SHOOT_PAIR_CCW;
      break;
    default:
      break;
    }
		
		
		//两对电机位置粗调
		static bool pair_position_zero_flag = 0;
		uint32_t current_pos = 0;
		if(pair_position_zero_flag)
		{
				if(rc_ctrl.ch[2]<=0)  current_pos = 0 ;
				else current_pos = rc_ctrl.ch[2] ;
				
				//cmd_for_shoot.pair_position = current_pos;
				/*上 设置目标为记忆位置 */
				/*中 设置目标为拨杆位置 */
				/*下    记忆拨杆位置    */
				static uint32_t memory_position = 0 ;
				switch (rc_ctrl.sw[6])
				{
						case 1800:
							if(rc_ctrl.sw[4] == 200)//添加记忆锁定拨杆
								cmd_for_shoot.pair_position = memory_position;
							break;
						case 1000:
							cmd_for_shoot.pair_position = current_pos;
							break;
						case 200:
							memory_position = current_pos;
							break;
						default:
							break;
				}	
		}
		else
		{
			cmd_for_shoot.pair_position = 0 ;
			if(rc_ctrl.ch[2]==-65) pair_position_zero_flag = true;
		}
		static float i=0.025;
		i+=0.005;if(i>0.125)i=0.025;
		BSP_PWM_SetComp(BSP_PWM_SERVO , i );
		
		
    osMessageQueuePut(task_runtime.msgq.shoot.cmd, &cmd_for_shoot, 0, 0);

    /* USER CODE END */
    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
  }
  
}