//*!!Sensor,    S1,             kCompass, sensorCustom,      ,                   !!*//
//*!!Sensor,    S2,          touchSensor, sensorTouch,      ,                    !!*//
//*!!                                                                            !!*//
//*!!Start automatically generated configuration code.                           !!*//
const tSensors touchSensor          = (tSensors) S2;   //sensorTouch        //*!!!!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//
//
//
// Title: LEGO Autopilot
// Created by Chris Anderson, DIYDrones.com, 2007. GPS and BT interface portions created by Steve Hassenplug for his
//      "Green Monster" outdoor robot.
// Published under a Creative Commons Attribution licence
//
//////////////////////////////////////////////////////////////////////////////////////////
//
// This program is started when the touch sensor is pushed (by a servo). It then moves the rudder to steer the airplane
// to a series of pre-programmed GPS coordinates. This is a navigation-only autopilot; it would have to be
// accompanied by a stabalization system, such as the FMA Co-Pilot, to maintain level flight autonomously.
//
// Connect the touch sensor to port 2, and the motor to port A
// Establish a BT connection with the GPS module manually one time before running this program
//
//////////////////////////////////////////////////////////////////////////////////////////
//

// Variables and constants

	int CurrentWaypoint = 1;
	int currentPosition = 0;
	bool DeflectLeft = false;
	bool DeflectRight = false;
	bool LoopStop = false;
	bool AutoPilotON;
	const int MaxWaypointCount = 10;
  int WayPointCount = 0;
  float WayPointLat[MaxWaypointCount + 1];
  float WayPointLon[MaxWaypointCount + 1];
  float gpsSats = 0.0;
  float gpsTime = 0.0;
  float fixIndicator = 0;
  float Lat1=0;
  float Lon1=0;
  float LastLat=0;
  float LastLon=0;


////////////////////////////////////////////
//
// Subroutines
//
//////////////////////////////////////////////
//
///////////////////////////////////////////////
//
// GetWaypoints()
//
//////////////////////////////////////////////

void GetWaypoints()
	{
	WayPointLat[1] = 37.890173;
	WayPointLon[1] = 122.254109;
	WayPointLat[2] = 37.890173;
	WayPointLon[2] = 122.254109;
	WayPointLat[3] = 37.890173;
	WayPointLon[3] = 122.254109;
	WayPointLat[4] = 37.890173;
	WayPointLon[4] = 122.254109;
	WayPointLat[5] = 37.890173;
	WayPointLon[5] = 122.254109;
	WayPointLat[6] = 37.890173;
	WayPointLon[6] = 122.254109;
	WayPointLat[7] = 37.890173;
	WayPointLon[7] = 122.254109;
	WayPointLat[8] = 37.890173;
	WayPointLon[8] = 122.254109;
	WayPointLat[9] = 37.890173;
	WayPointLon[9] = 122.254109;
	WayPointLat[10] = 37.890173;
	WayPointLon[10] = 122.254109;
  }

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                       getChar()
//
// Try for up to 200 milliseconds to get a character. Returns '-1' if no character available.
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

