#include "Arduino.h"
#include "BasicEncoder.h"
#include "LiquidCrystal_I2C.h"

#define STAGE_1_LED 50
#define STAGE_2_LED 51
#define STAGE_3_LED 52
LiquidCrystal_I2C lcd(0x27, 16, 2);

byte angryLeft[8] = { 0b10000, 0b10000, 0b01000, 0b00100, 0b00100,	0b00010, 0b00010, 0b00001 };
byte angryRight[8] = { 0b00001, 0b00010,	0b00010, 0b00100,	0b00100, 0b01000, 0b01000, 0b10000 };
byte smileLeft[8] = { 0b10000, 0b10000, 0b11000, 0b11000, 0b01100, 0b01100, 0b00111, 0b00000 };
byte smileRight[8] = { 0b00001, 0b00001, 0b00011, 0b00011, 0b00110, 0b00110, 0b11000, 0b00000 };
unsigned long currentMillis = 0;
unsigned short currentSeconds = 0;

//Stage 1
#define SWITCH_1_PIN 22
#define SWITCH_2_PIN 23
#define SWITCH_3_PIN 24
#define SWITCH_4_PIN 25
#define SWITCH_5_PIN 26

//Stage 2
//Rotary dial
const byte RGB_CLK_PIN = 18;
const byte RGB_DT_PIN = 19;
BasicEncoder rgb_control(RGB_CLK_PIN, RGB_DT_PIN);
//Buttons
#define RGB_R_BUTTON 28
#define RGB_G_BUTTON 29
#define RGB_B_BUTTON 30
byte RGB_Control = 0;
#define RGB_CONTROL_RED 0
#define RGB_CONTROL_GREEN 1
#define RGB_CONTROL_BLUE 2
//RGB Light
#define RGB_PIN_RED 44
#define RGB_PIN_GREEN 45
#define RGB_PIN_BLUE 46

#define STAGE2_RED_LIGHT 31

//Stage 3
#define STAGE3_GOOD_INPUT 33
#define STAGE3_BAD_INPUT 32
#define STAGE3_RED_LIGHT 34

String RBG_Color_Names[3] = { "red", "green", "blue" };
byte RGB_Color_Values[3] = {250, 130, 115};

boolean stage1 = false;
boolean stage2 = false;
boolean stage3 = false;
boolean stage2bad = false;
boolean stage3bad = false;

void setup() {
  Serial.begin(115200);
  Serial.println("Hello Arduino\n");

  //Results
  pinMode(STAGE_1_LED, OUTPUT);
  pinMode(STAGE_2_LED, OUTPUT);
  pinMode(STAGE_3_LED, OUTPUT);

  lcd.init(); //initialize the lcd
  lcd.backlight(); //open the backlight
  lcd.createChar(1, angryLeft);
  lcd.createChar(2, angryRight);
  lcd.createChar(3, smileLeft);
  lcd.createChar(4, smileRight);
  //Stage 1
  pinMode(SWITCH_1_PIN, INPUT_PULLUP);
  pinMode(SWITCH_2_PIN, INPUT_PULLUP);
  pinMode(SWITCH_3_PIN, INPUT_PULLUP);
  pinMode(SWITCH_4_PIN, INPUT_PULLUP);
  pinMode(SWITCH_5_PIN, INPUT_PULLUP);
  
  //Stage 2
  pinMode(RGB_R_BUTTON,INPUT_PULLUP);
  pinMode(RGB_G_BUTTON,INPUT_PULLUP);
  pinMode(RGB_B_BUTTON,INPUT_PULLUP);

  pinMode(RGB_PIN_RED, OUTPUT);
  pinMode(RGB_PIN_GREEN, OUTPUT);
  pinMode(RGB_PIN_BLUE, OUTPUT);
  pinMode(STAGE2_RED_LIGHT, OUTPUT);

  attachInterrupt(digitalPinToInterrupt(RGB_CLK_PIN), updateEncoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(RGB_DT_PIN), updateEncoder, CHANGE);

  //Stage 3
  pinMode(STAGE3_GOOD_INPUT, INPUT_PULLUP);
  pinMode(STAGE3_BAD_INPUT, INPUT_PULLUP);
  pinMode(STAGE3_RED_LIGHT, OUTPUT);
}

