#include <esp_now.h>
#include <WiFi.h>
#include <SPI.h>
#include <MFRC522.h>
#include "esp_wifi.h"

// ── Fixed MAC ─────────────────────────────────────────────────
uint8_t fixedMAC[] = {0x36, 0x33, 0x33, 0x33, 0x33, 0x33};

// ── Motor Pins (A4953) ────────────────────────────────────────
#define MOTOR_A_IN1   2
#define MOTOR_A_IN2   3
#define MOTOR_B_IN1   5
#define MOTOR_B_IN2   6

// ── Other Pins ────────────────────────────────────────────────
#define PIN_LED      21

// ── NFC (RC522) ───────────────────────────────────────────────
#define NFC_MOSI     10
#define NFC_MISO      9
#define NFC_RST       8
#define NFC_SCK      20
#define NFC_SS        7

MFRC522 nfc(NFC_SS, NFC_RST);

// ── Joystick calibration ──────────────────────────────────────
#define JOY_CENTER_X  2139
#define JOY_CENTER_Y  2248
#define JOY_DEAD       300
#define JOY_MAX       2048

// ── PWM config ────────────────────────────────────────────────
#define PWM_FREQ      5000
#define PWM_RES          8

// ── Signal watchdog ───────────────────────────────────────────
#define SIGNAL_TIMEOUT_MS 500
unsigned long lastReceiveTime = 0;

// ── Game config ───────────────────────────────────────────────
#define TOTAL_COINS    5
#define GAME_TIME_SEC  180

// ── NFC tag UIDs (7 bytes each) ───────────────────────────────
uint8_t coinTags[TOTAL_COINS][7] = {
  {0x1D, 0x05, 0x99, 0xC5, 0x08, 0x10, 0x80},
  {0x1D, 0x02, 0x99, 0xC5, 0x08, 0x10, 0x80},
  {0x1D, 0x04, 0x99, 0xC5, 0x08, 0x10, 0x80},
  {0x1D, 0xF7, 0x98, 0xC5, 0x08, 0x10, 0x80},
  {0x1D, 0x03, 0x99, 0xC5, 0x08, 0x10, 0x80}
};
bool coinCollected[TOTAL_COINS] = {false};

// ── Game state ────────────────────────────────────────────────
enum GameState { WAITING, PLAYING, WIN, GAMEOVER };
GameState gameState = WAITING;
unsigned long gameStartTime = 0;
int coinsCollected = 0;
unsigned long winTime = 0;

// ── Payloads ──────────────────────────────────────────────────
typedef struct {
  int   vrx;
  int   vry;
  bool  sw;
  float battPct;
} JoyData;

typedef struct {
  int           coins;
  int           timeLeft;
  bool          coinJustCollected;
  bool          win;
  bool          gameover;
  bool          waiting;
  unsigned long winTimeMs;
} CarData;

JoyData rxData;
volatile bool newData = false;
uint8_t joystickMac[6];
bool joystickMacKnown = false;

// ── ESP-NOW receive callback ──────────────────────────────────
void onReceive(const esp_now_recv_info_t *info,
               const uint8_t *data, int len) {
  if (len == sizeof(JoyData)) {
    memcpy(&rxData, data, sizeof(JoyData));
    newData = true;
    lastReceiveTime = millis();

    if (!joystickMacKnown) {
      memcpy(joystickMac, info->src_addr, 6);
      esp_now_peer_info_t peer = {};
      memcpy(peer.peer_addr, joystickMac, 6);
      peer.channel = 0;
      peer.encrypt = false;
      esp_now_add_peer(&peer);
      joystickMacKnown = true;
    }
  }
}

// ── Send state to joystick ────────────────────────────────────
void sendCarData(bool coinJust) {
  if (!joystickMacKnown) return;
  CarData cd;
  int elapsed  = (millis() - gameStartTime) / 1000;
  cd.timeLeft          = max(0, GAME_TIME_SEC - elapsed);
  cd.coins             = coinsCollected;
  cd.coinJustCollected = coinJust;
  cd.win               = (gameState == WIN);
  cd.gameover          = (gameState == GAMEOVER);
  cd.waiting           = (gameState == WAITING);
  cd.winTimeMs         = winTime;
  esp_now_send(joystickMac, (uint8_t*)&cd, sizeof(cd));
}

// ── Motor control ─────────────────────────────────────────────
void setMotorA(int speed) {
  speed = constrain(speed, -255, 255);
  if (speed > 0)      { ledcWrite(MOTOR_A_IN1, speed); ledcWrite(MOTOR_A_IN2, 0);      }
  else if (speed < 0) { ledcWrite(MOTOR_A_IN1, 0);     ledcWrite(MOTOR_A_IN2, -speed); }
  else                { ledcWrite(MOTOR_A_IN1, 0);     ledcWrite(MOTOR_A_IN2, 0);      }
}
void setMotorB(int speed) {
  speed = constrain(speed, -255, 255);
  if (speed > 0)      { ledcWrite(MOTOR_B_IN1, speed); ledcWrite(MOTOR_B_IN2, 0);      }
  else if (speed < 0) { ledcWrite(MOTOR_B_IN1, 0);     ledcWrite(MOTOR_B_IN2, -speed); }
  else                { ledcWrite(MOTOR_B_IN1, 0);     ledcWrite(MOTOR_B_IN2, 0);      }
}
void stopAll() { setMotorA(0); setMotorB(0); }

