//
// Lawnmower.nxc
//
// Autonomous lawnmower
//
// 26 July 2010 Mark Crosbie

#include "PFMate-lib.nxc"
#include "SE-lib.nxc"


// Channel 1
// Motor A: steering
// Motor B: drive
//
// Channel 4:
// Motor A: cutting blade

// S1: Mindsensors PFMate
// S2: Light sensor for steering centering
// S3: Mindsensors NXT Line Leader
// S4: Mindsensors NXT Sumo Eyes
//

#define PFMatePort S1
#define LIGHTSENSOR S2
#define LINELEADER S3
#define SUMOEYES S4

#define FORWARD 1
#define REVERSE 2
#define LEFT 3
#define RIGHT 4
#define STOPPED 0
#define STRAIGHT 5

const byte PFMateAddr = 0x48;

#define STEERSPEED 3
#define DRIVESPEED 5
#define THRESHOLD 50
#define ATTCHSPEED 7

#define DUTYCYCLE 100
#define TURNDELAY 400
#define I2CDELAY 20

int direction;		// direction of movement of the robot
int turnDirection;	// turn direction of the rear steering wheel

bool attachmentRunning;

void init() {
	
	SetSensorLight(LIGHTSENSOR);
	SetSensorLowspeed(PFMatePort);
    SumoEyes_SetShortRange(SUMOEYES);

	attachmentRunning = false;
}

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

//////////////////////////////////////////////////////////////////
// Prompt the user to center the steering wheels at the rear
// by using the Left/Right buttons on the NXT until the steering
// is centered with the light sensor
//
//////////////////////////////////////////////////////////////////
void centerSteering() {

	int r;
	
	ClearScreen();
	
	TextOut(0, LCD_LINE1, "Center steering");
	TextOut(0, LCD_LINE2, "Left/Right arrow");
	TextOut(0, LCD_LINE3, "Orange to exit");
	
	while(true) {
	
		r = Sensor(LIGHTSENSOR);
		if(r > THRESHOLD) {
			TextOut(0, LCD_LINE5, "Centered  ");
		} else {
			TextOut(0, LCD_LINE5, "Off center");
		}
		
		if(ButtonPressed(BTNCENTER, false)) {
			break;
		}
		
		if(ButtonPressed(BTNLEFT, false)) {
			PFMate_controlMotorA(PFMatePort, PFMateAddr, PFMATE_CHANNEL1,
				PFMATE_FORWARD, STEERSPEED);	
			Wait(50);
			PFMate_controlMotorA(PFMatePort, PFMateAddr, PFMATE_CHANNEL1,
				PFMATE_BRAKE, 0);		

		}	

		if(ButtonPressed(BTNRIGHT, false)) {
			PFMate_controlMotorA(PFMatePort, PFMateAddr, PFMATE_CHANNEL1,
				PFMATE_REVERSE, STEERSPEED);	
			Wait(50);
			PFMate_controlMotorA(PFMatePort, PFMateAddr, PFMATE_CHANNEL1,
				PFMATE_BRAKE, 0);		
		}	
		
		Wait(50);
		
	}
		
	ClearScreen();
	
}

//////////////////////////////////////////////////////////////////
// Start driving forwards
//////////////////////////////////////////////////////////////////
void forward() {

	PFMate_controlMotorB(PFMatePort, PFMateAddr, PFMATE_CHANNEL1,
		PFMATE_REVERSE, DRIVESPEED);	
	direction = FORWARD;
	Wait(I2CDELAY);
}

//////////////////////////////////////////////////////////////////
// Drive in reverse direction
//////////////////////////////////////////////////////////////////
void reverse() {

	PFMate_controlMotorB(PFMatePort, PFMateAddr, PFMATE_CHANNEL1,
		PFMATE_FORWARD, DRIVESPEED);	
	direction = REVERSE;
	Wait(I2CDELAY);
}

//////////////////////////////////////////////////////////////////
// Stop the driving motor
//////////////////////////////////////////////////////////////////
void brake() {
	
	PFMate_controlMotorB(PFMatePort, PFMateAddr, PFMATE_CHANNEL1,
		PFMATE_BRAKE, 0);		
	direction = STOPPED;
	Wait(I2CDELAY);
}

