import java.io.*;
import josx.rcxcomm.*;
import josx.platform.rcx.*;
import josx.robotics.*;

/**
  * 
  * 
  * 
  * Author : Swapnil Pathare
  *
 */
public class CarDriveMini extends Thread
{
	/* Power adjustment for motors to go straight */
	static int powerLeft=3;
	static int powerRight=7;

	/* The choices in useMotors	*/
	public static final int stopCar=10;
	public static final int driveStraight=11;
	public static final int verifyObstacle=12;
	public static final int floatCar=13;
	public static final int rotateCar=14;
	public static final int swerveLeft=15;
	public static final int turnBack=16;
	public static final int alignWall=17;

	TimingNavigator navigator =null;
	public boolean overtaking=true;
	public TouchSense ts=null;
	public LightSense ls=null;
	public InfraredSense is=null;
	public volatile int rightBumperHit=0;
	public volatile int leftBumperHit=0;
	public byte carId=20;				// MY CAR IS NUMBER 20
	


	public static void main(String args[]) throws Exception{
		CarDriveMini car=new CarDriveMini();
		car.start();
	}

	public void run()
	{
		Motor.A.setPower(powerLeft);
		Motor.C.setPower(powerRight);
		
		/* Motor.B is actually the TailLight  */
		Motor.B.setPower(powerRight);
		Motor.B.forward();
		

		navigator = new TimingNavigator(Motor.C, Motor.A, 8.12f, 3.835f);
		navigator.forward();

//		ts=new TouchSense(this);
		ls=new LightSense(this);
//		is=new InfraredSense(this);
//		ts.start(); 
		ls.start();
//		is.start();


	}
	
	/***
	* Method avoid_obstacle : accepts variable left,dist ; returns 0 if no collision
	* left=true : avoid by turning from left side
	* left=false: avoid by turning from right side
	* dist		: How far to go
	*
	* Function: moves car around the obstacle and places it ahead of obstacle
	***/
	public int avoidObstacle(boolean left, int dist)
	{	int num=60;	//first rotation
		if (!left)
			num=-60;
		//		|            |
		//		 \          /
		//		  \        /
		//		   |  OR  |     DEPENDING UPON VARIABLE left
		//		   |      |
		//		  /        \
		//		 /          \
		//		|            |

		navigator.rotate(num);	
		navigator.travel(dist/2);	
		if (Sensor.S1.readBooleanValue())
			return(99);
		if (Sensor.S3.readBooleanValue())
			return(88);
		navigator.travel(dist/2);	
		if (Sensor.S1.readBooleanValue())
			return(99);
		if (Sensor.S3.readBooleanValue())
			return(88);
		navigator.rotate(-num);	
		navigator.travel(dist/2);	
		if (Sensor.S1.readBooleanValue())
			return(99);
		if (Sensor.S3.readBooleanValue())
			return(88);
		navigator.travel(dist/2);	
		if (Sensor.S1.readBooleanValue())
			return(99);
		if (Sensor.S3.readBooleanValue())
			return(88);
		navigator.rotate(-num);	
		navigator.travel(dist/2);	
		if (Sensor.S1.readBooleanValue())
			return(99);
		if (Sensor.S3.readBooleanValue())
			return(88);
		navigator.travel(dist/2);	
		navigator.rotate(num);	
		navigator.forward();
		if (Sensor.S1.readBooleanValue())
			return(99);
		if (Sensor.S3.readBooleanValue())
			return(88);

		return 0;
	}

