// C++ code
//
#include <Servo.h>

//The code uses the sensors results to determine which motor to turn in order to get the center sensor alligned with the center of the users face.

Servo positional; // 360 servo motor setup
Servo continuous; // 180 servo motor setup

//assigning names to the pins
#define trigPin1 13
#define echoPin1 7
#define trigPin2 12
#define echoPin2 4
#define trigPin3 8
#define echoPin3 2

#define slideswitch   11

//set long variables to be used in upcoming code
long duration, distance, Face_left, Face_center, Face_right;

int angle = 30;

void setup()
{
Serial.begin (9600);
 
//setting up inputs and outputs for all used ports on the arduino
pinMode(trigPin1, OUTPUT);
pinMode(echoPin1, INPUT);
pinMode(trigPin2, OUTPUT);
pinMode(echoPin2, INPUT);
pinMode(trigPin3, OUTPUT);
pinMode(echoPin3, INPUT);

pinMode(slideswitch, INPUT);


//assigning the motors to a pin
continuous.attach(5);
positional.attach(3);

//set the 180 servo motor to a base angle.
positional.write(30);
}


void loop() {
 
//Next 10 lines were copied/altered naming only
//Uses SonarSenor function to gather the distance and assign the distance to a variable to be used for the motors
SonarSensor(trigPin1, echoPin1);
Face_left = distance;
SonarSensor(trigPin2, echoPin2);
Face_center = distance;
SonarSensor(trigPin3, echoPin3);
Face_right = distance;

//printout of the distances
Serial.print(Face_left);
Serial.print(" - ");
Serial.print(Face_center);
Serial.print(" - ");
Serial.println(Face_right);


//if the switch is flipped one way...
if (digitalRead(11)){
 delay(500);
 continuous.write(90); //Set 360 motor to about 0 speed
 positional.write(30); //set 180 motor to start angle of 30 degrees
}
 
//if the siwtch is flipped the other way
else{
 
  //Following if and else if statements cover the possible scenarios needed navigate towards the center of a face, assuming there is one
  if (Face_left > 50 && Face_right > 50 && Face_center > 50){
      continuous.write(100); //set motor spinning at low speed in one driection
       delay(200);
  }
 
   else if (Face_left < 50 && Face_right < 50 && Face_center < 50){ //ideally represents hitting a persons chest, so the next logical step would be to rotate the sensors up.
        continuous.write(90);
        delay(200);
        
       for (angle; angle <= 90; angle++){  //for loop to gradually increase the angle until either one or both side sensors hit a value greater than 2ft
     
           positional.write(angle);//sets the angle for the positional motor
           delay(100);
           //same copied code from before now included in the for loop
          SonarSensor(trigPin1, echoPin1);
          Face_left = distance;
          SonarSensor(trigPin2, echoPin2);
          Face_center = distance;
          SonarSensor(trigPin3, echoPin3);
          Face_right = distance;

          Serial.print(Face_left);
          Serial.print(" - ");
          Serial.print(Face_center);
          Serial.print(" - ");
          Serial.println(Face_right);
         
           if (Face_left > 50 && Face_right > 50 && Face_center < 50){
            delay(100);
             exit(0); //found the face
         }    
           else if(Face_left > 50 || Face_right > 50){
            break; //return back to spinning in 360 degrees
         }
           
     }
    }
       else if (Face_left > 50 && Face_right > 50 && Face_center < 50){
           exit(0);  // found the face
         }  
       else if(Face_right < 50 && Face_center < 50 && Face_left > 50){ 
          continuous.write(100);       //     rotate until all 3 sensors get a value less than 50cm or the center of the face is found
         }   
       else if(Face_left < 50 && Face_center < 50 && Face_right > 50){
         continuous.write(100);       //     rotate until all 3 sensors get a value less than 50cm or the center of the face is found

       }

       
}
}


//This portion of the code was copied, it just converts the signal from the snesors to a numercal value in cm
void SonarSensor(int trigPin,int echoPin)
{
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;

}