#include <LiquidCrystal.h>
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_PN532.h>

#include "station.h"
#include "statemachine.h"
#include "state.h"
#include "states.h"
#include "timer.h"

#define PN532_CS 10

// Using this library to set a timeout on readPassiveTagID
// The library in use is slightly modified to fix this issue: https://github.com/adafruit/Adafruit-PN532/issues/117
Adafruit_PN532 nfc(PN532_CS);

const int rs = 9, en = 8, d4 = 5, d5 = 4, d6 = 3, d7 = 2;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);

Station stations[4] = { Station(1680767519, "Yellow"), Station(3346823711, "Green"), Station(3569318175, "Pink"), Station(2174777887, "Blue") };
Station* currentStation;

Timer stationDetectionTimer(500, true);

StateMachine stateMachine(new Startup());

unsigned long lastLoopTime = 0;
unsigned long currentLoopTime = 0;
unsigned long deltaTime = 0;

void setup(void) {
  setupNFC();

  lcd.begin(16, 2);
  stationDetectionTimer.start();
}

void loop(void) {
  currentLoopTime = millis();
  deltaTime = currentLoopTime - lastLoopTime;

  bool doCheck = stationDetectionTimer.update(deltaTime);
  lcd.setCursor(0, 1);
  lcd.print("                ");
  if (doCheck) {
    checkStations();
    lcd.setCursor(7, 1);
    lcd.print("check");
  }
  lcd.setCursor(0, 1);
  lcd.print(String(deltaTime));

  stateMachine.update(Context(stations, deltaTime));

  lastLoopTime = currentLoopTime;
  delay(10);
}

void setupNFC() {
  nfc.begin();

  uint32_t versiondata = nfc.getFirmwareVersion();
  if (!versiondata) {
    while (1)
      ;  // halt
  }
  // configure board to read RFID tags and cards
  nfc.SAMConfig();
}

void checkStations() {
  Station* station = detectStation();

  if (station != 0) {
    lcd.setCursor(0, 0);
    lcd.print("                ");
    lcd.setCursor(0, 0);
    lcd.print("Station: " + station->getName());
    if (currentStation == 0) {
      currentStation = station;
      stateMachine.putDown(currentStation);
    }
  } else {
    lcd.setCursor(0, 0);
    lcd.print("                ");
    lcd.setCursor(0, 0);
    lcd.print("not docked");
    if (currentStation != 0) {
      currentStation = 0;
      stateMachine.pickedUp();
    }
  }
}

Station* detectStation() {
  uint8_t uid[7];
  uint8_t uidLength;
  uint16_t timeout = 100;
  bool success = nfc.readPassiveTargetID(PN532_MIFARE_ISO14443A, uid, &uidLength, timeout);

  if (!success) {
    return 0;
  }

  uint32_t id = 0;
    for (uint8_t i = 0; i < uidLength; i++) {
      id <<= 8;
      id |= uid[i];
    }
  

  Station* found = 0;
  for (int i = 0; i < sizeof(stations); i++) {
    Station* currentStation = &stations[i];
    if (currentStation->check(id)) {
      found = currentStation;
    }
  }
  return found;
}