#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_AND_ECHO_PIN, SONAR_MAX_DIST_CM);
RFIDReader rfid(SS_PIN, RST_PIN);
Engine engine(THROTTLE_PIN, CLUTCH_PIN, BRAKE_PIN);

unsigned long prevEngineUpdate = 0;
bool carEnabled = false;
unsigned long prevSonarUpdate = 0;
bool seatBelts = false;

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

  Serial.println(F("Podaj wagę samochodu w kg (np. 1200):"));
  while (true) {
    if (Serial.available() > 0) {
      float mass = Serial.parseFloat();
      if (mass > 0) {
        engine.setCarMass(mass);
        Serial.print(F("Waga samochodu ustawiona na: "));
        Serial.print(mass);
        Serial.println(F(" kg"));
        break;
      } else {
        Serial.println(F("Niepoprawna wartość, spróbuj ponownie:"));
      }
    }
  }

  pinMode(IN_1, OUTPUT);
  pinMode(IN_2, OUTPUT);
  pinMode(ENA_1, OUTPUT);
  pinMode(IN_3, OUTPUT);
  pinMode(IN_4, OUTPUT);
  pinMode(ENA_2, OUTPUT);

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

void loop() {
  if (rfid.check() && rfid.matches(AUTH_UID, AUTH_UID_SIZE)) {
    carEnabled = !carEnabled;

    if (carEnabled)
      Serial.println("Uruchamiam auto");
    else
      Serial.println("Gaszę auto");

    display.startOrEnd(carEnabled);
  };

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

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

    unsigned long now = millis();
    if (now - prevSonarUpdate >= 100) {
      prevSonarUpdate = now;
      long dist = sonar.measure();
      if (!seatBelts) {
        if (dist < 200) seatBelts = true;
      }
      else {
        if (dist >= 200) seatBelts = false;
      }
    }
  }
}