//
// Created by Administrator on 2026/4/22.
//

#ifndef SERVO_CONTROL_H
#define SERVO_CONTROL_H

#include <stdint.h>
#include <string>

#include <rtthread.h>

#include "common/curve/include/s_curve.h"
#include "common/type/servo_types.h"
#include "planner/s_curve_planner/include/s_curve_position_planner.h"
#include "servo_manager/include/servo_driver.h"

class ServoControl
{
public:
    explicit ServoControl(const ServoDriver::Config& cfg);
    ~ServoControl();

    rt_err_t init(bool enable_after_init = true, bool go_home = true);

    rt_err_t move(float angle_rad, MotionMode mode);
    rt_err_t stop();

    void setConstraints(double max_velocity_rad,
                        double max_acceleration_rad,
                        double max_jerk_rad);
    void setPositionGain(double position_gain);

    void update(rt_tick_t now_tick);

    const std::string& id() const;

    float currentAngle() const;
    float targetAngle() const;
    float currentVelocity() const;

    bool isActive() const;

private:
    void sampleStateNoLock(rt_tick_t now_tick);
    double ticksToSeconds(rt_tick_t delta_ticks) const;

private:
    static constexpr float ESP = 1e-4f;

    ServoDriver driver_;
    cmvr::SCurve curve_;
    cmvr::SCurvePositionPlanner1D follow_planner_;

    float current_angle_rad_;
    float current_velocity_rad_per_sec_;
    float target_angle_rad_;

    rt_tick_t start_tick_;
    rt_tick_t last_update_tick_;
    uint32_t generation_;
    MotionMode active_mode_;

    bool active_;
    bool dirty_;

    cmvr::SCurveProfile profile_;

    rt_mutex_t mutex_;
};

#endif // SERVO_CONTROL_H