#pragma config(Sensor, S1,     DGPS,                sensorI2CCustom)
#pragma config(Motor,  motorA,          DRIVE,         tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorB,          LIGHTS,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  motorC,          STEERING,      tmotorNormal, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//////////////////////////////////////////////////////////////////////
//
// LEGO Street View Car version 2
// by Mark Crosbie mark@mastincrosbie.com
//
// Uses code from Xander Soldaat and Dexter Industries for dGPS
// and DiWifi sensors.
//
// Drive under control of a Bluetooth remote and capture images on an
// iPhone while logging GPS coordinates.
//
// Stores GPS coordinates as a KML file to upload to Google Earth
//
//////////////////////////////////////////////////////////////////////


#define DEBUG_WIFI 0
#ifdef DEBUG_WIFI
#define writeRawHS(X, Y) debugnxtWriteRawHS(X, Y)
#else
#define writeRawHS(X, Y) nxtWriteRawHS(X, Y)
#endif

#define TRIGGER_FREQ 1400   // frequency to trigger the camera on
#define TIMEOUT 10000 // 10s timeout for wifi sensor
#define BAUDRATE 115200

#include "common.h"
#include "DGPS-driver.h"
#include "BTlibrary.h"
#include "lsvccommon.h"

//////////////////////////////////////////////////////////////////////
//
// KML file related global variables and includes
//
/////////////////////////////////////////////////////////////////////
#define MAX_COORDINATES 500
#define PATH_PREFIX "Images/"
#define FILENAME_PREFIX "IMG_"
#define FILENAME_EXTENSION ".jpg"
#define LAST_PICTURE_INDEX_FILE "picindex.txt"
#define INITIAL_PIC_INDEX 1

#define TURNRATIO 9

int currentPictureIndex = 1;
typedef struct coordinate {
	long longitude;
	long latitude;
	int pictureIndex;
} coordinate_t;
coordinate_t coordinates[MAX_COORDINATES];
int lastCoordIndex = 0;
bool capturing = false;
int kCaptureInterval = 19500;  // one picture every 20 seconds. Each iteration adds a slight delay, so adjust

#include "KMLcommon.h"

//////////////////////////////////////////////////////////////////////
//
// WIFI related global variables and includes
//
/////////////////////////////////////////////////////////////////////

// Declare the SSID and WPA_PSK we're connecting to
ubyte ssid[] = {'a', 'b', 'c'};
ubyte wpa_psk[] = {'p', 'a', 's', 's', 'w', 'o', 'r', 'd'};

ubyte BytesRead[8];
const ubyte newline[] = {0x0D};
typedef ubyte buff_t[128];
buff_t buffer;
ubyte rxbuffer[1500];

long baudrates[] = {9600, 19200, 38400, 57600, 115200, 230400,460800, 921600};

#include "DIWIFI-connect_WPA.h"
#include "tcp.h"

/////////////////////////////////////////////////////////////////////
//
// GPS related global variables and includes
//
/////////////////////////////////////////////////////////////////////
long longitude = 0;
long latitude = 0;
long utc = 0;
bool linkstatus = false;

/////////////////////////////////////////////////////////////////////
//
// Bluetooth communication related globals
//
/////////////////////////////////////////////////////////////////////
const int kMaxSizeOfMessage = 5;
const TMailboxIDs kQueueID = mailbox1;

void printWelcome() {

  nxtDisplayCenteredBigTextLine(0, "LSVC2");

  nxtDisplayString(2, "S1: DGPS");
  nxtDisplayString(3, "S4: DiWiFi");
  nxtDisplayString(4, "A: Steering");
  nxtDisplayString(5, "B: Lights");
  nxtDisplayString(6, "C: Drive");
  wait10Msec(200);
}

