// lsvc.nxc
// LEGO Street View Car
//
// By Mark Crosbie  mark@mastincrosbie.com
// http://www.mastincrosbie.com/Marks_LEGO_projects/LEGO_Projects.html
//
// Version: 0.5
// Uses PSPNx to send control messages to the car, but the NXTBee
// streams live telemetry back to a receiver
// Record the GPS coordinates into a KML file, and also associate each
// position with a photo. The filename for the photo on the camera
// has to be set
//
// Purpose:
// Simulate a Google Street View car by intergrating a dGPS and
// camera mount to take pictures as the car drives and log the 
// coordinates to later load into Google Earth.
// 
// This version streams the current coordinates from the dGPS over
// the NXTBee
//
// Wiring
// S1: dGPS
// S2: PSP-Nx
// S3: PFMate
// S4: NXTBee
//
// Motor A: steering control motor
//
// PF IR Link: Channel 1
// Channel A: (Red) Drive motor
// Channel B: (Blue) Drive Motor
// 
// PSP-Nx Controller Functions:
// Left Joystick: controls forward/backwards speed
// Right Joystick: controls steering input
// Triangle: Log a GPS Point
// Circle: Take a photo
// Cross: Exit program

#define DGPS S1
#define PSPNX S2
#define PFMATE S3
#define NXTBEE S4
#define STEERING OUT_A
#define SHUTTER OUT_C

#define VERSION "LSVC v0.5"

// Defns for the camera - file names, paths, picture index etc
#define MAX_COORDINATES 500
#define PATH_PREFIX "DCIM/100MEDIA/"
#define FILENAME_PREFIX "EKEN"
#define FILENAME_EXTENSION ".jpg"
#define LAST_PICTURE_INDEX_FILE "picindex.txt"
#define INITIAL_PIC_INDEX 1

#define PSPNXADDR     0x02
#define LONGDELAY 5000 // 5 seconds
#define SHORTDELAY 2000 // 2 seconds
#define DUTYCYCLE 250
#define TURNDELAY 400
#define I2CDELAY 20
#define SHUTTERROTATE 60

// Direction of Travel
#define FORWARD 1
#define REVERSE 2
#define LEFT 3
#define RIGHT 4
#define STOPPED 0
#define STRAIGHT 5

// Speed settings for the motors
#define STEERING_SPEED 100
#define DRIVESPEED 7
#define THRESHOLD 50  // distance to closest front obstacle tolerated

// For the PF IR Receiver
#define DRIVE_CHANNEL PFMATE_CHANNEL1

#define SHUTTER_SPEED 50

////////////////////////////////////
// Globals
////////////////////////////////////
unsigned int count = 0;
int heading;
const byte PFMateAddr = 0x48;
const byte ServoAddr = 0xb0;
int currentPictureIndex = 1;

long GPSlinkstatus = 0;
long longitude = 0;
long latitude = 0;
long utc = 0;
long batteryLevel = 0;

///////////////////////////////////
// Telemetry data from the car
///////////////////////////////////
struct value {
	int iter;
	int battery;
	long utc;
	long longitude;
	long latitude;
	bool linkstatus;
};

struct coordinate {
	long longitude;
	long latitude;
	int pictureIndex;
};

coordinate coordinates[MAX_COORDINATES];
int lastCoordIndex;

#include "logger.nxc"
#include "KMLCommon.nxc"
#include "PSP-Nx-lib.nxc"
#include "PFMate-lib.nxc"

//////////////////////////////////////////////////////////////////
///////
///////			UTILITY FUNCTIONS
///////
//////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////
// Say hello
//
//////////////////////////////////////////////////////////////////

void printWelcome() {

	TextOut(0, LCD_LINE1, VERSION);
	TextOut(0, LCD_LINE2, "S1: DGPS");
	TextOut(0, LCD_LINE3, "S2: PSPNX");
	TextOut(0, LCD_LINE4, "S3: PFMATE");
	TextOut(0, LCD_LINE5, "S4: NXTBEE");

	TextOut(0, LCD_LINE6, "ORANGE: Exit");
	
	TextOut(0, LCD_LINE7, "CENTER Steering!");
	
	Wait(2000);
	
	ClearScreen();
}

//////////////////////////////////////////////////////////////////
// See if the file exists that records the index of the last picture
// taken. If so then read it in and parse the contents
// If it doesn't exist then create it with the default value of 1
//////////////////////////////////////////////////////////////////
void readLastPictureIndex() {

	int r, fd, filesize;	
	r = OpenFileRead(LAST_PICTURE_INDEX_FILE, filesize, fd);

	if(r == LDR_SUCCESS) {
		// file exists, read the contents
		r = Read(fd, currentPictureIndex);
		// if we didn't read then just assign the default
		if(r != LDR_SUCCESS) {
			currentPictureIndex = INITIAL_PIC_INDEX;
		}
	} else {
		DeleteFile(LAST_PICTURE_INDEX_FILE);
		r = CreateFile(LAST_PICTURE_INDEX_FILE, 5, fd);
		Write(fd, INITIAL_PIC_INDEX); // write the initial index value
	}
	CloseFile(fd);
}

