#pragma config(Sensor, S1,     USSensor,            sensorSONAR)
#pragma config(Sensor, S2,     MSTMUX,              sensorLightInactive)
#pragma config(Sensor, S3,     HTMS,                sensorI2CCustom)
#pragma config(Sensor, S4,     DGPS,                sensorI2CCustom)
#pragma config(Motor,  motorA,          leftmoter,     tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          rightmotor,    tmotorNormal, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "drivers/common.h"
#include "drivers/HTMC-driver.h"
#include "drivers/MSTMUX-driver.h"
#include "drivers/DGPS-driver.h"

long targetHeading;
int side1, side2;
int currentHeading, TouchValue, distance, DGPSdistance;
string direction = "center";
int change = 0;
bool linkstatus = false;
long longitude = 0;
long latitude = 0;
long utc = 0;
long longitudeWaypoint[4];
long latitudeWaypoint[4];
long targetDistance = 0;
bool searchWaypoint = true;
int count = 1;

void Drive_Forward (int speed1, int speed2, int time) {  //set direction and speed of the 4 drive motors
  motor[motorA] = speed1;
  motor[motorC] = speed2;
  wait1Msec(time);
}

void Move_Forward (int duration, int speed1, int speed2) {
  nMotorEncoder[motorC] = 0;  //clear the LEGO motor encoders
  nMotorEncoder[motorA] = 0;
  wait1Msec(10);

  while (nMotorEncoder[motorA] > duration) { // 90 degrees to the right
    motor[motorC] = speed1; //turn both motors on at 30 percent power
    motor[motorA] = speed2;
  }
  motor[motorC] = 0; //turn both motors off
  motor[motorA] = 0;
}

void Compass_Check () {

  currentHeading = HTMCreadRelativeHeading(HTMS);  //take current compass reading

  side1 = currentHeading - 10; //create left side of compass range
  if (side1 < -179) {
    side1 = side1 + 180;
  }

  side2 = currentHeading + 10;  //create right side of compass range
  if (side2 > 180) {
    side2 = side2 - 180;
  }
}

void OrientToTarget () { //move robot closer to target compass heading

  if (currentHeading > targetHeading) {
    Drive_Forward (-45, 45, 100);
  }
  else if (currentHeading < targetHeading) {
    Drive_Forward (45, -45, 100);
  }
  Compass_Check(); //get new compass range before returning to main task
}

void ChangeDirection () {
  if (direction == "left") { //if robot turned left in the last 5 time continue to do so
    Drive_Forward (45, 45, 1000); //reverse
    Drive_Forward (45, -45, 1500); //turn left
    change = 5;  //now the robot should turn left unless it has gone forward 5 times in a row
  }
  else if (direction == "right") {
    Drive_Forward (45, 45, 1000); //reverse
    Drive_Forward (-45, 45, 1500); //turn right
    change = 5;  //now the robot should turn right unless it has gone forward 5 times in a row
  }
  else if (direction == "center") { // obstacle is detected look for a new direction to go for a bit
    Drive_Forward (45, 45, 2000); //reverse
    Drive_Forward (45, -45, 1500); //turn right
    int ScanRight = SensorValue(USSensor); //get ultrasonic sensor reading
    Drive_Forward (-45, 45, 3000); //turn to left side
    int ScanLeft = SensorValue(USSensor); //get ultrasonic sensor reading
    if (ScanRight < ScanLeft) {
//      Drive_Forward (45, -45, 1500); //turn left
      Drive_Forward (-75, -75, 2000);
      wait1Msec(2000);
      direction = "left"; //set direction to go left if obstacle is encountered before robot has gone forward less than 5 times
    }
    if (ScanRight > ScanLeft) {
      Drive_Forward (-45, 45, 3000);  //turn right
      Drive_Forward (-75, -75, 2000);
      wait1Msec(2000);
      direction = "right"; //set direction to go right if obstacle is encountered before robot has gone forward less than 5 times
    }
  }
}

void GPSSensorRead () {
  wait1Msec(500);
  utc = DGPSreadUTC(DGPS); // get coordinates and universal time code -- utc
  longitude = DGPSreadLongitude(DGPS);
  latitude = DGPSreadLatitude(DGPS);
  targetHeading = DGPSreadHeading(DGPS);
  targetHeading = targetHeading - 180;
  linkstatus = DGPSreadStatus(DGPS);
  DGPSdistance = DGPSreadDistToDestination(DGPS);
}

