#pragma once

#ifdef __cplusplus
extern "C" {
#endif

/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include <stdbool.h>

#include "bsp/uart.h"
#include "component/ahrs.h"
#include "component/user_math.h"
#include "component/kinematics.h"
#include "device/device.h"
#include "gom_protocol.h"

// #include 
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
    GO_LF_HIP = 0,
    GO_LF_THIGH = 1,
    GO_LF_CALF = 2,
    GO_RF_HIP = 3,
    GO_RF_THIGH = 4,
    GO_RF_CALF = 5,
    GO_LR_HIP = 6,
    GO_LR_THIGH = 7,
    GO_LR_CALF = 8,
    GO_RR_HIP = 9,
    GO_RR_THIGH = 10,
    GO_RR_CALF = 11,
} GO_MotorId_t;

typedef enum{
    GO_MOTOR_MODE_IDLE = 0, // 空闲状态
    GO_MOTOR_MODE_FOC = 1, // FOC控制
    GO_MOTOR_MODE_CALIBRATION = 2, // 电机标定
    GO_MOTOR_MODE_ERR,
    GO_MOTOR_MODE_NUM,
} GO_MotorMode_t;

typedef struct {
  BSP_UART_t left_leg;
  BSP_UART_t right_leg;
} GO_UART_Params_t;

enum GO_Motor_e {
    GO_MOTOR_LF_HIP = 0,
    GO_MOTOR_LF_THIGH,
    GO_MOTOR_LF_CALF,
    GO_MOTOR_RF_HIP,
    GO_MOTOR_RF_THIGH,
    GO_MOTOR_RF_CALF,
    GO_MOTOR_LR_HIP,  
    GO_MOTOR_LR_THIGH,
    GO_MOTOR_LR_CALF,
    GO_MOTOR_RR_HIP,
    GO_MOTOR_RR_THIGH,
    GO_MOTOR_RR_CALF,
    GO_MOTOR_NUM,
    GO_MOTOR_ERR,
};

/*电机数据*/
typedef union {
  MotorData_t id[GO_MOTOR_NUM];
  struct {
    MotorData_t lf_hip;   // 左前腿髋关节
    MotorData_t lf_thigh; // 左前腿大腿
    MotorData_t lf_calf;  // 左前腿小腿
    MotorData_t rf_hip;   // 右前腿髋关节
    MotorData_t rf_thigh; // 右前腿大腿
    MotorData_t rf_calf;  // 右前腿小腿
    MotorData_t lr_hip;   // 左后腿髋关节
    MotorData_t lr_thigh; // 左后腿大腿
    MotorData_t lr_calf;  // 左后腿小腿
    MotorData_t rr_hip;   // 右后腿髋关节
    MotorData_t rr_thigh; // 右后腿大腿
    MotorData_t rr_calf;  // 右后腿小腿
  } named;
}GO_MotorFeedback_t;

/*半侧电机数据*/
// typedef struct {
//   MotorData_t data[GO_MOTOR_NUM / 2];
// } GO_MotorFeedbackHalf_t;

/*控制数据*/
typedef union {
  MotorCmd_t id[GO_MOTOR_NUM]; 
  struct {
    MotorCmd_t lf_hip;   // 左前腿髋关节
    MotorCmd_t lf_thigh; // 左前腿大腿
    MotorCmd_t lf_calf;  // 左前腿小腿
    MotorCmd_t rf_hip;   // 右前腿髋关节
    MotorCmd_t rf_thigh; // 右前腿大腿
    MotorCmd_t rf_calf;  // 右前腿小腿
    MotorCmd_t lr_hip;   // 左后腿髋关节
    MotorCmd_t lr_thigh; // 左后腿大腿
    MotorCmd_t lr_calf;  // 左后腿小腿
    MotorCmd_t rr_hip;   // 右后腿髋关节
    MotorCmd_t rr_thigh; // 右后腿大腿
    MotorCmd_t rr_calf;  // 右后腿小腿
  } named;
} GO_MotorOutput_t;

