#include <Servo.h>

Servo pusherServo;

const int backgroundPin = A0;
const int obstaclePinLow = A1;
const int obstaclePinHigh = A2;
const int outputPin = 2;

const int runAngle=0;
const int jumpAngle=20;  

// light sensor cutoff between light and dark
int cutoffBackground;
int cutoffLowLightBG;
int cutoffHighLightBG;
int cutoffLowDarkBG = 350; // does not need calibration
int cutoffHighDarkBG = 350; // does not need calibration

int threshold = 30;

bool backgroundLight = false;

void setup() {
  Serial.begin(9600);

  pusherServo.attach(9);

  pinMode(outputPin, OUTPUT);
  digitalWrite(outputPin, LOW);

  // set threshold for light being on on sensor
  // hold up to the monitor to get brightness calibrated

  Serial.print("Calibrate please\n");
  Serial.print("Hold device so all sensors on bright part of screen\n");
  delay(300);
  Serial.print("Calibrating");
  delay(1000);
  Serial.print(".");
  delay(1000);
  Serial.print(".");
  delay(1000);
  Serial.print(".\n");

  // background cut off for light vs dark
  cutoffBackground = analogRead(backgroundPin) - threshold;

  // obstacle cut off vs light screen
  cutoffLowLightBG = analogRead(obstaclePinLow) - threshold;
  cutoffHighLightBG = analogRead(obstaclePinHigh) - threshold;

  // obstacle cut off vs dark screen, manually set as this is easier to detect
  // these declared earlier

  Serial.print("Calibration Complete\n\n");
}

void loop() {
  bool needJump = false;
  digitalWrite(outputPin, LOW); 
  
  bool lowLight = false;
  bool highLight = false;
    
  int backgroundVal = analogRead(backgroundPin);
  int lowVal = analogRead(obstaclePinLow);
  int highVal = analogRead(obstaclePinHigh);

  // first deal with light background case
  if (backgroundVal > cutoffBackground) {
    if (backgroundLight == false){
      delay(500);
      backgroundLight = true;
    }
    if (lowVal > cutoffLowLightBG) lowLight = true;
    if (highVal > cutoffHighLightBG) highLight = true;

    // if either obstacle areas appear dark we have to jump
    if (!lowLight || !highLight) {
      needJump = true;
    }
  } else { // now deal with dark background case
    if (backgroundLight == true){
      delay(500);
      backgroundLight = false;
    }
    if (lowVal > cutoffLowDarkBG) lowLight = true;
    if (highVal > cutoffHighDarkBG) highLight = true;

    // if either obstacle areas appear light we have to jump
    if (lowLight || highLight) {
      needJump = true;
    }
    
  }
    if (needJump) {
    digitalWrite(outputPin, HIGH);
    delay(10);
    pusherServo.write(jumpAngle);
    delay(200);
    pusherServo.write(runAngle);
    delay(25);

  }
}