// ── Tank drive ────────────────────────────────────────────────
void tankDrive(int vrx, int vry) {
  int y = vry - JOY_CENTER_Y;
  int x = vrx - JOY_CENTER_X;
  if (abs(y) < JOY_DEAD) y = 0;
  if (abs(x) < JOY_DEAD) x = 0;
  if (y == 0 && x == 0) { stopAll(); return; }
  int thrust = map(-y, -JOY_MAX, JOY_MAX, -255, 255);
  int turn   = map( x, -JOY_MAX, JOY_MAX, -255, 255);
  setMotorA(constrain(thrust + turn, -255, 255));
  setMotorB(constrain(thrust - turn, -255, 255));
}

// ── Match coin tag ────────────────────────────────────────────
int matchCoin(byte *uid, byte size) {
  if (size != 7) return -1;
  for (int i = 0; i < TOTAL_COINS; i++) {
    if (!coinCollected[i] &&
        memcmp(uid, coinTags[i], 7) == 0) {
      return i;
    }
  }
  return -1;
}

// ── Reset game ────────────────────────────────────────────────
void resetGame() {
  gameState      = PLAYING;
  coinsCollected = 0;
  winTime        = 0;
  gameStartTime  = millis();
  for (int i = 0; i < TOTAL_COINS; i++) coinCollected[i] = false;
  Serial.println("Game started!");
}

void setup() {
  Serial.begin(115200);

  ledcAttach(MOTOR_A_IN1, PWM_FREQ, PWM_RES);
  ledcAttach(MOTOR_A_IN2, PWM_FREQ, PWM_RES);
  ledcAttach(MOTOR_B_IN1, PWM_FREQ, PWM_RES);
  ledcAttach(MOTOR_B_IN2, PWM_FREQ, PWM_RES);
  stopAll();

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

  SPI.begin(NFC_SCK, NFC_MISO, NFC_MOSI, NFC_SS);
  nfc.PCD_Init();

  WiFi.mode(WIFI_STA);
  esp_wifi_set_mac(WIFI_IF_STA, fixedMAC);
  if (esp_now_init() != ESP_OK) return;
  esp_now_register_recv_cb(onReceive);

  lastReceiveTime = millis();
  Serial.println("Car ready. Press SW on joystick to start.");
}

void loop() {
  int elapsed  = (millis() - gameStartTime) / 1000;
  int timeLeft = max(0, GAME_TIME_SEC - elapsed);

  // ── Timer expired ─────────────────────────────────────────
  if (gameState == PLAYING && timeLeft == 0) {
    gameState = GAMEOVER;
    stopAll();
    sendCarData(false);
    Serial.println("Game Over!");
  }

  // ── Signal timeout watchdog ───────────────────────────────
  if (millis() - lastReceiveTime > SIGNAL_TIMEOUT_MS) {
    stopAll();
  }

  // ── Joystick input ────────────────────────────────────────
  if (newData) {
    newData = false;

    // SW button pressed = start or restart game
    if (rxData.sw) {
      resetGame();
      sendCarData(false);
    } else {
      // Only drive during PLAYING state
      if (gameState == PLAYING) {
        tankDrive(rxData.vrx, rxData.vry);
      } else {
        stopAll();
      }
    }
  }

  // ── NFC scan (only while playing) ────────────────────────
  if (gameState == PLAYING &&
      nfc.PICC_IsNewCardPresent() && nfc.PICC_ReadCardSerial()) {

    // ── DELETE this block after testing ──────────────────
    Serial.print("TAG UID: ");
    for (byte i = 0; i < nfc.uid.size; i++) {
      Serial.printf("%02X ", nfc.uid.uidByte[i]);
    }
    Serial.println();
    // ── END delete block ──────────────────────────────────

    int idx = matchCoin(nfc.uid.uidByte, nfc.uid.size);
    if (idx >= 0) {
      coinCollected[idx] = true;
      coinsCollected++;
      Serial.printf("Coin %d collected! Total: %d/%d\n",
                    idx + 1, coinsCollected, TOTAL_COINS);

      // Flash car LED
      for (int i = 0; i < 3; i++) {
        digitalWrite(PIN_LED, HIGH); delay(150);
        digitalWrite(PIN_LED, LOW);  delay(150);
      }

      // Check win
      if (coinsCollected >= TOTAL_COINS) {
        gameState = WIN;
        winTime   = millis() - gameStartTime;
        stopAll();
        Serial.printf("YOU WIN! Time: %lums\n", winTime);
      }

      sendCarData(true);
    }

    nfc.PICC_HaltA();
    nfc.PCD_StopCrypto1();
  }

  // ── Periodic state update to joystick ────────────────────
  static unsigned long lastSend = 0;
  if (millis() - lastSend > 200) {
    lastSend = millis();
    sendCarData(false);
  }
}