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

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

static float g_deadband   = 0.03f;
static float g_steer_gain = 1.5f;

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); }

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));
}

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