/* Includes ----------------------------------------------------------------- */
#include "rc_can.h"
#include "bsp/time.h"

/* USER INCLUDE BEGIN */

/* USER INCLUDE END */

/* Private define ----------------------------------------------------------- */
#define RC_CAN_SCALE_FACTOR    1000.0f  // 将[-1.0, 1.0]转换为[-1000, 1000]
#define RC_CAN_DATA_JOYSTICK   0
#define RC_CAN_DATA_SWITCH     1
#define RC_CAN_DATA_MOUSE      2
#define RC_CAN_DATA_KEYBOARD   3
#define RC_CAN_DATA_STATUS     4

/* USER DEFINE BEGIN */

/* USER DEFINE END */

/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */

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

/**
 * @brief 将浮点数转换为整数，缩放到指定范围
 * @param value 输入值 [-1.0, 1.0]
 * @return 缩放后的整数值 [-1000, 1000]
 */
static int16_t RC_CAN_ScaleFloat(float value) {
    if (value > 1.0f) value = 1.0f;
    if (value < -1.0f) value = -1.0f;
    return (int16_t)(value * RC_CAN_SCALE_FACTOR);
}

/**
 * @brief 更新遥杆数据
 * @param rc_can RC_CAN结构体指针
 * @param dr16 DR16数据结构体指针
 */
static void RC_CAN_UpdateJoystickData(RC_CAN_t *rc_can, const DR16_t *dr16) {
    rc_can->joystick_data.ch_l_x = RC_CAN_ScaleFloat(dr16->data.ch_l_x);
    rc_can->joystick_data.ch_l_y = RC_CAN_ScaleFloat(dr16->data.ch_l_y);
    rc_can->joystick_data.ch_r_x = RC_CAN_ScaleFloat(dr16->data.ch_r_x);
    rc_can->joystick_data.ch_r_y = RC_CAN_ScaleFloat(dr16->data.ch_r_y);
}

/**
 * @brief 更新拨杆数据
 * @param rc_can RC_CAN结构体指针
 * @param dr16 DR16数据结构体指针
 */
static void RC_CAN_UpdateSwitchData(RC_CAN_t *rc_can, const DR16_t *dr16) {
    rc_can->switch_data.sw_l = (uint8_t)dr16->data.sw_l;
    rc_can->switch_data.sw_r = (uint8_t)dr16->data.sw_r;
    rc_can->switch_data.ch_res = RC_CAN_ScaleFloat(dr16->data.ch_res);
    rc_can->switch_data.reserved = 0;
}

/**
 * @brief 更新鼠标数据
 * @param rc_can RC_CAN结构体指针
 * @param dr16 DR16数据结构体指针
 */
static void RC_CAN_UpdateMouseData(RC_CAN_t *rc_can, const DR16_t *dr16) {
    rc_can->mouse_data.mouse_x = dr16->data.mouse.x;
    rc_can->mouse_data.mouse_y = dr16->data.mouse.y;
    rc_can->mouse_data.mouse_z = dr16->data.mouse.z;
    rc_can->mouse_data.mouse_l = dr16->data.mouse.l_click ? 1 : 0;
    rc_can->mouse_data.mouse_r = dr16->data.mouse.r_click ? 1 : 0;
}

/**
 * @brief 更新键盘数据
 * @param rc_can RC_CAN结构体指针
 * @param dr16 DR16数据结构体指针
 */
static void RC_CAN_UpdateKeyboardData(RC_CAN_t *rc_can, const DR16_t *dr16) {
    rc_can->keyboard_data.key_value = dr16->data.keyboard.value;
    memset(rc_can->keyboard_data.reserved, 0, sizeof(rc_can->keyboard_data.reserved));
}

/**
 * @brief 更新状态数据
 * @param rc_can RC_CAN结构体指针
 * @param dr16 DR16数据结构体指针
 */
static void RC_CAN_UpdateStatusData(RC_CAN_t *rc_can, const DR16_t *dr16) {
    rc_can->status_data.online = dr16->header.online ? 1 : 0;
    rc_can->status_data.data_valid = 1; // 假设数据有效
    rc_can->status_data.timestamp = (uint32_t)(BSP_TIME_Get() / 1000); // 转换为ms
    rc_can->status_data.reserved = 0;
}

