// Library is used to communicate with I2C devices. Found at the Arduino website.
// http://arduino.cc/en/Reference/Wire
#include <Wire.h>
// Libraries ued for the 9DOF sensor. These two libraries are used to access data
// from the Gyro on the sensor.
// Sensor used below can be found on adafruit/gethub website at the links below
// https://github.com/adafruit/Adafruit_Sensor
#include <Adafruit_Sensor.h>
// Library below is used for gyro found at website blow.
// https://github.com/adafruit/Adafruit_L3GD20_U
#include <Adafruit_L3GD20_U.h>
// Servo library is used to control the servos and can be found at Arduino website
// http://arduino.cc/en/Reference/Servo
#include <Servo.h>


// Creating a varibale for the gyro
Adafruit_L3GD20_Unified gyro = Adafruit_L3GD20_Unified(20);

// Two instances for the servos.
Servo myservoR;
Servo myservoL;
// Variables for the keeping track of angle robot is facing
int mil = 0;
int rnd = 0;
float temp = 0;
float zspin = 0;
float degree = 0;
float lastDeg = 0;
float degPos = 0;


// Sensitivity variables. Control how fast the robot spins/moves.
float senPlus = .01;  //Gyro
float senNeg = .01;   //Gyro
float senDegree = .6;  //Gyro
float DistanceScale = .7; //Distance smaller = Farther


int driveFlag = 0;
int goalFlag = 0;
int stopFlag = 0;
int calcFlag = 0;


// Map info
float Xgoal = 60; //Stop coords
float Ygoal = 60; //Stop coords
float Xpos = 0; // Keep current position
float Ypos = 0;
long timeTrack;    //Keeps track of millis()
long timeTemp;
float tempVal = 0;
float calcAng = 0;
float time = 0;
float tempX;
float tempY;

//Dist sensor
int distF;
int distR;
int distL;
int clearObFlag = 0;
int angle;

void setup(void) 
{
  Serial.begin(9600);
  
  // lines of code below were found from adafruit library example.
  /* Enable auto-ranging */
  gyro.enableAutoRange(true);
  
  /* Initialise the sensor */
  if(!gyro.begin())
  {
    /* There was a problem detecting the L3GD20 ... check your connections */
    //Serial.println("Ooops, no L3GD20 detected ... Check your wiring!");
    while(1);
  }
  
  /* Display some basic information on this sensor */
  sensor_t sensor;
  gyro.getSensor(&sensor);
  delay(500);
  // End code from library.   

  //Assign servo to pin and set them to be still.
  myservoR.attach(9);
  myservoL.attach(10);
  myservoR.write(90);  // set servo to mid-point
  myservoL.write(90);
  
  // Just reading sensor to make sure that it is up and running.
  goalFlag = millis();
  driveFlag = goalFlag;
  while (goalFlag < 3000){
    goalFlag = millis();
    if (goalFlag != driveFlag){
      driveFlag = goalFlag;
      Degree();
    }
  }
  goalFlag = 1;
  driveFlag = 0;
}



