//Variables
//Libraries
  #include <Servo.h>
  #include<LiquidCrystal.h>

//LEDs_Mandrill
  int LED_red = 23;
  int LED_yellow = 22;
  int LED_green = 24;
  int LED_blue = 26;
    //LED_States
  int LED_red_state = LOW;
  int LED_yellow_state = LOW;
  int LED_green_state = LOW;
  int LED_blue_state = LOW;

//Buttons_Human
  int Button_Human_reset = 53;//black
  int Button_Human_red = 52;//red
  int Button_Human_yellow = 50;//yellow
  int Button_Human_green = 48;//green
  int Button_Human_blue = 46;//blue
    //Button_States_Human
  int buttonState_reset;
  int buttonState_red;
  int buttonState_yellow;
  int buttonState_green;
  int buttonState_blue;

//Buttons_Mandrill
  int Button_Mandrill_red = 13;//red
  int Button_Mandrill_yellow = 12;//yellow
  int Button_Mandrill_green = 11;//green
  int Button_Mandrill_blue = 10;//blue
    //Button_States_Mandrill
  int buttonState_Mandrill_red;
  int buttonState_Mandrill_yellow;
  int buttonState_Mandrill_green;
  int buttonState_Mandrill_blue;

//Buzzer
  int Buzzer = 42;

//Ultra_Sonic
  int trig = 2;//Pin for sound going out
  int echo = 3;//pin for sound going in
  float distance;//a number for distance
  float time;//a number for time the sound left

//Servo
  Servo myservo;

//LCD Display
  const int rs = 40, en = 38, d4 = 30, d5 = 32, d6 = 34, d7 = 36;
  LiquidCrystal lcd(rs, en, d4, d5, d6, d7);

//System_Variables
  int Game_Start = 0;
  int Color_Data = 0;


//Setup
void setup()
{
//Open_Serial
  Serial.begin(9600);

//LEDs_Mandrill
  pinMode(LED_red, OUTPUT);
  pinMode(LED_yellow, OUTPUT);
  pinMode(LED_green, OUTPUT);
  pinMode(LED_blue, OUTPUT);

//Buttons_Human
    //high if not pushed, low if pushed; No need for resistors this way
   pinMode(Button_Human_reset, INPUT_PULLUP);
   pinMode(Button_Human_red, INPUT_PULLUP);
   pinMode(Button_Human_yellow, INPUT_PULLUP);
   pinMode(Button_Human_green, INPUT_PULLUP);
   pinMode(Button_Human_blue, INPUT_PULLUP);
    //Button_States_Human
   buttonState_reset = digitalRead(Button_Human_reset);
   buttonState_red = digitalRead(Button_Human_red);
   buttonState_yellow = digitalRead(Button_Human_yellow);
   buttonState_green = digitalRead(Button_Human_green);
   buttonState_blue = digitalRead(Button_Human_blue);

//Buttons_Mandrill
    //high if not pushed, low if pushed; No need for resistors this way
   pinMode(Button_Mandrill_red, INPUT_PULLUP);
   pinMode(Button_Mandrill_yellow, INPUT_PULLUP);
   pinMode(Button_Mandrill_green, INPUT_PULLUP);
   pinMode(Button_Mandrill_blue, INPUT_PULLUP);
     //Button_States_Mandrill
   buttonState_Mandrill_red = digitalRead(Button_Mandrill_red);
   buttonState_Mandrill_yellow = digitalRead(Button_Mandrill_yellow);
   buttonState_Mandrill_green = digitalRead(Button_Mandrill_green);
   buttonState_Mandrill_blue = digitalRead(Button_Mandrill_blue);

//Buzzer
  pinMode(Buzzer, OUTPUT);

//Ultra_Sonic
  pinMode(echo, INPUT);//echo takes in the sound
  pinMode(trig, OUTPUT);//trig gives the sound

//Servo
  myservo.attach(28);//attaches servo pin 9 to the servo object (named myservo)

//LCD Display
  lcd.begin(16, 2);



}

//Basic Fuctions
  void read_buttons()//This reads all buttons
  {
    //Human
    buttonState_reset = digitalRead(Button_Human_reset);
    buttonState_red = digitalRead(Button_Human_red);
    buttonState_yellow = digitalRead(Button_Human_yellow);
    buttonState_green = digitalRead(Button_Human_green);
    buttonState_blue = digitalRead(Button_Human_blue);
    //Mandrill
    buttonState_Mandrill_red = digitalRead(Button_Mandrill_red);
    buttonState_Mandrill_yellow = digitalRead(Button_Mandrill_yellow);
    buttonState_Mandrill_green = digitalRead(Button_Mandrill_green);
    buttonState_Mandrill_blue = digitalRead(Button_Mandrill_blue);
  }

  void find_distance()//This uses the Ultra_Sonic sensor to find the distance of how far the object in front of it is in cm
  {
    //This section is to find the distance
    digitalWrite(trig, LOW);//turn off the sound being sent out
    delayMicroseconds(2);//wait
    digitalWrite(trig, HIGH);//turn on the sound being sent out
    delayMicroseconds(10);//wait
    digitalWrite(trig, LOW);//turn off the sound being sent out
    time = pulseIn(echo, HIGH);//how long it took for the sound to come back
    distance = (time*.0343)/2;;   //This value is the distance from the sensor in centimeters
    //this section is complete
  }

//Specific Fuctions
  void LCD_Start()
  {
    lcd.setCursor(3,0);//(column,row) is how the Cursor works       
    lcd.print("Press Reset"); 
    lcd.setCursor(4,1);           
    lcd.print("To Start");
    delay(500);
    lcd.clear();
  }

  void Button_Start()
  {
    read_buttons();
    if(buttonState_reset == LOW){
      Color_Data = 0;//resets color data
      Game_Start = 1;//1 means that the game is on

      lcd.clear();
      lcd.setCursor(3,0);//(column,row) is how the Cursor works       
      lcd.print("Select Four"); 
      lcd.setCursor(5,1);           
      lcd.print("Colors");
      delay(500);
    }
    else{
      LCD_Start();
      Game_Start = 0;//0 means that the game is off
    }
  }

void loop()
{
  read_buttons();//Important For Everytime Buttons Will Be Used
  Button_Start();

}