#include <Arduino.h>
// #include <Wire.h>
#include <Servo.h>
Servo servo1;

#include <Adafruit_Sensor.h>
#include <DHT.h>
#define DHTPIN 7    
#define DHTTYPE DHT22
DHT dht(DHTPIN, DHTTYPE);

#include <LiquidCrystal_I2C.h>
#define I2C_ADDR    0x27
#define LCD_COLUMNS 20
#define LCD_LINES   4
LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

const int redLEDpin = 13;
const int blueLEDpin = 12;
const int yellowLEDpin = 11;
const int PB1pin = 9;
const int PB2pin = 8;
const int encoderCLK = 2;
const int encoderDT = 3;
const int analogSignal = A3;

volatile bool toggleState = false;


void blinkRed();
void blinkBlue();
void blinkYellow();
float readTHsens();
void INT0_ISR() {
  // buttonPressed = true;
  
    if (digitalRead(buttonpinRE) == LOW) {
      toggleState = !toggleState;
      // Serial.print("test");
      static bool state = toggleState;
      digitalWrite(redLEDpin, state);
    }
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);

  pinMode(redLEDpin, OUTPUT);
  pinMode(blueLEDpin, OUTPUT);
  pinMode(yellowLEDpin, OUTPUT);
  pinMode(encoderCLK, INPUT);
  pinMode(encoderDT, INPUT);
  pinMode(PB1pin, INPUT_PULLUP);
  pinMode(PB2pin, INPUT_PULLUP);
  pinMode(analogSignal, INPUT);

  attachInterrupt(digitalPinToInterrupt(encoderCLK), INT0_ISR, FALLING);

  servo1.attach(5);

  dht.begin();

  // Init
  lcd.init();
  lcd.backlight();
  lcd.setCursor(3, 0);
  lcd.print("Testing Zone");
  lcd.setCursor(3, 1);
  lcd.print("Temp:");
  lcd.setCursor(3, 2);
  lcd.print("Humi:");
}

void loop() {



}

float readTHsens() {
  static unsigned long THsensmillis = millis();
  if (millis() - THsensmillis > 2000){
    // Reading temperature or humidity takes about 250 milliseconds!
    // Sensor readings may also be up to 2 seconds 'old' (its a very slow sensor)
    float h = dht.readHumidity();
    // Read temperature as Celsius (the default)
    float t = dht.readTemperature();
    // Read temperature as Fahrenheit (isFahrenheit = true)
    float f = dht.readTemperature(true);


    // Check if any reads failed and exit early (to try again).
    if (isnan(h) || isnan(t) || isnan(f)) {
      // Serial.println(F("Failed to read from DHT sensor!"));
      lcd.setCursor(9, 1);
      lcd.print("Failed to read");
      lcd.setCursor(9, 2);
      lcd.print("error");
      return -1.0;
    }

    // Compute heat index in Fahrenheit (the default)
    // float hif = dht.computeHeatIndex(f, h);
    // // Compute heat index in Celsius (isFahreheit = false)
    // float hic = dht.computeHeatIndex(t, h, false);
    
    lcd.setCursor(9, 1);
    lcd.print(String(t));
    lcd.setCursor(9, 2);
    lcd.print(String(h));
    
    return t;
    THsensmillis = millis();
  }
}

void blinkRed(){
  static unsigned long redLEDmillis = millis();
  if (millis() - redLEDmillis > 1500) {
    
    digitalWrite(redLEDpin, !digitalRead(redLEDpin));
    redLEDmillis = millis();
  }
}

void blinkBlue(){
  static unsigned long blueLEDmillis = millis();
  if (millis() - blueLEDmillis > 500) {
    digitalWrite(blueLEDpin, !digitalRead(blueLEDpin));
    blueLEDmillis = millis();
  }
}

void blinkYellow(){
  static unsigned long yellowLEDmillis = millis();
  if (millis() - yellowLEDmillis > 1000) {
    digitalWrite(yellowLEDpin, !digitalRead(yellowLEDpin));
    yellowLEDmillis = millis();
  }
}

void toggleStopLight() {

  static unsigned long processMillis = millis();

    // Delay until elevator arrives
    static uint16_t stateDelay;

    // Timer for notification process has completed
    static unsigned long goMillis;
    static unsigned long readyMillis;
    static unsigned long stopMillis;

    // Declare the states in meaningful English. Enums start enumerating
    // at zero, incrementing in steps of 1 unless overridden. We use an
    // enum 'class' here for type safety and code readability
    enum class stopLightState : uint8_t {
        IDLE,      // defaults to 0
        REDSTOP,    // defaults to 1
        YLWREADY,   // defaults to 2
        GRNGO, // defaults to 3
    };

    // Keep track of the current State (it's an stopLightState variable)
    static stopLightState currState = stopLightState::IDLE;

    switch (currState) {

        // Initial state (or final returned state)
        case stopLightState::IDLE:
            displayState("IDLE state");

            // Someone pushed the button yet?
            if (digitalRead(pushBtn) == LOW) {
                // Set the millis counter for the elevator arrival timer
                processMillis = millis();

                // Not really part of this process, simulates background action
                {
                    // Calculate random elevator arrival delay (seconds * milliSeconds)
                    elevatorDelay = random(3, 8) * 1000;
                    Serial.print("Elevator delay:");
                    Serial.println(elevatorDelay);
                }

                // Move to next state
                currState = stopLightState::CALLED;
            }
            break;

        // Someone pushed the 'call elevator' button - an input
        case stopLightState::CALLED:
            displayState("CALLED state");

            // Light the 'elevator called' LED
            digitalWrite(ylwLED, HIGH);

            // Has the elevator arrived yet?
            if (millis() - processMillis >= elevatorDelay) {
                // Move to next state
                currState = stopLightState::ARRIVED;
            }
            break;

        // Elevator has arrived
        case stopLightState::ARRIVED:
            displayState("ARRIVED State");

            // Quick beep to alert user that elevator has arrived
            digitalWrite(beepPin, HIGH);

            // Set the timer for the notification
            beepMillis = millis();

            // Move to next state
            currState = stopLightState::DOORSOPEN;
            break;

        case stopLightState::DOORSOPEN:
            displayState("DOORS OPEN state");

            // Extinguish the LED
            digitalWrite(ylwLED, LOW);

            // Time to turn off the beeper yet?
            if (millis() - beepMillis >= 100) {
                // Turn off beeper
                digitalWrite(beepPin, LOW);

                // Move to next state
                currState = stopLightState::IDLE;
            }
            break;

        default:
            // Nothing to do here
            Serial.println("'Default' Switch Case reached - Error");
    }
}

void displayState(String currState) {
    static String prevState = "";

    if (currState != prevState) {
        Serial.println(currState);
        prevState = currState;
    }
}