#include <LiquidCrystal_I2C.h>
#include <Servo.h>
int bigGoals=0, smallBottomGoals=0, smallTopGoals=0; //Variables for couting each goal type
int bigMult=5, smallBottomMult=2, smallTopMult=3; //Variables for setting points for each goal type
int bigPin=5, smallTopPin=4, smallBottomPin=3; //Variables for setting pins for each goal
int bottomServoPin=6, topServoPin=7; //Sets the pins for each servo
int angle=30; //Sets the angle for the bottom serv to rotate
boolean bigState=false, smallTopState=false, smallBottomState=false, bottomServoState=false; //Sets states for all sensors and servos
Servo bottomServo, topServo; //Creates both servos
LiquidCrystal_I2C lcd(0x27, 16, 2); //Creates display
long pastBottomMilli = 0, interval=5000, curMilli=millis(), topInterval=15000, pastTopMilli = 0;//All used for doing hings on an interval
//PastBottomMilli: Last time bottom moved, Interval: Time between bottom moves
//curMilli: Uses Milli() command to get current time in Milliseconds, topInterval: Time between top turns
//pastTopMilli: Time when top last moved
void setup() {
  // put your setup code here, to run once:
  lcd.init(); lcd.backlight(); //Setup for LCD
  Serial.begin(9600); //Sets up Serial
  bottomServo.attach(bottomServoPin); //Tells board bottomServo is attached on bottomServoPin
  topServo.attach(topServoPin); //Tells board topServo is attached on topServoPin
  bottomServo.write(90+angle); //Rotates bottomServo to its start position
  topServo.write(90);
}

void loop() {
  // put your main code here, to run repeatedly:
  curMilli = millis(); //Updates time
if(!digitalRead(bigPin)){ //If no object in front of sensor bigState is true
  bigState=true; //Stores whether there has been time where no object is in front of the sensor
}
if(bigState&&digitalRead(bigPin)){ //Checks if there has been a time with no object in front and an object is currently in front 
  bigGoals++; //Increments bigGoals
  bigState=false; //Resets bigGoals
}
if(!digitalRead(smallBottomPin)){ //If no object in front of sensor smallBottomState is true
  smallBottomState=true; //Stores whether there has been time where no object is in front of the sensor
}
if(smallBottomState&&digitalRead(smallBottomPin)){ //Checks if there has been a time with no object in front and an object is currently in front 
  smallBottomGoals++; //Increments smallBottomGoals
  smallBottomState=false; //Resets smallBottomState
}
if(!digitalRead(smallTopPin)){ //If no object in front of sensor smallTopState is true
  smallTopState=true; //Stores whether there has been time where no object is in front of the sensor
}
if(smallTopState&&digitalRead(smallTopPin)){ //Checks if there has been a time with no object in front and an object is currently in front 
  smallTopGoals++; //Increments smallTopGoals
  smallTopState=false; //Resets smallTopState
}
if(curMilli-pastBottomMilli>=interval){ //Checks if more time has passed than the interval
  pastBottomMilli=curMilli; //Resets time passed to 0
  if(bottomServoState){ //Checks if servo is one way and sets it to the other
    bottomServo.write(90+angle); //Tells servo to go right 
    bottomServoState=false; //Tells servo to next time go left
  }
  else{
    bottomServo.write(90-angle); //Tells servo to go left
    bottomServoState=true; //Tells servo to next time go right
  }
}
if(curMilli-pastTopMilli>=topInterval){ //Checks if more time has passed than the interval
  if(curMilli-pastTopMilli<17000){
  topServo.write(0);
  }
  else{
    topServo.write(90);
    pastTopMilli=curMilli; //Resets time passed to 0
  }
}
PrintScore(GetScore()); //Calls printscore with the score from GetScore()
}

void PrintScore(int score){ //Prints the scoreboard
lcd.setCursor(0,0); //Sets cursor to the start
lcd.print("B:"); lcd.print(bigGoals); //Prints B: for big goals followed by the amount of Big goals
lcd.print(" SB:"); lcd.print(smallBottomGoals); //Prints SB: for small bottom goals followed by the amount of small bottom goals
lcd.print(" ST:"); lcd.print(smallTopGoals); //Prints ST: for small top goals followed by the amount of small top goals
lcd.setCursor(0,1); //Sets cursor to the start of line 2
lcd.print("Score: "); lcd.print(score); //Prints Score: followed by the score
}

int GetScore(){ //Gets score by multiplying each goal by its point value
int total=0;
total+=bigGoals*bigMult;
total+=smallTopGoals*smallTopMult;
total+=smallBottomGoals*smallBottomMult;
return total;
}