#include "motor_driver.h"
#include "hoverboard.h"
#include "config.h"
#include <stdlib.h>

void motor_driver_init(motor_driver_t *m) {
    m->steer_actual = 0;
    m->estop        = false;
}

void motor_driver_update(motor_driver_t *m,
                         int16_t balance_cmd,
                         int16_t steer_cmd,
                         uint32_t now) {
    static uint32_t s_last_tick = 0;

    /* Delta-time for ramp (cap at 100ms to handle first call / long gaps).
     * Always update tick so ramp starts fresh when estop clears. */
    int32_t dt_ms = (int32_t)(now - s_last_tick);
    if (dt_ms > 100) dt_ms = 100;
    s_last_tick = now;

    /* Emergency stop: send zero, hold latch */
    if (m->estop) {
        hoverboard_send(0, 0);
        return;
    }

    /* Ramp steer toward target at MOTOR_STEER_RAMP_RATE counts/ms */
    int32_t max_step = (int32_t)MOTOR_STEER_RAMP_RATE * dt_ms;
    int32_t steer_error = (int32_t)steer_cmd - (int32_t)m->steer_actual;
    if (steer_error > max_step)       steer_error = max_step;
    else if (steer_error < -max_step) steer_error = -max_step;
    m->steer_actual = (int16_t)((int32_t)m->steer_actual + steer_error);

    /* Headroom clamp: ensure |speed| + |steer| <= MOTOR_CMD_MAX
     * Reduce steer (not balance) to preserve PID authority. */
    int32_t speed = (int32_t)balance_cmd;
    int32_t steer = (int32_t)m->steer_actual;
    int32_t headroom = MOTOR_CMD_MAX - abs((int)speed);
    if (headroom < 0) headroom = 0;
    if (steer >  headroom) steer =  headroom;
    if (steer < -headroom) steer = -headroom;

    hoverboard_send((int16_t)speed, (int16_t)steer);
}

void motor_driver_estop(motor_driver_t *m) {
    m->estop        = true;
    m->steer_actual = 0;
    /* Don't call hoverboard_send here — caller must drive sends via
     * motor_driver_update() at the normal 50Hz ESC rate to avoid
     * flooding the UART with 1kHz zero packets. */
}

void motor_driver_estop_clear(motor_driver_t *m) {
    m->estop = false;
}