//////////////////////////////////////////////////////////////////
// Increment the last picture index by one
//////////////////////////////////////////////////////////////////
void incrLastPictureIndex() {

	int r, fd, filesize;	
	r = OpenFileRead(LAST_PICTURE_INDEX_FILE, filesize, fd);

	if(r == LDR_SUCCESS) {
		// file exists, read the contents, add one, write it back out
		r = Read(fd, currentPictureIndex);
		currentPictureIndex++;
		CloseFile(fd);
			
		DeleteFile(LAST_PICTURE_INDEX_FILE);
		r = CreateFile(LAST_PICTURE_INDEX_FILE, 5, fd);
		Write(fd, currentPictureIndex); // write the initial index value
		CloseFile(fd);
	}

}


//////////////////////////////////////////////////////////////////
// Set the sensors to their default state.
// Initialise the variables used by the GPS
//
//////////////////////////////////////////////////////////////////
void initialise(){
	int r;

	startLogging();
	
	LOG("LSVC: initialise");
	
	// configure the port for low speed I2C
	SetSensorLowspeed(PFMATE);
	SetSensorLowspeed(DGPS);
	SetSensorLowspeed(PSPNX);

	// configure the S4 port as RS485
	UseRS485();

	// make sure the RS485 system is turned on
	// hi level API function call
  	RS485Enable();
	RS485Initialize();
	RS485Uart(HS_BAUD_9600, HS_MODE_DEFAULT); // use 9600 baud

	// write the KML file header
	dGPS_writeKMLHeader();
	
  	GPSlinkstatus = 0;
  	utc = 0;
	count = 0;	
	lastCoordIndex = 0;
	
	// configure the S4 port as RS485
	UseRS485();

	// make sure the RS485 system is turned on
	// hi level API function call
  	RS485Enable();
	RS485Initialize();
	
  	Wait(MS_10);

	readLastPictureIndex();
	
	TextOut(0, LCD_LINE1, VERSION);
	
}

inline void WaitForMessageToBeSent()
{
  // use hi level API functions (preferred)
  while (RS485SendingData())
    Wait(MS_1);
}

//////////////////////////////////////////////////////////////////
// Print the current GPS link status, UTC, longitude and lattitude
//
//////////////////////////////////////////////////////////////////
void printGPSData() {
	
	if (GPSlinkstatus) {
		TextOut(0, LCD_LINE2, "Link: -UP-");
	} else {
		TextOut(0, LCD_LINE2, "Link: DOWN");
	}
	 
    TextOut(0, LCD_LINE3, FormatNum("UTC: %d", utc));
    TextOut(0, LCD_LINE4, FormatNum("Long: %d", longitude));
	TextOut(0, LCD_LINE5, FormatNum("Latt: %d", latitude));

	TextOut(0, LCD_LINE6, FormatNum("Count: %d", count));
	TextOut(0, LCD_LINE7, FormatNum("Battery: %d", batteryLevel));
	
	count++;
}

//////////////////////////////////////////////////////////////////
// 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) {
	
	LOG("recordGPSCoord");
	if(lastCoordIndex < MAX_COORDINATES) {
		coordinates[lastCoordIndex].longitude = longitude;
		coordinates[lastCoordIndex].latitude = latitude;
		coordinates[lastCoordIndex].pictureIndex = currentPictureIndex;
		lastCoordIndex++;
	}
}

//////////////////////////////////////////////////////////////////
// Take a photo by cycling the shutter release motor
// Press the circle on the PSP remote to take the picture 
// Records the current GPS location associated with the picture
//////////////////////////////////////////////////////////////////
void takePicture() {	
	
	LOG("takePicture");
	
	OnFwd(SHUTTER, SHUTTER_SPEED);
	Wait(500);
	OnRev(SHUTTER, SHUTTER_SPEED);
	Wait(500);
	Off(SHUTTER);

	
}

//////////////////////////////////////////////////////////////////
// Send the current GPS data to the NXTBee in numeric format
//
//////////////////////////////////////////////////////////////////
void sendGPSData2NXTBee() {	

	value v;
	string msg;

	LOG("sendGPSData2NXTBee");	
	v.battery = batteryLevel;
	v.iter = count;
	v.utc = utc;
	v.longitude = longitude;
	v.latitude = latitude;
	v.linkstatus = GPSlinkstatus;

	msg = FlattenVar(v);
	SendRS485String(msg); 	
    WaitForMessageToBeSent();

}


