/**
 * @file motor_base.hpp
 * @brief 电机抽象基类 - C++多态实现
 */

#pragma once

#include <stdint.h>
#include "component/user_math.h"
#include "device/motor_lz.h"
#include "device/motor_dm.h"

namespace arm {

// ============================================================================
// Motor 抽象基类（接口）
// ============================================================================
class IMotor {
public:
    virtual ~IMotor() = default;
    
    // 纯虚函数（接口）
    virtual int8_t Register() = 0;
    virtual int8_t Enable() = 0;
    virtual int8_t Update() = 0;
    virtual int8_t Relax() = 0;
    
    virtual float GetAngle() const = 0;
    virtual float GetVelocity() const = 0;
    virtual float GetTorque() const = 0;
    virtual bool IsOnline() const = 0;
    
    virtual int8_t PositionControl(float angle, float velocity, float kp, float kd, float torque) = 0;
};

// ============================================================================
// LZ电机实现
// ============================================================================
class MotorLZ : public IMotor {
private:
    MOTOR_LZ_Param_t params_;
    MOTOR_LZ_t* motor_instance_;  // 指向内部管理器的实例
    
public:
    explicit MotorLZ(const MOTOR_LZ_Param_t& params) 
        : params_(params), motor_instance_(nullptr) {}
    
    int8_t Register() override {
        int8_t ret = MOTOR_LZ_Register(&params_);
        if (ret == DEVICE_OK) {
            motor_instance_ = MOTOR_LZ_GetMotor(&params_);
        }
        return ret;
    }
    
    int8_t Enable() override {
        return MOTOR_LZ_Enable(&params_);
    }
    
    int8_t Update() override {
        int8_t ret = MOTOR_LZ_Update(&params_);
        if (ret == DEVICE_OK && motor_instance_) {
            motor_instance_ = MOTOR_LZ_GetMotor(&params_);
        }
        return ret;
    }
    
    int8_t Relax() override {
        return MOTOR_LZ_Relax(&params_);
    }
    
    float GetAngle() const override {
        if (!motor_instance_) return 0.0f;
        
        // LZ电机范围: -4π ~ 4π，归一化到 -π ~ π
        float angle = motor_instance_->motor.feedback.rotor_abs_angle;
        // while (angle > M_PI) {
        //     angle -= 2.0f * M_PI;
        // }
        // while (angle < -M_PI) {
        //     angle += 2.0f * M_PI;
        // }
        return angle;
    }
    
    float GetVelocity() const override {
        return motor_instance_ ? motor_instance_->motor.feedback.rotor_speed : 0.0f;
    }
    
    float GetTorque() const override {
        return motor_instance_ ? motor_instance_->motor.feedback.torque_current : 0.0f;
    }
    
    bool IsOnline() const override {
        return motor_instance_ ? motor_instance_->motor.header.online : false;
    }
    
    int8_t PositionControl(float angle, float velocity, float kp, float kd, float torque) override {
        MOTOR_LZ_MotionParam_t output;
        output.target_angle = angle;
        output.target_velocity = velocity;
        output.kp = kp;
        output.kd = kd;
        output.torque = torque;
        return MOTOR_LZ_MotionControl(&params_, &output);
    }
};

// ============================================================================
// DM电机实现
// ============================================================================
class MotorDM : public IMotor {
private:
    MOTOR_DM_Param_t params_;
    MOTOR_DM_t* motor_instance_;
    
public:
    explicit MotorDM(const MOTOR_DM_Param_t& params) 
        : params_(params), motor_instance_(nullptr) {}
    
    int8_t Register() override {
        int8_t ret = MOTOR_DM_Register(&params_);
        if (ret == DEVICE_OK) {
            motor_instance_ = MOTOR_DM_GetMotor(&params_);
        }
        return ret;
    }
    
    int8_t Enable() override {
        return MOTOR_DM_Enable(&params_);
    }
    
    int8_t Update() override {
        int8_t ret = MOTOR_DM_Update(&params_);
        if (ret == DEVICE_OK && motor_instance_) {
            motor_instance_ = MOTOR_DM_GetMotor(&params_);
        }
        return ret;
    }
    
    int8_t Relax() override {
        return MOTOR_DM_Relax(&params_);
    }
    
    float GetAngle() const override {
        if (!motor_instance_) return 0.0f;
        
        // DM电机范围: 0 ~ 2π，归一化到 -π ~ π
        float angle = motor_instance_->motor.feedback.rotor_abs_angle;
        // if (angle > M_PI) {
        //     angle -= 2.0f * M_PI;  // 例如 1.5π → -0.5π
        // }
        return angle;
    }
    
    float GetVelocity() const override {
        return motor_instance_ ? motor_instance_->motor.feedback.rotor_speed : 0.0f;
    }
    
    float GetTorque() const override {
        return motor_instance_ ? motor_instance_->motor.feedback.torque_current : 0.0f;
    }
    
    bool IsOnline() const override {
        return motor_instance_ ? motor_instance_->motor.header.online : false;
    }
    
    int8_t PositionControl(float angle, float velocity, float kp, float kd, float torque) override {
        MOTOR_MIT_Output_t output;
        output.angle = angle;
        output.velocity = velocity;
        output.kp = kp;
        output.kd = kd;
        output.torque = torque;
        return MOTOR_DM_MITCtrl(&params_, &output);
    }
};

} // namespace arm