void loop() {
  currentMillis = millis();
  currentSeconds = currentMillis % 1000;

  //Stage 1
  if (digitalRead(SWITCH_1_PIN) == LOW
      && digitalRead(SWITCH_2_PIN) == LOW
      && digitalRead(SWITCH_3_PIN) == LOW
      && digitalRead(SWITCH_4_PIN) == LOW
      && digitalRead(SWITCH_5_PIN) == LOW) {
    stage1 = true;
  } else {
    stage1 = false;
  }

  //debugStage1(stage1);
  if(stage1) {
    //Stage 2
    if (digitalRead(RGB_R_BUTTON) == LOW) {
      RGB_Control = RGB_CONTROL_RED;
    } else if (digitalRead(RGB_G_BUTTON) == LOW) {
      RGB_Control = RGB_CONTROL_GREEN;
    } else if (digitalRead(RGB_B_BUTTON) == LOW) {
      RGB_Control = RGB_CONTROL_BLUE;
    }
    
    long rgbControlChange = rgb_control.get_change();
    if(rgbControlChange) {
        Serial.print("RGB Control Color ");
        Serial.println(RBG_Color_Names[RGB_Control]);
        
        Serial.print("RGB Control Change ");
        Serial.println(rgbControlChange);
        int colorChangeAmount = RGB_Color_Values[RGB_Control] + rgbControlChange * -15;
        if (colorChangeAmount < 0) {
          colorChangeAmount = 0;
        } else if (colorChangeAmount > 255) {
          colorChangeAmount = 255;
        }
        RGB_Color_Values[RGB_Control]  = colorChangeAmount;

        Serial.print("Red ");
        Serial.print(RGB_Color_Values[RGB_CONTROL_RED]);
        Serial.print(" Green ");
        Serial.print(RGB_Color_Values[RGB_CONTROL_GREEN]);
        Serial.print(" Blue ");
        Serial.println(RGB_Color_Values[RGB_CONTROL_BLUE]);
      }
      analogWrite(RGB_PIN_RED, RGB_Color_Values[RGB_CONTROL_RED]);
      analogWrite(RGB_PIN_GREEN, RGB_Color_Values[RGB_CONTROL_GREEN]);
      analogWrite(RGB_PIN_BLUE, RGB_Color_Values[RGB_CONTROL_BLUE]);

    if (RGB_Color_Values[RGB_CONTROL_RED] >= 240
        && RGB_Color_Values[RGB_CONTROL_GREEN] >= 240
        && RGB_Color_Values[RGB_CONTROL_BLUE] >= 240) {
        digitalWrite(STAGE2_RED_LIGHT, HIGH);
        stage2bad = true;
    }
  } else {
    analogWrite(RGB_PIN_RED, 0);
    analogWrite(RGB_PIN_GREEN, 0);
    analogWrite(RGB_PIN_BLUE, 0);
  }

  stage2 = RGB_Color_Values[RGB_CONTROL_RED] >= 240
          && RGB_Color_Values[RGB_CONTROL_GREEN] <= 15
          && RGB_Color_Values[RGB_CONTROL_BLUE] >= 240;


  if (digitalRead(STAGE3_BAD_INPUT) == LOW) {
    digitalWrite(STAGE3_RED_LIGHT, HIGH);
    stage3bad = true;
  } else if (digitalRead(STAGE3_GOOD_INPUT) == LOW) {
    stage3 = true;
  }


  //Final Results
  if(currentMillis % 4000 < 1000) {
    if(!stage1 || !stage2 || !stage3) {
      stageLightsOff();
      angryFaceOne();
    }
  }
  if(currentMillis % 4000 > 1000) {
    if(stage1) {
      digitalWrite(STAGE_1_LED, HIGH);
    }
  }
  if(currentMillis % 4000 > 2000) {
    if(stage1 && stage2) {
      digitalWrite(STAGE_2_LED, HIGH);
    } else {
      digitalWrite(STAGE_2_LED, LOW);
    }
  }
  if(currentMillis % 4000 > 3000) {
    if(stage1 && stage2 && stage3) {
      digitalWrite(STAGE_3_LED, HIGH);
      if(stage2bad || stage3bad) {
        smirkyFace();
      } else {
        happyFace();
      }
      
    } else {
      angryFaceTwo();
    }
  }
}

void stageLightsOff() {
  digitalWrite(STAGE_1_LED, LOW);
  digitalWrite(STAGE_2_LED, LOW);
  digitalWrite(STAGE_3_LED, LOW);
}

void updateEncoder() {
  rgb_control.service();  // Call BasicEncoder library .service()
}

void angryFaceOne() {
  lcd.setCursor(0, 0);
  lcd.write(1);lcd.print("  ");lcd.write(2);lcd.write(1);lcd.print("  ");lcd.write(2);
  lcd.write(1);lcd.print("  ");lcd.write(2);lcd.write(1);lcd.print("  ");lcd.write(2);
  lcd.setCursor(0, 1);
  lcd.print(" ");lcd.write(1);lcd.write(2);lcd.print("  ");lcd.write(1);lcd.write(2);
  lcd.print("  ");lcd.write(1);lcd.write(2);lcd.print("  ");lcd.write(1);lcd.write(2);lcd.print(" ");
}

void angryFaceTwo() {
  lcd.setCursor(0, 0);
  lcd.print(" ");lcd.write(2);lcd.write(1);lcd.print("  ");lcd.write(2);
  lcd.write(1);lcd.print("  ");lcd.write(2);lcd.write(1);lcd.print("  ");lcd.write(2);lcd.write(1);lcd.print(" ");
  lcd.setCursor(0, 1);
  lcd.write(2);lcd.print("  ");lcd.write(1);lcd.write(2);
  lcd.print("  ");lcd.write(1);lcd.write(2);lcd.print("  ");lcd.write(1);lcd.write(2);lcd.print("  ");lcd.write(1);
}

void happyFace() {
  lcd.setCursor(0,0);
  lcd.write(1);lcd.print("              ");lcd.write(2);
  lcd.setCursor(0, 1);
  lcd.print(" ");lcd.write(3);lcd.print("____________");lcd.write(4);lcd.print(" ");
}

void smirkyFace() {
  lcd.setCursor(0,0);
  lcd.print("               ");lcd.write(2);
  lcd.setCursor(0, 1);
  lcd.print("______________");lcd.write(4);lcd.print(" ");
}

void debugStage1(bool stage1) {
  Serial.print("Switch 1 ");
  Serial.print(digitalRead(SWITCH_1_PIN));
  Serial.print(" Switch 2 ");
  Serial.print(digitalRead(SWITCH_2_PIN));
  Serial.print(" Switch 3 ");
  Serial.print(digitalRead(SWITCH_3_PIN));
  Serial.print(" Switch 4 ");
  Serial.print(digitalRead(SWITCH_4_PIN));
  Serial.print(" Switch 5 ");
  Serial.println(digitalRead(SWITCH_5_PIN));
  if(stage1) {
    Serial.println("Stage 1 complete");
  } else {
    Serial.println("Stage 1 not complete");
    return;
  }
}