/*
 * receiver.ino  --  UART -> two LEGO Powered Up hubs (ESP32 "B")
 *
 * Board package: esp32 (the normal Espressif one)
 * Libraries:     Legoino  +  NimBLE-Arduino 1.4.x  (both via Library Manager)
 *
 * Listens for gamepad frames from the Bluepad32 board on Serial2 and drives
 * two Powered Up / Technic hubs over BLE using Legoino.
 *
 * Wiring to the transmitter board:
 *   RX GPIO16  <-  TX GPIO17 on transmitter
 *   TX GPIO17  ->  RX GPIO16 on transmitter
 *   GND        ->  GND
 *
 * Created by: Jess Rogerson (yelling commands at Claude.AI)
 */

#include "Lpf2Hub.h"

// ---------------------------------------------------------------- settings

// Hub BLE addresses. Run tools/hub_scanner to find them - do not guess.
// Lower case, colon separated.
static const char *HUB_TRACKS_ADDR    = "90:84:2b:61:e6:8c";
static const char *HUB_UPPERBODY_ADDR = "90:84:2b:61:f2:d7";

// Port numbers are just bytes in the LEGO protocol, same on every hub type:
static const byte PORT_A = 0x00;
static const byte PORT_B = 0x01;
static const byte PORT_C = 0x02;
static const byte PORT_D = 0x03;

// Technic / Control+ motors (42100 etc.) are tacho motors -> leave this at 1.
// Plain train motors and the simple Powered Up motors -> set it to 0.
#define USE_TACHO_MOTORS 1

static const int  DEADZONE       = 40;    // raw stick counts ignored around centre
static const int  TRACK_MAX      = 100;   // LEGO speed range is -100..100
static const int  HEAD_MAX       = 60;
static const unsigned long MOTOR_MIN_INTERVAL_MS = 60;   // per port throttle
static const unsigned long LINK_TIMEOUT_MS       = 400;  // failsafe

static const int  LINK_RX_PIN = 16;
static const int  LINK_TX_PIN = 17;
static const long LINK_BAUD   = 115200;

static const int  STATUS_LED_PIN = 2;

// ------------------------------------------------------------------ state
Lpf2Hub hubTracks;
Lpf2Hub hubUpperBody;

static bool tracksInitialised = false;
static bool upperInitialised  = false;
static unsigned long tracksRetryAt = 0;
static unsigned long upperRetryAt  = 0;

static unsigned long lastFrameAt = 0;
static bool failsafeEngaged = true;

struct PortState {
    int lastSpeed;
    unsigned long lastSentAt;
};
static PortState trackLeft  = {0, 0};
static PortState trackRight = {0, 0};
static PortState headTurn   = {0, 0};

// ----------------------------------------------------------------- helpers

// Apply a deadzone, then scale what is left so the usable travel still
// reaches full speed instead of jumping from 0 to a third of the range.
static int stickToSpeed(int raw, int maxSpeed) {
    if (raw > -DEADZONE && raw < DEADZONE) return 0;
    int sign = (raw < 0) ? -1 : 1;
    long magnitude = abs((long)raw) - DEADZONE;
    long span = 512L - DEADZONE;
    long scaled = (magnitude * maxSpeed) / span;
    if (scaled > maxSpeed) scaled = maxSpeed;
    return sign * (int)scaled;
}

static void driveMotor(Lpf2Hub &hub, byte port, int speed, PortState &state) {
    if (!hub.isConnected()) return;

    unsigned long now = millis();
    bool changed = (speed != state.lastSpeed);
    bool stopping = (speed == 0 && state.lastSpeed != 0);

    // Stops always go out immediately; everything else is throttled so we
    // don't flood the hub's BLE queue and stall it.
    if (!stopping && (!changed || (now - state.lastSentAt) < MOTOR_MIN_INTERVAL_MS)) return;

#if USE_TACHO_MOTORS
    hub.setTachoMotorSpeed(port, speed);
#else
    hub.setBasicMotorSpeed(port, speed);
#endif

    state.lastSpeed = speed;
    state.lastSentAt = now;
}