int getChar()
{
	ubyte BytesRead[1]; // Circular buffer of last bytes read.
	int nNumbBytesRead;
	int nAttempts;

	for (nAttempts = 0; nAttempts < 200; ++nAttempts)
	{
		// Get a single character from BT
		nNumbBytesRead = nxtReadRawBluetooth(BytesRead[0], 1);
		if (nNumbBytesRead != 0)
		  return BytesRead[0];
		wait1Msec(10);
	}

	// No character is available in the last 200 milliseconds

	return -1; // No character
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                       parseNumber()
//
// Parses a floating point number
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

bool parseNumber(float &fNumber)
{
	int aChar;
	bool  bFoundDecimalPoint = false;
	bool bFoundDigit = false;
	float fDivisor = 1.0;

	fNumber = 0.0;
	while (true)
	{
		aChar = getChar();

		switch (aChar)
		{
		case '0':
		case '1':
		case '2':
		case '3':
		case '4':
		case '5':
		case '6':
		case '7':
		case '8':
		case '9':
		  fNumber *= 10;
		  fNumber += aChar - '0';
		  if (bFoundDecimalPoint)
		    fDivisor *= 10;
		  bFoundDigit = true;
		  break;

		case '.':
		  if (bFoundDecimalPoint)
		    return false;
		  bFoundDecimalPoint = true;
		  break;

		default:
		  if (!bFoundDigit)
		    return false;
		  fNumber /= fDivisor;
		  return true;
	  }
	}
}

bool parseDirection()
{
	switch (getChar())
	{
	case 'N':
	case 'S':
	case 'E':
	case 'W':
	case ' ':
	  break;

	default:
	  return false;
  }

	if (getChar() != ',')
	  return false;

	return true;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                        ConvertToCoord
//
// This function takes a floating point value received from the GPS in a format DDDMM.MMMM
// and converts it to DDD.DDDD
//
// D=Degrees; M=Minutes
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

float ConvertToCoord(float ValueIn)
{
	int Degrees = ValueIn / 100;
	return (ValueIn - Degrees * 100) / 60 + Degrees;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                        taskWatchGPS
//
// Infinite loop reading data from GPS receiver
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

task taskWatchGPS()
{
	// process gps string
	// Sample string:
	// $GPGGA,140818.000,4433.2984,N,08056.3964,W,6,00,50.0,168.7,M,-36.0,M,,0000*5C

//	float Lat1,Lon1;
	static const short cHeader[6] = {'$', 'G', 'P', 'G', 'G', 'A'};

	//	set the task priority very high.  This task will run whenever it's not waiting.
 	nSchedulePriority = 200;

	setBluetoothRawDataMode();
		wait1Msec(50);

	while (!bBTRawMode)
	{
		wait1Msec(5);
	}

	while (true)
	{
		int nIndex = 0;
		int aChar  = -1;

		//
		// Scan input characters until Header string ("GPGGA" is found)
		//
		while (nIndex < 5)
		{
			while (aChar < 0)
			{
				aChar = getChar();
				nIndex = 0;
			}
			if (aChar != cHeader[nIndex])
			{
			  nIndex = 0;
			  aChar = -1;
			  continue;
			}

			++nIndex;
			aChar = getChar();
		}

		aChar = getChar();
		//
		// "Shadow" copy of data. Only update when all data is received.
		//
		float tempLat;
		float tempLon;

		if (!parseNumber(gpsTime))
			continue;
		if (!parseNumber(tempLat))
			continue;
		if (!parseDirection())
			continue;
		if (!parseNumber(tempLon))
			continue;
		if (!parseDirection())
			continue;
		if (!parseNumber(fixIndicator))
			continue;
		if (!parseNumber(gpsSats))
			continue;

		// Transfer data in one block

		Lat1  = ConvertToCoord(tempLat);
		Lon1  = ConvertToCoord(tempLon);

			nxtDisplayTextLine(3,"Lat: %3.8f",Lat1);
			nxtDisplayTextLine(4,"Lon: %3.8f",Lon1);

		if (fixIndicator!=0)
		{
			nxtDisplayTextLine(3,"Lat: %3.8f",Lat1);
			nxtDisplayTextLine(4,"Lon: %3.8f",Lon1);

			nxtDisplayTextLine(0,"Autopilot:%2.0f",gpsSats);
		} else {
			nxtDisplayTextLine(0,"Autopilot:00");
		}


	}
}


////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                        checkBTLinkConnected
//
// Utility function to check whether Bluetooth link is set up. It should be manually set up before
// launching this program. [You can also set it up within an application program; there's a separate
// sample program for this.
//
////////////////////////////////////////////////////////////////////////////////////////////////////////

void checkBTLinkConnected()
{
	if (nBTCurrentStreamIndex >= 0)
	  return;  // An existing Bluetooth connection is present.

	nxtDisplayCenteredTextLine(3, "Attempting");
	nxtDisplayCenteredTextLine(4, "To Connect");

//	TFileIOResult nResult;

//	static const short sPIN[4] = {'0', '0', '0', '0'};
//	setDefaultPIN(nResult, sPIN);
	string pinnumber = "0000";

		setDefaultPIN(pinnumber);
//		setSessionPIN(pinnumber);
    bBTSkipPswdPrompt = true;

	// connect to GPS, here...
	btConnect(1, "iBT-GPS");

	while (bBTBusy)
	{
		wait1Msec(1);
	}

	if (nBTCurrentStreamIndex >= 0)
	  return;  // An existing Bluetooth connection is present.

	//
	// Not connected. Audible notification and LCD error display
	//
//	PlaySound(soundLowBuzz);
//	PlaySound(soundLowBuzz);
//	eraseDisplay();
	nxtDisplayCenteredTextLine(3, "GPS not");
	nxtDisplayCenteredTextLine(4, "Connected");
	wait1Msec(3000);
//	StopAllTasks();
}

/////////////////////////////////////////////////

//////////////////////////////////////////////////


void Initialize()
{
	checkBTLinkConnected();
	StartTask(taskWatchGPS);
	LastLat = Lat1; // set initial GPS coordinates
	LastLon = Lon1;
	nxtDisplayTextLine(4, "Autopilot Initialized");  // Display the text on line number 4 of 8 on the LCD
}
//////////////////////////////////////////
task CheckAutoPilot()
{
 while(true)
 {
  if (SensorValue(touchSensor) == 1) AutoPilotON=true;
  else AutoPilotON=false;
  wait1Msec(1000);
 }
}

///////////////////////////////////
//  Turn
/////////////////////////////////
void Turn(int NewSetting)
{
	if (NewSetting > currentPosition)
		{
	   nMotorEncoderTarget[motorA] = NewSetting - currentPosition;       // incremental amount to move motor
		 motor[motorA] = 100;								//motor A is run at a 100 power level
 		 while ((nMotorRunState[motorA] != runStateIdle) && AutoPilotON)
    	{}  																						// wait for action to complete
		}
	if (NewSetting < currentPosition)
		{
		nMotorEncoderTarget[motorA] = currentPosition - NewSetting;       // incremental amount to move motor
  	motor[motorA] = -100;								//motor A is run at a 100 power level
		while ((nMotorRunState[motorA] != runStateIdle) && AutoPilotON)
		 {}  																						// wait for action to complete
		}
	motor[motorA] = 0;
	currentPosition = nMotorEncoder[motorA];
}
////////////////////////////////
// Set the rudder straight
///////////////////////////////

void StraightenUp()
{
			if (DeflectLeft) // return the rudder to the neutral position
				{
				 Turn(currentPosition - 100);
				 DeflectLeft = false;
				}
			if (DeflectRight)
				{
					Turn(currentPosition + 100);
					DeflectRight = false;
				}
}
///////////////////////////////////
//  Turn left
/////////////////////////////////
void TurnLeft()
  {
		Turn(100);
		DeflectLeft = true;
	}
///////////////////////////////////
//  Turn right
/////////////////////////////////
void TurnRight()
  {
		Turn(-100);
  	DeflectRight = true;
	}

///////////////////////////////////////////////
// This is the section where we turn the rudder to head the plane to the new course, and stop turning when we're on
// that course.
//////////////////////////////////////////////

void SteeringLoop ()
{
	float DeltaX = 0;
	float DeltaY = 0;
	float Angle =  0;
	float TargetAngle = 0;
	float CurrentAngle = 0;
	bool OnLat = false;
	bool OnLon = false;
	LoopStop = false;
	while (!LoopStop)
		{
		nxtDisplayTextLine(1,"TargetLat: %3.8f",WayPointLat[CurrentWaypoint]); //display target GPS heading
		nxtDisplayTextLine(2,"TargetLon: %3.8f",WayPointLon[CurrentWaypoint]); //display target GPS heading
		nxtDisplayTextLine(3, "Deflect Left: %d", (short) DeflectLeft); //display left rudder command
		nxtDisplayTextLine(4, "Deflect Right: %d",(short) DeflectRight); //display right rudder command

		if ((Lon1 < WayPointLon[WayPointCount] + .001) && (Lon1 > WayPointLon[WayPointCount] - .001)) OnLon = true;
		if ((Lat1 < WayPointLat[WayPointCount] + .001) && (Lat1 > WayPointLat[WayPointCount] - .001)) OnLat = true;

		if (!OnLon || !OnLat) // we're not on target; must turn
			{
		   DeltaX = WayPointLon[WayPointCount] - Lon1;  // Get target heading direction
		   DeltaY = WayPointLat[WayPointCount] - Lat1;
		   if (DeltaX != 0) Angle = atan(DeltaY/DeltaX) * 57.2958; else Angle = 45;
		   if (DeltaX >= 0) TargetAngle = 90 - Angle;
		   if (DeltaX < 0) TargetAngle = 270- Angle;

		   DeltaX = LastLon - Lon1;  // Get current heading direction
		   DeltaY = LastLat - Lat1;
		   if (DeltaX != 0) Angle = atan(DeltaY/DeltaX) * 57.2958; else Angle = 45;
		   if (DeltaX >= 0) CurrentAngle = 90 - Angle;
		   if (DeltaX < 0) CurrentAngle = 270- Angle;
			 if (!DeflectLeft && !DeflectRight) // rudder not already turned
				{
				WayPointCount = WayPointCount;
				}
			}
		else WayPointCount = WayPointCount + 1;  // we hit a waypoint; go to next one
		if ((CurrentAngle < TargetAngle + 5) && (CurrentAngle > TargetAngle - 5)) //we're heading in the right direction
			{
			if (DeflectLeft || DeflectRight) StraightenUp();
		  }
		else // we need to turn
			{
			if (CurrentAngle < TargetAngle) // need to turn right
				{
				if (!DeflectRight) TurnRight();
				}
			if (CurrentAngle > TargetAngle) // need to turn left
				{
				if (!DeflectLeft) TurnLeft();
				}
			}
		if ((SensorValue(touchSensor) == 0) || (WayPointCount == MaxWaypointCount)) // If user stops autopilot or we hit the end of the waypoints, exit
			{
			LoopStop = true;
			StraightenUp();
		  }
		LastLat = Lat1; // Set previous position to current position, and prepare to start the loop again.
		LastLon = Lon1;
		}
}


/////////////////////////////////////////
// Take inititial readings and start steering loop
/////////////////////////////////////////

void Autopilot()
{
	nxtDisplayTextLine(4, " Autopilot ON  ");  // Display the text on line number 4 of 8 on the LCD
	StartTask(CheckAutoPilot);
	SteeringLoop();                     // go to the steering loop and do all the work
	StopTask(CheckAutoPilot);
	nxtDisplayTextLine(4, " Autopilot OFF  ");  // Display the text on line number 4 of 8 on the LCD
}

// Main program

  task main()
{
	GetWaypoints();
	Initialize(); //Initialize sensors//
	while(true)   //Loop forever
   {
     alive(); //ensure that the NXT doesn't automatically power down while waiting for the autopilot to be turned on
     if (SensorValue(touchSensor) == 1) Autopilot();
    }
}