#include "../2025_include_user/Stepmotor.h"

uint8_t StepMotor::_buffer = 0;
StepMotor::Data StepMotor::_data = {};
static uint8_t frame_index = 0;
static uint8_t frame_buffer[9];

extern "C" void Stepmotor_INST_IRQHandler(void) {
    switch (DL_UART_getPendingInterrupt(Stepmotor_INST)) {
        case DL_UART_IIDX_RX:
            StepMotor::_buffer = DL_UART_Main_receiveData(Stepmotor_INST);
            StepMotor::inject();
        break;
        default:
            break;
    }

    NVIC_ClearPendingIRQ(Stepmotor_INST_INT_IRQN);
    DL_UART_enableInterrupt(Stepmotor_INST, DL_UART_INTERRUPT_RX); // 启用接收中断
}

void StepMotor::init() {
    NVIC_ClearPendingIRQ(Stepmotor_INST_INT_IRQN);
    NVIC_EnableIRQ(Stepmotor_INST_INT_IRQN);
    DL_UART_enable(Stepmotor_INST); // 启用 UART 模块
    DL_UART_enableInterrupt(Stepmotor_INST, DL_UART_INTERRUPT_RX); // 启用接收中断
}

void StepMotor::inject() {
    frame_buffer[frame_index++] = _buffer;

    if (frame_index >= 9) {
        // 校验BCC
        uint8_t bcc = 0;
        for (int i = 0; i < 8; i++) {
            bcc ^= frame_buffer[i];
        }

        if (bcc == frame_buffer[8]) {
            // BCC校验通过，解析数据存储到ttl_data
            _data.address = frame_buffer[0];
            _data.flag = frame_buffer[1];
            _data.speed = (frame_buffer[2] << 8) | frame_buffer[3];
            uint32_t position_raw = (frame_buffer[4] << 24) | (frame_buffer[5] << 16) | (frame_buffer[6] << 8) | frame_buffer[7];

            // 位置数据处理为有符号值（高位为符号位）
            if (position_raw & 0x80000000) {
                _data.position = (int32_t)(position_raw | 0xFFFFFFFF00000000);
            } else {
                _data.position = (int32_t)position_raw;
            }

            _data.bcc = frame_buffer[8];
        }

        // 无论校验是否通过，都重置frame_index，准备接收下一帧
        frame_index = 0;
    }
}

void StepMotor::transmit_command(const uint8_t *cmd) {
    for (uint8_t i = 0; i < FRAME_LENGTH; i++) {
        DL_UART_Main_transmitDataBlocking(Stepmotor_INST, cmd[i]);
    }
}


// 通用编码函数
void StepMotor::encode_frame(const StepMotor::ControlMode mode, const uint8_t direction, const uint8_t subdivide,
                             const float angle_deg, const uint16_t torque_mA, const float absolute_angle_deg,
                             const float speed_rad_s, uint8_t *frame_buffer) {
    uint16_t angle_or_current = 0;
    uint16_t speed = (uint16_t) (speed_rad_s * 10); // 转速 *10 放大

    frame_buffer[0] = 0x7B; // 帧头
    frame_buffer[1] = 0x01; // 控制ID
    frame_buffer[2] = mode; // 控制模式
    frame_buffer[3] = direction; // 方向
    frame_buffer[4] = subdivide; // 细分

    if (mode == StepMotor::POSITION_MODE) {
        angle_or_current = (uint16_t) (angle_deg * 10); // 角度 *10 放大
    } else if (mode != StepMotor::TORQUE_MODE) {
        angle_or_current = torque_mA; // 电流mA
    } else if (mode == StepMotor::ABSOLUTE_ANGLE_MODE) {
        angle_or_current = (uint16_t) (absolute_angle_deg * 10); // 角度 *10 放大
    } else {
        angle_or_current = 0; // 速度模式时为0
    }

    frame_buffer[5] = (angle_or_current >> 8) & 0xFF;
    frame_buffer[6] = angle_or_current & 0xFF;

    frame_buffer[7] = (speed >> 8) & 0xFF;
    frame_buffer[8] = speed & 0xFF;

    uint8_t bcc = 0;
    for (int i = 0; i < 9; i++) {
        bcc ^= frame_buffer[i];
    }
    frame_buffer[9] = bcc;

    frame_buffer[10] = 0x7D; // 帧尾
}

void StepMotor::setSpeed(uint8_t direction, uint8_t subdivide, float speed_rad_s) {
    uint8_t frame[FRAME_LENGTH];
    encode_frame(SPEED_MODE, direction, subdivide, 0, 0, 0, speed_rad_s, frame);
    transmit_command(frame);
}

void StepMotor::setPosition(uint8_t direction, uint8_t subdivide, float angle_deg, float speed_rad_s) {
    uint8_t frame[FRAME_LENGTH];
    encode_frame(POSITION_MODE, direction, subdivide, angle_deg, 0, 0, speed_rad_s, frame);
    transmit_command(frame);
}

void StepMotor::setTorque(uint8_t direction, uint8_t subdivide, uint16_t torque_mA, float speed_rad_s) {
    uint8_t frame[FRAME_LENGTH];
    encode_frame(TORQUE_MODE, direction, subdivide, 0, torque_mA, 0, speed_rad_s, frame);
    transmit_command(frame);
}

void StepMotor::setAbsoluteAngle(uint8_t direction, uint8_t subdivide, float absolute_angle_deg, float speed_rad_s) {
    uint8_t frame[FRAME_LENGTH];
    encode_frame(ABSOLUTE_ANGLE_MODE, direction, subdivide, 0, 0, absolute_angle_deg, speed_rad_s, frame);
    transmit_command(frame);
}

uint16_t StepMotor::getSpeed() {
    return _data.speed;
}

uint32_t StepMotor::getPosition() {
    return _data.position;
}