#include "servo.hpp"
#include "hal/pwm.hpp"
#include <algorithm>
#include <cstdio>
#include <cmath>

static PWM* servo_pwm = nullptr;
static bool serv_initialized = false;

static float g_deadband   = 0.03f;    // 死区 (归一化偏差)
static float g_steer_gain = 1.5f;     // 转向增益
static constexpr float HALF_RANGE = 300000.0f;  // 半行程 (ns), ±0.30ms

void servo_init()
{
    if (serv_initialized) return;
    servo_pwm = new PWM(SERVO_PWMCHIP, SERVO_PWMID, SERVO_PERIOD_NS);
    servo_pwm->setDutyCycle(SERVO_CENTER_NS);
    servo_pwm->enable();
    serv_initialized = true;
}

void servo_set_deadband(float v)   { g_deadband = std::max(0.0f, v); }
void servo_set_steer_gain(float v) { g_steer_gain = std::max(0.1f, v); }
void servo_set_pid(double, double, double) {}  // 保留接口兼容, 不再使用 PID

// 纯比例转向: deviation(-1~+1) → 线性映射到舵机脉宽范围
// deviation=0   → 中位 1.50ms
// deviation=±1  → 极限 1.20ms / 1.80ms
// steer_gain 控制灵敏度 (1.0=标准, 1.5=灵敏, 0.5=柔和)
void servo_set_angle(float deviation)
{
    if (!serv_initialized) return;
    deviation = std::clamp(deviation, -1.0f, 1.0f);

    if (std::abs(deviation) < g_deadband)
    {
        servo_pwm->setDutyCycle(SERVO_CENTER_NS);
        return;
    }

    float offset = deviation * g_steer_gain * HALF_RANGE;
    float duty_ns = SERVO_CENTER_NS + offset;
    duty_ns = std::clamp(duty_ns, 1200000.0f, 1800000.0f);
    servo_pwm->setDutyCycle(static_cast<unsigned int>(duty_ns));
}

void servo_center()
{
    if (serv_initialized)
        servo_pwm->setDutyCycle(SERVO_CENTER_NS);
}