//Cyclops5: Hori and Vert, also: bring back the motors and indicators...

#include <AFMotor.h>       // Adafruit MotorShield
#include <VarSpeedServo.h> // VarSpeedServo library gives more options, but variable speed isnt used yet

const int PR = A1; // connect PR to A1
int Array[125];    // array to store sweep values

VarSpeedServo Hori;
int HoriPos = 90; //  centre is 90. hardware modififed to prevent clashes. 30 (left) to 150 (right)
int CWHoriDir = 0;
int CCWHoriDir = 0;

VarSpeedServo Vert;
int VertPos = 90; // 130 = level, 0 is back on itself, 30 is straight up
int UpVertDir = 0;
int DownVertDir = 0;

AF_DCMotor motorL(3, MOTOR12_1KHZ);  
AF_DCMotor motorR(2, MOTOR12_1KHZ);
const int RLED = 13; // indicator LED: 13 and 2 are the only properly unused digi pins on the motorshield.
const int LLED = 2;  // indicator LED

int VertDelay;    // integer used to calculate "distance" to travel. Shallow angle on Vert indicates distance to light is greater,and increases time to run motors. 
int HoriDelay;    // Steep (vertical) angle indicates closer to light and therefore moves in small steps to help prevent over run

// front sensors and integers for calibration
const int LeftPR = A2;    // pin that the sensor is attached to
const int CentrePR = A3;    // pin that the sensor is attached to
const int RightPR = A4;    // pin that the sensor is attached to
int LeftValue = 0;         // the sensor value
int LeftMin = 1023;        // minimum sensor value
int LeftMax = 0;           // maximum sensor value
int CentreValue = 0;         // the sensor value
int CentreMin = 1023;        // minimum sensor value
int CentreMax = 0;           // maximum sensor value
int RightValue = 0;         // the sensor value
int RightMin = 1023;        // minimum sensor value
int RightMax = 0;           // maximum sensor value

unsigned long previous = 0;
unsigned long current = 0;
unsigned long interval = 1000;




