/*
 * transmitter.ino  --  PS4 gamepad -> UART bridge (ESP32 "A")
 *
 * Board package: esp32_bluepad32  (NOT the plain esp32 package)
 * Board:         "ESP32 Dev Module" listed under esp32_bluepad32
 * Libraries:     none (Bluepad32 ships inside the board package)
 *
 * Reads a Bluetooth Classic gamepad via Bluepad32 and streams the full stick,
 * trigger, d-pad and button state out of Serial2 as plain ASCII lines. The
 * second ESP32 (see ../receiver) picks those up and drives the LEGO hubs.
 *
 * This board does NOT talk to the hubs and must not have Legoino or NimBLE
 * installed. Keeping BTstack and NimBLE on separate chips is the entire point.
 *
 * Wiring to the receiver board:
 *   TX GPIO17  ->  RX GPIO16 on receiver
 *   RX GPIO16  <-  TX GPIO17 on receiver   (unused today, reserved for acks)
 *   GND        ->  GND                      (mandatory - common ground)
 *
 * Line format:  G,<lx>,<ly>,<rx>,<ry>,<buttons>,<dpad>,<l2>,<r2>*<XX>\n
 *   lx/ly/rx/ry : -512..511   (Bluepad32 raw axis range)
 *   buttons     : 16-bit mask, decimal
 *   dpad        : 8-bit mask, decimal
 *   l2/r2       : analog triggers, 0..1023
 *   XX          : XOR checksum of everything before the '*', two hex digits
 *
 * Created by: Jess Rogerson (yelling commands at Claude.AI)
 */

#include <Bluepad32.h>

// ---------------------------------------------------------------- settings
static const int  LINK_TX_PIN      = 17;
static const int  LINK_RX_PIN      = 16;
static const long LINK_BAUD        = 115200;
static const int  STATUS_LED_PIN   = 2;
static const unsigned long SEND_INTERVAL_MS = 20;   // 50 Hz ceiling
static const unsigned long HEARTBEAT_MS     = 200;  // resend even if idle

// Set to 1 to echo every outgoing frame to the USB serial monitor.
#define DEBUG_FRAMES 0

// ------------------------------------------------------------------ state
ControllerPtr myControllers[BP32_MAX_CONTROLLERS];

static unsigned long lastSend      = 0;
static unsigned long lastHeartbeat = 0;
static int  lastLx = 0, lastLy = 0, lastRx = 0, lastRy = 0;
static int  lastL2 = 0, lastR2 = 0;
static uint16_t lastButtons = 0;
static uint8_t  lastDpad = 0;

// -------------------------------------------------------------- callbacks
void onConnectedController(ControllerPtr ctl) {
    for (int i = 0; i < BP32_MAX_CONTROLLERS; i++) {
        if (myControllers[i] == nullptr) {
            myControllers[i] = ctl;
            Serial.printf("Controller connected in slot %d\n", i);
            digitalWrite(STATUS_LED_PIN, HIGH);
            return;
        }
    }
    Serial.println("Controller connected but no free slot");
}

void onDisconnectedController(ControllerPtr ctl) {
    for (int i = 0; i < BP32_MAX_CONTROLLERS; i++) {
        if (myControllers[i] == ctl) {
            myControllers[i] = nullptr;
            Serial.printf("Controller disconnected from slot %d\n", i);
            digitalWrite(STATUS_LED_PIN, LOW);
            return;
        }
    }
}

// ----------------------------------------------------------------- helpers
static uint8_t xorChecksum(const char *s) {
    uint8_t c = 0;
    while (*s) c ^= (uint8_t)*s++;
    return c;
}

static void sendFrame(int lx, int ly, int rx, int ry,
                      uint16_t buttons, uint8_t dpad, int l2, int r2) {
    char payload[80];
    snprintf(payload, sizeof(payload), "G,%d,%d,%d,%d,%u,%u,%d,%d",
             lx, ly, rx, ry, buttons, dpad, l2, r2);
    Serial2.printf("%s*%02X\n", payload, xorChecksum(payload));
#if DEBUG_FRAMES
    Serial.printf("%s*%02X\n", payload, xorChecksum(payload));
#endif
}

// Only transmit when something actually moved, or on heartbeat.
static bool worthSending(int lx, int ly, int rx, int ry,
                         uint16_t buttons, uint8_t dpad, int l2, int r2) {
    const int DELTA = 4;    // ignore stick jitter of a few counts
    const int TRIG  = 8;    // triggers are noisier at rest
    if (buttons != lastButtons) return true;
    if (dpad != lastDpad) return true;
    if (abs(lx - lastLx) >= DELTA) return true;
    if (abs(ly - lastLy) >= DELTA) return true;
    if (abs(rx - lastRx) >= DELTA) return true;
    if (abs(ry - lastRy) >= DELTA) return true;
    if (abs(l2 - lastL2) >= TRIG) return true;
    if (abs(r2 - lastR2) >= TRIG) return true;
    return (millis() - lastHeartbeat) >= HEARTBEAT_MS;
}

// -------------------------------------------------------------------- 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.printf("Bluepad32 firmware: %s\n", BP32.firmwareVersion());

    BP32.setup(&onConnectedController, &onDisconnectedController);

    // Bluepad32 keeps pairing keys in NVS and reflashing does NOT clear them.
    // Leave this in for the first pairing, then comment it out - otherwise you
    // re-pair the pad on every boot.
    BP32.forgetBluetoothKeys();

    // We only want real gamepads, not the mouse/keyboard virtual device.
    BP32.enableVirtualDevice(false);

    Serial.println("Ready - put the PS4 pad in pairing mode (SHARE + PS)");
}

void loop() {
    bool dataUpdated = BP32.update();

    ControllerPtr gp = nullptr;
    for (int i = 0; i < BP32_MAX_CONTROLLERS; i++) {
        if (myControllers[i] && myControllers[i]->isConnected() &&
            myControllers[i]->isGamepad()) {
            gp = myControllers[i];
            break;
        }
    }

    unsigned long now = millis();
    if (now - lastSend < SEND_INTERVAL_MS) return;
    lastSend = now;

    if (gp == nullptr) {
        // Nothing connected: keep sending neutral so the receiver's failsafe
        // never has to guess.
        sendFrame(0, 0, 0, 0, 0, 0, 0, 0);
        lastHeartbeat = now;
        lastLx = lastLy = lastRx = lastRy = 0;
        lastL2 = lastR2 = 0;
        lastButtons = 0;
        lastDpad = 0;
        return;
    }

    int lx = gp->axisX();
    int ly = gp->axisY();
    int rx = gp->axisRX();
    int ry = gp->axisRY();
    int l2 = gp->brake();
    int r2 = gp->throttle();
    uint16_t buttons = gp->buttons();
    uint8_t dpad = gp->dpad();

    if (dataUpdated || worthSending(lx, ly, rx, ry, buttons, dpad, l2, r2)) {
        sendFrame(lx, ly, rx, ry, buttons, dpad, l2, r2);
        lastLx = lx; lastLy = ly; lastRx = rx; lastRy = ry;
        lastL2 = l2; lastR2 = r2;
        lastButtons = buttons;
        lastDpad = dpad;
        lastHeartbeat = now;
    }
}