#include <Wire.h>

#include "Config.h"
#include "Display.h"
#include "Gear.h"
// #include "Sonar.h"
#include "RFID.h"
#include "Engine.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);
Engine engine(THROTTLE_PIN, CLUTCH_PIN, BRAKE_PIN);

unsigned long prevUpdate = 0;

bool carEnabled = false;

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

  pinMode(IN_1, OUTPUT);
  pinMode(IN_2, OUTPUT);

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

void loop() {
  bool check = rfid.check();
  bool matches = rfid.matches(AUTH_UID, AUTH_UID_SIZE);

  bool rfidOk = check && matches;

  if (rfid.check() && rfid.matches(AUTH_UID, AUTH_UID_SIZE)) {
    carEnabled = !carEnabled;
    Serial.print("Auto działa: ");
    Serial.println(carEnabled);
    display.startOrEnd(carEnabled);0
  };

  if (carEnabled) {
    if (gear.update())
      display.updateGear(gear.getGearChar());

    unsigned long now = millis();
    float dt = (now - prevUpdate) / 1000.0f;
    if (dt >= 0.02f) {
      prevUpdate = now;
      engine.update(dt, gear.getGear());
      display.updateRPM(engine.getRPM());
      display.updateSpeed(engine.getSpeedKmh());
    }

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