#include "dmmotor.h"
#include "general_def.h"
#include "string.h"
#include "daemon.h"
#include "stdlib.h"
#include "bsp_log.h"
#include "bsp_dwt.h"

static uint8_t idx;
static DMMotorInstance *dm_motor_instance[DM_MOTOR_CNT];

/* ---- float <-> uint conversion for MIT mode ---- */

static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
{
    float span = x_max - x_min;
    float offset = x_min;
    return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span);
}

static float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
    float span = x_max - x_min;
    float offset = x_min;
    return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
}

/* ---- Low-level CAN send functions (all three modes) ---- */

/* MIT mode: CAN ID = motor_id */
void DMMotorSendMIT(CANInstance *can, float pos, float vel, float kp, float kd, float torq)
{
    uint16_t pos_uint  = float_to_uint(pos,  DM_P_MIN, DM_P_MAX, 16);
    uint16_t vel_uint  = float_to_uint(vel,  DM_V_MIN, DM_V_MAX, 12);
    uint16_t kp_uint   = float_to_uint(kp,   DM_KP_MIN, DM_KP_MAX, 12);
    uint16_t kd_uint   = float_to_uint(kd,   DM_KD_MIN, DM_KD_MAX, 12);
    uint16_t torq_uint = float_to_uint(torq, DM_T_MIN, DM_T_MAX, 12);

    uint8_t *buf = can->tx_buff;
    buf[0] = (uint8_t)(pos_uint >> 8);
    buf[1] = (uint8_t)(pos_uint);
    buf[2] = (uint8_t)(vel_uint >> 4);
    buf[3] = (uint8_t)(((vel_uint & 0xF) << 4) | (kp_uint >> 8));
    buf[4] = (uint8_t)(kp_uint);
    buf[5] = (uint8_t)(kd_uint >> 4);
    buf[6] = (uint8_t)(((kd_uint & 0xF) << 4) | (torq_uint >> 8));
    buf[7] = (uint8_t)(torq_uint);

    can->txconf.Identifier = can->tx_id;
    CANTransmit(can, 1);
}

/* Position-Velocity mode: CAN ID = motor_id + 0x100 */
void DMMotorSendPosVel(CANInstance *can, float pos, float vel)
{
    uint8_t *buf = can->tx_buff;
    uint8_t *pos_bytes = (uint8_t *)&pos;
    uint8_t *vel_bytes = (uint8_t *)&vel;

    buf[0] = pos_bytes[0];
    buf[1] = pos_bytes[1];
    buf[2] = pos_bytes[2];
    buf[3] = pos_bytes[3];
    buf[4] = vel_bytes[0];
    buf[5] = vel_bytes[1];
    buf[6] = vel_bytes[2];
    buf[7] = vel_bytes[3];

    can->txconf.Identifier = can->tx_id + 0x100;
    CANTransmit(can, 1);
}

/* Velocity mode: CAN ID = motor_id + 0x200 */
void DMMotorSendVelocity(CANInstance *can, float vel)
{
    uint8_t *buf = can->tx_buff;
    uint8_t *vel_bytes = (uint8_t *)&vel;

    buf[0] = vel_bytes[0];
    buf[1] = vel_bytes[1];
    buf[2] = vel_bytes[2];
    buf[3] = vel_bytes[3];
    buf[4] = 0;
    buf[5] = 0;
    buf[6] = 0;
    buf[7] = 0;

    can->txconf.Identifier = can->tx_id + 0x200;
    CANTransmit(can, 1);
}

/* ---- Motor commands (enable/disable/zero/clear error) ---- */

void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor)
{
    memset(motor->motor_can_instance->tx_buff, 0xFF, 7);
    motor->motor_can_instance->tx_buff[7] = (uint8_t)cmd;
    motor->motor_can_instance->txconf.Identifier = motor->motor_can_instance->tx_id;
    CANTransmit(motor->motor_can_instance, 1);
}

/* ---- Feedback decode: CAN RX interrupt callback ---- */

static void DMMotorDecode(CANInstance *motor_can)
{
    uint8_t *rxbuff = motor_can->rx_buff;
    DMMotorInstance *motor = (DMMotorInstance *)motor_can->id;
    DM_Motor_Measure_s *measure = &motor->measure;
    uint16_t tmp;

    //DaemonReload(motor->motor_daemon);

    measure->id    = rxbuff[0] & 0x0F;
    measure->state = (rxbuff[0] >> 4) & 0x0F;

    measure->last_position = measure->position;

    tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
    measure->position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16);

    tmp = (uint16_t)((rxbuff[3] << 4) | (rxbuff[4] >> 4));
    measure->velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12);

    tmp = (uint16_t)(((rxbuff[4] & 0x0F) << 8) | rxbuff[5]);
    measure->torque = uint_to_float(tmp, DM_T_MIN, DM_T_MAX, 12);

    measure->T_Mos   = (float)rxbuff[6];
    measure->T_Rotor = (float)rxbuff[7];

    /* multi-turn tracking */
    if (measure->position - measure->last_position > 10.0f)
        measure->total_round--;
    else if (measure->position - measure->last_position < -10.0f)
        measure->total_round++;

    measure->total_angle = (measure->position + measure->total_round * 25.1327f) * RAD_2_DEGREE;
    measure->velocity    = measure->velocity * RAD_2_DEGREE;
}