//////////////////////////////////////////////////////////////////
// Align the steering wheel with the light sensor so the robot
// drives straight
//////////////////////////////////////////////////////////////////
void goStraight() {

	int r;
		
	if(turnDirection == RIGHT) {
		// wheels are turned to the right, so move them left until
		// we reach the light sensor
		r = Sensor(LIGHTSENSOR);
		while(r < THRESHOLD) {	
			PFMate_controlMotorA(PFMatePort, PFMateAddr, PFMATE_CHANNEL1,
				PFMATE_FORWARD, STEERSPEED);	
			Wait(50);
			r = Sensor(LIGHTSENSOR);
		}
		PFMate_controlMotorA(PFMatePort, PFMateAddr, PFMATE_CHANNEL1,
			PFMATE_BRAKE, 0);		

	} else if(turnDirection == LEFT) {
		// wheels are turned to the left, so move them right until
		// we reach the light sensor
		r = Sensor(LIGHTSENSOR);
		while(r < THRESHOLD) {	
			PFMate_controlMotorA(PFMatePort, PFMateAddr, PFMATE_CHANNEL1,
				PFMATE_REVERSE, STEERSPEED);	
			Wait(50);
			r = Sensor(LIGHTSENSOR);
		}
		PFMate_controlMotorA(PFMatePort, PFMateAddr, PFMATE_CHANNEL1,
			PFMATE_BRAKE, 0);		
	}
	
	turnDirection = STRAIGHT;
}

//////////////////////////////////////////////////////////////////
// Turn the steering wheels so the robot drives to the left
//////////////////////////////////////////////////////////////////
void turnLeft() {
	
	// first ensure that we are steering straight
	goStraight();
	
	// now do the turn
	PFMate_controlMotorA(PFMatePort, PFMateAddr, PFMATE_CHANNEL1,
			PFMATE_FORWARD, STEERSPEED);	
	Wait(TURNDELAY);
	PFMate_controlMotorA(PFMatePort, PFMateAddr, PFMATE_CHANNEL1,
		PFMATE_BRAKE, 0);		
	turnDirection = LEFT;
	Wait(I2CDELAY);
}

//////////////////////////////////////////////////////////////////
// Turn the steering wheels so the robot drives to the right
//////////////////////////////////////////////////////////////////
void turnRight() {
	
	// first ensure that we are steering straight
	goStraight();
	
	// now do the turn
	PFMate_controlMotorA(PFMatePort, PFMateAddr, PFMATE_CHANNEL1,
			PFMATE_REVERSE, STEERSPEED);	
	Wait(TURNDELAY);
	PFMate_controlMotorA(PFMatePort, PFMateAddr, PFMATE_CHANNEL1,
		PFMATE_BRAKE, 0);		
	turnDirection = RIGHT;
	Wait(I2CDELAY);
}

//////////////////////////////////////////////////////////////////
// Start the attachment motor turning
//////////////////////////////////////////////////////////////////
void startAttachment() {
	PFMate_controlMotorA(PFMatePort, PFMateAddr, PFMATE_CHANNEL4,
		PFMATE_REVERSE, ATTCHSPEED);		
	attachmentRunning = true;
	Wait(I2CDELAY);
}

//////////////////////////////////////////////////////////////////
// Stop the attachment motor
//////////////////////////////////////////////////////////////////
void stopAttachment() {
	PFMate_controlMotorA(PFMatePort, PFMateAddr, PFMATE_CHANNEL4,
		PFMATE_BRAKE, 0);	
	attachmentRunning = false;	
	Wait(I2CDELAY);
}

//////////////////////////////////////////////////////////////////
// Return true if an obstacle is detected in front of the robot
// Set the direction of the object in the ob reference parameter
//////////////////////////////////////////////////////////////////
bool obstacleDetected(int &ob) {
	
    ob = SumoEyes_DetectObstacleZone(SUMOEYES);
	if(ob != SE_NONE) {
		return true;
	} else {
		return false;
	}
}

task main() {
	
	int ob;
	int timer;
	bool obstacle;
	
	init();
	
	centerSteering();
		
	TextOut(0, LCD_LINE1, "Lawnmower");
	
	Wait(1000);
		
	while(!ButtonPressed(BTNCENTER, false)) {
		obstacle = obstacleDetected(ob);
		// check for obstacles before starting blades
		if(obstacle) {
			//PlayTone(800, 25);
			stopAttachment();
		} else {
			startAttachment();
		}	

		if(obstacle) {
		
			TextOut(0, LCD_LINE3, "Obstacle");

			// All stop!
			brake();
			
			if(ob == SE_LEFT) {
				turnLeft();
			}
			
			if(ob == SE_RIGHT) {
				turnRight();
			}
			
			if(ob == SE_FRONT) {
				// occasionally we'll randomly turn if just reversing straight
				// to avoid getting stuck in a corner
				if(Random(100) <= 50) {
					turnLeft();
				}
			}
			
			// We'll reverse for a random time amount each go
			// but be sure to not stop until the obstacle is gone	
			timer = Random(5);
			reverse();
			while(timer--) {
				PlayTone(1500, 500);
				Wait(800);
				
				// clumsy ugly logic
				if( (timer == 1) && obstacleDetected(ob)) {
					timer++; // I should be shot for this...
				}
			}
			brake();
			
			goStraight();
			
		} else {
			
			TextOut(0, LCD_LINE3, "Forward  ");
			forward();
		}

		Wait(DUTYCYCLE);
	}	
	
	PlayTone(800, 50);	
	stopAttachment();
	brake();
	
}