	public synchronized int useMotors(int ch1,int ch2)
	{
		if (ch1==CarDriveMini.driveStraight)
		{
			navigator.forward();
			return 0;
		}
		if (ch1==CarDriveMini.stopCar)
		{
			navigator.stop();
			return 0;
		}
		if (ch1==CarDriveMini.floatCar)
		{
			navigator.stop();
			return 0;
		}
		if (ch1==CarDriveMini.rotateCar)
		{
			navigator.rotate(ch2);
			return 0;
		}
		if (ch1==CarDriveMini.swerveLeft)
		{	//need to check Touch Sensor!
			boolean wasRunning=Motor.A.isMoving() || Motor.C.isMoving();
			navigator.rotate(60);
			for (int i=0;i<4;i++ )
			{
				navigator.travel(8);
				if ((Sensor.S1.readBooleanValue()) || (Sensor.S3.readBooleanValue()))
				{
					return(99);
				}
			}
			navigator.rotate(-60);
			if ((Sensor.S1.readBooleanValue()) || (Sensor.S3.readBooleanValue()))
			{
				return(99);
			}
			if (wasRunning)
			{
				navigator.forward();
			}
			return 0;
		}
		if (ch1==CarDriveMini.verifyObstacle)
		{

			//	perform all operations and give back
			//	aligned car in proper direction
			// return value 0 indicates it is ready to drive
			// return value 99 indicates that there was a collision.


			// move back a little in any case
			navigator.travel(-10);
			int result=1;
			if (ch2<10)
			{	result=avoidObstacle(false,25);	//avoid from right
			}
			else
			{
				result=avoidObstacle(true,25);	//avoid from left
			}
			return result;
		}
		if (ch1==CarDriveMini.turnBack)
		{
			navigator.travel(-10);
			if (ch2==99)
			{
				// left bumper hit most recently...turn left
				navigator.rotate(160);
			}
			else if (ch2==88)
			{
				// right bumper hit most recently...turn right
				navigator.rotate(-160);
			}
			navigator.forward();
			return 0;
		}
		if (ch1==CarDriveMini.alignWall)
		{
			navigator.travel(-10);
			if (ch2==1)
			{	//wall on the right...try rotating left
				navigator.rotate(65);
			}
			else if (ch2==2)
			{	//wall on the left...try rotating right
				navigator.rotate(-65);
			}
			navigator.forward();
			return 0;
		}

		return 0;
	}
}


class TouchSense extends Thread
{
	CarDriveMini car=null;
	public TouchSense(CarDriveMini parentObj)
	{
		// Set the 2 touch sensors in boolean mode
		Sensor.S1.setTypeAndMode(SensorConstants.SENSOR_TYPE_TOUCH, SensorConstants.SENSOR_MODE_BOOL);
		Sensor.S3.setTypeAndMode(SensorConstants.SENSOR_TYPE_TOUCH, SensorConstants.SENSOR_MODE_BOOL);
		car=parentObj;
	}

	public void run()
	{
		Sensor.S3.activate();
		Sensor.S1.activate();
		boolean s1=false,s3=false;
		try{Thread.sleep(100);}catch (Exception e)	{}

		while(true)
		{
			s1=Sensor.S1.readBooleanValue();
			s3=Sensor.S3.readBooleanValue();
			if(s1 || s3)	//touched...handle the collision
			{
//				try{Thread.sleep(1000);}catch (Exception e)	{} 
				//	signal other threads to stop, take exclusive control of the car 
				//	but what if other threads are inside exclusively used methods useIR / useMotors?
				//	Other threads should check for stopFlag as soon as they exit useIR/useMotors
				//
//				car.ls.stopFlag=true;
				car.is.stopFlag=true;

//				while( ! car.ls.stopped);	// wait till other sensors stop

				car.useMotors(CarDriveMini.stopCar,0);


				int bumpers=0;	
				if (s1)
				{
					bumpers++;
				}
				if (s3)
				{
					bumpers+=10;
				}
				int motorValue=car.useMotors(CarDriveMini.verifyObstacle,bumpers);
				if (motorValue==0)
				{
					// obstacle handled. proceed
					car.leftBumperHit=0;
					car.rightBumperHit=0;
					car.useMotors(CarDriveMini.driveStraight,0);
				}
				else
				{
					LCD.showNumber(motorValue);
					if (motorValue==88)
						car.rightBumperHit++;
					else if (motorValue==99)
						car.leftBumperHit++;
					
					if ((car.rightBumperHit>=3) && (car.leftBumperHit >=3))
					{
						// squeezed in the corner or someplace similar. turn back & come out
						car.useMotors(CarDriveMini.turnBack,motorValue);
						car.leftBumperHit=0;
						car.rightBumperHit=0;
					}
					else if (car.rightBumperHit>=3)
					{
						// cud be a wall on the right... angle myself a little left
						car.useMotors(CarDriveMini.alignWall,1);
					}
					else if (car.leftBumperHit>=3)
					{
						// cud be a wall on the left... angle myself a little right
						car.useMotors(CarDriveMini.alignWall,2);
					}
					else
						continue;
				}
				// AFTER COMPLETING all activities & setting car back to driving line, 
				// restart both light & IR
//				car.ls.stopFlag=false;
				car.is.stopFlag=false;

			}
		}
	}
};

class LightSense extends Thread
{
	public boolean stopFlag=false;
	public boolean stopped=false;
	CarDriveMini car=null;
	public LightSense(CarDriveMini parentObj)
	{
		// Set the light sensor in RAW mode
		Sensor.S2.setTypeAndMode(SensorConstants.SENSOR_TYPE_LIGHT, SensorConstants.SENSOR_MODE_RAW);
		car=parentObj;
	}

