//*!!Sensor,    S1,            frontDROD, sensorLightActive,      ,              !!*//
//*!!Sensor,    S2,             rearDROD, sensorLightActive,      ,              !!*//
//*!!Motor,  motorA,           DriveMotor, tmotorNxtEncoderClosedLoop,           !!*//
//*!!Motor,  motorB,        SteeringMotor, tmotorNxtEncoderClosedLoop,           !!*//
//*!!                                                                            !!*//
//*!!Start automatically generated configuration code.                           !!*//
const tSensors frontDROD            = (tSensors) S1;   //sensorLightActive  //*!!!!*//
const tSensors rearDROD             = (tSensors) S2;   //sensorLightActive  //*!!!!*//
const tMotor   DriveMotor           = (tMotor) motorA; //tmotorNxtEncoderClosedLoop //*!!!!*//
const tMotor   SteeringMotor        = (tMotor) motorB; //tmotorNxtEncoderClosedLoop //*!!!!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//

//
// Synchro Drive robot v4
//
// Copyright 2008 Mark Crosbie mark@mastincrosbie.com
// 3/2/2008
//
// Uses mindsensors.com DROD-Nx on "front" and "rear" to detect obstacles
//
// Motor A:	steering motor
// Motor B: drive motor
//
// Sensor 1: "Forward" facing DROD-Nx
// Sensor 2: "Rear" facing DROD-Nx
//
// Even though the robot has no real front or rear as it can drive in any
// direction we need to nominate a "forward" direction to keep coding
// simple. So the NXT is pointing "forward" and hence that side of the
// robot is the front for our purposes.
//


// Constants

#define ORANGE_BUTTON 3

// DROD-Nx readings for obstacle location
const int kLeftObstacle = 33;
const int kFrontObstacle = 78;
const int kRightObstacle = 67;
const int DRODerror = 3; // readings from DROD may vary by this much

// display related constants for drawing the obstacle map
const int kDisplayWidth = 100;
const int kDisplayHeight = 64;

// dimensions of the central "box" that represents the robot
const int t = 12;	// distance from box corners of "obstacle" line endpoints
const int ax=30, ay=48; // top left of central box
const int w=25, h=32; // width and height of box

const int standardDriveSpeed = 50;
const int warpDriveSpeed = 100;
const int slowDriveSpeed = 15;

// Bitmask defining which segments around the vehicle perimeter an
// obstacle was detected in. Obstacles can be located all around
// the robot so we need to know every obstacle in one reading
// to plan a route away from them
typedef enum {
	kNoObstacles = 0,
	kFrontLeft = 1,
	kFront = 2,
	kFrontRight = 4,
	kRearLeft = 8,
	kRear = 16,
	kRearRight = 32
} tObstacleLocation;

// Global variables
int line = 1;
tObstacleLocation obstacleLocation;

// This array contains the start and endpoints of each line
// segment we draw around the robot graphic on the screen
// Each segment is represented as four coordiantes
// 0: start x
// 1: start y
// 2: end x
// 3: end y
//
// The contents are computed once based on the dimensions specified
// in ax, ay, h, t during initialisation
int obstacleMap[6][4];

//
// Function prototypes
//
void initialiseRobot();
void sayHello();
tObstacleLocation computeObstacleLocation(int reading);
void drawObstacleMap(tObstacleLocation obstacles);
void precomputeObstacleMap();

//
// Monitor the front and rear DROD sensors and update the global
// variable obstacleLocation when an obstacle is detected
task detectObstacles() {

	int front, rear;

	while(1) {

		obstacleLocation = kNoObstacles; // clear previous reading

		// compute the distance between the front DROD reading and each
		// return value. The one with the smallest error is the location
		// of the obstacle
		front = SensorValue[frontDROD];
		rear = SensorValue[rearDROD];

		obstacleLocation |= computeObstacleLocation(front);
		obstacleLocation |= (computeObstacleLocation(rear)*8); // left shift three bits

		wait10Msec(1); // don't hog CPU
	}
}


task main()
{
	// say hello
	sayHello();

	initialiseRobot();

	StartTask(detectObstacles);

	while(1) {
		//nxtDisplayTextLine(1, "Obstacle: %d", obstacleLocation);
		drawObstacleMap(obstacleLocation);
		wait10Msec(30);
	}
	//motor[DriveMotor] = standardDriveSpeed;
	//wait10Msec(200);
	//motor[DriveMotor] = 0;

}

