#ifndef DMMOTOR_H
#define DMMOTOR_H

#include <stdint.h>
#include "bsp_can.h"
#include "controller.h"
#include "motor_def.h"
#include "daemon.h"

#define DM_MOTOR_CNT 4

/* DM motor parameter ranges (from reference DM_6220 driver) */
#define DM_P_MIN  (-12.5f)
#define DM_P_MAX  12.5f
#define DM_V_MIN  (-45.0f)
#define DM_V_MAX  45.0f
#define DM_T_MIN  (-10.0f)
#define DM_T_MAX  10.0f
#define DM_KP_MIN 0.0f
#define DM_KP_MAX 500.0f
#define DM_KD_MIN 0.0f
#define DM_KD_MAX 5.0f

/* DM motor control modes */
/* DM motor command codes */
typedef enum
{
    DM_CMD_ENABLE_MODE   = 0xFC,  // enable motor
    DM_CMD_DISABLE_MODE  = 0xFD,  // disable motor
    DM_CMD_ZERO_POSITION = 0xFE,  // set current position as encoder zero
    DM_CMD_CLEAR_ERROR   = 0xFB,  // clear motor error
} DM_Motor_Mode_e;

/* DM motor control modes */
typedef enum
{
    DM_MODE_MIT       = 0,  // MIT mode: CAN ID, packed pos/vel/KP/KD/torque
    DM_MODE_POS_VEL   = 1,  // Position-Velocity mode: CAN ID + 0x100, float pos + float vel
    DM_MODE_VELOCITY  = 2,  // Velocity mode: CAN ID + 0x200, float velocity only
} DM_Motor_Ctrl_Mode_e;

/* DM motor measurement data (parsed from feedback CAN frame) */
typedef struct
{
    uint8_t id;            // motor ID from feedback byte[0:3]
    uint8_t state;         // motor state from feedback byte[7:4]: 0=reset, 1=run
    float position;        // current position (rad, range ±12.5)
    float velocity;        // current velocity (rad/s, range ±45)
    float torque;          // current torque (N·m, range ±10)
    float T_Mos;           // MOS temperature (Celsius)
    float T_Rotor;         // rotor temperature (Celsius)
    float last_position;   // previous position for multi-turn calculation
    int32_t total_round;   // accumulated full revolutions
    float total_angle;     // multi-turn angle (degrees)
} DM_Motor_Measure_s;

typedef struct{
    float Kp;
    Output_Reverse_Flag_e dir;
    float feed_input;
} DM_Feedforward_Controller_s;

/* DM motor instance (runtime state) */
typedef struct
{
    DM_Motor_Measure_s measure;
    Motor_Controller_s motor_controller;
    DM_Feedforward_Controller_s feedforward_controller;

    CANInstance *motor_can_instance;  // CAN communication instance
    DaemonInstance *motor_daemon;     // daemon watchdog
    Output_Reverse_Flag_e output_reverse; // output reverse flag
    DM_Motor_Ctrl_Mode_e ctrl_mode;        // current control mode (MIT / Pos-Vel / Velocity)
    DM_Motor_Mode_e motor_mode;         // last command mode (enable/disable/zero/clear error)
    float mit_kp;                     // MIT mode KP (motor internal position gain, 0~500)
    float mit_kd;                     // MIT mode KD (motor internal velocity gain, 0~5)

} DMMotorInstance;

/* Feedforward controller init config */
typedef struct
{
    float Kp;
    Output_Reverse_Flag_e dir;
} DM_Feedforward_Init_Config_s;

/* DM motor init config */
typedef struct
{
    Motor_Init_Config_s motor_config;   // generic motor PID and setting config
    DM_Motor_Ctrl_Mode_e ctrl_mode;          // default control mode
    DM_Feedforward_Init_Config_s feedforward_config;
    float mit_kp;                       // MIT mode KP (only for MIT mode)
    float mit_kd;                       // MIT mode KD (only for MIT mode)
} DM_Motor_Init_Config_s;

/* ---- API ---- */

DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config);

void DMMotorSetRef(DMMotorInstance *motor, float ref);
void DMMotorSetMode(DM_Motor_Mode_e mode, DMMotorInstance *motor);

/**
 * @brief Main control function called from Motor Task at 1kHz.
 *        Iterates all registered motors, runs PID cascade, sends CAN frames.
 */
void DMMotorControl(void);

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

/* MIT mode: send position + velocity + KP + KD + torque in packed 8-byte frame */
void DMMotorSendMIT(CANInstance *can, float pos, float vel, float kp, float kd, float torq);

/* Position-Velocity mode: send float position + float velocity */
void DMMotorSendPosVel(CANInstance *can, float pos, float vel);

/* Velocity mode: send float velocity only */
void DMMotorSendVelocity(CANInstance *can, float vel);

#endif // DMMOTOR_H