//////////////////////////////////////////////////////////////////
// Drive around under manual control of the PSPNX
// LEFT joystick controls speed forwards and backwards
// RIGHT joystick controls turn motor speed
// Press TRIANGLE to take a GPS reading
//
//////////////////////////////////////////////////////////////////

void runLoop() {
	psp currState;
	int left_X, left_Y;
	int right_X, right_Y;
	int driveSpeed, turnSpeed;

	while (true ) {
	    if(ButtonPressed(BTNCENTER, false)) 
    		break;

	   // Read the sensor's data
		utc = DGPSreadUTC(DGPS);
		longitude = DGPSreadLongitude(DGPS);
		latitude = DGPSreadLatitude(DGPS);
		GPSlinkstatus = DGPSreadStatus(DGPS);
		batteryLevel = BatteryLevel();
    	
    	printGPSData();
 		sendGPSData2NXTBee();
		count++; 	

    	PSP_ReadButtonState(PSPNX, PSPNXADDR, currState);

		left_X = currState.l_j_x;
		left_Y = currState.l_j_y;
		right_X = currState.r_j_x;
		right_Y = currState.r_j_y;

	    if(ButtonPressed(BTNCENTER, false)) 
    		break;

		// TRIANGLE - CAPTURE GPS COORDINATE and PICTURE
		if(currState.triang == 0) {

			// do we have room left for the coordinates?
			if(currentPictureIndex < MAX_COORDINATES) {			
				// cycle the shutter motor
				takePicture();
				
				// 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
				dGPS_writePlacemark(latitude, longitude, currentPictureIndex);
				
				// Increment the picture index
				currentPictureIndex++;
	
				PlayTone(500, 100);
				Wait(200); // debounce
			}
		}
				
		// Speed forward or reverse of motor is determined by the 
		// Y position of the LEFT joystick on the PSP remote
		// As the PF controller only has seven possible speeds (and 0)
		// we scale down the reading
		// Pushing the joystick forwards gives a negative reading
		// so negative is forward, positive is reverse		
		driveSpeed = left_Y / 14;
		driveSpeed = abs(driveSpeed);
		
		if(driveSpeed <= 1) {
			// just brake the motor on very low speed settings
			// to avoid draining the battery and jitter
				PFMate_controlBothMotors(PFMATE, PFMateAddr, DRIVE_CHANNEL,
					PFMATE_BRAKE, 0, PFMATE_BRAKE, 0);	
		} else {
			if(left_Y > 0) {
				PFMate_controlBothMotors(PFMATE, PFMateAddr, DRIVE_CHANNEL,
					PFMATE_REVERSE, driveSpeed, PFMATE_FORWARD, driveSpeed);				
			}
			if(left_Y < 0) {
				PFMate_controlBothMotors(PFMATE, PFMateAddr, DRIVE_CHANNEL,
					PFMATE_FORWARD, driveSpeed, PFMATE_REVERSE, driveSpeed);	
			}
		}
		Wait(I2CDELAY);

		// Steering left or right is determined by the X position of the RIGHT
		// joystick forward or reverse of motor is determined by the 
		// As the PF controller only has seven possible speeds (and 0)
		// we scale down the reading
		// Pushing the joystick right gives a negative reading
		// so negative is forward, positive is reverse		
		turnSpeed = right_X / 14;
		turnSpeed = abs(turnSpeed);
		
		if(turnSpeed <= 1) {
			// just brake the motor on very low speed settings
			// to avoid draining the battery and jitter
			Off(STEERING);		
		} else {
			if(right_X > 0) {
			OnFwd(STEERING, STEERING_SPEED);
			}
			if(right_X < 0) {
			OnRev(STEERING, STEERING_SPEED);
			
			}
		}		

		Wait(DUTYCYCLE);	
	
	}	
}

//////////////////////////////////////////////////////////////////
// Main 
// Set everything up and call runLoop()
//////////////////////////////////////////////////////////////////
task main(){
	
	printWelcome();
	initialise();
	    
    // configure the S4 port as RS485
  	UseRS485();

  	// make sure the RS485 system is turned on
  	// hi level API function call
  	RS485Enable();

  	// initialize the UART to default values
  	// hi level API function call
  	RS485Uart(HS_BAUD_9600, HS_MODE_DEFAULT); // use 9600 baud rather than default rate

	PlayTone(400, 200);
	Wait(200);
	PlayTone(700, 200);
	Wait(200);
    
    LOG("main, entering main loop");

	runLoop();
	
	PlayTone(700, 200);
	Wait(200);
	PlayTone(400, 200);
	Wait(200);

	LOG("main, writing path string");
	dGPS_write_Pathstring(currentPictureIndex-1);
	
	LOG("main, write KML footer");
	dGPS_writeKMLFooter();
	
	LOG("main, DONE.");
	CloseFile(logfileFd);
}