void sayHello() {

	eraseDisplay();
	nxtDisplayTextLine(line++, "Synchro Drive");
	nxtDisplayTextLine(line++, "DriveMotor: A");
	nxtDisplayTextLine(line++, "SteeringMotor: B");
	nxtDisplayTextLine(line++, "DROD-Nx 1: S1");
	nxtDisplayTextLine(line++, "DROD-Nx 2: S2");
	nxtDisplayTextLine(line++, "Press orange...");

	// wait for keypress
	while(nNxtButtonPressed != ORANGE_BUTTON) {
		wait10Msec(10);
	}

	wait10Msec(10);
	eraseDisplay();
}


void initialiseRobot()
{
	// clear the encoders on each motor
	nMotorEncoder[DriveMotor] = 0;
	nMotorEncoder[SteeringMotor] = 0;

	obstacleLocation = kNoObstacles;
	precomputeObstacleMap();

}

// Drive around and avoid hitting anything
void driveAround()
{

	nxtDisplayTextLine(line++, "Driving.");

}

tObstacleLocation computeObstacleLocation(int reading) {

		bool errorLeft, errorFront, errorRight;
		tObstacleLocation o;

		errorLeft = ( abs(kLeftObstacle - reading) <= DRODerror);
		errorFront = ( abs(kFrontObstacle - reading) <= DRODerror);
		errorRight = ( abs(kRightObstacle - reading) <= DRODerror);

		o = kNoObstacles;

		if(errorLeft) {
			o |= kFrontLeft;
		}
		if(errorRight) {
			o |= kFrontRight;
		}
		if(errorFront) {
			o |= kFront;
		}

		return o;
}

// Draw an line map of the obstacles around the robot
// using segments to display the location of obstacles detected

void drawObstacleMap(tObstacleLocation obstacles) {

	int i;

	// first erase the screen
	eraseDisplay();

	// draw the central box that represents the robot
	nxtDrawRect(ax, ay, ax+w, ay-h);

	// now see which bits are set in the obstacle map and draw the line
	// segment corresponding to those bits set to one
	if(obstacles == kNoObstacles) {
		return;
	}

	if(obstacles & kFrontLeft) {
		nxtDrawLine(obstacleMap[0][0],
								obstacleMap[0][1],
								obstacleMap[0][2],
								obstacleMap[0][3]);
	}

	if(obstacles & kFront) {
		nxtDrawLine(obstacleMap[1][0],
								obstacleMap[1][1],
								obstacleMap[1][2],
								obstacleMap[1][3]);
	}

	if(obstacles & kFrontRight) {
		nxtDrawLine(obstacleMap[2][0],
								obstacleMap[2][1],
								obstacleMap[2][2],
								obstacleMap[2][3]);
	}

	if(obstacles & kRearLeft) {
		nxtDrawLine(obstacleMap[3][0],
								obstacleMap[3][1],
								obstacleMap[3][2],
								obstacleMap[3][3]);
	}

	if(obstacles & kRear) {
		nxtDrawLine(obstacleMap[4][0],
								obstacleMap[4][1],
								obstacleMap[4][2],
								obstacleMap[4][3]);
	}

	if(obstacles & kRearRight) {
		nxtDrawLine(obstacleMap[5][0],
								obstacleMap[5][1],
								obstacleMap[5][2],
								obstacleMap[5][3]);
	}

}

void precomputeObstacleMap() {

	int i, j;


	for(i=0; i < 6; i++) {
		for(j=0; j < 4; j++) {
			obstacleMap[i][j] = 0;
		}
	}

	// obstacle segment 1
	obstacleMap[0][0] = ax - t;
	obstacleMap[0][1] = ay;
	obstacleMap[0][2] = ax;
	obstacleMap[0][3] = ay + t;

	// obstacle segment 2
	obstacleMap[1][0] = ax;
	obstacleMap[1][1] = ay + t;
	obstacleMap[1][2] = ax + w;
	obstacleMap[1][3] = ay + t;

	// obstacle segment 3
	obstacleMap[2][0] = ax + w;
	obstacleMap[2][1] = ay + t;
	obstacleMap[2][2] = ax + w + t;
	obstacleMap[2][3] = ay;

	// obstacle segment 4
	obstacleMap[3][0] = ax + w;
	obstacleMap[3][1] = ay - h - t;
	obstacleMap[3][2] = ax + w + t;
	obstacleMap[3][3] = ay - h;

	// obstacle segment 5
	obstacleMap[4][0] = ax;
	obstacleMap[4][1] = ay - h - t;
	obstacleMap[4][2] = ax + w;
	obstacleMap[4][3] = ay - h - t;

	// obstacle segment 6
	obstacleMap[5][0] = ax - t;
	obstacleMap[5][1] = ay - h;
	obstacleMap[5][2] = ax;
	obstacleMap[5][3] = ay - h - t;
}