#ifndef GIMBAL_H
#define GIMBAL_H

#include <stdint.h>
#include <stdbool.h>

/*
 * gimbal.h — Pan/tilt gimbal controller for ST3215 bus servos (Issue #547)
 *
 * Manages dual ST3215 serial bus servos:
 *   Pan  servo: ID GIMBAL_PAN_ID  (config.h, default 1)
 *   Tilt servo: ID GIMBAL_TILT_ID (config.h, default 2)
 *
 * Position units: degrees x10 (int16), matching JLink protocol convention.
 *   e.g. 900 = 90.0°, -450 = -45.0°
 *
 * Limits:
 *   Pan:  -1800..+1800 (x10 deg) = -180..+180 deg
 *   Tilt:  -900..+900  (x10 deg) = -90..+90 deg
 *
 * The gimbal_tick() function polls servo feedback at GIMBAL_TLM_HZ (50 Hz).
 * Alternates reading pan position on even ticks, tilt on odd ticks — each
 * servo polled at 25 Hz to keep bus utilization low.
 */

typedef struct {
    /* Command state */
    int16_t  cmd_pan_x10;    /* Commanded pan  (deg x10) */
    int16_t  cmd_tilt_x10;   /* Commanded tilt (deg x10) */
    uint16_t cmd_speed;      /* Servo bus speed (0=max, 1-4095) */
    bool     torque_enabled; /* True when torques are enabled */

    /* Feedback state (updated at ~25 Hz per axis) */
    int16_t  fb_pan_x10;     /* Measured pan  (deg x10) */
    int16_t  fb_tilt_x10;    /* Measured tilt (deg x10) */
    uint16_t fb_pan_speed;   /* Raw speed register, pan servo */
    uint16_t fb_tilt_speed;  /* Raw speed register, tilt servo */

    /* Diagnostics */
    uint32_t rx_ok;          /* Successful position reads */
    uint32_t rx_err;         /* Failed position reads */

    uint32_t _last_tick_ms;  /* Internal: last tick timestamp */
    uint8_t  _poll_phase;    /* Internal: alternates 0=pan 1=tilt */
} gimbal_t;

/*
 * gimbal_init(g) — enable torque on both servos, center them.
 * servo_bus_init() must be called first.
 */
void gimbal_init(gimbal_t *g);

/*
 * gimbal_set_pos(g, pan_x10, tilt_x10, speed) — command a new pan/tilt
 * position.  pan_x10 and tilt_x10 are degrees×10, clamped to servo limits.
 * speed: 0=max servo speed, 1-4095 = scaled.
 */
void gimbal_set_pos(gimbal_t *g, int16_t pan_x10, int16_t tilt_x10,
                    uint16_t speed);

/*
 * gimbal_torque(g, enable) — enable or disable torque on both servos.
 */
void gimbal_torque(gimbal_t *g, bool enable);

/*
 * gimbal_tick(g, now_ms) — poll servo feedback at GIMBAL_TLM_HZ.
 * Call every 1 ms from the main loop; function self-throttles.
 */
void gimbal_tick(gimbal_t *g, uint32_t now_ms);

#endif /* GIMBAL_H */