#include <LiquidCrystal.h>

#include "led_control.h"
#include "sensors_handle.h"
#include "parking_control.h"


// Инициализируем объект-экран, передаём использованные
LiquidCrystal lcd(13, 12, 11, 10, 9, 8);

// sensor
SensorUltraDistance sensor(3, 2);

Led r_led(6, HIGH);
Led g_led(5, HIGH);
Led b_led(4, HIGH);

void setup() {
  lcd.begin(16, 2);
  lcd.clear();
  lcd.print("Dist. in meters:");
  LedSwitchOff();
}


State cur_state = State::NORMAL;
size_t timer;

void LedSwitchOff() {
  r_led.switch_off();
  g_led.switch_off();
  b_led.switch_off();
}

void LedNormal() {
  r_led.switch_on();
  g_led.switch_on();
  b_led.switch_on();
}

bool state_is_changed = false;
bool is_on = true;

void LedWarning() {
  if (state_is_changed) {
    LedSwitchOff();
    timer = millis();
    state_is_changed = false;
  }
  if (timer + 500 < millis()) {
    timer += 500;
    is_on = !is_on;
  }
  if (is_on) {
    r_led.switch_on();
    g_led.switch_on();
    b_led.switch_off();
  } else {
    LedSwitchOff();
  }
}

void LedDanger() {
  if (state_is_changed) {
    LedSwitchOff();
    timer = millis();
    state_is_changed = false;
  }
  if (timer + 100 < millis()) {
    timer += 100;
    is_on = !is_on;
  }
  if (is_on) {
    r_led.switch_on();
    g_led.switch_off();
    b_led.switch_off();
  } else {
    LedSwitchOff();
  }
}

void LedControl(State state) {
  switch (state) {
    case State::NORMAL:
      LedNormal();
      break;
    case State::WARNING:
      LedWarning();
      break;
    case State::DANGER:
      LedDanger();
      break;
  }
}

void PrintLCD(float distance_m) {
  lcd.setCursor(0,1);
  lcd.print("    ");

  lcd.setCursor(2,1);
  lcd.print(distance_m, 1);
}

void CheckStateChanged(float distance) {
  if (distance < 100 && cur_state != State::DANGER) {
    state_is_changed = true;
    is_on = true;
    cur_state = State::DANGER;
  } else if (distance >= 200 && cur_state != State::NORMAL) {
    state_is_changed = true;
    is_on = true;
    cur_state = State::NORMAL;
  } else if (distance < 200 && distance >= 100 && cur_state != State::WARNING){
    state_is_changed = true;
    is_on = true;
    cur_state = State::WARNING;
  }
} 

void ParkingControl() {
  float cm = sensor.get_distance_cm();
  PrintLCD(cm / 100);
  CheckStateChanged(cm);
  LedControl(cur_state);
}

void loop() {
  ParkingControl();

  delay(100);  
}