#ifndef __GO_M8010_6_H
#define __GO_M8010_6_H
#ifdef __cplusplus
extern "C"{
#endif
#include "main.h"
#include "crc_ccitt.h"
#include "usart.h"
#include "string.h"
#include <math.h>
#define GO_NUM 3
/**
 * @brief 电机模式状态信息
 * 
 */
typedef struct
{
    uint8_t id     :4;      // 电机ID: 0,1...,14 15表示广播模式(暂时无法用)
    uint8_t status :3;      // 工作模式: 0.停止 1.FOC校准 2.编码器校准 3.运行
    uint8_t none   :1;      // 保留位
} RIS_Mode_t /*__attribute__((packed))*/;   // 电机模式 1Byte



/**
 * @brief 电机状态控制信息
 * 
 */
typedef struct
{
    int16_t tor_des;        // 电机目标力矩 unit: N.m     (q8)
    int16_t spd_des;        // 电机目标速度 unit: rad/s   (q7)
    int32_t pos_des;        // 电机目标位置 unit: rad     (q15)
    uint16_t  k_pos;        // 电机位置环系数 unit: 0.0-1.0 (q15)
    uint16_t  k_spd;        // 电机速度环系数 unit: 0.0-1.0 (q15)
    
} RIS_Comd_t;   // 控制命令 12Byte




/**
 * @brief 电机控制数据包格式
 * 
 */
typedef struct
{
    uint8_t head[2];    // 包头         2Byte
    RIS_Mode_t mode;    // 电机工作模式  1Byte
    RIS_Comd_t comd;    // 电机控制命令 12Byte
    uint16_t   CRC16;   // CRC校验      2Byte

} ControlData_t;    // 电机控制数据包 17Byte




/**
 * @brief 电机状态信息结构体
 * 
 */
typedef struct {              
    unsigned short id;              // 电机ID, 0xBB表示广播
    unsigned short mode;            // 工作模式: 0.停止 5.正转 10.FOC控制
    uint16_t correct;               // 校正状态: 1.校正完成 0.未校正
    int MError;                     // 电机错误: 0.正常 1.过流 2.过载 3.欠压 4.过温
    int Temp;                       // 温度
    float tar_pos;                  // 目标位置
    float tar_w;                    // 目标速度
    float T;                        // 当前实际力矩
    float W;                        // 当前实际速度 (单位: rad/s)
    float Pos;                      // 当前实际位置
    int footForce;                  // 预留字段, 暂未使用
    uint8_t buffer[17];             // 数据发送缓冲区
    uint8_t Rec_buffer[16];         // 数据接收缓冲区
    ControlData_t motor_send_data;  // 电机控制数据结构
} GO_Motorfield;

/**
    *@brief		电机初始化
    */
void GO_M8010_init(void);

/**
    *@brief				go发送控制命令
    *@input[in]		huart 所用串口指针
    *@input[in]		id 被控电机id
    *@input[in]		rev 暂时不知道干啥用的，github为回答issue
    *@input[in]		T	力矩，单位N·m
    *@input[in]		Pos 目标位置，单位rad
    *@input[in]		W 目标速度，单位rad/s
    *@input[in]		K_P 位置环kp
    *@input[in]		K_W 速度环kp
    *@note				控制公式 ： t = T + （设定位置 - 实际位置）*K_P + （设定速度 - 实际速度）*K_W
    */
void GO_M8010_send_data(UART_HandleTypeDef *huart,int id, int rev,float T,float Pos,float W,float K_P,float K_W);

/**
    *@brief		用户自定义挂载发送完成中断回调函数
    */
void uartTxCB(UART_HandleTypeDef *huart);


// 基础力控函数
void basic_ForceControl (UART_HandleTypeDef *huart, int id, float bias, float length, float mass, 
                        float tar_pos, float tar_w, float K_P,float K_W);


/**
    *@brief				go获取电机指针
    *@input[in]		id 电机的id
    *@revtal			返回电机的指针
    */
GO_Motorfield* getGoPoint(uint8_t id);

#ifdef __cplusplus
}
#endif
#endif /*__GO_M8010_6_H */