#include "gimbal.h"
#include "servo_bus.h"
#include "config.h"
#include <stddef.h>

/*
 * gimbal.c — Pan/tilt gimbal controller for ST3215 bus servos (Issue #547)
 *
 * Tick rate: called every 1 ms from main loop; self-throttles to GIMBAL_TLM_HZ.
 * Feedback polling alternates: pan on even ticks, tilt on odd ticks.
 * This gives ~25 Hz per axis, keeping per-read latency under 2 ms total.
 *
 * Safety limits:
 *   Pan:  -GIMBAL_PAN_LIMIT_DEG  .. +GIMBAL_PAN_LIMIT_DEG  (±180 deg)
 *   Tilt: -GIMBAL_TILT_LIMIT_DEG .. +GIMBAL_TILT_LIMIT_DEG  (± 90 deg)
 */

#define TICK_PERIOD_MS          (1000u / GIMBAL_TLM_HZ)  /* 20 ms at 50 Hz */

/* Clamp int16 to [lo, hi] */
static int16_t _clamp16(int16_t v, int16_t lo, int16_t hi)
{
    if (v < lo) return lo;
    if (v > hi) return hi;
    return v;
}

/* ---- gimbal_init() ---- */

void gimbal_init(gimbal_t *g)
{
    g->cmd_pan_x10  = 0;
    g->cmd_tilt_x10 = 0;
    g->cmd_speed    = 0;           /* 0 = max speed */
    g->fb_pan_x10   = 0;
    g->fb_tilt_x10  = 0;
    g->fb_pan_speed = 0;
    g->fb_tilt_speed = 0;
    g->rx_ok        = 0;
    g->rx_err       = 0;
    g->_last_tick_ms = 0;
    g->_poll_phase  = 0;

    /* Enable torque and center both servos */
    servo_bus_write_torque(GIMBAL_PAN_ID,  true);
    servo_bus_write_torque(GIMBAL_TILT_ID, true);
    g->torque_enabled = true;

    uint16_t center = servo_bus_deg_to_raw(0.0f);
    servo_bus_write_pos(GIMBAL_PAN_ID,  center, 0);
    servo_bus_write_pos(GIMBAL_TILT_ID, center, 0);
}

/* ---- gimbal_set_pos() ---- */

void gimbal_set_pos(gimbal_t *g, int16_t pan_x10, int16_t tilt_x10,
                    uint16_t speed)
{
    /* Clamp to hardware limits */
    pan_x10  = _clamp16(pan_x10,
                        -(int16_t)(GIMBAL_PAN_LIMIT_DEG  * 10),
                        (int16_t)(GIMBAL_PAN_LIMIT_DEG   * 10));
    tilt_x10 = _clamp16(tilt_x10,
                        -(int16_t)(GIMBAL_TILT_LIMIT_DEG * 10),
                        (int16_t)(GIMBAL_TILT_LIMIT_DEG  * 10));
    if (speed > SB_SPEED_MAX) speed = SB_SPEED_MAX;

    g->cmd_pan_x10  = pan_x10;
    g->cmd_tilt_x10 = tilt_x10;
    g->cmd_speed    = speed;

    float pan_deg  = (float)pan_x10  / 10.0f;
    float tilt_deg = (float)tilt_x10 / 10.0f;

    servo_bus_write_pos(GIMBAL_PAN_ID,
                        servo_bus_deg_to_raw(pan_deg),  speed);
    servo_bus_write_pos(GIMBAL_TILT_ID,
                        servo_bus_deg_to_raw(tilt_deg), speed);
}

/* ---- gimbal_torque() ---- */

void gimbal_torque(gimbal_t *g, bool enable)
{
    servo_bus_write_torque(GIMBAL_PAN_ID,  enable);
    servo_bus_write_torque(GIMBAL_TILT_ID, enable);
    g->torque_enabled = enable;
}

/* ---- gimbal_tick() ---- */

void gimbal_tick(gimbal_t *g, uint32_t now_ms)
{
    if ((now_ms - g->_last_tick_ms) < TICK_PERIOD_MS) return;
    g->_last_tick_ms = now_ms;

    uint16_t raw = 0;

    if (g->_poll_phase == 0u) {
        /* Poll pan position */
        if (servo_bus_read_pos(GIMBAL_PAN_ID, &raw)) {
            g->fb_pan_x10 = (int16_t)(servo_bus_raw_to_deg(raw) * 10.0f);
            g->rx_ok++;
        } else {
            g->rx_err++;
        }

        /* Also refresh pan speed */
        uint16_t spd = 0;
        (void)servo_bus_read_speed(GIMBAL_PAN_ID, &spd);
        g->fb_pan_speed = spd;
    } else {
        /* Poll tilt position */
        if (servo_bus_read_pos(GIMBAL_TILT_ID, &raw)) {
            g->fb_tilt_x10 = (int16_t)(servo_bus_raw_to_deg(raw) * 10.0f);
            g->rx_ok++;
        } else {
            g->rx_err++;
        }

        uint16_t spd = 0;
        (void)servo_bus_read_speed(GIMBAL_TILT_ID, &spd);
        g->fb_tilt_speed = spd;
    }

    g->_poll_phase ^= 1u;  /* toggle 0 / 1 */
}