#include "jetson_cmd.h"
#include <stdio.h>

/*
 * Parsed drive state — updated by jetson_cmd_process() in the main loop.
 * Raw fields are ints parsed from "C<speed>,<steer>\n".
 */
static volatile int16_t jcmd_speed = 0;  /* -1000..+1000 */
static volatile int16_t jcmd_steer = 0;  /* -1000..+1000 */

/* Clamp helper (avoids including math.h) */
static int16_t clamp16(int v, int lo, int hi) {
    if (v < lo) return (int16_t)lo;
    if (v > hi) return (int16_t)hi;
    return (int16_t)v;
}

/*
 * Called from main loop when jetson_cmd_ready is set.
 * Parses jetson_cmd_buf — safe to use sscanf here (not in ISR).
 * The ISR only copies bytes and sets the ready flag.
 */
void jetson_cmd_process(void) {
    int speed = 0, steer = 0;
    /* buf format: "C<speed>,<steer>" — skip leading 'C' */
    if (sscanf((const char *)jetson_cmd_buf + 1, "%d,%d", &speed, &steer) == 2) {
        jcmd_speed = clamp16(speed, -1000, 1000);
        jcmd_steer = clamp16(steer, -1000, 1000);
    }
    /* If parse fails, keep previous values — don't zero-out mid-motion */
}

/*
 * Returns true if the last heartbeat (H or C command) arrived within
 * JETSON_HB_TIMEOUT_MS.  jetson_hb_tick is updated in the ISR.
 */
bool jetson_cmd_is_active(uint32_t now_ms) {
    /* jetson_hb_tick == 0 means we've never received any command */
    if (jetson_hb_tick == 0) return false;
    return (now_ms - jetson_hb_tick) < JETSON_HB_TIMEOUT_MS;
}

/* Steer command for motor_driver_update() */
int16_t jetson_cmd_steer(void) {
    return jcmd_steer;
}

/*
 * Convert speed command to balance setpoint offset (degrees).
 * Positive speed → lean forward → robot moves forward.
 * Scaled linearly: speed=1000 → +JETSON_SPEED_MAX_DEG degrees.
 */
float jetson_cmd_sp_offset(void) {
    return ((float)jcmd_speed / 1000.0f) * JETSON_SPEED_MAX_DEG;
}