/**
 * @brief 将整数转换为浮点数，反缩放
 * @param value 输入值 [-1000, 1000]
 * @return 反缩放后的浮点数值 [-1.0, 1.0]
 */
static float RC_CAN_UnscaleInt(int16_t value) {
    float result = (float)value / RC_CAN_SCALE_FACTOR;
    if (result > 1.0f) result = 1.0f;
    if (result < -1.0f) result = -1.0f;
    return result;
}

/**
 * @brief 处理接收到的CAN消息
 * @param rc_can RC_CAN结构体指针
 * @param msg CAN消息指针
 */
static void RC_CAN_ProcessRxMessage(RC_CAN_t *rc_can, BSP_CAN_Message_t *msg) {
    uint32_t current_time = (uint32_t)(BSP_TIME_Get() / 1000); // 转换为ms
    
    switch (msg->parsed_id) {
        case RC_CAN_DR16_JOYSTICK_ID:
            if (msg->dlc >= sizeof(RC_CAN_JoystickData_t)) {
                memcpy(&rc_can->joystick_data, msg->data, sizeof(RC_CAN_JoystickData_t));
                rc_can->rx_status.joystick_updated = true;
                rc_can->rx_status.last_joystick_time = current_time;
            }
            break;
            
        case RC_CAN_DR16_SWITCH_ID:
            if (msg->dlc >= sizeof(RC_CAN_SwitchData_t)) {
                memcpy(&rc_can->switch_data, msg->data, sizeof(RC_CAN_SwitchData_t));
                rc_can->rx_status.switch_updated = true;
                rc_can->rx_status.last_switch_time = current_time;
            }
            break;
            
        case RC_CAN_DR16_MOUSE_ID:
            if (msg->dlc >= sizeof(RC_CAN_MouseData_t)) {
                memcpy(&rc_can->mouse_data, msg->data, sizeof(RC_CAN_MouseData_t));
                rc_can->rx_status.mouse_updated = true;
                rc_can->rx_status.last_mouse_time = current_time;
            }
            break;
            
        case RC_CAN_DR16_KEYBOARD_ID:
            if (msg->dlc >= sizeof(RC_CAN_KeyboardData_t)) {
                memcpy(&rc_can->keyboard_data, msg->data, sizeof(RC_CAN_KeyboardData_t));
                rc_can->rx_status.keyboard_updated = true;
                rc_can->rx_status.last_keyboard_time = current_time;
            }
            break;
            
        case RC_CAN_DR16_STATUS_ID:
            if (msg->dlc >= sizeof(RC_CAN_StatusData_t)) {
                memcpy(&rc_can->status_data, msg->data, sizeof(RC_CAN_StatusData_t));
                rc_can->rx_status.status_updated = true;
                rc_can->rx_status.last_status_time = current_time;
                // 更新整体在线状态
                rc_can->header.online = (rc_can->status_data.online == 1);
                rc_can->header.last_online_time = BSP_TIME_Get();
            }
            break;
            
        default:
            // 未知ID，忽略
            break;
    }
}

/* USER FUNCTION BEGIN */

/* USER FUNCTION END */

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

