/*
  四足机器人12个关节驱动 TODO:改为DMA
*/

/* Includes ----------------------------------------------------------------- */
#include "bsp/uart.h"
#include "go.h"

#include <stdbool.h>
#include <string.h>

/* Private define ----------------------------------------------------------- */


/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static bool inited = false;

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

/* Exported functions ------------------------------------------------------- */

int8_t GO_Init(GO_t *go, const GO_UART_Params_t *param){
    if (go == NULL) return DEVICE_ERR_NULL;
    if (param == NULL) return DEVICE_ERR_NULL;
    if (inited) return DEVICE_ERR_INITED;

    go->uart_param = param;
    memset(&go->motor.output, 0, sizeof(go->motor.output));
    memset(&go->motor.feedback, 0, sizeof(go->motor.feedback));


    inited = true;
    return DEVICE_OK;
}



int8_t GO_Motor_Control(GO_t *go){
    if (go == NULL) return DEVICE_ERR_NULL;
    if (!inited) return DEVICE_ERR_INITED;

    // 发送控制命令
    for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
        go->motor.output.id[i].id = i; // 设置电机ID
        go->motor.output.id[i].mode = GO_MOTOR_MODE_FOC; // 设置电机模式
        modify_data(&go->motor.output.id[i]); 
        if (i < GO_MOTOR_NUM / 4 || i >= GO_MOTOR_NUM/2 && i < GO_MOTOR_NUM * 3 / 4) {
        // if (i < GO_MOTOR_NUM / 4) {
            HAL_UART_Transmit(BSP_UART_GetHandle(go->uart_param->left_leg), 
                              (uint8_t *)&go->motor.output.id[i].motor_send_data, 
                              sizeof(go->motor.output.id[i].motor_send_data), 1);
            HAL_UART_Receive(BSP_UART_GetHandle(go->uart_param->left_leg),
                             (uint8_t *)&go->motor.feedback.id[i].motor_recv_data, 
                             sizeof(go->motor.feedback.id[i].motor_recv_data), 1);
        } else {
            HAL_UART_Transmit(BSP_UART_GetHandle(go->uart_param->right_leg), 
                              (uint8_t *)&go->motor.output.id[i].motor_send_data, 
                              sizeof(go->motor.output.id[i].motor_send_data), 1);
            HAL_UART_Receive(BSP_UART_GetHandle(go->uart_param->right_leg),
                             (uint8_t *)&go->motor.feedback.id[i].motor_recv_data, 
                             sizeof(go->motor.feedback.id[i].motor_recv_data), 1);
        }
        extract_data(&go->motor.feedback.id[i]); // 解析接收到的数据
    }
    return DEVICE_OK;
}

// int8_t GO_Half_Init(GO_Half_t *go_half, const GO_UART_Params_t *param) {
//     if (go_half == NULL) return DEVICE_ERR_NULL;
//     if (param == NULL) return DEVICE_ERR_NULL;

//     go_half->uart_param = param;
//     memset(&go_half->motor.output, 0, sizeof(go_half->motor.output));
//     memset(&go_half->motor.feedback, 0, sizeof(go_half->motor.feedback));

//     return DEVICE_OK;
// }

// int8_t GO_Control_Left(GO_Half_t *go_half) {
//     if (go_half == NULL) return DEVICE_ERR_NULL;

//     // 发送控制命令
//     for (uint8_t i = 0; i < GO_MOTOR_NUM / 2; i++) {
//         // 0,1,2 -> id 0,1,2；3,4,5 -> id 6,7,8
//         if (i < 3) {
//             go_half->motor.output.cmd[i].id = i;
//         } else {
//             go_half->motor.output.cmd[i].id = i + 3; // 3->6, 4->7, 5->8
//         }
//         go_half->motor.output.cmd[i].mode = GO_MOTOR_MODE_FOC; // 设置电机模式
//         modify_data(&go_half->motor.output.cmd[i]); // 修改数据
//         HAL_UART_Transmit(BSP_UART_GetHandle(go_half->uart_param->left_leg), 
//                           (uint8_t *)&go_half->motor.output.cmd[i].motor_send_data, 
//                           sizeof(go_half->motor.output.cmd[i].motor_send_data), 1);
//         HAL_UART_Receive(BSP_UART_GetHandle(go_half->uart_param->left_leg),
//                          (uint8_t *)&go_half->motor.feedback.data[i].motor_recv_data, 
//                          sizeof(go_half->motor.feedback.data[i].motor_recv_data), 1);
//         extract_data(&go_half->motor.feedback.data[i]); // 解析接收到的数据
//     }
//     return DEVICE_OK;
// }

// int8_t GO_Control_Right(GO_Half_t *go_half) {
//     if (go_half == NULL) return DEVICE_ERR_NULL;

//     // 发送控制命令
//     for (uint8_t i = 0; i < GO_MOTOR_NUM / 2; i++) {
//         // 0,1,2 -> id 3,4,5；3,4,5 -> id 9,10,11
//         if (i < 3) {
//             go_half->motor.output.cmd[i].id = i + 3; // 0->3, 1->4, 2->5
//         } else {
//             go_half->motor.output.cmd[i].id = i + 6; // 3->9, 4->10, 5->11
//         }
//         go_half->motor.output.cmd[i].mode = GO_MOTOR_MODE_FOC; // 设置电机模式
//         modify_data(&go_half->motor.output.cmd[i]); // 修改数据
//         HAL_UART_Transmit(BSP_UART_GetHandle(go_half->uart_param->right_leg), 
//                           (uint8_t *)&go_half->motor.output.cmd[i].motor_send_data, 
//                           sizeof(go_half->motor.output.cmd[i].motor_send_data), 1);
//         HAL_UART_Receive(BSP_UART_GetHandle(go_half->uart_param->right_leg),
//                          (uint8_t *)&go_half->motor.feedback.data[i].motor_recv_data, 
//                          sizeof(go_half->motor.feedback.data[i].motor_recv_data), 1);
//         extract_data(&go_half->motor.feedback.data[i]); // 解析接收到的数据
//     }
//     return DEVICE_OK;
// }

int8_t GO_ParseCmd(GO_t *go, const GO_ChassisCMD_t *chassis_cmd){
    if (go == NULL) return DEVICE_ERR_NULL;
    if (chassis_cmd == NULL) return DEVICE_ERR_NULL;

    for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
        go->motor.output.id[i].K_P = chassis_cmd->id[i].K_P; // 设置位置控制增益
        go->motor.output.id[i].K_W = chassis_cmd->id[i].K_W;
        go->motor.output.id[i].Pos = chassis_cmd->id[i].Pos; // 设置目标位置
        go->motor.output.id[i].W = chassis_cmd->id[i].W; // 设置
        go->motor.output.id[i].T = chassis_cmd->id[i].T; // 设置目标扭矩
    }
    return DEVICE_OK;
}

int8_t GO_ParseFeedback(GO_t *go, GO_ChassisFeedback_t *chassis_feedback){
    if (go == NULL) return DEVICE_ERR_NULL;
    if (chassis_feedback == NULL) return DEVICE_ERR_NULL;

    for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
        chassis_feedback->id[i].Pos = go->motor.feedback.id[i].Pos; // 设置位置反馈
        chassis_feedback->id[i].W = go->motor.feedback.id[i].W; // 设置速度反馈
        chassis_feedback->id[i].T = go->motor.feedback.id[i].T; // 设置扭矩反馈
    }
    return DEVICE_OK;
}