task main() {

  longitudeWaypoint[1] = -114097512;
  longitudeWaypoint[2] = -114097640;
  longitudeWaypoint[3] = -114097416;
  latitudeWaypoint[1] = 51793988;
  latitudeWaypoint[2] = 51793856;
  latitudeWaypoint[3] = 51793232;

  DGPSsetDestination(DGPS, latitudeWaypoint[count], longitudeWaypoint[count]); //input first of target coordinates

  while (true) {
//    while (linkstatus) {

      GPSSensorRead();  //get GPS information
      TouchValue = MSTMUXgetActive(MSTMUX); //get value from touch sensor multiplexer

      if (linkstatus) {
        Compass_Check (); //get compass reading and set range values
      }

      if ((side1 < targetHeading) && (side2 > targetHeading) && (linkstatus)) {
        distance = SensorValue(USSensor);
        if ((distance > 30) && (TouchValue == 0)) { // check for obstacles otherwise drive forward
          Drive_Forward (-75, -75, 100);
          change = change - 1;
          if (change < 0) {
            change = 0;
            direction = "center";
          }
        }
        else {
          ChangeDirection (); //obstacle detected move in a different direction
        }
      }

      if (TouchValue == 1) {  //left touch sensor pressed
        Drive_Forward (75, 75, 1000);  //backup and move to left side, then move forward
        Drive_Forward (-75, 75, 1000);
        Drive_Forward (-75, -75, 1000);
      }
      else if (TouchValue == 2) {  //right touch sensor pressed
        Drive_Forward (75, 75, 1000);  //backup and move to right side, then move forward
        Drive_Forward (75, -75, 1000);
        Drive_Forward (-75, -75, 1000);
      }
      else if (TouchValue == 3) {  //both touch sensors pressed use ChangeDirection to pick which side to use to avoid obstacle
        Drive_Forward (75, 75, 1000);
        ChangeDirection ();
      }

      if ((side1 > targetHeading) && (side2 > targetHeading) && (linkstatus)) {  //check which side of the target heading the robot is on and turn
        OrientToTarget ();
      }
      else if ((side1 < targetHeading) && (side2 < targetHeading) && (linkstatus)) {
        OrientToTarget ();
      }

      long a1 = longitudeWaypoint[count] - longitude;  //calculate distance to target
      long b1 = latitudeWaypoint[count] - latitude;
      long a2 = a1 * a1;
      long b2 = b1 * b1;
      targetDistance = sqrt(a2 + b2);
      if (targetDistance < 100) {
        count = count + 1;
      }
      eraseDisplay(); //display some imformation
      nxtDisplayTextLine(1, "Target: %4d", targetHeading);  //DGPS compass direction
      nxtDisplayTextLine(2, "Compass:   %4d", HTMCreadRelativeHeading(HTMS));  //HT compass heading
      nxtDisplayTextLine(3, "Lon: %d", longitude); //current longitude value
      nxtDisplayTextLine(4, "Lat: %d", latitude);  //current latitude value
      nxtDisplayTextLine(5, "Distance:  %4d", targetDistance);  //calculated distance to target
      nxtDisplayTextLine(6, "DGPS_dis: %4d", DGPSdistance);  //DGPS distance to target from sensor
    }
//  }
}

#pragma config(Sensor, S4,     DGPS,                sensorI2CCustom)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "drivers/DGPS-driver.h"

long targetHeading;
int DGPSdistance;
bool linkstatus = false;
long longitude = 0;
long latitude = 0;
long utc = 0;
long longitudeWaypoint[4];
long latitudeWaypoint[4];
long targetDistance = 0;
int count = 1;




void GPSSensorRead () {
  wait1Msec(500);
  utc = DGPSreadUTC(DGPS); // get coordinates and universal time code -- utc
  longitude = DGPSreadLongitude(DGPS);
  latitude = DGPSreadLatitude(DGPS);
  targetHeading = DGPSreadHeading(DGPS);
  targetHeading = targetHeading - 180;
  linkstatus = DGPSreadStatus(DGPS);
  DGPSdistance = DGPSreadDistToDestination(DGPS);
}

