/*
 * far♂蛇模块
 */

#pragma once

#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif

#include "main.h"
#include "component/pid.h"
#include "device/motor_rm.h"
#include "module/chassis.h"
/* Exported constants ------------------------------------------------------- */
#define TRACK_MOTOR_NUM 2
#define TRACK_CMD_RPM_MAX 8000.0f 
#define TRACK_OK (0)        /* 运行正常 */
#define TRACK_ERR_NULL (-1) /* 运行时发现NULL指针 */    
#define TRACK_ERR_ERR (-2)  /* 运行时发现了其他错误 */
#define TRACK_ERR_MODE (-3) /* 运行时配置了错误的Mode */
#define TRACK_ERR_MOTOR (-4) /* 运行时配置了不存在的电机类型 */
#define TRACK_ERR_MALLOC (-5) /* 内存分配失败 */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct {
  float now;            /* 当前时间，单位秒 */
  uint64_t lask_wakeup; /* 上次唤醒时间，单位微秒 */
  float dt;             /* 两次唤醒间隔时间，单位秒 */
} Track_Timer_t;

typedef struct {
  MOTOR_RM_Param_t motor[TRACK_MOTOR_NUM];
  KPID_Params_t pid[TRACK_MOTOR_NUM];
  KPID_Params_t pid_w[TRACK_MOTOR_NUM];
} Track_Params_t;

typedef struct {
  bool enable;
  float vel;
  float omega;
} Track_CMD_t;

typedef struct {
  Track_Params_t *param;
  Track_Timer_t timer;

  KPID_t pid[TRACK_MOTOR_NUM];
  KPID_t pid_w;

  MOTOR_RM_t motor[TRACK_MOTOR_NUM]; 

  struct {
    float seata;
    float w;
    float mix_motor[TRACK_MOTOR_NUM];
  }middle_val;
  struct {
    float currentOut[TRACK_MOTOR_NUM];
  }output;
} Track_t;

/* Exported functions prototypes -------------------------------------------- */
uint8_t Track_Init(Track_t *t, Track_Params_t *param, float target_freq);
uint8_t Track_UpdateFeedback(Track_t *t);
uint8_t Track_Control(Track_t *t, Track_CMD_t *cmd);
uint8_t Track_AutoControl(Track_t *t, const Chassis_t *chassis);
uint8_t Track_Output(Track_t *t);
#ifdef __cplusplus
}
#endif