#ifndef MODE_MANAGER_H
#define MODE_MANAGER_H

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

/*
 * SaltyLab Mode Manager
 *
 * Resolves three operating modes selected by RC CH6 (3-pos switch):
 *
 *   RC_MANUAL   — RC steer (CH4) and speed bias (CH3) applied directly.
 *                 Balance PID remains active for stability.
 *   RC_ASSISTED — RC inputs blended 50/50 with Jetson autonomous commands.
 *   AUTONOMOUS  — Jetson commands only; RC CH5 arm switch still kills motors.
 *
 * Transitions between modes are smoothed over MODE_BLEND_MS (~500ms) to
 * prevent jerky handoffs. A single `blend` scalar (0=pure RC, 1=pure auto)
 * drives all interpolation; adjacent-mode steps take ~250ms each.
 *
 * RC safety rule: if RC is alive and CH5 is disarmed, the main loop MUST
 * disarm regardless of mode. mode_manager only blends commands — kill
 * authority lives in the main loop.
 *
 * Autonomous commands are set by the Jetson serial bridge via
 * mode_manager_set_auto_cmd(). They default to zero (no motion).
 */

typedef enum {
    MODE_RC_MANUAL   = 0,
    MODE_RC_ASSISTED = 1,
    MODE_AUTONOMOUS  = 2,
} robot_mode_t;

typedef struct {
    robot_mode_t target;         /* Mode requested by CH6 (or fallback) */
    float        blend;          /* 0.0=pure RC .. 1.0=pure auto, smoothly ramped */
    bool         rc_alive;       /* Cached RC liveness (set in update) */
    int16_t      auto_steer;     /* Jetson steer cmd (-1000..+1000) */
    int16_t      auto_speed_bias;/* Jetson speed bias (-MOTOR_RC_SPEED_MAX..+) */
} mode_manager_t;

/* Initialise — call once before the main loop */
void mode_manager_init(mode_manager_t *m);

/*
 * Call every main-loop tick (1ms) to:
 *   - read CH6, update target mode
 *   - cache RC liveness
 *   - advance blend ramp toward target blend value
 */
void mode_manager_update(mode_manager_t *m, uint32_t now);

/* Set autonomous commands from the Jetson serial bridge */
void mode_manager_set_auto_cmd(mode_manager_t *m,
                                int16_t steer,
                                int16_t speed_bias);

/*
 * Blended steer command to pass to motor_driver_update().
 * Returns 0 when RC is not alive and no autonomous steer set.
 */
int16_t mode_manager_get_steer(const mode_manager_t *m);

/*
 * Blended speed bias to add to bal.motor_cmd before motor_driver_update().
 * Returns 0 when RC is not alive and no autonomous speed set.
 */
int16_t mode_manager_get_speed_bias(const mode_manager_t *m);

/* Quantised current mode (based on blend position, not target) */
robot_mode_t mode_manager_active(const mode_manager_t *m);

#endif