/*
  四足机器人12个关节驱动
*/

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

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


#include "component/user_math.h"

#include "cmsis_os2.h"

static osSemaphoreId_t sem_left_rx = NULL;
static osSemaphoreId_t sem_right_rx = NULL;

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


/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
// static RIS_MotorData_t raw_left __attribute__ ((section(".AXI_SRAM")));
static uint8_t raw_left_buff[16] __attribute__ ((section(".AXI_SRAM")));
static uint8_t raw_right_buff[16] __attribute__ ((section(".AXI_SRAM")));
// static RIS_MotorData_t raw_right __attribute__ ((section(".AXI_SRAM")));
static MotorCmd_t motor_cmd __attribute__ ((section(".AXI_SRAM")));

static osThreadId_t thread_alert;
static GO_t *go_rx;
static bool inited = false;

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

// static void GO_LeftRxCpltCallback(void) {
//     osMessageQueuePut(go_rx->msgq_raw, &raw_left_buff, 0, 0);
//     osSemaphoreRelease(sem_left_rx); // 通知接收完成
    // memset(raw_left_buff, 0, sizeof(raw_left_buff)); // 清空缓存区
    // HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_LEFT_LEG),
    //                     (uint8_t *)&raw_left, sizeof(RIS_MotorData_t));
// }
// static void GO_RightRxCpltCallback(void) {
//     osMessageQueuePut(go_rx->msgq_raw, &raw_right_buff, 0, 0);
//     osSemaphoreRelease(sem_right_rx); // 通知接收完成
    // memset(raw_right_buff, 0, sizeof(raw_right_buff)); // 清空缓存区
    // HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_RIGHT_LEG),
    //                     (uint8_t *)&raw_right, sizeof(RIS_MotorData_t));
// }

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

int8_t GO_Init(GO_t *go, const GO_UART_Params_t *param){
    if (go == NULL) return DEVICE_ERR_NULL;
    if (inited) return DEVICE_ERR_INITED;
    if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
    
    go_rx->msgq_raw = osMessageQueueNew(36, 16u, NULL);
    
    go->param = param;
    sem_left_rx = osSemaphoreNew(1, 0, NULL);
    sem_right_rx = osSemaphoreNew(1, 0, NULL);
    // BSP_UART_RegisterCallback(BSP_UART_LEFT_LEG, BSP_UART_RX_CPLT_CB,
    //                             GO_LeftRxCpltCallback);
    // BSP_UART_RegisterCallback(BSP_UART_RIGHT_LEG, BSP_UART_RX_CPLT_CB,
    //                             GO_RightRxCpltCallback);
    
    inited = true;
    return DEVICE_OK;
}

int8_t GO_Motor_Control(GO_MotorOutput_t *output, GO_t *go){
    if (go == NULL) return DEVICE_ERR_NULL;
    if (output == NULL) return DEVICE_ERR_NULL;
    memset(raw_right_buff, 0, sizeof(raw_right_buff));
    memset(raw_left_buff, 0, sizeof(raw_left_buff));
    // HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_LEFT_LEG),
    //                     (uint8_t *)&raw_left_buff, 16);
    // HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_RIGHT_LEG),
    //                     (uint8_t *)&raw_right_buff, 16);

    for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
        motor_cmd.id = i;
        motor_cmd.mode = GO_MOTOR_MODE_FOC;
        motor_cmd.T = output->as_array[i].tor_des;
        motor_cmd.W = output->as_array[i].spd_des;
        motor_cmd.Pos = output->as_array[i].pos_des;
        motor_cmd.K_P = output->as_array[i].k_pos;
        motor_cmd.K_W = output->as_array[i].k_spd;

        modify_data(&motor_cmd);

        if (i <= GO_MOTOR_LF_CALF || (i >= GO_MOTOR_LR_HIP && i <= GO_MOTOR_LR_CALF)) {
            HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_LEFT_LEG),
                              (uint8_t *)&motor_cmd.motor_send_data,
                              sizeof(RIS_ControlData_t));

            HAL_UART_Receive(BSP_UART_GetHandle(BSP_UART_LEFT_LEG),
                            (uint8_t *)&raw_left_buff, 16, 5);
        } else {
            HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_RIGHT_LEG),
                              (uint8_t *)&motor_cmd.motor_send_data,
                              sizeof(RIS_ControlData_t));

            HAL_UART_Receive(BSP_UART_GetHandle(BSP_UART_RIGHT_LEG),
                            (uint8_t *)&raw_right_buff, 16, 5);
        }
        
    }
    return DEVICE_OK;
}

int8_t GO_StoreMsg(GO_t *go, RIS_MotorData_t *go_rx){

    return DEVICE_OK;
}