#include "interface.h"
#include "drive_rs04.h"

// 限制值在最小值和最大值之间
float constrain(float value, float min_val, float max_val)
{
    if (value < min_val)
    {
        return min_val;
    }
    else if (value > max_val)
    {
        return max_val;
    }
    else
    {
        return value;
    }
}



/**
 * @brief 带死区的原始数据到物理量转换函数（简单版）
 * @param raw_value 原始16位无符号整数值 [0, 65535]
 * @param min 物理量最小值（如 -10.0）
 * @param max 物理量最大值（如 +10.0）
 * @param deadzone 死区范围（物理量单位，如 1.0 表示 ±1.0 内为死区）
 * @return 转换后的物理量值（若在死区内返回0，否则返回实际值）
 */
float convertPhysical(uint16_t raw_value, float min, float max, float deadzone) 
{
    // 1. 计算实际物理量值
    float physical_value = min + ((float)raw_value / 65535.0f) * (max - min);

    // 2. 判断是否在死区内（绝对值 ≤ deadzone）
    if (fabs(physical_value) <= deadzone) 
    {
        return 0.0f;  // 死区内返回0
    } 
    else 
    {
        return physical_value;  // 死区外返回实际值
    }
}


/**
 * @brief 将浮点数转换为uint32_t（按小端序存储）
 * @param num 输入的浮点数
 * @return 转换后的uint32_t值（直接内存拷贝结果）
 * @note 此函数通过内存直接拷贝实现转换，不进行数值计算，结果受平台字节序影响
 */
uint32_t floatToUint32(float num) 
{
    uint32_t result;
    // 将浮点数的内存数据直接拷贝到uint32_t变量
    memcpy(&result, &num, sizeof(num));
    return result;
}


/**
 * @brief 电机失能函数（停止电机运行）
 * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
 * @param master_id 主控制器ID (用于标识发送方)
 * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
 * @return 0: 成功, -1: 参数无效
 * @note 此函数会修改unsdodata指向的结构体内容，调用后需及时发送CAN报文
 */
int8_t motorDisable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata)
{
    /* 参数有效性检查 */
    if (unsdodata == NULL) {
        return -1;
    }

    /* 设置CAN报文ID域 */
    unsdodata->tx_can_id.bits.mode = 3;           /* 通信模式3：电机失能 */
    unsdodata->tx_can_id.bits.motor_id = motor_id;  /* 目标电机ID */
    unsdodata->tx_can_id.bits.res = 0;            /* 保留位清零 */
    unsdodata->tx_can_id.bits.data = master_id;   /* 主控制器ID */
    
    /* 清零数据域 */
    unsdodata->tx_can_data.bit_data.data = 0;
    unsdodata->tx_can_data.bit_data.index = 0;
    unsdodata->tx_can_data.bit_data.object_index = 0;    
    
    return 0;
}

/**
 * @brief 电机使能函数（启动电机运行）
 * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
 * @param master_id 主控制器ID (用于标识发送方)
 * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
 * @return 0: 成功, -1: 参数无效
 * @note 通信模式4：电机使能
 */
int8_t motorEnable(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata)
{
    /* 参数有效性检查 */
    if (unsdodata == NULL) {
        return -1;
    }

    /* 设置CAN报文ID域 */
    unsdodata->tx_can_id.bits.mode = 3;           /* 通信模式4：电机使能 */
    unsdodata->tx_can_id.bits.motor_id = motor_id;  /* 目标电机ID */
    unsdodata->tx_can_id.bits.res = 0;            /* 保留位清零 */
    unsdodata->tx_can_id.bits.data = master_id;   /* 主控制器ID */
    
    /* 清零数据域 */
    unsdodata->tx_can_data.bit_data.data = 0;
    unsdodata->tx_can_data.bit_data.index = 0;
    unsdodata->tx_can_data.bit_data.object_index = 0;  
    
    return 0;
}

/**
 * @brief 设置电机运行模式
 * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
 * @param master_id 主控制器ID (用于标识发送方)
 * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
 * @param mode 要设置的模式值 (具体含义需参考电机协议文档)
 * @return 0: 成功, -1: 参数无效
 * @note RUM_MODE应为预定义的宏，表示运行模式索引
 */
int8_t setMotorMode(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint8_t mode)
{
    /* 参数有效性检查 */
    if (unsdodata == NULL) {
        return -1;
    }

    /* 设置CAN报文ID域 */
    unsdodata->tx_can_id.bits.mode = 0x12;        /* 通信模式0x12：参数写入 */
    unsdodata->tx_can_id.bits.motor_id = motor_id;  /* 目标电机ID */
    unsdodata->tx_can_id.bits.res = 0;            /* 保留位清零 */
    unsdodata->tx_can_id.bits.data = master_id;   /* 主控制器ID */
    
    /* 设置数据域 */
    unsdodata->tx_can_data.bit_data.index = RUM_MODE;      /* 运行模式索引 */
    unsdodata->tx_can_data.bit_data.object_index = 0;      /* 子索引通常为0 */
    unsdodata->tx_can_data.bit_data.data = mode;           /* 模式值 */
    
    return 0;
}

/**
 * @brief 写入电机参数
 * @param motor_id 目标电机ID (范围取决于系统设计，通常0-255)
 * @param master_id 主控制器ID (用于标识发送方)
 * @param unsdodata 指向UnSdoOutput联合体的指针，用于填充CAN报文数据
 * @param index 要写入的参数索引 (具体含义需参考电机协议文档)
 * @param ref 要写入的参数值 (浮点数，会自动转换为uint32_t)
 * @return 0: 成功, -1: 参数无效
 * @note 使用floatToUint32函数转换浮点参数
 */
int8_t setMotorWrite(uint8_t master_id, uint8_t motor_id, StrTxCanFrame *unsdodata, uint16_t index, float ref)
{
    /* 参数有效性检查 */
    if (unsdodata == NULL) {
        return -1;
    }

    /* 设置CAN报文ID域 */
    unsdodata->tx_can_id.bits.mode = 0x12;        /* 通信模式0x12：参数写入 */
    unsdodata->tx_can_id.bits.motor_id = motor_id;  /* 目标电机ID */
    unsdodata->tx_can_id.bits.res = 0;            /* 保留位清零 */
    unsdodata->tx_can_id.bits.data = master_id;   /* 主控制器ID */
    
    /* 设置数据域 */
    unsdodata->tx_can_data.bit_data.index = index;         /* 参数索引 */
    unsdodata->tx_can_data.bit_data.object_index = 0;      /* 子索引通常为0 */
    unsdodata->tx_can_data.bit_data.data = floatToUint32(ref); /* 转换并写入参数值 */
    
    return 0;
}


/**
 * @brief 动态斜率限制（支持变时间间隔）
 * @param last_command   上一次的电流指令值
 * @param target_current 本次目标电流指令
 * @param delta_time     距离上一次调用的时间间隔 (s)
 * @return              限制后的安全电流指令
 */
float dynamic_current_limit(float *last_command, float target_current, float delta_time) 
{
    // 计算期望的变化量
    float desired_change = target_current - *last_command;

    // 计算两种限制
    float step_limit = MAX_STEP;
    float time_limit = MAX_DI_DT * delta_time; // 动态计算时间限制

    // 选择更严格的限制
    float max_allowed_change = (step_limit < time_limit) ? step_limit : time_limit;

    // 应用限制并返回新指令
    float actual_change = constrain(desired_change, -max_allowed_change, max_allowed_change);
    *last_command = *last_command + actual_change;//更新过去值

    return *last_command; 
}