static void stopEverything() {
    driveMotor(hubTracks,    PORT_A, 0, trackLeft);
    driveMotor(hubTracks,    PORT_B, 0, trackRight);
    driveMotor(hubUpperBody, PORT_A, 0, headTurn);
}

// Connect the hubs one at a time. Kicking off two scans at once upsets the
// shared NimBLE scanner and you end up with one hub connected and one sulking.
static void serviceHub(Lpf2Hub &hub, const char *address, bool &initialised,
                       unsigned long &retryAt, const char *label) {
    if (hub.isConnected()) return;

    if (hub.isConnecting()) {
        hub.connectHub();
        if (hub.isConnected()) {
            Serial.printf("[%s] connected\n", label);
            hub.setLedColor(GREEN);
        } else {
            Serial.printf("[%s] connect failed, retrying\n", label);
            initialised = false;
            retryAt = millis() + 2000;
        }
        return;
    }

    if (!initialised && millis() >= retryAt) {
        Serial.printf("[%s] scanning for %s\n", label, address);
        hub.init(std::string(address));
        initialised = true;
    }
}

static uint8_t xorChecksum(const char *s, size_t len) {
    uint8_t c = 0;
    for (size_t i = 0; i < len; i++) c ^= (uint8_t)s[i];
    return c;
}

// Returns true if a valid frame was parsed.
static bool parseFrame(char *line, int &lx, int &ly, int &rx, int &ry, unsigned &buttons) {
    char *star = strrchr(line, '*');
    if (!star) return false;
    *star = '\0';

    unsigned expected = 0;
    if (sscanf(star + 1, "%2x", &expected) != 1) return false;
    if (xorChecksum(line, strlen(line)) != (uint8_t)expected) return false;

    return sscanf(line, "G,%d,%d,%d,%d,%u", &lx, &ly, &rx, &ry, &buttons) == 5;
}

// -------------------------------------------------------------------- main
void setup() {
    Serial.begin(115200);
    Serial2.begin(LINK_BAUD, SERIAL_8N1, LINK_RX_PIN, LINK_TX_PIN);

    pinMode(STATUS_LED_PIN, OUTPUT);
    digitalWrite(STATUS_LED_PIN, LOW);

    Serial.println("LEGO hub receiver starting");
}

void loop() {
    // 1. Keep the hubs connected, tracks first.
    serviceHub(hubTracks, HUB_TRACKS_ADDR, tracksInitialised, tracksRetryAt, "tracks");
    if (hubTracks.isConnected()) {
        serviceHub(hubUpperBody, HUB_UPPERBODY_ADDR, upperInitialised, upperRetryAt, "upper");
    }

    digitalWrite(STATUS_LED_PIN,
                 (hubTracks.isConnected() && hubUpperBody.isConnected()) ? HIGH : LOW);

    // 2. Pull whole lines off the link.
    static char buf[96];
    static size_t idx = 0;

    while (Serial2.available()) {
        char c = (char)Serial2.read();
        if (c == '\r') continue;
        if (c == '\n') {
            buf[idx] = '\0';
            int lx, ly, rx, ry;
            unsigned buttons;
            if (idx > 0 && parseFrame(buf, lx, ly, rx, ry, buttons)) {
                lastFrameAt = millis();
                failsafeEngaged = false;

                int leftTrack  = stickToSpeed(-ly, TRACK_MAX);   // push forward = positive
                int rightTrack = stickToSpeed(-ry, TRACK_MAX);
                int head       = stickToSpeed(lx,  HEAD_MAX);

                driveMotor(hubTracks,    PORT_A, leftTrack,  trackLeft);
                driveMotor(hubTracks,    PORT_B, rightTrack, trackRight);
                driveMotor(hubUpperBody, PORT_A, head,       headTurn);
            }
            idx = 0;
        } else if (idx < sizeof(buf) - 1) {
            buf[idx++] = c;
        } else {
            idx = 0;   // overrun, throw the line away
        }
    }

    // 3. Failsafe - link went quiet, stop before something drives off a table.
    if (!failsafeEngaged && (millis() - lastFrameAt) > LINK_TIMEOUT_MS) {
        Serial.println("Link timeout - stopping motors");
        stopEverything();
        failsafeEngaged = true;
    }
}