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

// ── 全局对象 ──────────────────────────────────────────
static PWM* servo_pwm = nullptr;      // pwmchip1/pwm0, 3ms 周期
static PID* servo_pid = nullptr;      // 位置式 PID, 偏差→脉宽偏移量 (ns)
static bool serv_initialized = false;

// ── 运行时热调参数 ──────────────────────────────────
// 这些值会被 scheduler.cpp 每 1 秒从文件热读覆盖
static float g_deadband   = 0.03f;    // 死区 (归一化偏差, 默认 ±0.03)
static float g_steer_gain = 1.5f;     // 转向增益 (偏差放大系数)

// ============================================================
// servo_init — 初始化舵机 PWM + PID
//
// 调用时机: scheduler_init() 启动时调用一次
//
// 硬件: pwmchip1/pwm0
//   周期 = 3ms (333Hz)    — 舵机标准周期
//   中位 = 1.52ms         — 前轮摆正
//   行程 = 1.52 ± 0.21ms  — 约 ±45° 物理摆角
//
// PID: 位置式
//   Kp=400000, Ki=200000, Kd=0
//   输出限幅 = ±SERVO_MAX_DELTA_NS (±210000ns = ±0.21ms)
//   这组参数等效于线性增益 (因为 Ki 很大, 积分瞬间到位)
// ============================================================
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();

    servo_pid = new PID(400000, 200000, 0, 0, PID_POSITIONAL,
                        static_cast<float>(SERVO_MAX_DELTA_NS),
                        static_cast<float>(SERVO_MAX_DELTA_NS));

    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 kp, double ki, double kd) { if (servo_pid) servo_pid->setPID(kp, ki, kd); }

// ============================================================
// servo_set_angle — 偏差 → 舵机脉宽 (核心控制回路)
//
// 调用频率: scheduler.cpp sched_control() 每 5ms 一次
//
// 输入: deviation  — 归一化中线偏差 (-1=偏左极限, 0=正中, +1=偏右极限)
//        由 strategy/deviation.cpp 的 calc_deviation() 计算
//
// 处理流程:
//   1. 钳位偏差至 [-1, +1]
//   2. 死区: |deviation| < g_deadband → 直接回中, 跳过 PID
//      典型值 g_deadband=0.03, 即偏差 < 3% 时不转向
//   3. 增益缩放: deviation *= g_steer_gain
//      典型值 g_steer_gain=1.5, 把微小偏差放大供 PID 处理
//   4. PID 计算: out = PID(deviation)
//      位置式 PID, 输出为脉宽偏移量 (ns)
//   5. 叠加中位: duty_ns = 1520000 + out
//      正偏差(偏右) → out>0 → 脉宽>中位 → 舵机右转
//      负偏差(偏左) → out<0 → 脉宽<中位 → 舵机左转
//   6. 限幅: 1.52 ± 0.21ms (1310000ns ~ 1730000ns)
//   7. 写入 PWM sysfs
// ============================================================
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 scaled = deviation * g_steer_gain;
    float out = -servo_pid->update(scaled);  // 负号: 偏差正(右偏)→输出负→左转回正
    float duty_ns = SERVO_CENTER_NS + out;
    duty_ns = std::clamp(duty_ns,
                         static_cast<float>(SERVO_CENTER_NS - SERVO_MAX_DELTA_NS),
                         static_cast<float>(SERVO_CENTER_NS + SERVO_MAX_DELTA_NS));
    servo_pwm->setDutyCycle(static_cast<unsigned int>(duty_ns));
}

// servo_center — 紧急回中 (停止时调用)
void servo_center()
{
    if (serv_initialized)
        servo_pwm->setDutyCycle(SERVO_CENTER_NS);
}