// Send a long to the remote
void sendInteger(TMailboxIDs queue, long val) {

	ubyte nXmitBuffer[kMaxSizeOfMessage] = {0x00, 0x00, 0x00, 0x00, 0x00}; // For NXT-G compatability, last byte of message must be zero because of string messsages.
	TFileIOResult nBTCmdErrorStatus;

	nXmitBuffer[0] = (val >> 24) & 0xFF;
	nXmitBuffer[1] = (val >> 16) & 0xFF;
	nXmitBuffer[2] = (val >>  8) & 0xFF;
	nXmitBuffer[3] = (val >>  0) & 0xFF;

	if (bBTBusy)
	{
		return;
	}

	nBTCmdErrorStatus = cCmdMessageAddToQueue(queue, nXmitBuffer, kMaxSizeOfMessage);


	// nBTCmdErrorStatus = cCmdMessageWriteToBluetooth(nXmitBuffer, kMaxSizeOfMessage, queue);
	switch (nBTCmdErrorStatus)
	{
		case ioRsltSuccess:
		case ioRsltCommPending:
		break;

		case ioRsltCommChannelBad:
		default:
		break;
	}
	return;
}

void sendGPSToRemote() {

    //writeDebugStreamLine("sendGPSToRemote");
    //sendInteger(kPictureCountID, currentPictureIndex);

}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// Start up the wifi subsystem and get an IP address
////////////////////////////////////////////////////////////////////////////////////////////////////////

void wifi_startup(){

	nxtScrollText("wifi_startup");

	closeAllConns();

	config_wifi();            // This function sets up the wifi sensor for operation.  Basic housekeeping.

	nxtScrollText("wifi_auth_mode");
	wifi_auth_mode(2);        // Set the authentication mode.
	                        // 0 is none or WPA // 1 is Open network // 2 is Shared with WEP

	nxtScrollText("setDHCP");
	setDHCP(1);               // Get us an IP number

	nxtScrollText("Call set_wpa_psk");
	set_wpa_psk();            // Calculates your psk
	Receive(true);            // Housekeeping
	Receive(true);            // Housekeeping

	nxtScrollText("connect_to_ssid");
	connect_to_ssid();        // Connects to the wifi network you've designated
	Receive(false);            // Housekeeping.
}

//////////////////////////////////////////////////////////////////
// Record a GPS coordinate in the array
// This array is dumped out to disk once the logging is completed
//////////////////////////////////////////////////////////////////
void recordGPSCoord(long latitude, long longitude) {

	if(lastCoordIndex < MAX_COORDINATES) {
		coordinates[lastCoordIndex].longitude = longitude;
		coordinates[lastCoordIndex].latitude = latitude;
		coordinates[lastCoordIndex].pictureIndex = currentPictureIndex;
		lastCoordIndex++;
	}
}

bool checkBTLinkConnected()
{
	if (nBTCurrentStreamIndex >= 0)
	  return true;
    else
      return false;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// The lights task manages the power to the headlights. If the Bluetooth connection is not connected
// then it flashes the lights, otherwise they are on full power
//
////////////////////////////////////////////////////////////////////////////////////////////////////////
task lights() {

    while(true) {
        if(checkBTLinkConnected()) {
            motor[LIGHTS] = 100;
        } else {
            motor[LIGHTS] = 100;
            wait10Msec(25);
            motor[LIGHTS] = 0;
            wait10Msec(25);
        }
        EndTimeSlice();
    }
}


////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Read a message from the Bluetooth link. A message from the remote is composed of 4 bytes
// command-power-turn-zero
// Returns -1 if an error occurred
// Returns 0 if no message is waiting
// Returns >0 if a message was read
////////////////////////////////////////////////////////////////////////////////////////////////////////
int readDataMsg(byte &cmd, byte &power, byte &turn) {

	const bool bWaitForReply = false;
	TFileIOResult nBTCmdRdErrorStatus;
	int nSizeOfMessage;
	ubyte nRcvBuffer[kMaxSizeOfMessage];
    int ret = -1;

	// Check to see if a message is available
	nSizeOfMessage = cCmdMessageGetSize(kQueueID);
	if (nSizeOfMessage <= 0) {
		wait1Msec(1);    // Give other tasks a chance to run
		ret=0;           // No message this time
	}

	if (nSizeOfMessage > kMaxSizeOfMessage)
        nSizeOfMessage = kMaxSizeOfMessage;

    nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, kQueueID);

    if (nBTCmdRdErrorStatus == ioRsltSuccess) {
  	    ret = 1;
  	    cmd = nRcvBuffer[0];
  	    power = nRcvBuffer[1];
  	    turn = nRcvBuffer[2];
	} else {
	    ret = -1;
    }
	return ret;
}

