#include <WiFi.h>
#include <esp_now.h>
#include <esp_mac.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// ================= Display =================
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define SCREEN_ADDRESS 0x3C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// ================= Touch Buttons =================
#define N_TOUCH 6
#define THRESHOLD 100000UL
int  touch_pins[N_TOUCH]      = {9, 8, 7, 2, 3, 1};
int  touch_values[N_TOUCH]    = {0};
bool pin_now[N_TOUCH]         = {0};
bool pin_old[N_TOUCH]         = {0};

#define IDX_LEFT  1
#define IDX_RIGHT 0
#define IDX_JUMP  2

// ================= Player Types =================
struct PlayerState {
  int x;
  int y;
  int vx;
  int vy;
  bool onGround;
};

struct NetPacket {
  PlayerState st;
  bool ready;
};

PlayerState myState, otherState;
bool otherReady = false;
bool isP1 = false;

uint8_t selfMac[6];
uint8_t peerMac[6];

// GAME BOARD MACs
uint8_t macP1[] = {0xD8,0x3B,0xDA,0x75,0x05,0xAC};
uint8_t macP2[] = {0xD8,0x3B,0xDA,0x75,0xE1,0x9C};

// CAMERA MACs (receive only)
uint8_t camA_MAC[] = {0xB8,0xF8,0x62,0xF9,0xE2,0xC0};
uint8_t camB_MAC[] = {0xB8,0xF8,0x62,0xF9,0xD6,0x38};

// ================= ESP-NOW callbacks =================
void onDataSent(const wifi_tx_info_t*, esp_now_send_status_t) {}

void onDataRecv(const esp_now_recv_info_t *info, const uint8_t *data, int len) {
  if (len != sizeof(NetPacket)) return;

  NetPacket pkt;
  memcpy(&pkt, data, sizeof(pkt));
  otherState = pkt.st;
  if (pkt.ready) otherReady = true;
}

// ================= ESP-NOW INIT =================
void addPeer(const uint8_t *mac) {
  esp_now_peer_info_t p = {};
  memcpy(p.peer_addr, mac, 6);
  p.channel = 0;
  p.encrypt = false;
  esp_now_add_peer(&p);
}

bool initESPNowAndPeers() {
  if (esp_now_init() != ESP_OK) return false;

  esp_now_register_recv_cb(onDataRecv);
  esp_now_register_send_cb(onDataSent);

  // identify game role
  if (memcmp(selfMac, macP1, 6)==0) {
    isP1 = true;
    memcpy(peerMac, macP2, 6);
  } else {
    isP1 = false;
    memcpy(peerMac, macP1, 6);
  }

  // Add all peers:
  addPeer(peerMac);    // other game board
  addPeer(camA_MAC);   // camera A
  addPeer(camB_MAC);   // camera B

  return true;
}

// ================= Touch =================
void update_touch() {
  for (int i = 0; i < N_TOUCH; i++) {
    int v = touchRead(touch_pins[i]);
    touch_values[i] = v;
    pin_old[i] = pin_now[i];
    pin_now[i] = (v > THRESHOLD);
  }
}

// ================= Physics =================
#define PLAYER_SIZE    8
#define MOVE_SPEED     2
#define JUMP_VELOCITY -6
#define GRAVITY        1
#define MAX_FALL_SPEED 4
#define GROUND_Y       (SCREEN_HEIGHT - PLAYER_SIZE)

void resetPlayer(PlayerState &p, bool leftSide) {
  p.x = leftSide ? 20 : (SCREEN_WIDTH - 20 - PLAYER_SIZE);
  p.y = GROUND_Y;
  p.vx = 0;
  p.vy = 0;
  p.onGround = true;
}

void updatePlayer(PlayerState &p, bool left, bool right, bool jump) {
  if (left && !right) p.vx = -MOVE_SPEED;
  else if (right && !left) p.vx = MOVE_SPEED;
  else p.vx = 0;

  if (jump && p.onGround) {
    p.vy = JUMP_VELOCITY;
    p.onGround = false;
  }

  p.vy += GRAVITY;
  if (p.vy > MAX_FALL_SPEED) p.vy = MAX_FALL_SPEED;

  p.x += p.vx;
  p.y += p.vy;

  if (p.y >= GROUND_Y) { p.y = GROUND_Y; p.vy = 0; p.onGround = true; }
  if (p.x < 0) p.x = 0;
  if (p.x > SCREEN_WIDTH - PLAYER_SIZE) p.x = SCREEN_WIDTH - PLAYER_SIZE;
}

// ================= SETUP =================
void setup() {
  Serial.begin(115200);

  pinMode(44, INPUT); // stability fix

  display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(0,0);
  display.println("Boot...");
  display.display();

  WiFi.mode(WIFI_STA);
  esp_read_mac(selfMac, ESP_MAC_WIFI_STA);

  if (!initESPNowAndPeers()) {
    display.clearDisplay();
    display.println("ESP-NOW FAIL");
    display.display();
    while(1);
  }

  // warm touch
  for (int k = 0; k < 4; k++) {
    for (int i = 0; i < N_TOUCH; i++) touchRead(touch_pins[i]);
    delay(10);
  }

  resetPlayer(myState, isP1);
  resetPlayer(otherState, !isP1);
}

// ================= LOOP =================
void loop() {
  update_touch();

  bool left  = pin_now[IDX_LEFT];
  bool right = pin_now[IDX_RIGHT];
  bool jump  = pin_now[IDX_JUMP];

  updatePlayer(myState, left, right, jump);

  // send to all peers
  NetPacket pkt;
  pkt.st = myState;
  pkt.ready = true;

  esp_now_send(peerMac,   (uint8_t*)&pkt, sizeof(pkt));
  esp_now_send(camA_MAC,  (uint8_t*)&pkt, sizeof(pkt));
  esp_now_send(camB_MAC,  (uint8_t*)&pkt, sizeof(pkt));

  // ================= DRAW =================
  display.clearDisplay();

  if (!otherReady) {
    display.setCursor(10, 22);
    display.println("Waiting Player");
  } else {
    display.drawFastHLine(0, SCREEN_HEIGHT-1, SCREEN_WIDTH, SSD1306_WHITE);

    if (isP1) {
      display.fillRect(myState.x, myState.y, PLAYER_SIZE, PLAYER_SIZE, SSD1306_WHITE);
      display.fillCircle(otherState.x+4, otherState.y+4, 4, SSD1306_WHITE);
    } else {
      display.fillCircle(myState.x+4, myState.y+4, 4, SSD1306_WHITE);
      display.fillRect(otherState.x, otherState.y, PLAYER_SIZE, PLAYER_SIZE, SSD1306_WHITE);
    }
  }

  display.display();
  delay(30);
}