	public void run()
	{
		Sensor.S2.activate();
		int lightValue=0;
		try{Thread.sleep(100);}catch (Exception e)	{}
		while (true)
		{
			stopped=false;
			while(! stopFlag)
			{	
				// problem to use the useMotors method is that if collision occurs while tailing car
				//	then collision is to be given priority, and tailing has to be ABANDONED...
				//	so a 'break; ' will be required below, if proper tailing/overtaking used.
				
				lightValue=Sensor.S2.readRawValue();
				while((lightValue<700) && (lightValue>600))	
				{
					LCD.showNumber(lightValue);
					car.useMotors(CarDriveMini.floatCar,0);

					if (stopFlag)	//	check if stop request
						break;

					car.useMotors(CarDriveMini.driveStraight,0);
					lightValue=Sensor.S2.readRawValue();
				}
				if (lightValue<=600)
				{
					// Taillight of car in front gets brighter, even though i'm going slow.
					// The car may be stopping just like that, or communicating with IR
					// Overtaking the car directly may miss IR (landmark). Wait and see for sometime
					//  if the car moves. If it doesn't move, overtake it. 
					car.useMotors(CarDriveMini.stopCar,0);
					//wait for 5 seconds to see if the car in front moves
					try{Thread.sleep(5000);}catch (Exception e)	{} 
					lightValue=Sensor.S2.readRawValue();
					if (lightValue>650)
					{
						car.useMotors(CarDriveMini.driveStraight,0);

						if (stopFlag)	//	check if stop request
							break;

						continue;
					}
					//car hasn't moved or has moved very little. Overtake it !
					
					// what if obstacle comes during overtaking? keep goal co-ord. in memory
					car.overtaking=true;
					int result=car.avoidObstacle(false,30);	// overtake from right, cover distance of 60
					car.overtaking=false;

				}
				if (stopFlag)	//	check if stop request
					break;
			}
			stopped=true;
			try
			{
				while (stopFlag)
				{
					Thread.sleep(500);
				}
			}
			catch (Exception e)	{}
		}
	}
};

class InfraredSense extends Thread
{
	CarDriveMini car=null;
	public InfraReceive iRecv ;
	public InfraSend iSend;
	public volatile boolean read=false;
	public volatile boolean written=false;
	public boolean landmark=false;
	public boolean stopFlag=false;
	public boolean stopped=false;
	public int readValue=0;
	public int writeValue=0;
	
	public RCXPort rcxport=null;
	public DataInputStream  ris;
	public DataOutputStream ros;