void loop() {

  // timeTrack is incremented every 5ms and the sensors are read.
  if ((timeTrack + 5) < millis()){
    timeTrack = millis();
    // Reading the gyro
    Degree();
    // Reading the distance sensors.
    distF = distanceRead(6);  
    distL = distanceRead(0);
    distR = distanceRead(7);
    // Check is a object is in range of the sensor.
    if (distF < 10 && distF > 3){
      distF = 1;
    }
    else{
      distF = 0;
    }
    if (distR < 8 && distR > 3){
      distR = 1; 
    }
    else{
      distR = 0;
    }
    if (distL < 10 && distL > 3){
      distL = 1;
    }
    else{
      distL = 0;
    }
  }
  
  
  
  // NO obstacal so go towards goal.
  if (goalFlag == 1){ //If goal flag is 1 move towards goal else move around object
    //Rotate towards goal
    if ((driveFlag == 0) && (stopFlag == 0)){
      // Get angle of robot
      tempX = Xgoal - Xpos;
      tempY = Ygoal - Ypos;
      calcAng = atan(tempY/tempX);
      // If robot not in tolerance rotate.
      if (((calcAng + .07) < degPos) || ((calcAng - .07) > degPos)){
        if ((calcAng - degPos) > 0){
          driveMotor('l');     
        }
        else{
          driveMotor('r');         
        }
        timeTemp = timeTrack + 1;  
        calcFlag = 0;
        stopFlag = 1;
      }
      else{
        // If robot in tolerance set flag to move towards goal
        driveFlag = 1;
      }
    }
    
    //Drive towards goal
    if ((driveFlag == 1) && (stopFlag == 0)){
      driveMotor('f');    
      timeTemp = timeTrack + 100;  
      calcFlag = 1;
      stopFlag = 1;
    }  
 
    // If an object is in range of sensor stop go to move around object.
    if (distF == 1 || distR == 1){
      driveMotor('s');
      if (calcFlag == 1){
	// Adding distance moved to variables Xpos, Ypos
        time = timeTemp - timeTrack;
        time = time / float(100);        
        time = time * DistanceScale;
        tempVal = cos(degPos);
        tempVal = time*tempVal;
        Xpos = Xpos + tempVal;
        tempVal = sin(degPos);
        tempVal = time*tempVal;
        Ypos = Ypos + tempVal;
        calcFlag = 0;
      }
      goalFlag = 0;
      stopFlag = 0;
      driveFlag = 0;
    }
 
    // Stop after each l turn, r turn or move forward
    // Stop after time
    // Once stopFlag == 1 stop robot.
    // If the robot is told to move forward 100 timeTemp will be set timeTrack + 100
    // and once timeTrack is > timeTemp the robot will be told to stop. The code was
    // written this way so the gyro and distance sensors can be continuously read while
    // the robot is moving.
    if ((stopFlag == 1) && (timeTrack > timeTemp)){
      driveMotor('s');
      stopFlag = 0;
      driveFlag = 0;
      if (calcFlag == 1){
        tempVal = cos(degPos);
        tempVal = DistanceScale*tempVal;
        Xpos = Xpos + tempVal;
        tempVal = sin(degPos);
        tempVal = DistanceScale*tempVal;
        Ypos = Ypos + tempVal;
        calcFlag = 0;
      }
    } 
  }
  
  
  
  
  
  // An object is in sensor range we will move around it.
  // If statement to follow object
  if (goalFlag == 0){
    
    // If object is in front of robot turn left.
    if (distF == 1 && distL == 0){
      // Turn left
      if (driveFlag == 0 && stopFlag == 0){
        driveMotor('l');
        timeTemp = timeTrack + 1;
        stopFlag = 1;   
      }
    }
    
    
    // If object is in front and to the right of robot rotate left.
    // Porbably redurdant 
    if (distF == 1 && distR == 1 && distL == 0){
      if (driveFlag == 0 && stopFlag == 0){
        driveMotor('l');
        timeTemp = timeTrack + 1;
        stopFlag = 1;        
      }
    }
    
   
      
      
    // if object is on the right and nothing is in front of robot.
    if (distF == 0 && distR == 1  && distL == 0){
      // Find angle the robot 
      tempX = Xgoal - Xpos;
      tempY = Ygoal - Ypos;
      calcAng = atan(tempY/tempX);
      clearObFlag = 0;
      
     
      if (driveFlag == 0 && stopFlag == 0){
        // If object is to close to object turn away from object
        if (distR < 10){
          driveMotor('l');
          timeTemp = timeTrack + 1;
          stopFlag = 1;
        }
	// If object is to far move closer to object.
        else if (distR > 7){
          driveMotor('r');
          timeTemp = timeTrack + 1;
          stopFlag = 1;
        }
	// If object is pointing towards goal and nothing in front of robot move towards goal
        else if (((calcAng + .07) < degPos) || ((calcAng - .07) > degPos)){
          driveMotor('f');
          timeTemp = timeTrack + 100;
          stopFlag = 1;
          driveFlag = 1;
          calcFlag = 1;
          goalFlag = 1;
        }
	// Otherwise just move forward.
        else{
          driveMotor('f');
          timeTemp = timeTrack + 20;
          tempVal = cos(degPos);
          tempVal = DistanceScale*tempVal;
          tempVal = tempVal/5;
          Xpos = Xpos + tempVal;
          tempVal = sin(degPos);
          tempVal = DistanceScale*tempVal;
          tempVal = tempVal/5;
          Ypos = Ypos + tempVal;
          stopFlag = 1;
          driveFlag = 1;
        }
      }
    }
    
    // If nothing around robot set goalFlag = 1 so the robot will run off code above and
    // move towards goal
    if (distF == 0 && distR == 0 && distL == 0){
      
      if (driveFlag == 0 && stopFlag == 0){
          driveMotor('f');
          timeTemp = timeTrack + 100;
          stopFlag = 1;
          driveFlag = 1;
          calcFlag = 1;
          clearObFlag++;
          if (clearObFlag == 2){
            clearObFlag = 0;
            goalFlag = 1;
          }
      }
      
    }
    
    
   
    
    // Stop robot after robot has moved for desired amount of time.
    // Same code as stop code in if statement above.
    if ((stopFlag == 1) && (timeTrack > timeTemp)){
      driveMotor('s');
      stopFlag = 0;
      driveFlag = 0;
      if (calcFlag == 1){
        tempVal = cos(degPos);
        tempVal = DistanceScale*tempVal;
        Xpos = Xpos + tempVal;
        tempVal = sin(degPos);
        tempVal = DistanceScale*tempVal;
        Ypos = Ypos + tempVal;
        calcFlag = 0;
      } 
    
    }
  }
  
  // Once robot reaches goal stop robot.
  if (Xpos > Xgoal && Ypos > Ygoal){
      driveMotor('s');
      stopFlag = 3;   
  }
}

