/*
 * 英雄履带控制
 */
 

/* Includes ----------------------------------------------------------------- */
#include <math.h>
#include <stdint.h>
#include <string.h>
#include "track.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/filter/filter.h"
#include "component/user_math.h"
#include "module/chassis.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */

/* Private macro ------------------------------------------------------------ */
#define WHEEL_RADIUS 0.03f  //驱动轮半径，单位米
#define TRACK_RADIUS 1.0f   //履带驱动轮半径，单位米
/* Private variables -------------------------------------------------------- */

/* Private function  -------------------------------------------------------- */

/* Exported functions ------------------------------------------------------- */
uint8_t Track_Init(Track_t *t, Track_Params_t *param, float target_freq)
{
    if (t == NULL || param == NULL) {
        return TRACK_ERR_NULL; // 参数错误
    }
    t->param = param;

    for (int i=0; i<TRACK_MOTOR_NUM; i++) {
    PID_Init(&t->pid[i], KPID_MODE_CALC_D, target_freq, &param->pid[i]);
    }
    return TRACK_OK;
}

uint8_t Track_Control(Track_t *t, Track_CMD_t *cmd)
{
    if (t == NULL || cmd == NULL) {
        return TRACK_ERR_NULL; // 参数错误
    }
    t->timer.now = BSP_TIME_Get_us()/1000000.0f;
    t->timer.dt = t->timer.now - t->timer.lask_wakeup/1000000.0f;
    t->timer.lask_wakeup = BSP_TIME_Get_us();
    
    t->middle_val.w=PID_Calc(&t->pid_w, 0, t->middle_val.seata, 0.0f, t->timer.dt);
    
    for (int i=0; i<TRACK_MOTOR_NUM; i++) {
      t->middle_val.mix_motor[i]=cmd->vel + t->middle_val.w*TRACK_RADIUS;
      t->output.currentOut[i] = PID_Calc(&t->pid[i], t->middle_val.mix_motor[i], t->motor[i].feedback.rotor_speed, 0.0f, t->timer.dt);
    }
    return TRACK_OK;
}

uint8_t Track_AutoControl(Track_t *t, const Chassis_t *chassis)
{
    if (t == NULL) {
        return TRACK_ERR_NULL; // 参数错误
    }
    t->timer.now = BSP_TIME_Get_us()/1000000.0f;
    t->timer.dt = t->timer.now - t->timer.lask_wakeup/1000000.0f;
    t->timer.lask_wakeup = BSP_TIME_Get_us();
    if (chassis->mode==CHASSIS_MODE_FOLLOW_GIMBAL||chassis->mode==CHASSIS_MODE_FOLLOW_GIMBAL_35)
    {
      float target_rpm[2];
      for (int i=0; i<TRACK_MOTOR_NUM; i++) {
      float rpm = (chassis->feedback.motor[i].rotor_speed + chassis->feedback.motor[i+1].rotor_speed)/2; 
      target_rpm[i] = rpm*(WHEEL_RADIUS/TRACK_RADIUS);
      t->output.currentOut[i] = PID_Calc(&t->pid[i], target_rpm[i], t->motor[i].feedback.rotor_speed, 0.0f, t->timer.dt);
    }
    } else {
      for (int i=0; i<TRACK_MOTOR_NUM; i++) {
        t->output.currentOut[i] = 0.0f;
      }
      PID_Reset(t->pid);
    }

    return TRACK_OK;
} 

uint8_t Track_UpdateFeedback(Track_t *t)
{
    if (t == NULL) {
        return TRACK_ERR_NULL; // 参数错误
    }
    for (int i=0; i<TRACK_MOTOR_NUM; i++) {
      MOTOR_RM_Update(&t->param->motor[i]);
      MOTOR_RM_t *rm_motor = MOTOR_RM_GetMotor(&t->param->motor[i]);
      t->motor[i] = *rm_motor;
    }
    return TRACK_OK;
}
 
uint8_t Track_Output(Track_t *t)
{
    if (t == NULL) {
        return TRACK_ERR_NULL; // 参数错误
    }
    for (int i=0; i<TRACK_MOTOR_NUM; i++) {
      MOTOR_RM_SetOutput(&t->param->motor[i], t->output.currentOut[i]);
      MOTOR_RM_Ctrl(&t->param->motor[i]);
    }
    return TRACK_OK;
}