//
// Created by iflyt on 2025/3/3.
//

#ifndef MOTOR_STATUS_H
#define MOTOR_STATUS_H


#include "cmsis_os2.h"
#include <stdint.h>

#include "elc_motor_helper.h"

// 读取速率档位
typedef enum {
    READ_RATE_LOW,
    READ_RATE_MEDIUM,
    READ_RATE_HIGH
} ReadRate;

typedef struct {
    uint8_t event_type; // 事件类型
    int32_t event_data; // 事件相关数据，如编码器位置
} HomeEvent;

typedef enum {
    EVENT_ORIGIN_LEAVE, // 触发原点离开事件
    EVENT_IN_ORIGIN, // 当前已经在原点
    EVENT_ORIGIN_ENTER, // 触发原点进入事件
    EVENT_ENCODER_BIG_DIFF, // 编码器位置与实际位置偏差过大
    EVENT_ENCODER_FAR_ORIGIN,   // 编码器位置距离原点较远
    EVENT_ENCODER_MODERATE_ORIGIN,  // 编码器位置合适的距离
    EVENT_ENCODER_NEAR_ORIGN,   // 编码器位置距离原点较近
    EVENT_MOTOR_MOVE_FINISHED,  // 电机移动结束
    EVENT_START_HOMING // 开始回原点事件
} HomeEventType;

// 定义回原点状态机枚举
typedef enum {
    STATE_INIT,
    STATE_BACK_TO_ORIGIN_HIGH, // 高速回原点
    STATE_BACK_TO_ORIGIN_MIDDLE, // 中速回原点
    STATE_BACK_TO_ORIGIN_LOW, // 低速回到原点
    STATE_LEAVE_ORIGIN_MIDDLE, // 低速离开原点
    STATE_FORWARD_TO_ORIGIN_MIDDLE, // 中速向前 (向前5mm)
    STATE_HOMED // 成功回到原点
} HomeState;

/**
 * Encoder Close Loop Motor Control
 */
class ECLMotor {
private:
    int32_t index_;  // 电机索引

    int32_t x_actual_;
    int32_t enc_val_;
    int32_t rt_speed_;       // 实时速度
    int32_t target_pos_;        // 目标位置
    int32_t target_speed_;      // 目标速度
    bool is_move_finished_;     // 是否已经移动结束

    osTimerId_t readTimer;
    osMutexId_t mutex_;

    bool isRunHomingTask { true };  // 是否运行回原点任务
    osThreadAttr_t home_task_attr = {0};
    osThreadId_t homingStateMachineThreadId;    // 回原点线程
    HomeEvent home_event_;
    osMessageQueueId_t homeEventQueue;  // 回原点事件队列
    HomeState home_state_{STATE_INIT};       // home 状态机


    bool is_home_init_{ false };       // 是否已经第一次（初始化回过原点）回过原点
    bool is_home_suc_ { false };        // 是否已经成功回原点

    uint8_t seq_id_home_ {0};       // 回 HOME 点 指令  SEQ ID
    uint8_t seq_id_move_ {0};       // 移动到指定到位置 SEQ ID

    static void readTimerCallback(void* arg);
public:
    ECLMotor();
    ~ECLMotor();
    void setMotorIndex(int32_t index);

    // 是否正在回原点的逻辑
    bool isHomeSuc();

    void startMoveHome();

    void setHomeSeqId(uint8_t seq_id);

    void setMoveFinishSeqId(uint8_t seq_id);

    int32_t getPosition();
    void setTargetPosition(int32_t target);
    void setShiftPosition(int32_t shift_target);

    void moveToWithSpeed(int32_t target, int32_t speed);

    void setSpeed(int32_t speed);

    bool isMoveFinished();
    int32_t getEncoderPosition();

    void moveToHome();

    void runZeroLimit(bool isEnter);     // 停止运动
    void runEndLimit(bool isEnter);     // 停止运动
    void runE_Stop();
    void runPause();
    void runStop();

    void ECL_Rotate(int32_t speed, const bool is_flip = true);

    void readMotorPosition();
    void startReadTimer();

    void setEncoderPosition(int32_t encoderPos);

    int32_t getRTSpeed();
private:
    void runhomeSuc();
    // 回原点任务
    static void homingTask(void *arg);

};


#endif //MOTOR_STATUS_H