#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#include <Stepper.h>

LiquidCrystal_I2C lcd(0x27,16,2);

const int steps = 200;
Stepper myStepper(steps, 4, 5, 6, 7);

const int triggerPin = 3;
const int echoPin = 2;

const int t = A0;
const int f = A1;

const int redd = 10;
const int greenn = 11;
const int bluee = 12;

long readUltrasonicDistance(int triggerPin, int echoPin) {
  pinMode(triggerPin, OUTPUT);
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  pinMode(echoPin, INPUT);
  return pulseIn(echoPin, HIGH);
}

void setup() {
  lcd.init();
  lcd.backlight();
  
  pinMode(t, INPUT_PULLUP);
  pinMode(f, INPUT_PULLUP);

  pinMode(redd, OUTPUT);
  pinMode(greenn, OUTPUT);
  pinMode(bluee, OUTPUT);
  
  standbyMode();
}

void loop() {
  float cm = 0.0344 / 2 * readUltrasonicDistance(triggerPin, echoPin);
  
  if (cm < 50) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Is 1 + 1 = 2?");
    
    standbyMode();

    while (digitalRead(t) == HIGH && digitalRead(f) == HIGH) {
      // Wait for button press
    }

    if (digitalRead(t) == LOW) {
      correctAnswer();
    } 
    else if (digitalRead(f) == LOW) {
      incorrectAnswer();
    }

    myStepper.step(steps / 4);
    delay(2000);
  } else {
    lcd.clear();
    lcd.noBacklight();
    resetRGB();
  }
}

void standbyMode() {
  digitalWrite(redd, LOW);
  digitalWrite(greenn, LOW);
  digitalWrite(bluee, HIGH);
  lcd.backlight();
}

void correctAnswer() {
  digitalWrite(redd, LOW);
  digitalWrite(greenn, HIGH);
  digitalWrite(bluee, LOW);
}

void incorrectAnswer() {
  digitalWrite(redd, HIGH);
  digitalWrite(greenn, LOW);
  digitalWrite(bluee, LOW);
}

void resetRGB() {
  digitalWrite(redd, LOW);
  digitalWrite(greenn, LOW);
  digitalWrite(bluee, LOW);
}