//*!!Sensor,    S1,          touchSensor, sensorTouch,      ,                    !!*//
//*!!Sensor,    S2,      soundSensorLeft, sensorSoundDB,      ,                  !!*//
//*!!Sensor,    S3,     soundSensorRight, sensorSoundDB,      ,                  !!*//
//*!!Sensor,    S4,          sonarSensor, sensorSONAR,      ,                    !!*//
//*!!Motor,  motorA,         motorStinger, tmotorNormal,                         !!*//
//*!!Motor,  motorB,            motorLeft, tmotorNormal,                         !!*//
//*!!Motor,  motorC,           motorRight, tmotorNormal,                         !!*//
//*!!                                                                            !!*//
//*!!Start automatically generated configuration code.                           !!*//
const tSensors touchSensor          = (tSensors) S1;   //sensorTouch        //*!!!!*//
const tSensors soundSensorLeft      = (tSensors) S2;   //sensorSoundDB      //*!!!!*//
const tSensors soundSensorRight     = (tSensors) S3;   //sensorSoundDB      //*!!!!*//
const tSensors sonarSensor          = (tSensors) S4;   //sensorSONAR        //*!!!!*//
const tMotor   motorStinger         = (tMotor) motorA; //tmotorNormal       //*!!!!*//
const tMotor   motorLeft            = (tMotor) motorB; //tmotorNormal       //*!!!!*//
const tMotor   motorRight           = (tMotor) motorC; //tmotorNormal       //*!!!!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//

//********************************************************
// Spike
//
// Rob Torok, 22/10/06
//
// My first attempt at a RobotC program!!
//
// 1.  Move around randomly (either forward, left, right, or stop).
//
// 2.  Move towards sound.
// [I'm still not sure of the best way of dealing with turning towards sound...]
//
// 3. Move away when someone comes close-ish, sting if they
//    get	too close.
// [Update 28/10 - I had to use SensorRaw rather than SensorValue to avoid the
// ultrasound readings getting 'stuck']
//
// 4. Say "Spike" if we strike!
// [Or "Woops" for the purposes of testing...]
//
//
//Subsumption idea/structure from Knudsen (1999),
//"The Unofficial Guide to LEGO MINDSTORMS Robots"
//
//********************************************************


// I think these thresholds were quite different because one of my
// sound sensors is a pre-release model.
const int leftSoundThreshold = 30;
const int rightSoundThreshold = 55;

// These variables are how I dealt with the sound senors giving different
// readings. The idea was to subtract the thresholds (set above) from the
// actual sensor readings to produce comparable readings.
int leftSoundDifference;
int rightSoundDifference;


// Distances in centimetres
const int mediumCloseThreshold = 50;
const int muchTooCloseThreshold = 15;

const int COMMAND_NONE = 0;
const int COMMAND_STOP = 1;
const int COMMAND_FLOAT = 2;
const int COMMAND_FORWARD = 3;
const int COMMAND_REVERSE = 4;
const int COMMAND_LEFT = 5;
const int COMMAND_RIGHT = 6;
const int COMMAND_STRIKE = 7;
const int COMMAND_TO_SOUND = 8;

int cruiseCommand;
int listenCommand;
int sonarCommand;
int motorCommand;

int leftMotorPower;
int rightMotorPower;

int randTime;
int randMovement;

//	bFloatDuringInactiveMotorPWM = false;

task stinger()
{
	while (true)
	{
		if(SensorValue(touchSensor) == 1)
		{
//			PlaySound(soundFastUpwardTones);
			PlaySoundFile("Woops.rso");
			wait10Msec(100);
		}
	}
}

task cruise()
{
	while (true)
  {
 		randMovement = random(4);
		switch (randMovement)
		{
		case 1:
		  cruiseCommand = COMMAND_FORWARD;
		  break;
		 case 2:
		  cruiseCommand = COMMAND_LEFT;
		  break;
		 case 3:
		  cruiseCommand = COMMAND_RIGHT;
		  break;
		default:
		  cruiseCommand = COMMAND_STOP;
		}

		randTime = random(300);
		wait10Msec(randTime+200);

  }
}

task listen()
{
  while (true)
  {
  	leftSoundDifference = (SensorValue(soundSensorLeft) - leftSoundThreshold);
  	rightSoundDifference = (SensorValue(soundSensorRight) - rightSoundThreshold);

		if ( (leftSoundDifference > 0) || (rightSoundDifference > 0) )
		{
			listenCommand = COMMAND_TO_SOUND;
		}
		else
		{
			listenCommand = COMMAND_NONE;
		}
  }
}

