#include <Wire.h>

#include "Config.h"
#include "Display.h"
#include "Gear.h"
#include "Sonar.h"
#include "RFID.h"

Display display;
GearSelector gear(GEAR_X_PIN, GEAR_Y_PIN, GEAR_THRESH_LOW, GEAR_THRESH_HIGH);
Sonar sonar(TRIG_PIN, ECHO_PIN, SONAR_MAX_DIST_CM);
RFIDReader rfid(SS_PIN, RST_PIN);

bool leftBlink = false;
bool rightBlink = false;

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

  pinMode(TS_LEFT, INPUT);
  pinMode(TS_RIGHT, INPUT);

  rfid.begin();
  display.begin(LCD_COLS, LCD_ROWS);
}

void loop() {
  if (gear.update()) {
    display.updateGear(gear.getGearChar());
  }

  leftBlink = digitalRead(TS_LEFT);
  rightBlink = digitalRead(TS_RIGHT);
  display.updateDirection(leftBlink, rightBlink);

  long dist = sonar.measure();
  // Serial.println(dist);

  if (rfid.check()) {
    rfid.printUID();
  }
}