void positionMotor(char motorToTurn, int degrees){

    //writeDebugStreamLine("positionMotor degrees=%d", degrees);
    //writeDebugStreamLine("positionMotor encoder=%d", nMotorEncoder[motorToTurn]);

    // allow some degree of slop in the motor
    if( abs(nMotorEncoder[motorToTurn] - degrees) < 20) return;

    if (degrees < 0)
    {
        if(nMotorEncoder[motorToTurn] > degrees) {
	        while (nMotorEncoder[motorToTurn] > degrees)
	        {
	            motor[motorToTurn] = -50;
	            EndTimeSlice();
	        }
	    } else {
	        while (nMotorEncoder[motorToTurn] < degrees)
	        {
	            motor[motorToTurn] = 50;
	            EndTimeSlice();
	        }
        }
    }
    else
    {
        if(nMotorEncoder[motorToTurn] < degrees) {
	        while (nMotorEncoder[motorToTurn] < degrees)
	        {
	            motor[motorToTurn] = 50;
	            EndTimeSlice();
	        }
	    } else {
	        while (nMotorEncoder[motorToTurn] > degrees)
	        {
	            motor[motorToTurn] = -50;
	            EndTimeSlice();
	        }
        }
    }
    motor[motorToTurn] = 0;
}

void parseTCP(int cid_connection)
{
  nxtScrollText("ParseTCP %d", cid_connection);

  ubyte BytesRead[20];
  ubyte currByte[] = {0};
  ubyte prevByte[] = {0};
  ubyte conn[] = {0};
  string tmpString;

  int index = 0;
  while(true)
  {
      alive();
      if (nxtGetAvailHSBytes() > 0)
        {
        nxtReadRawHS(currByte[0], 1);
        if ((prevByte[0] == 27) && (currByte[0] == 'S')) {
          index = 0;
          memset(rxbuffer, 0, sizeof(rxbuffer));
          wait1Msec(1);
          nxtReadRawHS(conn[0], 1);
          while (true) {
			while (nxtGetAvailHSBytes() == 0) EndTimeSlice();
				nxtReadRawHS(currByte[0], 1);
				if ((prevByte[0] == 27) && (currByte[0] == 'E')) {
			    	rxbuffer[index--] = 0;
				    rxbuffer[index--] = 0;
				    break;
				}
			    prevByte[0] = currByte[0];
			    rxbuffer[index++] = currByte[0];
		    }
            for (int i = 0; i < ((index / 19) + 1); i++) {
                memset(BytesRead[0], 0, 20);
                memcpy(BytesRead[0], rxbuffer[i*19], 19);
                StringFromChars(tmpString, BytesRead);
                writeDebugStream(tmpString);
               }
        }
        prevByte[0] = currByte[0];
        }
       wait1Msec(1);
 }
}