void setup() { // Setup*******************

  Serial.begin(9600);  // Serial at 9600 bps
  Hori.attach(9);           // 9 is "Servo2"
  Vert.attach(10);          // 10 is "Servo1"
  motorL.setSpeed(250);     // 0 is stop, 255 is full speed:
  motorR.setSpeed(250);     // 
  pinMode (RLED,OUTPUT);    // indicator LEDS. HIGH is on, LOW is off
  pinMode (LLED,OUTPUT);
 
 
  // **************calibrate ************
  {  
    while (millis() < 5000) { 

      //Spin on the spot 
    motorL.run(FORWARD);  
    motorR.run(BACKWARD);  
    digitalWrite(LLED, LOW);
    digitalWrite(RLED, HIGH);

      //Calibrate 
      LeftValue = analogRead(LeftPR);
      // record the maximum sensor value
      if (LeftValue > LeftMax) {
        LeftMax = LeftValue;
      }
      // record the minimum sensor value
      if (LeftValue < LeftMin) {
        LeftMin = LeftValue;
      }
      CentreValue = analogRead(CentrePR);
      // record the maximum sensor value
      if (CentreValue > CentreMax) {
        CentreMax = CentreValue;
      }
      // record the minimum sensor value
      if (CentreValue < CentreMin) {
        CentreMin = CentreValue;
      }
      RightValue = analogRead(RightPR);
      // record the maximum sensor value
      if (RightValue > RightMax) {
        RightMax = RightValue;
      }
      // record the minimum sensor value
      if (RightValue < RightMin) {
        RightMin = RightValue;
      }
    }


    motorL.run(RELEASE);  
    motorR.run(RELEASE);  
    digitalWrite(LLED, LOW);
    digitalWrite(RLED, LOW);

    // ***************end of the calibration******************

    BlindSpot();         //BlindSpot Check
    unsigned long current = millis();
  }
  
}
void BlindSpot (){

  for(VertPos = 100; VertPos >= 1; VertPos -= 1) 
  {    
    Array[VertPos-1] = analogRead(PR);
    Vert.write(VertPos);
    if ((Array[VertPos-1] < Array[VertPos]) && (VertPos<100)) {
      UpVertDir = VertPos;
    }
    delay (10) ;       
  }

  for(VertPos = 1; VertPos < 100; VertPos += 1) 
  {    
    Array[VertPos-1] = analogRead(PR);
    Vert.write(VertPos);
    if ((Array[VertPos-1] < Array[VertPos+1]) && (VertPos>1)) {
      DownVertDir = VertPos;
    }
    delay (10) ;       
  }
  VertPos = ((UpVertDir+DownVertDir)/2);   // Average the results to get the centre
  Vert.write(VertPos);   // Go to the position and print the results
  Serial.print("UpSweep: ");
  Serial.print(UpVertDir);
  Serial.print(", DownSweep: ");
  Serial.print(DownVertDir);
  Serial.print(", VertPos: ");
  Serial.println(VertPos);
  delay (100);

  Vert.write(100);

  if(VertPos < 50){   
    motorL.run(FORWARD);  
    motorR.run(BACKWARD);  
    digitalWrite(LLED, LOW);
    digitalWrite(RLED, HIGH);
    delay (1750);
    motorL.run(RELEASE);  
    motorR.run(RELEASE);  
    digitalWrite(LLED, LOW);
    digitalWrite(RLED, LOW);

  }
}
void Decisions(){

 

  if((HoriPos > 110) && (HoriPos <= 150)){// single wheel moving
    HoriDelay = map (HoriPos, 110, 150, 100,1000);

    if (current - previous < interval){
    motorL.run(FORWARD);  
    motorR.run(BACKWARD);  
    digitalWrite(LLED, LOW);
    digitalWrite(RLED, HIGH);
  }
else{
    motorL.run(RELEASE);  
    motorR.run(RELEASE);  
    digitalWrite(LLED, LOW);
    digitalWrite(RLED, LOW);

    }
  }


  //LEFT
  if((HoriPos < 70) && (HoriPos >= 30)){ // Shallow angle results in single wheel moving
    HoriDelay = map (HoriPos, 70, 30, 100, 1000);
  
  if (current - previous < interval){
    motorL.run(BACKWARD);  
    motorR.run(FORWARD);  
    digitalWrite(LLED, HIGH);
    digitalWrite(RLED, LOW);
  }
else{
    motorL.run(RELEASE);  
    motorR.run(RELEASE);  
    digitalWrite(LLED, LOW);
    digitalWrite(RLED, LOW);
    }
  }

  // FORWARD run forward: Window of 20 degrees in the centre results in running forward
  if((HoriPos >= 70) && (HoriPos <= 110)){ 
    VertDelay = map (VertPos, 130, 30, 10000, 500);
      
  if (current - previous < interval){
    motorL.run(FORWARD);  
    motorR.run(FORWARD);  
    digitalWrite(LLED, HIGH);
    digitalWrite(RLED, HIGH);

  }

    else{

    motorL.run(RELEASE);  
    motorR.run(RELEASE);  
    digitalWrite(LLED, LOW);
    digitalWrite(RLED, LOW);

    }
  }

}
void HoriSweep (){ // Horizontal Sweep********

  for(HoriPos = 30; HoriPos < 150; HoriPos += 1) 
  {    
    Array[HoriPos-30] = analogRead(PR);
    Hori.write(HoriPos);
    if ((Array[HoriPos-30] < Array[HoriPos-31]) && (HoriPos>30)) {
      CWHoriDir = HoriPos;
    }
    delay (10) ;       
  }
  for(HoriPos = 150; HoriPos >= 30; HoriPos -= 1)
  {    
    Array[HoriPos-30] = analogRead(PR);
    Hori.write(HoriPos);
    if ((Array[HoriPos-30] < Array[HoriPos-29]) && (HoriPos<150)) {
      CCWHoriDir = HoriPos;
    }
    delay (10) ;       
  }
  HoriPos = ((CWHoriDir+CCWHoriDir)/2); // Average the two value to get the centre
  Hori.write(HoriPos); // Go to the position, and print the results to serial
  Serial.print("CWSweep: ");
  Serial.print(CWHoriDir);
  Serial.print(", CCWSweep: ");
  Serial.print(CCWHoriDir);
  Serial.print(", HoriPos: ");
  Serial.println(HoriPos);    
  delay (500);
}

void VertSweep (){ // Vertical Sweep*************

  for(VertPos = 130; VertPos >= 30; VertPos -= 1) 
  {    
    Array[VertPos-30] = analogRead(PR);
    Vert.write(VertPos);
    if ((Array[VertPos-30] < Array[VertPos-29]) && (VertPos<130)) {
      UpVertDir = VertPos;
    }
    delay (10) ;       
  }

  for(VertPos = 30; VertPos < 130; VertPos += 1) 
  {    
    Array[VertPos-30] = analogRead(PR);
    Vert.write(VertPos);
    if ((Array[VertPos-30] < Array[VertPos-31]) && (VertPos>30)) {
      DownVertDir = VertPos;
    }
    delay (10) ;       
  }
  VertPos = ((UpVertDir+DownVertDir)/2);   // Average the results to get the centre
  Vert.write(VertPos);   // Go to the position and print the results
  Serial.print("UpSweep: ");
  Serial.print(UpVertDir);
  Serial.print(", DownSweep: ");
  Serial.print(DownVertDir);
  Serial.print(", VertPos: ");
  Serial.println(VertPos);
  delay (100);

}


void loop() {
  

  HoriPos = constrain (HoriPos, 30, 150); // constrain servos, Just in case.
  VertPos = constrain (VertPos, 30, 130);
  HoriSweep();        //Horizontal Scan
  VertSweep();        // Vertical Scan
//********************************
  previous = current;
//********************************


    
  //Stop under light
  if(VertPos < 40){ 
    motorL.run(RELEASE);  
    motorR.run(RELEASE);  
    digitalWrite(LLED, LOW);
    digitalWrite(RLED, LOW);
    delay (99999999); // lazy mans ending
  }

  else {
    Decisions();        // Decisions Decisions
  }
}