/*
    chassis Task
    
*/

/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "module/chassis.h"
#include "module/config.h"
#include "device/ai.h"
#include "device/supercap.h"
#include "component/limiter.h"
/* USER INCLUDE END */

/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
Chassis_t chassis;
Chassis_CMD_t cmd_chassis;
AI_result_t c_cmd_ai_result; /* 新增的 ai 结果变量 主要是给底盘接收自瞄相关的命令*/
float temp_for_cap[7];//cap-add//supercap-add
float P3508[4],P6020[4],I6020[4],S6020[4],I3508[4],S3508[4],power_limit_3508=100;
/* USER STRUCT END */

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

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

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

  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
  /* USER CODE INIT BEGIN */
  /*底盘初始化*/
chassis_init(&chassis,&Config_GetRobotParam()->chassis,CHASSIS_FREQ);

  /* USER CODE INIT END */
  
  while (1) {
    tick += delay_tick; /* 计算下一个唤醒时刻 */
    /* USER CODE BEGIN */

osMessageQueueGet(task_runtime.msgq.navi.c_cmd, &c_cmd_ai_result, NULL, 0);

 osMessageQueueGet(task_runtime.msgq.gimbal.yaw4310,&chassis.motorfeedback.gimbal_yaw_encoder,NULL,0);
    /*接受cmd任务数据*/
    if(osMessageQueueGet(task_runtime.msgq.chassis.cmd, &cmd_chassis, NULL, 0)==osOK)
    {
      if(cmd_chassis.ctrl_mode == CHASSIS_MODE_DAOHANG){
        cmd_chassis.Vx = c_cmd_ai_result.chassis_t.Vx;
        cmd_chassis.Vy = c_cmd_ai_result.chassis_t.Vy;
        cmd_chassis.Vw = c_cmd_ai_result.chassis_t.Vw;
      }
      else{
        cmd_chassis.Vx = cmd_chassis.Vx;
        cmd_chassis.Vy = cmd_chassis.Vy;
        cmd_chassis.Vw = cmd_chassis.Vw;
      }
      Chassis_update(&chassis);
      Chassis_Control(&chassis, &cmd_chassis,tick);
    }else
{
	 Chassis_CMD_t safe_cmd = {.mode = STOP, .Vx = 0, .Vy = 0, .Vw = 0};
    Chassis_Control(&chassis, &safe_cmd,tick);
}
    		
				// float P3=0,P6=0;
				// for(int i=0;i<4;i++)
		    // {
        //   S6020[i] = MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&chassis.param->motor_6020_param[i])->motor));
        //   I6020[i] = MOTOR_GetTorqueCurrent(&(MOTOR_RM_GetMotor(&chassis.param->motor_6020_param[i])->motor));
				// 	 S3508[i] = MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&chassis.param->motor_3508_param[i])->motor));
        //   I3508[i] = MOTOR_GetTorqueCurrent(&(MOTOR_RM_GetMotor(&chassis.param->motor_3508_param[i])->motor));
        //   P3508[i]=fabsf(S3508[i]*I3508[i]);
				// 	P6020[i]=fabsf(S6020[i]*I6020[i]);
				// 	P3+=P3508[i];
				// 	P6+=P6020[i];
				// }
				// temp_for_cap[0] = P6*0.000012;											// 计算6020总功率 P6
        // temp_for_cap[1] = P3*0.0000048;		
        // float P_3508 = 0; 									// 计算3508总功率 P3
      // for(int i=0;i<4;i++)
      // {
      //   P_3508+=chassis.out.rotor3508_out[i]*S3508[i]*0.0000048;
      // }

      
    // if(CAN_SuperCapRXData.SuperCapReady == 1)
    // {
    //   if(CAN_SuperCapRXData.SuperCapEnergy<=10)
    //     power_limit_3508 = 100;
    //   else  
    //     power_limit_3508 = 320;

    //   if(CAN_SuperCapRXData.ChassisPower>power_limit_3508&&power_limit_3508>=0)
    // {
    //     for(int i=0;i<4;i++)
    //     {
    //         chassis.out.rotor3508_out[i]*=(power_limit_3508)/
    //           CAN_SuperCapRXData.ChassisPower>power_limit_3508;	// 如果总功率超过限制，按比例降低3508的输出
    //     }

    // }
    // }
    // else
    // {
    //   //启动老方案进行限制
    //    PowerLimit_ChassicOutput(power_limit_3508,chassis.out.rotor3508_out,
    //                             chassis.motorfeedback.rotor_rpm3508, 4);  // 根据功率限制调整3508输出
      
    // }
    

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