#pragma config(Sensor, S1,     DGPS,           sensorI2CCustom)
#pragma config(Sensor, S4,     HTMC,           sensorI2CCustom)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop, reversed, encoder)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop, encoder)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//20 inches of movement per motor rotation

#include "C:\Program Files (x86)\Robomatter Inc\ROBOTC Development Environment\Sample Programs\NXT\3rd Party Sensor Drivers\drivers\dexterind-gps.h"
#include "C:\Program Files (x86)\Robomatter Inc\ROBOTC Development Environment\Sample Programs\NXT\3rd Party Sensor Drivers\drivers\hitechnic-compass.h"

//Test Latitude = 42.000389
//Test Longitude = -88.038040
//Longitude = x axis
//Latitude = y axis
//Smallest unit that GPS can measure equals roughly 5.282321853711464 inches.

task main()
{
   float latitude = 0;
   float Longitude = 0;

   int heading;
   wait1Msec(1000);

   while(nNxtButtonPressed != kEnterButton)
   {
      nxtDisplayCenteredTextLine(3, "%f", (float)DGPSreadLatitude(DGPS) / 1000000);
      nxtDisplayCenteredTextLine(4, "%f", (float)DGPSreadLongitude(DGPS) / 1000000);
      wait1Msec(500);
   }

   //float TarLong = (float)DGPSreadLongitude(DGPS) / 1000000;
   //float TarLat = (float)DGPSreadLatitude(DGPS) / 1000000;

   float TarLong = -88.038040;
   float TarLat = 42.000389;

   DGPSsetDestination(DGPS, TarLat, TarLong);

   time1[T1] = 0;

   nMotorEncoder[motorA] = 0;
   nMotorEncoder[motorB] = 0;
   nMotorEncoder[motorC] = 0;
   
   wait1Msec(1000);
   
   Longitude = (float)DGPSreadLongitude(DGPS) / 1000000;
   latitude = (float)DGPSreadLatitude(DGPS) / 1000000;

   while(true)
   {

      /*if(time1[T1] > 500)
      {
         Longitude = Longitude - (Longitude - ((float)DGPSreadLongitude(DGPS) / 1000000));
         latitude = (float)DGPSreadLatitude(DGPS) / 1000000;
         time1[T1] = 0;
      }*/

      if(Longitude > TarLong + 0.00001) HTMCsetTarget(HTMC, 270);
      else if(Longitude < TarLong - 0.00001) HTMCsetTarget(HTMC, 90);
      else if(Longitude > TarLong - 0.00001 && Longitude < TarLong + 0.00001) StopAllTasks();
      //if(Latitude > TarLat + 0.000025) HTMCsetTarget(HTMC, 180);
      //else if(Latitude < TarLat - 0.000025) HTMCsetTarget(HTMC, 0);
      //else if(Longitude > TarLong - 0.00001 && Longitude < TarLong + 0.00001  && Latitude > TarLat - 0.00001 && Latitude < TarLat + 0.00001) StopAllTasks();

      float compass = HTMCreadRelativeHeading(HTMC);

      nxtDisplayCenteredTextLine(0, "TLon: %f", TarLong);
      nxtDisplayCenteredTextLine(1, "TLat: %f", TarLat);
      nxtDisplayCenteredTextLine(2, "Lon: %f", Longitude);
      nxtDisplayCenteredTextLine(3, "Lat: %f", latitude);
      nxtDisplayCenteredTextLine(4, "Rel Comp: %d", compass);
      nxtDisplayCenteredTextLine(5, "Act Comp: %d", HTMCreadHeading(HTMC));
      nxtDisplayCenteredTextLine(6, "En: %d", nMotorEncoder[motorA]);

      float bPwr = 100 - (compass * 2);
      float cPwr = 100 + (compass * 2);

      if(bPwr < 50) motor[motorB] = 50;
      else motor[motorB] = bPwr;
      if(cPwr < 50) motor[motorC] = 50;
      else motor[motorC] = cPwr;
      motor[motorA] = (motorPWMLevel[motorB] + motorPWMLevel[motorC]) / 2;

      //if((nMotorEncoder[motorA] + nMotorEncoder[motorB] + nMotorEncoder[motorC]) / 3 = 95)
      if(nMotorEncoder[motorA] >= 95)
      {
         if(compass > 10 && compass < 170) Longitude = Longitude + (float)0.000001;
         else Longitude = Longitude - (float)0.000001;

         nMotorEncoder[motorA] = 0;
      }
   }
}