	public InfraredSense(CarDriveMini parentObj)
	{
		// Set the infrared light port
		car=parentObj;
		writeValue=car.carId;
		try
		{
			rcxport=new RCXPort();
			ris=new DataInputStream(rcxport.getInputStream());
			ros=new DataOutputStream(rcxport.getOutputStream());
		}
		catch (Exception e)
		{
		}
	}
	public void run()
	{
		boolean respawnRead=true;
		boolean respawnWrite=true;

		boolean touched;
		iRecv=new InfraReceive(this);
		iRecv.start();
		iSend=new InfraSend(this);
		iSend.start();
		while (true)
		{
			Sound.beep();
			touched=false;
			stopped=false;
			read=false;
			written=false;
			if (respawnRead)
			{
				iRecv.resume=true;
			}
			if (respawnWrite)
			{
				iSend.resume=true;
			}
			while (true)
			{
				landmark=false;
				if (stopFlag)
				{
					break;
				}
				if (read && written)
				{
					// a value read at this point of time, 
					//	> 40 is communication between other IRs
					if ((readValue>=30) && (readValue <=40))
					{
						// Landmark in front. quit
						landmark=true;
						break;
					}
					else if (readValue<30)
					{
						// car in front. swerve left
						// also reset the booleans to avoid false alarms while car swerves left
						read=false;
						written=false;
						touched=(car.useMotors(CarDriveMini.swerveLeft,0)==99)?true:false;
						if (touched)
						{
							break;
						}
						iRecv.resume=true;
						iSend.resume=true;
					}
					else 
					{
						// landmark is communicating with another car.
						//	wait for sometime and retry.
						car.useMotors(CarDriveMini.stopCar,0);
						LCD.showNumber(3100);
						try{Thread.sleep(4000);}catch (Exception e)	{}
						iRecv.resume=true;
						iSend.resume=true;
					}
				}
				else if (read && (!written))
				{
					// value is read, but communication broke before value was written
					// or maybe i'm checking just before value is about to be written
					// give it some time...
					try{Thread.sleep(500);}catch (Exception e)	{}
					if (!written)
					{
						// surely communication error. now the read thread has ended!
						// restart the read thread, so that next time there is no communication
						//problem bcos of 'only write'

						iRecv.resume=true;
						iSend.resume=false;
					}	
					else 
					{
						iRecv.resume=false;
						iSend.resume=false;
					}

				}
				else if ((!read) && written)
				{
					// value is written, but communication broke before value was read
					// or maybe i'm checking just before value is about to be read
					// give it some time...
					try{Thread.sleep(500);}catch (Exception e)	{}
					if (!read)
					{
						// surely communication error. now the write thread has ended!
						// restart the write thread, so that next time there is no communication
						//problem bcos of 'only read'
						iRecv.resume=false;
						iSend.resume=true;
					}
					else 
					{
						iRecv.resume=false;
						iSend.resume=false;
					}
				}
			}
			if (touched)
			{
				stopFlag=true;
				stopped=true;
			}
			//landmark is present. stop the car, communicate with it, get directions.
			// first stop light sensor. stopping touch sensor is unnecesary, since it is not 
			// going to feel anything on bumpers
			// what if IR is found WHILE touch sensor module was circumventing an obstacle?
//swapz:do
			if ((!stopFlag) && landmark)
			{
				// TouchSense has not asked me to stop... hence it is not performing any task
				// After stopping car, there is rare chance that TouchSense may perform any op.
				//  now once again verify that Landmark is in front, else blocking calls will
				//  put car in trouble ..
				car.useMotors(CarDriveMini.stopCar,0);

				read=false;
				written=false;
				iRecv.resume=true;
				iSend.resume=true;
				try{Thread.sleep(1500);}catch (Exception ex)	{}

				if(read && written && (readValue>=30) && (readValue<=40))
				{	//yes, Landmark surely is in front. proceed with data exchange
					int tower=readValue;
					int angle=0,direction=0;
					try
					{
						ros.write(199);		//value to suggest i'm ready for pt.to pt. communication
						if(ris.read()==199)
						{
							// the angle received is pre-incremented by 50, to avoid another
							// car assuming it as TowerId or CarId
							angle=ris.read();		//values: 0 - 180 sent as (angle+50).
							direction=ris.read();	//values: 190 (anticlock), 200 (clock)
							if (direction==190)
							{
								direction=angle-50;
							}
							else if (direction==200)
							{
								direction=-(angle-50);
							}
							car.useMotors(CarDriveMini.rotateCar,direction);
							car.useMotors(CarDriveMini.driveStraight,0);

							try{Thread.sleep(2000);}catch (Exception ex)	{}
							
						}
					}
					catch (Exception e)
					{
						LCD.showNumber(666);
					}
				}
				else if (read && (!written))
				{
					// some problem, so go to next iteration, but spawn read thread only,
					// write thread is already on, and blocked
					respawnRead=true;
					respawnWrite=false;
					continue;
				}
				else if (written && (!read))
				{
					// some problem, so go to next iteration, but spawn read thread only,
					// write thread is already on, and blocked
					respawnRead=false;
					respawnWrite=true;
					continue;
				}
				else if ((!read) && (!written))
				{
					//	!read && !written
					respawnRead=false;
					respawnWrite=false;
					continue;
				}
			}
			
			try
			{
				while (stopFlag)
				{
					stopped=true;
					Thread.sleep(1000);
				}
			}
			catch (Exception e)	{}
			
			respawnRead=true;
			respawnWrite=true;
			
		}
	}

};

class InfraReceive extends Thread
{
	public DataInputStream ris;
	public InfraredSense is;
	public boolean resume;
	public InfraReceive(InfraredSense infra)
	{
		is=infra;
		ris=infra.ris;
	}
	public void run()
	{
		while (true)
		{
			while(!resume)
			{
				try{Thread.sleep(200);}catch (Exception ex)	{}
			}
			resume=false;
			is.read=false;
			try
			{
				is.readValue=ris.read();
			}
			catch (Exception e)
			{			LCD.showNumber(777);
					try{Thread.sleep(800);}catch (Exception ex)	{}
			}
			is.read=true;
			
		}
	}
};

class InfraSend extends Thread
{
	public DataOutputStream ros;
	public InfraredSense is;
	public boolean resume;
	public InfraSend(InfraredSense infra)
	{
		is=infra;
		ros=infra.ros;
	}
	public void run()
	{
		while (true)
		{
			while(!resume)
			{
				try{Thread.sleep(200);}catch (Exception ex)	{}
			}
			resume=false;
			is.written=false;
			try
			{
				ros.write(is.writeValue);
			}
			catch (Exception e)
			{
				LCD.showNumber(888);
					try{Thread.sleep(300);}catch (Exception ex)	{}
			}
			
			is.written=true;
		}
	}
};