/*
 * pid_schedule.h — Speed-dependent PID gain scheduling (Issue #550)
 *
 * Maps robot velocity to PID gain triplets (Kp, Ki, Kd) using a lookup
 * table with linear interpolation between adjacent entries.  The table
 * supports 1–PID_SCHED_MAX_BANDS entries, each associating a velocity
 * breakpoint (m/s) with gains that apply AT that velocity.
 *
 * HOW IT WORKS:
 * 1. Each entry in the table defines: {speed_mps, kp, ki, kd}.
 *    The table is sorted by speed_mps ascending (pid_schedule_set_table
 *    sorts automatically).
 *
 * 2. pid_schedule_get_gains(speed_mps, ...) finds the two adjacent entries
 *    that bracket the query speed and linearly interpolates:
 *      t  = (speed - bands[i-1].speed_mps) /
 *           (bands[i].speed_mps - bands[i-1].speed_mps)
 *      kp = bands[i-1].kp + t * (bands[i].kp - bands[i-1].kp)
 *    Speeds below the first entry or above the last entry clamp to the
 *    nearest endpoint (no extrapolation).
 *    The query speed is ABS(motor_speed) — scheduling is symmetric.
 *
 * 3. Default 3-entry table (loaded when flash has no valid schedule):
 *      Band 0: speed=0.00 m/s  kp=40.0  ki=1.5  kd=1.2  (stopped — tight)
 *      Band 1: speed=0.30 m/s  kp=35.0  ki=1.0  kd=1.0  (slow — balanced)
 *      Band 2: speed=0.80 m/s  kp=28.0  ki=0.5  kd=0.8  (fast — relaxed)
 *
 * 4. pid_schedule_apply(balance, speed_mps) interpolates and writes the
 *    result directly into balance->kp/ki/kd.  Call from the main loop at
 *    the same rate as the balance PID update (1 kHz) or slower (100 Hz
 *    for scheduling, 1 kHz for PID execution — gains change slowly enough).
 *
 * 5. Flash persistence: pid_schedule_flash_save() calls pid_flash_save_all()
 *    which erases sector 7 once and writes both the single-PID record at
 *    PID_FLASH_STORE_ADDR and the schedule at PID_SCHED_FLASH_ADDR.
 *
 * 6. JLINK interface (Issue #550):
 *      0x0C  SCHED_GET  — no payload; triggers TLM_SCHED response
 *      0x0D  SCHED_SET  — upload new table (num_bands + N×16-byte entries)
 *      0x0E  SCHED_SAVE — save current table + single PID to flash
 *      0x85  TLM_SCHED  — table dump response to SCHED_GET
 */

#ifndef PID_SCHEDULE_H
#define PID_SCHEDULE_H

#include <stdint.h>
#include <stdbool.h>
#include "pid_flash.h"   /* pid_sched_entry_t, PID_SCHED_MAX_BANDS */
#include "balance.h"     /* balance_t */

/* ---- Default gain table ---- */
/* Motor ESC range is ±1000 counts; 1000 counts ≈ full drive.
 * Speed scale: MOTOR_CMD_MAX=1000 → ~0.8 m/s max tangential velocity.
 * Adjust PID_SCHED_SPEED_SCALE if odometry calibration changes this. */
#define PID_SCHED_SPEED_SCALE   0.0008f   /* motor_cmd counts → m/s: 1000 × 0.0008 = 0.8 m/s */

/* ---- API ---- */

/*
 * pid_schedule_init() — load table from flash (via pid_flash_load_schedule).
 * Falls back to the built-in 3-band default if flash is empty or invalid.
 * Call once after flash init during system startup.
 */
void pid_schedule_init(void);

/*
 * pid_schedule_get_gains(speed_mps, *kp, *ki, *kd) — interpolate gains.
 * |speed_mps| is used (scheduling is symmetric for forward/reverse).
 * Clamps to table endpoints; does not extrapolate outside the table range.
 */
void pid_schedule_get_gains(float speed_mps, float *kp, float *ki, float *kd);

/*
 * pid_schedule_apply(b, speed_mps) — compute interpolated gains and write
 * them into b->kp, b->ki, b->kd.  b->integral is reset to 0 when the
 * active band changes to avoid integrator windup on transitions.
 */
void pid_schedule_apply(balance_t *b, float speed_mps);

/*
 * pid_schedule_set_table(entries, n) — replace the active gain table.
 * Entries are copied and sorted by speed_mps ascending.
 * n is clamped to [1, PID_SCHED_MAX_BANDS].
 * Does NOT automatically save to flash — call pid_schedule_flash_save().
 */
void pid_schedule_set_table(const pid_sched_entry_t *entries, uint8_t n);

/*
 * pid_schedule_get_table(out_entries, out_n) — copy current table out.
 * out_entries must have room for PID_SCHED_MAX_BANDS entries.
 */
void pid_schedule_get_table(pid_sched_entry_t *out_entries, uint8_t *out_n);

/*
 * pid_schedule_get_num_bands() — return current number of table entries.
 */
uint8_t pid_schedule_get_num_bands(void);

/*
 * pid_schedule_flash_save(kp_single, ki_single, kd_single) — save the
 * current schedule table PLUS the caller-supplied single-PID values to
 * flash in one atomic sector erase (pid_flash_save_all).
 * Must NOT be called while armed (sector erase takes ~1s).
 * Returns true on success.
 */
bool pid_schedule_flash_save(float kp_single, float ki_single, float kd_single);

/*
 * pid_schedule_active_band_idx() — index (0-based) of the lower bracket
 * entry used in the most recent interpolation.  Useful for telemetry.
 * Returns 0 if speed is below the first entry.
 */
uint8_t pid_schedule_active_band_idx(void);

/*
 * pid_schedule_get_default_table(out_entries, out_n) — fill the 3-band
 * default table into caller's buffer.  Used for factory-reset.
 */
void pid_schedule_get_default_table(pid_sched_entry_t *out_entries, uint8_t *out_n);

#endif /* PID_SCHEDULE_H */