task printGPSData() {

    while(true) {
	  // Read the sensor's data
	  utc = DGPSreadUTC(DGPS);
	  longitude = DGPSreadLongitude(DGPS);
	  latitude = DGPSreadLatitude(DGPS);
	  linkstatus = DGPSreadStatus(DGPS);

	  nxtDisplayTextLine(2, "UTC: %d", utc);
	  nxtDisplayTextLine(3, "Lon: %d", longitude);
	  nxtDisplayTextLine(4, "Lat: %d", latitude);
	  if (linkstatus)
	    nxtDisplayTextLine(5, "Link Stat: [UP]");
	  else
	    nxtDisplayTextLine(5, "Link Stat: [DOWN]");

	       wait1Msec(500);
    }
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//  Capture Task - monitor the boolean capturing flag, and if it is set we start capturing GPS data points
//  every INTERVAL second
//
////////////////////////////////////////////////////////////////////////////////////////////////////////
task capture() {

    while(true) {

        if(capturing) {

            nVolume = 3;
            PlayImmediateTone(TRIGGER_FREQ, 80);
            while(bSoundActive) wait1Msec(1);

			// record a GPS coordinate for the path string
			recordGPSCoord(latitude, longitude);

			// write a GPS Placemark with the associated picture
			// this is written directly to the KML file
			writeKMLPlacemark(latitude, longitude, currentPictureIndex);

			// Increment the picture index
			currentPictureIndex++;

			ClearTimer(T1);
			while(time1[T1] < kCaptureInterval) wait1Msec(5);

		} else {
		    wait10Msec(5);
		}
    }
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
//                                        Main Task
////////////////////////////////////////////////////////////////////////////////////////////////////////
task main()
{
    byte power, turn, cmd;
    bool done = false;
    bool useWifi = false;

    printWelcome();

	eraseDisplay();

	nxtDisplayCenteredTextLine(0, "LSVC2");

	PlaySound(soundBeepBeep);
	wait10Msec(50);

    StartTask(lights);

	// initialise the KML file
	writeKMLHeader();

	StartTask(capture);

	StartTask(printGPSData);

	nMotorEncoder[STEERING] = 0;

	while(!done) {

	    //printGPSData();

	    //sendGPSToRemote();

	    if(nNxtButtonPressed == ORANGEBUTTON) {
	        done = true;
	        continue;
	    }

	    cmd = NOP;

	    // If we lose the BT connection then stop, we don't want the car to keep driving without
	    // direct human control
	    if(checkBTLinkConnected()) {
            readDataMsg(cmd, power, turn);
        } else {
            power = cmd = turn = 0;
        }

        if(capturing)
            nxtDisplayCenteredTextLine(6, "Capturing");
        else
            nxtDisplayClearTextLine(6);

        nxtDisplayTextLine(7, "%d %d %d %d", cmd, power, turn, currentPictureIndex);

	    switch(cmd) {

	        case ABORT:
	            StopAllTasks();
	            break;

     		case SNAPSHOT:
     		    if(capturing)
     		        capturing = false;
     		    else
     		        capturing = true;
     			break;

     		case STOPCAPTURE:
     			done = true;
     			capturing = false;
     			break;

     		case STARTWIFI:
     			useWifi = true;
     			break;

     		case NOP:
     		    break;

     		case BRAKE:
     		    power = 0;
     		    break;

     		default:
     			//writeLogString("runLoop: unknown command");
     			break;
     	}

     	motor[DRIVE] = -power;
     	positionMotor(STEERING, turn * TURNRATIO);

	    wait10Msec(5);
	}

	// Reset the motors
	motor[DRIVE] = 0;
	positionMotor(STEERING, 0);
	capturing = false;

	StopTask(capture);
    StopTask(printGPSData);

	writeKMLPathstring(currentPictureIndex - 1);
	writeKMLfooter();

	if(useWifi) {
	    PlaySound(soundFastUpwardTones);

	    nxtSetHSBaudRate(115200);

		eraseDisplay();
		nxtDisplayCenteredTextLine(0, "LSVC2");
		nxtDisplayCenteredTextLine(1, "Wifi Start");

		wifi_startup();
		PlaySound(soundBeepBeep);   // Make some noise when we're connected.

		wait10Msec(100);

		nxtScrollText("CloseAllConns");

		closeAllConns();                      // Close all connections.  If any other CID's are open, close them.

		nxtScrollText("startTCP 20");

		int cid = startTCP(20);               // Start TCP on Port 20.

		nxtScrollText("CID: %d", cid);  // Write the CID assigned by the sensor.
		writeDebugStreamLine("CID:%d", cid);  // Write the CID assigned by the sensor.

        clear_read_buffer();                  // Clean up.
        echo_tcp(cid);                        // Wait for a response and run.

	}

	PlaySound(soundBeepBeep);
}