#include "mode_manager.h"
#include "crsf.h"
#include "config.h"

/* -----------------------------------------------------------------------
 * Internal helpers
 * --------------------------------------------------------------------- */

static int16_t clamp16(int32_t v, int16_t lo, int16_t hi) {
    if (v < lo) return lo;
    if (v > hi) return hi;
    return (int16_t)v;
}

static float clampf(float v, float lo, float hi) {
    if (v < lo) return lo;
    if (v > hi) return hi;
    return v;
}

/*
 * Map a CRSF raw value to [-out_max, +out_max] with a symmetric deadband
 * around center (992). Within ±CRSF_DEADBAND counts of center → returns 0.
 * Outside deadband the remaining range is rescaled linearly to ±out_max.
 */
static int16_t crsf_stick(uint16_t raw, int16_t out_max) {
    int32_t centered = (int32_t)raw - 992;
    if (centered >  CRSF_DEADBAND)  centered -= CRSF_DEADBAND;
    else if (centered < -CRSF_DEADBAND) centered += CRSF_DEADBAND;
    else return 0;
    /* CRSF half-range from centre ≈ 820 counts; subtract deadband */
    const int32_t half_range = 820 - CRSF_DEADBAND;
    int32_t out = centered * out_max / half_range;
    return clamp16(out, -out_max, out_max);
}

/* Blend target values for each mode (0=pure RC, 1=pure autonomous) */
static const float k_blend_target[3] = {
    [MODE_RC_MANUAL]   = 0.0f,
    [MODE_RC_ASSISTED] = 0.5f,
    [MODE_AUTONOMOUS]  = 1.0f,
};

/* Blend advance rate: 1/MODE_BLEND_MS per ms → full 0..1 transition in
 * MODE_BLEND_MS. Adjacent mode steps (covering 0.5 of range) take 250ms. */
#define BLEND_RATE  (1.0f / (float)MODE_BLEND_MS)

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

void mode_manager_init(mode_manager_t *m) {
    m->target          = MODE_RC_MANUAL;
    m->blend           = 0.0f;
    m->rc_alive        = false;
    m->auto_steer      = 0;
    m->auto_speed_bias = 0;
}

void mode_manager_update(mode_manager_t *m, uint32_t now) {
    static uint32_t s_last_tick = 0;

    /* Delta-time (cap at 100ms for first call / resume after gap) */
    int32_t dt_ms = (int32_t)(now - s_last_tick);
    if (dt_ms > 100) dt_ms = 100;
    s_last_tick = now;

    /* Cache RC liveness — checked by main loop too, but needed by getters */
    m->rc_alive = (crsf_state.last_rx_ms != 0) &&
                  ((now - crsf_state.last_rx_ms) < RC_TIMEOUT_MS);

    /* Determine mode target from CH6 */
    if (m->rc_alive) {
        uint16_t ch6 = crsf_state.channels[CRSF_CH_MODE];
        if (ch6 <= CRSF_MODE_LOW_THRESH)
            m->target = MODE_RC_MANUAL;
        else if (ch6 >= CRSF_MODE_HIGH_THRESH)
            m->target = MODE_AUTONOMOUS;
        else
            m->target = MODE_RC_ASSISTED;
    }
    /* If RC is not alive, keep existing target — don't flap to MANUAL just
     * because the stub returns zeros; kill authority is a separate concern. */

    /* Advance blend toward target value */
    float target_blend = k_blend_target[m->target];
    float step = BLEND_RATE * (float)dt_ms;
    if (m->blend < target_blend)
        m->blend = clampf(m->blend + step, 0.0f, target_blend);
    else
        m->blend = clampf(m->blend - step, target_blend, 1.0f);
}

void mode_manager_set_auto_cmd(mode_manager_t *m,
                                int16_t steer,
                                int16_t speed_bias) {
    m->auto_steer      = clamp16(steer,      -1000, 1000);
    m->auto_speed_bias = clamp16(speed_bias,
                                 -MOTOR_RC_SPEED_MAX,
                                  MOTOR_RC_SPEED_MAX);
}

int16_t mode_manager_get_steer(const mode_manager_t *m) {
    int16_t rc_steer = 0;
    if (m->rc_alive)
        rc_steer = crsf_stick(crsf_state.channels[CRSF_CH_STEER], 1000);

    /* lerp: rc_steer → auto_steer over blend */
    int32_t mixed = (int32_t)rc_steer +
                    (int32_t)((float)(m->auto_steer - rc_steer) * m->blend);
    return clamp16(mixed, -1000, 1000);
}

int16_t mode_manager_get_speed_bias(const mode_manager_t *m) {
    int16_t rc_bias = 0;
    if (m->rc_alive)
        rc_bias = crsf_stick(crsf_state.channels[CRSF_CH_SPEED],
                             MOTOR_RC_SPEED_MAX);

    int32_t mixed = (int32_t)rc_bias +
                    (int32_t)((float)(m->auto_speed_bias - rc_bias) * m->blend);
    return clamp16(mixed, -MOTOR_RC_SPEED_MAX, MOTOR_RC_SPEED_MAX);
}

robot_mode_t mode_manager_active(const mode_manager_t *m) {
    if (m->blend < 0.25f) return MODE_RC_MANUAL;
    if (m->blend > 0.75f) return MODE_AUTONOMOUS;
    return MODE_RC_ASSISTED;
}