#ifndef STEPMOTOR_H
#define STEPMOTOR_H

#define FRAME_LENGTH 11

#include "ti_msp_dl_config.h"
#include <../../example/2025_include_user/Print.h>

class StepMotor {
public:
    typedef enum {
        SPEED_MODE = 1,
        POSITION_MODE,
        TORQUE_MODE,
        ABSOLUTE_ANGLE_MODE
    } ControlMode;

    struct Data {
        uint8_t address;
        uint8_t flag;
        uint16_t speed;
        int32_t position;
        uint8_t bcc;
    };

    static void init();
    static void inject();

    static void setSpeed(uint8_t direction, uint8_t subdivide, float speed_rad_s);
    static void setPosition(uint8_t direction, uint8_t subdivide, float angle_deg, float speed_rad_s);
    static void setTorque(uint8_t direction, uint8_t subdivide, uint16_t torque_mA, float speed_rad_s);
    static void setAbsoluteAngle(uint8_t direction, uint8_t subdivide, float absolute_angle_deg, float speed_rad_s);

    static uint16_t getSpeed();
    static uint32_t getPosition();

    static uint8_t _buffer;
    static Data _data;

private:
    static void transmit_command(const uint8_t *cmd);
    static void encode_frame(ControlMode mode, uint8_t direction, uint8_t subdivide, float angle_deg, uint16_t torque_mA, float absolute_angle_deg, float speed_rad_s, uint8_t *frame_buffer);
};

#endif //STEPMOTOR_H