task main() {

  longitudeWaypoint[1] = -114097512;
  longitudeWaypoint[2] = -114097640;
  longitudeWaypoint[3] = -114097416;
  latitudeWaypoint[1] = 51793988;
  latitudeWaypoint[2] = 51793856;
  latitudeWaypoint[3] = 51793232;

  DGPSsetDestination(DGPS, latitudeWaypoint[count], longitudeWaypoint[count]); //input first of target coordinates

  while (true) {
      GPSSensorRead();  //get GPS information

      long a1 = longitudeWaypoint[count] - longitude;  //calculate distance to target
      long b1 = latitudeWaypoint[count] - latitude;
      long a2 = a1 * a1;
      long b2 = b1 * b1;
      targetDistance = sqrt(a2 + b2);
      if (targetDistance < 100) {
        count = count + 1;
      }
      eraseDisplay(); //display some imformation
      nxtDisplayTextLine(1, "Target: %4d", targetHeading);  //DGPS compass direction
      nxtDisplayTextLine(3, "Lon: %d", longitude); //current longitude value
      nxtDisplayTextLine(4, "Lat: %d", latitude);  //current latitude value
      nxtDisplayTextLine(5, "Distance:  %4d", targetDistance);  //calculated distance to target
      nxtDisplayTextLine(6, "DGPS_dis: %4d", DGPSdistance);  //DGPS distance to target from sensor
    }
}

#pragma config(Sensor, S4,     DGPS,                sensorI2CCustom)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "drivers/common.h" //needed by my setup - I'm not sure why??? If someone knows please let me know.
#include "drivers/DGPS-driver.h"

long targetHeading;
int DGPSdistance;
bool linkstatus = false;
float longitude;
float latitude;
long utc;
float longitudeWaypoint = -114.097528;
float latitudeWaypoint = 51.793932;
float targetDistance1;
float targetDistance2;
float a1, a2, b1, b2, b3, bb;

void GPSSensorRead () {
  wait1Msec(500);
  utc = DGPSreadUTC(DGPS); // get coordinates and universal time code -- utc
  longitude = (float)DGPSreadLongitude(DGPS) / 1000000;
  latitude = (float)DGPSreadLatitude(DGPS) / 1000000;
  targetHeading = DGPSreadHeading(DGPS);
  targetHeading = targetHeading - 180;
  linkstatus = DGPSreadStatus(DGPS);
  DGPSdistance = DGPSreadDistToDestination(DGPS);
}

task main() {

  DGPSsetDestination(DGPS, latitudeWaypoint, longitudeWaypoint); //input first of target coordinates

  while (true) {

      GPSSensorRead();  //get GPS information

      b1 = 53.0 * (longitudeWaypoint - longitude);  //calculate distance to target
      a1 = 69.1 * (latitudeWaypoint - latitude);
      bb = 69.1 * (longitudeWaypoint - longitude) * cosDegrees(latitude);
      a2 = a1 * a1;
      b2 = b1 * b1;
      b3 = bb * bb;
      targetDistance1 = sqrt(a2 + b2); //sqrt(x*x + y*y) where x = 69.1 *(lat2 - lat1), y = 53.0 * (long2 - long1)
      targetDistance2 = sqrt(a2 + b3); //sqrt(x*x + y*y) where x = 69.1 *(lat2 - lat1), y = 69.1 * (long2 - long1) * cosDegrees(lat1/57.3)

      eraseDisplay(); //display some imformation
      nxtDisplayTextLine(1, "Target: %4d", targetHeading);  //DGPS compass direction
      nxtDisplayTextLine(2, "Lon: %+3.6f", longitude); //current longitude value
      nxtDisplayTextLine(3, "Lat: %+3.6f", latitude);  //current latitude value
      nxtDisplayTextLine(4, "Di1:  %+1.6f", targetDistance1);  //calculated distance to target
      nxtDisplayTextLine(5, "Di2:  %+1.6f", targetDistance2);  //calculated distance to target with cosine added
      nxtDisplayTextLine(6, "DGPS_dis: %4d", DGPSdistance);  //DGPS distance to target from sensor
    }
}