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

#include "servo_manager/include/servo_driver.h"

ServoDriver::ServoDriver(const Config& cfg, float limit_min_angle_rad, float limit_max_angle_rad)
    : cfg_(cfg),
      limit_min_angle_rad_(limit_min_angle_rad),
      limit_max_angle_rad_(limit_max_angle_rad),
      pwm_dev_(RT_NULL),
      last_pulse_(0),
      inited_(false),
      mutex_(RT_NULL)
{
    mutex_ = rt_mutex_create("sv_drv", RT_IPC_FLAG_PRIO);
}

ServoDriver::~ServoDriver()
{
    if (mutex_) {
        rt_mutex_delete(mutex_);
        mutex_ = RT_NULL;
    }
}

rt_err_t ServoDriver::init(float initial_angle_rad, bool enable_after_init)
{
    if (cfg_.pwm_dev_name.empty()) {
        rt_kprintf("[ServoDriver][E] init: invalid cfg, pwm is empty\n");
        return -RT_EINVAL;
    }

    if (!mutex_) {
        return -RT_ENOMEM;
    }

    rt_mutex_take(mutex_, RT_WAITING_FOREVER);

    pwm_dev_ = (rt_device_pwm*)rt_device_find(cfg_.pwm_dev_name.c_str());
    if (!pwm_dev_) {
        rt_kprintf("[ServoDriver][E] init: rt_device_find failed, pwm=%s ch=%d\n",
                   cfg_.pwm_dev_name.c_str(),
                   cfg_.pwm_channel);
        rt_mutex_release(mutex_);
        return -RT_ENOSYS;
    }

    const float limited_angle = clampf(initial_angle_rad, limit_min_angle_rad_, limit_max_angle_rad_);
    const rt_uint32_t pulse = angleToPulseNs(cfg_, limited_angle);

    rt_err_t err = rt_pwm_set(pwm_dev_, cfg_.pwm_channel, cfg_.period_ns, pulse);
    if (err != RT_EOK) {
        rt_kprintf("[ServoDriver][E] init: rt_pwm_set failed, ch=%d pulse=%u err=%d\n",
                   cfg_.pwm_channel,
                   (unsigned)pulse,
                   (int)err);
        rt_mutex_release(mutex_);
        return err;
    }

    last_pulse_ = pulse;
    inited_ = true;

    if (enable_after_init) {
        err = rt_pwm_enable(pwm_dev_, cfg_.pwm_channel);
        if (err != RT_EOK) {
            rt_kprintf("[ServoDriver][E] init: rt_pwm_enable failed, ch=%d err=%d\n",
                       cfg_.pwm_channel,
                       (int)err);
            rt_mutex_release(mutex_);
            return err;
        }
    }

    rt_mutex_release(mutex_);

    rt_kprintf("[ServoDriver] init OK: pwm=%s ch=%d enable=%d initial=%.4f\n",
               cfg_.pwm_dev_name.c_str(),
               cfg_.pwm_channel,
               (int)enable_after_init,
               (double)initial_angle_rad);

    return RT_EOK;
}

rt_err_t ServoDriver::enable(bool on)
{
    if (!mutex_) {
        return -RT_ENOMEM;
    }

    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
    const rt_err_t err = enableNoLock(on);
    rt_mutex_release(mutex_);
    return err;
}

rt_err_t ServoDriver::setAngle(float angle_rad)
{
    if (!mutex_) {
        return -RT_ENOMEM;
    }

    rt_mutex_take(mutex_, RT_WAITING_FOREVER);
    const rt_err_t err = setAngleNoLock(angle_rad);
    rt_mutex_release(mutex_);
    return err;
}

const ServoDriver::Config& ServoDriver::config() const
{
    return cfg_;
}

rt_err_t ServoDriver::enableNoLock(bool on)
{
    if (!inited_ || !pwm_dev_) {
        return -RT_ERROR;
    }

    return on ? rt_pwm_enable(pwm_dev_, cfg_.pwm_channel)
              : rt_pwm_disable(pwm_dev_, cfg_.pwm_channel);
}

rt_err_t ServoDriver::setAngleNoLock(float angle_rad)
{
    if (!inited_ || !pwm_dev_) {
        return -RT_ERROR;
    }

    const float limited_angle = clampf(angle_rad, limit_min_angle_rad_, limit_max_angle_rad_);
    const rt_uint32_t pulse = angleToPulseNs(cfg_, limited_angle);
    if (pulse == last_pulse_) {
        return RT_EOK;
    }

    const rt_err_t err = rt_pwm_set(pwm_dev_, cfg_.pwm_channel, cfg_.period_ns, pulse);
    if (err != RT_EOK) {
        return err;
    }

    last_pulse_ = pulse;
    return RT_EOK;
}

float ServoDriver::clampf(float v, float lo, float hi)
{
    if (v < lo) {
        return lo;
    }
    if (v > hi) {
        return hi;
    }
    return v;
}

rt_uint32_t ServoDriver::angleToPulseNs(const Config& cfg, float angle_rad)
{
    const float a = clampf(angle_rad, cfg.min_angle_rad, cfg.max_angle_rad);

    const float in_span = cfg.max_angle_rad - cfg.min_angle_rad;
    const float out_span = (float)(cfg.max_pulse_ns - cfg.min_pulse_ns);

    float t = 0.0f;
    if (in_span > 1e-6f) {
        t = (a - cfg.min_angle_rad) / in_span;
    }

    float pulse = (float)cfg.min_pulse_ns + t * out_span;

    if (pulse < (float)cfg.min_pulse_ns) {
        pulse = (float)cfg.min_pulse_ns;
    }
    if (pulse > (float)cfg.max_pulse_ns) {
        pulse = (float)cfg.max_pulse_ns;
    }

    return (rt_uint32_t)(pulse + 0.5f);
}