int8_t RC_CAN_Init(RC_CAN_t *rc_can, RC_CAN_Param_t *param) {
    if (rc_can == NULL || param == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    if (rc_can->initialized) {
        return DEVICE_ERR_INITED;
    }
    
    // 复制参数
    memcpy(&rc_can->param, param, sizeof(RC_CAN_Param_t));
    
    // 初始化头部信息
    rc_can->header.online = false;
    rc_can->header.last_online_time = 0;
    
    // 清零数据包
    memset(&rc_can->joystick_data, 0, sizeof(RC_CAN_JoystickData_t));
    memset(&rc_can->switch_data, 0, sizeof(RC_CAN_SwitchData_t));
    memset(&rc_can->mouse_data, 0, sizeof(RC_CAN_MouseData_t));
    memset(&rc_can->keyboard_data, 0, sizeof(RC_CAN_KeyboardData_t));
    memset(&rc_can->status_data, 0, sizeof(RC_CAN_StatusData_t));
    
    // 初始化接收状态
    memset(&rc_can->rx_status, 0, sizeof(RC_CAN_RxStatus_t));
    
    // 如果是接收模式，注册CAN ID
    if (param->mode == RC_CAN_MODE_RECEIVER || param->mode == RC_CAN_MODE_BOTH) {
        BSP_CAN_RegisterId(param->can, RC_CAN_DR16_JOYSTICK_ID, 0);
        BSP_CAN_RegisterId(param->can, RC_CAN_DR16_SWITCH_ID, 0);
        BSP_CAN_RegisterId(param->can, RC_CAN_DR16_MOUSE_ID, 0);
        BSP_CAN_RegisterId(param->can, RC_CAN_DR16_KEYBOARD_ID, 0);
        BSP_CAN_RegisterId(param->can, RC_CAN_DR16_STATUS_ID, 0);
    }
    
    // 初始化状态
    rc_can->last_send_time = 0;
    rc_can->initialized = true;
    
    return DEVICE_OK;
}

int8_t RC_CAN_SendData(RC_CAN_t *rc_can, const void *dr16_ptr) {
    if (rc_can == NULL || dr16_ptr == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    const DR16_t *dr16 = (const DR16_t *)dr16_ptr;
    
    if (!rc_can->initialized) {
        return DEVICE_ERR;
    }
    
    if (!rc_can->param.enabled) {
        return DEVICE_OK; // 未启用时直接返回成功
    }
    
    uint64_t current_time = BSP_TIME_Get() / 1000; // 转换为ms
    
    // 检查发送周期
    if (current_time - rc_can->last_send_time < rc_can->param.send_period) {
        return DEVICE_OK; // 未到发送时间
    }
    
    // 更新所有数据包
    RC_CAN_UpdateJoystickData(rc_can, dr16);
    RC_CAN_UpdateSwitchData(rc_can, dr16);
    RC_CAN_UpdateMouseData(rc_can, dr16);
    RC_CAN_UpdateKeyboardData(rc_can, dr16);
    RC_CAN_UpdateStatusData(rc_can, dr16);
    
    int8_t result = DEVICE_OK;
    
    // 发送所有数据包
    if (RC_CAN_SendJoystickData(rc_can) != DEVICE_OK) result = DEVICE_ERR;
    if (RC_CAN_SendSwitchData(rc_can) != DEVICE_OK) result = DEVICE_ERR;
    if (RC_CAN_SendMouseData(rc_can) != DEVICE_OK) result = DEVICE_ERR;
    if (RC_CAN_SendKeyboardData(rc_can) != DEVICE_OK) result = DEVICE_ERR;
    if (RC_CAN_SendStatusData(rc_can) != DEVICE_OK) result = DEVICE_ERR;
    
    // 更新发送时间和在线状态
    rc_can->last_send_time = (uint32_t)current_time;
    rc_can->header.online = true;
    rc_can->header.last_online_time = BSP_TIME_Get();
    
    return result;
}

int8_t RC_CAN_SendJoystickData(RC_CAN_t *rc_can) {
    if (rc_can == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    BSP_CAN_StdDataFrame_t tx_frame;
    tx_frame.id = RC_CAN_DR16_JOYSTICK_ID;
    tx_frame.dlc = sizeof(RC_CAN_JoystickData_t);
    
    memcpy(tx_frame.data, &rc_can->joystick_data, sizeof(RC_CAN_JoystickData_t));
    
    return BSP_CAN_TransmitStdDataFrame(rc_can->param.can, &tx_frame) == BSP_OK ? 
           DEVICE_OK : DEVICE_ERR;
}

int8_t RC_CAN_SendSwitchData(RC_CAN_t *rc_can) {
    if (rc_can == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    BSP_CAN_StdDataFrame_t tx_frame;
    tx_frame.id = RC_CAN_DR16_SWITCH_ID;
    tx_frame.dlc = sizeof(RC_CAN_SwitchData_t);
    
    memcpy(tx_frame.data, &rc_can->switch_data, sizeof(RC_CAN_SwitchData_t));
    
    return BSP_CAN_TransmitStdDataFrame(rc_can->param.can, &tx_frame) == BSP_OK ? 
           DEVICE_OK : DEVICE_ERR;
}

int8_t RC_CAN_SendMouseData(RC_CAN_t *rc_can) {
    if (rc_can == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    BSP_CAN_StdDataFrame_t tx_frame;
    tx_frame.id = RC_CAN_DR16_MOUSE_ID;
    tx_frame.dlc = sizeof(RC_CAN_MouseData_t);
    
    memcpy(tx_frame.data, &rc_can->mouse_data, sizeof(RC_CAN_MouseData_t));
    
    return BSP_CAN_TransmitStdDataFrame(rc_can->param.can, &tx_frame) == BSP_OK ? 
           DEVICE_OK : DEVICE_ERR;
}

int8_t RC_CAN_SendKeyboardData(RC_CAN_t *rc_can) {
    if (rc_can == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    BSP_CAN_StdDataFrame_t tx_frame;
    tx_frame.id = RC_CAN_DR16_KEYBOARD_ID;
    tx_frame.dlc = sizeof(RC_CAN_KeyboardData_t);
    
    memcpy(tx_frame.data, &rc_can->keyboard_data, sizeof(RC_CAN_KeyboardData_t));
    
    return BSP_CAN_TransmitStdDataFrame(rc_can->param.can, &tx_frame) == BSP_OK ? 
           DEVICE_OK : DEVICE_ERR;
}

int8_t RC_CAN_SendStatusData(RC_CAN_t *rc_can) {
    if (rc_can == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    BSP_CAN_StdDataFrame_t tx_frame;
    tx_frame.id = RC_CAN_DR16_STATUS_ID;
    tx_frame.dlc = sizeof(RC_CAN_StatusData_t);
    
    memcpy(tx_frame.data, &rc_can->status_data, sizeof(RC_CAN_StatusData_t));
    
    return BSP_CAN_TransmitStdDataFrame(rc_can->param.can, &tx_frame) == BSP_OK ? 
           DEVICE_OK : DEVICE_ERR;
}

int8_t RC_CAN_SetEnabled(RC_CAN_t *rc_can, bool enabled) {
    if (rc_can == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    rc_can->param.enabled = enabled;
    return DEVICE_OK;
}

bool RC_CAN_IsOnline(const RC_CAN_t *rc_can) {
    if (rc_can == NULL) {
        return false;
    }
    
    return rc_can->header.online;
}

int8_t RC_CAN_Update(RC_CAN_t *rc_can) {
    if (rc_can == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    if (!rc_can->initialized) {
        return DEVICE_ERR;
    }
    
    // 只在接收模式下处理接收
    if (rc_can->param.mode != RC_CAN_MODE_RECEIVER && rc_can->param.mode != RC_CAN_MODE_BOTH) {
        return DEVICE_OK;
    }
    
    BSP_CAN_Message_t rx_msg;
    int8_t result = DEVICE_OK;
    
    // 处理所有可能的接收ID
    uint32_t can_ids[] = {
        RC_CAN_DR16_JOYSTICK_ID,
        RC_CAN_DR16_SWITCH_ID,
        RC_CAN_DR16_MOUSE_ID,
        RC_CAN_DR16_KEYBOARD_ID,
        RC_CAN_DR16_STATUS_ID
    };
    
    for (int i = 0; i < 5; i++) {
        while (BSP_CAN_GetMessage(rc_can->param.can, can_ids[i], &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
            RC_CAN_ProcessRxMessage(rc_can, &rx_msg);
        }
    }
    
    return result;
}

int8_t RC_CAN_GetJoystickData(const RC_CAN_t *rc_can, float *ch_l_x, float *ch_l_y, 
                              float *ch_r_x, float *ch_r_y) {
    if (rc_can == NULL || ch_l_x == NULL || ch_l_y == NULL || 
        ch_r_x == NULL || ch_r_y == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    *ch_l_x = RC_CAN_UnscaleInt(rc_can->joystick_data.ch_l_x);
    *ch_l_y = RC_CAN_UnscaleInt(rc_can->joystick_data.ch_l_y);
    *ch_r_x = RC_CAN_UnscaleInt(rc_can->joystick_data.ch_r_x);
    *ch_r_y = RC_CAN_UnscaleInt(rc_can->joystick_data.ch_r_y);
    
    return DEVICE_OK;
}

int8_t RC_CAN_GetSwitchData(const RC_CAN_t *rc_can, uint8_t *sw_l, uint8_t *sw_r, 
                           float *ch_res) {
    if (rc_can == NULL || sw_l == NULL || sw_r == NULL || ch_res == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    *sw_l = rc_can->switch_data.sw_l;
    *sw_r = rc_can->switch_data.sw_r;
    *ch_res = RC_CAN_UnscaleInt(rc_can->switch_data.ch_res);
    
    return DEVICE_OK;
}

int8_t RC_CAN_GetMouseData(const RC_CAN_t *rc_can, int16_t *mouse_x, int16_t *mouse_y, 
                          int16_t *mouse_z, bool *mouse_l, bool *mouse_r) {
    if (rc_can == NULL || mouse_x == NULL || mouse_y == NULL || 
        mouse_z == NULL || mouse_l == NULL || mouse_r == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    *mouse_x = rc_can->mouse_data.mouse_x;
    *mouse_y = rc_can->mouse_data.mouse_y;
    *mouse_z = rc_can->mouse_data.mouse_z;
    *mouse_l = (rc_can->mouse_data.mouse_l != 0);
    *mouse_r = (rc_can->mouse_data.mouse_r != 0);
    
    return DEVICE_OK;
}

int8_t RC_CAN_GetKeyboardData(const RC_CAN_t *rc_can, uint16_t *key_value) {
    if (rc_can == NULL || key_value == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    *key_value = rc_can->keyboard_data.key_value;
    
    return DEVICE_OK;
}

int8_t RC_CAN_GetRemoteStatus(const RC_CAN_t *rc_can, bool *remote_online, 
                             bool *data_valid, uint32_t *timestamp) {
    if (rc_can == NULL || remote_online == NULL || 
        data_valid == NULL || timestamp == NULL) {
        return DEVICE_ERR_NULL;
    }
    
    *remote_online = (rc_can->status_data.online != 0);
    *data_valid = (rc_can->status_data.data_valid != 0);
    *timestamp = rc_can->status_data.timestamp;
    
    return DEVICE_OK;
}

bool RC_CAN_IsDataUpdated(RC_CAN_t *rc_can, uint8_t data_type) {
    if (rc_can == NULL || data_type > RC_CAN_DATA_STATUS) {
        return false;
    }
    
    bool updated = false;
    
    switch (data_type) {
        case RC_CAN_DATA_JOYSTICK:
            updated = rc_can->rx_status.joystick_updated;
            rc_can->rx_status.joystick_updated = false; // 清除更新标志
            break;
        case RC_CAN_DATA_SWITCH:
            updated = rc_can->rx_status.switch_updated;
            rc_can->rx_status.switch_updated = false;
            break;
        case RC_CAN_DATA_MOUSE:
            updated = rc_can->rx_status.mouse_updated;
            rc_can->rx_status.mouse_updated = false;
            break;
        case RC_CAN_DATA_KEYBOARD:
            updated = rc_can->rx_status.keyboard_updated;
            rc_can->rx_status.keyboard_updated = false;
            break;
        case RC_CAN_DATA_STATUS:
            updated = rc_can->rx_status.status_updated;
            rc_can->rx_status.status_updated = false;
            break;
        default:
            updated = false;
            break;
    }
    
    return updated;
}

bool RC_CAN_IsDataTimeout(const RC_CAN_t *rc_can) {
    if (rc_can == NULL) {
        return true;
    }
    
    uint32_t current_time = (uint32_t)(BSP_TIME_Get() / 1000); // 转换为ms
    uint32_t timeout_ms = rc_can->param.timeout_ms;
    
    // 检查所有数据类型是否超时
    bool timeout = false;
    
    if (current_time - rc_can->rx_status.last_joystick_time > timeout_ms) timeout = true;
    if (current_time - rc_can->rx_status.last_switch_time > timeout_ms) timeout = true;
    if (current_time - rc_can->rx_status.last_mouse_time > timeout_ms) timeout = true;
    if (current_time - rc_can->rx_status.last_keyboard_time > timeout_ms) timeout = true;
    if (current_time - rc_can->rx_status.last_status_time > timeout_ms) timeout = true;
    
    return timeout;
}

/* USER FUNCTION BEGIN */

/* USER FUNCTION END */