static void DMMotorLostCallback(void *motor_ptr)
{
    DMMotorInstance *motor = (DMMotorInstance *)motor_ptr;
    LOGWARNING("[DMmotor] Motor lost, id [%d]", motor->motor_can_instance->tx_id);
}

/* ---- Init ---- */

DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config)
{
    DMMotorInstance *motor = (DMMotorInstance *)malloc(sizeof(DMMotorInstance));
    memset(motor, 0, sizeof(DMMotorInstance));

    motor->output_reverse = config->motor_config.output_reverse;
    motor->ctrl_mode = config->ctrl_mode;
    motor->mit_kp = config->mit_kp;
    motor->mit_kd = config->mit_kd;

    /* init angle PID controller */
    PIDInit(&motor->motor_controller.angle_PID, &config->motor_config.controller_param_init_config.angle_PID);

    motor->motor_controller.other_angle_feedback_ptr  = config->motor_config.controller_param_init_config.other_angle_feedback_ptr;
    motor->motor_controller.other_speed_feedback_ptr   = config->motor_config.controller_param_init_config.other_speed_feedback_ptr;

    /* register CAN instance */
    config->motor_config.can_init_config.can_module_callback = DMMotorDecode;
    config->motor_config.can_init_config.id = motor;
    motor->motor_can_instance = CANRegister(&config->motor_config.can_init_config);

    /* register daemon */
    Daemon_Init_Config_s conf = {
        .callback = DMMotorLostCallback,
        .owner_id = motor,
        .reload_count = 10,
    };
    //motor->motor_daemon = DaemonRegister(&conf);

    /* enable motor */
    DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);

    dm_motor_instance[idx++] = motor;
    return motor;
}

/* ---- Runtime control ---- */

void DMMotorSetRef(DMMotorInstance *motor, float ref)
{
    motor->motor_controller.pid_ref = ref;
}

void DMMotorSetCtrlMode(DMMotorInstance *motor, DM_Motor_Mode_e mode)
{
    motor->ctrl_mode = mode;
}

/* ---- Main control: called from Motor Task at 1kHz ---- */
/* 硬编码: IMU 角度反馈 → 角度 PID → 速度指令 → 电机速度模式 */

void DMMotorControl(void)
{
    for (uint8_t i = 0; i < idx; i++)
    {
        DMMotorInstance *motor = dm_motor_instance[i];
        CANInstance *can = motor->motor_can_instance;

        /* handle motor error states */
        if (motor->measure.state != 0x00 && motor->measure.state != 0x01)
        {
            DMMotorSetMode(DM_CMD_CLEAR_ERROR, motor);
            DWT_Delay(0.001);
            DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
            continue;
        }

        /* angle PID: IMU feedback → velocity output */
        float pid_ref = PIDCalculate(&motor->motor_controller.angle_PID,
                                     *motor->motor_controller.other_angle_feedback_ptr,
                                     motor->motor_controller.pid_ref);

        if (motor->output_reverse == OUTPUT_DIRECTION_REVERSE)
            pid_ref *= -1;

        float vel_cmd = pid_ref * DEGREE_2_RAD;
        LIMIT_MIN_MAX(vel_cmd, DM_V_MIN, DM_V_MAX);

        switch (motor->ctrl_mode)
        {
        case DM_MODE_VELOCITY:{
            DMMotorSendVelocity(can, vel_cmd);
            break;
        }
        case DM_MODE_MIT: {
            float pos_cmd = pid_ref * DEGREE_2_RAD;
            LIMIT_MIN_MAX(pos_cmd, DM_P_MIN, DM_P_MAX);
            DMMotorSendMIT(can, pos_cmd, 0.0f, motor->mit_kp, motor->mit_kd, 0.0f);
            break;
        }
        case DM_MODE_POS_VEL: {
            float pos_cmd = pid_ref * DEGREE_2_RAD;
            LIMIT_MIN_MAX(pos_cmd, DM_P_MIN, DM_P_MAX);
            DMMotorSendPosVel(can, pos_cmd, vel_cmd);
            break;
        }
        }
    }
}