/*
 * 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 stick /
 * button state out of Serial2 as plain ASCII lines. The second ESP32 (see
 * ../receiver) picks those up and drives the LEGO hubs over BLE.
 *
 * 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>*<XX>\n
 *   lx/ly/rx/ry : -512..511   (Bluepad32 raw axis range)
 *   buttons     : 16-bit mask, decimal
 *   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
static const unsigned long HEARTBEAT_MS     = 200;  // resend even if idle

// ------------------------------------------------------------------ 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 uint16_t lastButtons = 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) {
    char payload[64];
    snprintf(payload, sizeof(payload), "G,%d,%d,%d,%d,%u", lx, ly, rx, ry, buttons);
    Serial2.printf("%s*%02X\n", payload, xorChecksum(payload));
}

// Only bother transmitting when something actually moved, or on heartbeat.
static bool worthSending(int lx, int ly, int rx, int ry, uint16_t buttons) {
    const int DELTA = 4;   // ignore stick jitter of a few counts
    if (buttons != lastButtons) 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;
    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);

    // Clear stored pairings so a controller that was last paired to a phone
    // will bind here. Comment this out once you are happy with the pairing,
    // otherwise you re-pair on every boot.
    BP32.forgetBluetoothKeys();

    // We only care about 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);
        lastHeartbeat = now;
        lastLx = lastLy = lastRx = lastRy = 0;
        lastButtons = 0;
        return;
    }

    int lx = gp->axisX();
    int ly = gp->axisY();
    int rx = gp->axisRX();
    int ry = gp->axisRY();
    uint16_t buttons = gp->buttons();

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