task sonar()
{
	while(true)
	{
		if (SensorRaw(sonarSensor) < muchTooCloseThreshold)
		{
			sonarCommand = COMMAND_STRIKE;
		}
		else if (SensorRaw(sonarSensor) < mediumCloseThreshold)
		{
			sonarCommand = COMMAND_REVERSE;
		}
		else
		{
			sonarCommand = COMMAND_NONE;
		}
	}
}


void motorControl()
{
	if ((motorCommand == COMMAND_STOP) || (motorCommand == COMMAND_FLOAT))
	{
		motor[motorLeft] = 0;
		motor[motorRight] = 0;
	}
	else if (motorCommand == COMMAND_FORWARD)
	{
		motor[motorLeft] = 75;
		motor[motorRight] = 75;
	}
	else if (motorCommand == COMMAND_REVERSE)
	{
		motor[motorLeft] = -75;
		motor[motorRight] = -75;
	}
	else if (motorCommand == COMMAND_LEFT)
	{
		motor[motorLeft] = -75;
		motor[motorRight] = 75;
	}
	else if (motorCommand == COMMAND_RIGHT)
	{
		motor[motorLeft] = 75;
		motor[motorRight] = -75;
	}
	else if (motorCommand == COMMAND_TO_SOUND)
	{
		if (rightSoundDifference > 0)
		{
//			leftMotorPower = 50+(rightSoundDifference * 50)/(100-rightSoundThreshold);
			leftMotorPower = 50+rightSoundDifference;
		}
		else
		{
			leftMotorPower = 0;
		}

		if (leftSoundDifference > 0)
		{
//			rightMotorPower = 50+(leftSoundDifference * 50)/(100-leftSoundThreshold);
			rightMotorPower = 50+leftSoundDifference;

		}
		else
		{
			rightMotorPower = 0;
		}

		motor[motorLeft] = leftMotorPower;
		motor[motorRight] = rightMotorPower;
		wait10Msec(100);

	}
	else if (motorCommand == COMMAND_STRIKE)
	{
		motor[motorLeft] = 0;
		motor[motorRight] = 0;
		motor[motorStinger] = 100;
		wait10Msec(27);
		motor[motorStinger] = -100;
		wait10Msec(35);
		motor[motorStinger] = 0;
		wait10Msec(70);
	}
}

task arbitrate()
{
	while (true)
	{
		motorCommand = cruiseCommand;
		if (sonarCommand != COMMAND_NONE) motorCommand = sonarCommand;
		if (listenCommand != COMMAND_NONE) motorCommand = listenCommand;
		if (sonarCommand == COMMAND_STRIKE) motorCommand = sonarCommand;
		motorControl();
	}
}

task main()
{

	//reset the stinger
	motor[motorStinger] = -100;
	wait10Msec(30);
	motor[motorStinger] = 0;

	//start the tasks
	StartTask(stinger);
	StartTask(cruise);
	StartTask(sonar);
	StartTask(listen);
	StartTask(arbitrate);

	while(true)
	{
		int x = SensorRaw(sonarSensor);								//stores the sonar sensor reading as an integer variable
		eraseDisplay();
		nxtDisplayTextLine(0,"Sonar: %d",x);			//displays the reading on the LCD screen

		if ( (leftSoundDifference > 0) || (rightSoundDifference > 0) )
		{
			if (leftSoundDifference == rightSoundDifference)
			{
				nxtDisplayTextLine(1,"   Centre");
			}
			else if (leftSoundDifference > rightSoundDifference)
			{
				nxtDisplayTextLine(1,"Left: %d",leftSoundDifference-rightSoundDifference);
			}
			else if (leftSoundDifference < rightSoundDifference)
			{
				nxtDisplayTextLine(1,"    %d :Right",rightSoundDifference-leftSoundDifference);
			}
		}
		nxtDisplayTextLine(2,"Sound, Left: %d",SensorValue(soundSensorLeft));
		nxtDisplayTextLine(3,"Sound, LDiff:%d",leftSoundDifference);
		nxtDisplayTextLine(4,"Sound, Right:%d",SensorValue(soundSensorRight));
		nxtDisplayTextLine(5,"Sound, RDiff:%d",rightSoundDifference);
		nxtDisplayTextLine(6,"Left Power: %d",leftMotorPower);
		nxtDisplayTextLine(7,"Right Power:%d",rightMotorPower);
		wait10Msec(20);
	}
}