typedef union {
  Kinematics_JointCMD_t id[GO_MOTOR_NUM];
  struct {
    Kinematics_JointCMD_t lf_hip;   // 左前腿髋关节
    Kinematics_JointCMD_t lf_thigh; // 左前腿大腿
    Kinematics_JointCMD_t lf_calf;  // 左前腿小腿
    Kinematics_JointCMD_t rf_hip;   // 右前腿髋关节
    Kinematics_JointCMD_t rf_thigh; // 右前腿大腿
    Kinematics_JointCMD_t rf_calf;  // 右前腿小腿
    Kinematics_JointCMD_t lr_hip;   // 左后腿髋关节
    Kinematics_JointCMD_t lr_thigh; // 左后腿大腿
    Kinematics_JointCMD_t lr_calf;  // 左后腿小腿
    Kinematics_JointCMD_t rr_hip;   // 右后腿髋关节
    Kinematics_JointCMD_t rr_thigh; // 右后腿大腿
    Kinematics_JointCMD_t rr_calf;  // 右后腿小腿
  } named;
} GO_ChassisCMD_t;

typedef union {
  Kinematics_JointData_t id[GO_MOTOR_NUM];
  struct {
    Kinematics_JointData_t lf_hip;   // 左前腿髋关节
    Kinematics_JointData_t lf_thigh; // 左前腿大腿
    Kinematics_JointData_t lf_calf;  // 左前腿小腿
    Kinematics_JointData_t rf_hip;   // 右前腿髋关节
    Kinematics_JointData_t rf_thigh; // 右前腿大腿
    Kinematics_JointData_t rf_calf;  // 右前腿小腿
    Kinematics_JointData_t lr_hip;   // 左后腿髋关节
    Kinematics_JointData_t lr_thigh; // 左后腿大腿
    Kinematics_JointData_t lr_calf;  // 左后腿小腿
    Kinematics_JointData_t rr_hip;   // 右后腿髋关节
    Kinematics_JointData_t rr_thigh; // 右后腿大腿
    Kinematics_JointData_t rr_calf;  // 右后腿小腿
  } named;
} GO_ChassisFeedback_t;


// /*半侧电机控制数据*/
// typedef struct {
//   MotorCmd_t cmd[GO_MOTOR_NUM / 2];
// } GO_MotorOutputHalf_t;

// typedef struct {
//   GO_MotorOutputHalf_t output; // 电机控制数据
//   GO_MotorFeedbackHalf_t feedback; // 电机反馈数据
// } GO_MotorHalf_t;

typedef struct {
  GO_MotorOutput_t output; // 电机控制数据
  GO_MotorFeedback_t feedback; // 电机反馈数据
}GO_Motor_t;



typedef struct {
  GO_Motor_t motor;
  const GO_UART_Params_t *uart_param; // UART参数
} GO_t;

// typedef struct {
//   GO_MotorHalf_t motor; // 电机数据
//   const GO_UART_Params_t *uart_param; // UART参数
// } GO_Half_t;



/* Exported functions prototypes -------------------------------------------- */
int8_t GO_Init(GO_t *go, const GO_UART_Params_t *param);
// int8_t GO_Half_Init(GO_Half_t *go_half, const GO_UART_Params_t *param);
// int8_t GO_Left_Init(GO_Half_t *go_half, const GO_UART_Params_t *param);
// int8_t GO_Right_Init(GO_Half_t *go_half, const GO_UART_Params_t *param);
// int8_t GO_Control_Left(GO_Half_t *go_half);
// int8_t GO_Control_Right(GO_Half_t *go_half);


int8_t GO_Motor_Control(GO_t *go);

int8_t GO_ParseCmd(GO_t *go, const GO_ChassisCMD_t *chassis_cmd);
int8_t GO_ParseFeedback(GO_t *go, GO_ChassisFeedback_t *chassis_feedback);
// int8_t GO_Control_Left(GO_t *go);
// int8_t GO_Control_Right(GO_t *go);
// int8_t GO_CMD_Parse(GO_t *go, const uint8_t *rx_buff, uint16_t len);
#ifdef __cplusplus
}
#endif