/*
 * steering_pid.c — closed-loop yaw-rate PID for differential drive (Issue #616).
 *
 * Converts Jetson Twist.angular.z (encoded as steer * STEER_OMEGA_SCALE deg/s)
 * into a differential wheel speed offset using IMU gyro Z as feedback.
 *
 * Anti-windup:   integral clamped to ±STEER_INTEGRAL_MAX before Ki multiply.
 * Rate limiter:  output changes at most STEER_RAMP_RATE_PER_MS per ms so that
 *                a step in steering demand cannot disturb the balance PID.
 */

#include "steering_pid.h"
#include "jlink.h"

/* ---- steering_pid_init() ---- */
void steering_pid_init(steering_pid_t *s)
{
    s->target_omega_dps = 0.0f;
    s->actual_omega_dps = 0.0f;
    s->integral         = 0.0f;
    s->prev_error       = 0.0f;
    s->output           = 0;
    s->enabled          = true;
    /* Initialize so first send_tlm call fires immediately */
    s->last_tlm_ms = (uint32_t)(-(uint32_t)(1000u / (STEER_TLM_HZ > 0u ? STEER_TLM_HZ : 1u)));
}

/* ---- steering_pid_reset() ---- */
void steering_pid_reset(steering_pid_t *s)
{
    s->target_omega_dps = 0.0f;
    s->integral         = 0.0f;
    s->prev_error       = 0.0f;
    s->output           = 0;
}

/* ---- steering_pid_set_target() ---- */
void steering_pid_set_target(steering_pid_t *s, float omega_dps)
{
    if (!s->enabled) return;
    s->target_omega_dps = omega_dps;
}

/* ---- steering_pid_update() ---- */
int16_t steering_pid_update(steering_pid_t *s, float actual_omega_dps, float dt)
{
    if (!s->enabled || dt <= 0.0f) {
        s->output = 0;
        return 0;
    }

    s->actual_omega_dps = actual_omega_dps;

    /* PID error */
    float error = s->target_omega_dps - actual_omega_dps;

    /* Proportional */
    float p_term = STEER_KP * error;

    /* Integral with anti-windup clamp */
    s->integral += error * dt;
    if (s->integral >  STEER_INTEGRAL_MAX) s->integral =  STEER_INTEGRAL_MAX;
    if (s->integral < -STEER_INTEGRAL_MAX) s->integral = -STEER_INTEGRAL_MAX;
    float i_term = STEER_KI * s->integral;

    /* Derivative on error (avoids setpoint kick for smooth yaw changes) */
    float d_term = 0.0f;
    if (dt > 0.0f) {
        d_term = STEER_KD * (error - s->prev_error) / dt;
    }
    s->prev_error = error;

    /* Sum and clamp raw output */
    float raw = p_term + i_term + d_term;
    if (raw >  (float)STEER_OUTPUT_MAX) raw =  (float)STEER_OUTPUT_MAX;
    if (raw < -(float)STEER_OUTPUT_MAX) raw = -(float)STEER_OUTPUT_MAX;

    /* Rate limiter: bound change per step by STEER_RAMP_RATE_PER_MS * dt */
    float max_step = (float)STEER_RAMP_RATE_PER_MS * (dt * 1000.0f);
    float delta = raw - (float)s->output;
    if (delta >  max_step) delta =  max_step;
    if (delta < -max_step) delta = -max_step;

    float limited = (float)s->output + delta;
    /* Final clamp after rate limit */
    if (limited >  (float)STEER_OUTPUT_MAX) limited =  (float)STEER_OUTPUT_MAX;
    if (limited < -(float)STEER_OUTPUT_MAX) limited = -(float)STEER_OUTPUT_MAX;

    s->output = (int16_t)limited;
    return s->output;
}

/* ---- steering_pid_get_output() ---- */
int16_t steering_pid_get_output(const steering_pid_t *s)
{
    return s->output;
}

/* ---- steering_pid_set_enabled() ---- */
void steering_pid_set_enabled(steering_pid_t *s, bool en)
{
    if (!en && s->enabled) {
        /* Disabling: zero out state */
        s->target_omega_dps = 0.0f;
        s->integral         = 0.0f;
        s->prev_error       = 0.0f;
        s->output           = 0;
    }
    s->enabled = en;
}

/* ---- steering_pid_send_tlm() ---- */
void steering_pid_send_tlm(const steering_pid_t *s, uint32_t now_ms)
{
    if (STEER_TLM_HZ == 0u) return;

    uint32_t interval_ms = 1000u / STEER_TLM_HZ;
    if ((now_ms - s->last_tlm_ms) < interval_ms) return;
    /* Cast away const for timestamp update — only mutable field */
    ((steering_pid_t *)s)->last_tlm_ms = now_ms;

    jlink_tlm_steering_t tlm;

    /* Scale to ×10 for 0.1 deg/s resolution, clamped to int16 range */
    float t = s->target_omega_dps * 10.0f;
    if (t >  32767.0f) t =  32767.0f;
    if (t < -32768.0f) t = -32768.0f;
    tlm.target_x10 = (int16_t)t;

    float a = s->actual_omega_dps * 10.0f;
    if (a >  32767.0f) a =  32767.0f;
    if (a < -32768.0f) a = -32768.0f;
    tlm.actual_x10 = (int16_t)a;

    tlm.output  = s->output;
    tlm.enabled = s->enabled ? 1u : 0u;
    tlm._pad    = 0u;

    jlink_send_steering_tlm(&tlm);
}