#include <Keypad.h>
#include <Eventually.h>
#include <LiquidCrystal_I2C.h>


/********************************** KEYPAD **********************************/

#define ROWS 4
#define COLS 4

char keys[ROWS][COLS] = {
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '#', 'D' }
};

const uint8_t rowPins[ROWS] = { 5, 4, 3, 2 }; // Pins connected to R1, R2, R3, R4
const uint8_t colPins[COLS] = { A3, A2, A1, A0 }; // Pins connected to C1, C2, C3, C4

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

#define PASS_LEN 6

const char passcode[PASS_LEN] = {'0', '1', '2', '3', '4', '5'};

/********************************* SWITCHES *********************************/

#define SW_HAZARD_PIN 11
#define SW_LEFT_PIN 12
#define SW_RIGHT_PIN 13

#define LANE_CHANGE_THRESHOLD 500
#define HAZARD_LED_ON_TIME 500
#define HAZARD_LED_OFF_TIME 500

#define HAZARD_TURNOFF_THRESHOLD 200
#define HAZARD_TURNON_THRESHOLD 200

double startHazardHoldTime;
double stopHazardHoldTime;

/*********************************** LEDS ***********************************/

#define LED_LEFT_PIN 10
#define LED_RIGHT_PIN 6

const uint8_t LEDS[] = {LED_LEFT_PIN, LED_RIGHT_PIN};

/******************************** LISTENERS *********************************/

EvtManager eventManager;
EvtListener *start_blinking_left_check_listener;
EvtListener *start_blinking_right_check_listener;
EvtListener *start_hazard_check_listener;
EvtListener *start_hazard_listener;
EvtListener *hazard_blink_listener;
EvtListener *stop_hazard_check_listener;
EvtListener *stop_hazard_listener;

/******************************** LCD *********************************/

LiquidCrystal_I2C lcd(0x27,20,4);


/********************************* FUNCTIONS ********************************/

void lane_change_blink(int idx) {
  for (int i = 0; i < 3; ++i) {
    digitalWrite(LEDS[idx], HIGH);
    delay(300);
    digitalWrite(LEDS[idx], LOW);
    delay(700);
  }
}

void normal_blink(int idx, double period) {
  digitalWrite(LEDS[idx], HIGH);
  delay(period / 2.0);
  digitalWrite(LEDS[idx], LOW);
  delay(period / 2.0);
}

void hazard_blink() {
  digitalWrite(LEDS[0], HIGH);
  digitalWrite(LEDS[1], HIGH);
  delay(HAZARD_LED_ON_TIME);
  digitalWrite(LEDS[0], LOW);
  digitalWrite(LEDS[1], LOW);
}

void addEventListeners() {
  //eventManager.addListener(start_blinking_left_check_listener = new EvtPinListener(SW_LEFT_PIN, (EvtAction)start_blinking_left_check));
  //eventManager.addListener(start_blinking_right_check_listener = new EvtPinListener(SW_RIGHT_PIN, (EvtAction)start_blinking_right_check));
  eventManager.addListener(start_hazard_check_listener = new EvtPinListener(SW_HAZARD_PIN, (EvtAction)start_hazard_check));
}

bool start_hazard_check() {
  startHazardHoldTime = millis();
  eventManager.addListener(start_hazard_listener = new EvtPinListener(SW_HAZARD_PIN, LOW, (EvtAction)start_hazard));

  return true;
}

bool start_hazard() {
  if (millis() - startHazardHoldTime > HAZARD_TURNON_THRESHOLD) {
    eventManager.addListener(hazard_blink_listener = new EvtTimeListener(HAZARD_LED_OFF_TIME, true, (EvtAction)hazard_blink));
    eventManager.addListener(stop_hazard_check_listener = new EvtPinListener(SW_HAZARD_PIN, (EvtAction)stop_hazard_check));
    //eventManager.removeListener(start_hazard_check_listener);
    //eventManager.removeListener(start_hazard_listener);
  }
  else {
    eventManager.resetContext();
    addEventListeners();
  }

  return true;
}

bool stop_hazard_check() {
  stopHazardHoldTime = millis();
  eventManager.addListener(stop_hazard_listener = new EvtPinListener(SW_HAZARD_PIN, LOW, (EvtAction)stop_hazard));
  
  return false;
}

bool stop_hazard() {
  if (millis() - stopHazardHoldTime > HAZARD_TURNOFF_THRESHOLD) {
    eventManager.resetContext();
    addEventListeners();
  }
  else {
    eventManager.removeListener(stop_hazard_listener);
  }

  return true;
}


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

  pinMode(LED_LEFT_PIN, OUTPUT);
  pinMode(LED_RIGHT_PIN, OUTPUT);

  pinMode(SW_LEFT_PIN, INPUT);
  pinMode(SW_RIGHT_PIN, INPUT);
  pinMode(SW_HAZARD_PIN, INPUT);

  digitalWrite(LED_LEFT_PIN, LOW);
  digitalWrite(LED_RIGHT_PIN, LOW);

 
  // security phase
  char attempt[PASS_LEN];
  bool guessed = false;
  while (!guessed) {
    for (int i = 0; i < PASS_LEN; ++i) {
      while (true) {
        char key = keypad.getKey();
        if (key != NO_KEY) {
          attempt[i] = key;
          break;
        }
      }
    }
     
    guessed = true;
    for (int i = 0; i < PASS_LEN; ++i) {
      if (attempt[i] != passcode[i]) {
        guessed = false;
        Serial.println("Access denied. Try again");
        break;
      }
    }
  }

  Serial.println("Access granted.");

  // listeners registering
  addEventListeners();

}

// for eventually
USE_EVENTUALLY_LOOP(eventManager)