// Read gyro.
void Degree(){
  sensors_event_t event; 
  gyro.getEvent(&event);
  // Read sensor.  
  zspin = event.gyro.z;
  temp = abs(zspin);
  // Round value read so sensor wont drift so much.
  rnd = int(temp + .95);
  
  // Interpreting the data read so the degPos will be between 0 - 360 degrees
  if (rnd != 0){
    if (zspin > 0){
      degree = degree + zspin*senPlus;
      temp = abs(degree - lastDeg);
      temp = temp * senDegree;
      degPos = degPos + temp;
    }
    else{
      degree = degree + zspin*senNeg;
      temp = abs(degree - lastDeg);
      temp = temp * senDegree;
      degPos = degPos - temp;
    }
    lastDeg = degree;   
    if (degPos < 0){
      degPos = abs(degPos);
      degPos = 2*PI - degPos;
    }    
    if (degPos > 2*PI){
      degPos = degPos - 2*PI;
    }
  }
}

// Drive the servo motors.
void driveMotor(char direct){
  // Turn robot right
  if (direct == 'r'){
    myservoR.write(80); //Right
    myservoL.write(80); 
  }
  // Turn robot right
  else if (direct == 'l'){
    myservoR.write(100); //Left
    myservoL.write(100);
  }
  // Drive forward
  else if (direct == 'f'){
    myservoR.write(70); //Forward  60
    myservoL.write(110);           //170
  }
  // Stop robot
  else{
    myservoR.write(90); //Stop
    myservoL.write(90);
  } 
}

// Meth to read the distance sensors.
int distanceRead(unsigned char inpin){
        int input = analogRead(inpin);   
        if (input < 100){
          return 255;
        }   
	unsigned char distance = (2076/(input - 11));
	return distance;
}