#include ".\robot.h"
#include <time.h>
#include <math.h>

Robot::Robot(void)
{

	minParticleProbability = 0.001;

	linearSpawnSpread = 2.0;
	angularSpawnSpread = 5.0;

	// this times odometer reading is our best estimate of the real world value
	linearCalibrationFactor = 0.9886;
	angularCalibrationFactor = 1.002;

	// we add something between + and - the uncertainty fraction to the calibration factor
	// before multiplying it to the odometer reading.  this creates our uncertainty model for movement
	linearUncertaintyFraction = 0.1; // our robot statistical data suggests this is 0.0188,
	angularUncertaintyFraction = 0.05;  // our robot statistical data suggests this is 0.00968

	// Sensor Model info
	// flags for which sensors we want to incorporate into the MCL algorithm
	usingCamera = true;
	usingSonar = true;
	usingIR = true;
	ignoreCenterIR = true; // we're not using our center IR (we use it to check for stairs to not fall off of)

	// Sensor Model info
	sonarUncertaintyFraction = .15; 
	sonarMax = 170; 
	IRMax = 1024;
	IRMaxThreshold = 1000; // if the IR reading is greater than this it counts as no bump
	IRBumpThreshold = 150; // if the IR reading is less than this it counts as a bump
	IRBumpRange = 30; // range in cm that the IR sensor can see

	sonarParticlePlaceUncertainty = 30; // particles can be +/- 30cm from the actual sonar reading when using WALL_RELATIVE placement

	realRobot = false;

	mapMaxX = 300;
	mapMaxY = 300;

	// seeding dummy sensor data
	double son_dist = 25.0;
	this->ir[0] = 25.0;
	this->ir[1] = 25.0;
	this->ir[2] = 25.0;
	this->is_red = 0;         // 1 will be if the camera sees red wall, 0 will be sees no red wall

	PI = 3.141592653589793;
	//setting the origin of the robot
	this->x_orig = 0.0;
	this->y_orig = 0.0;
	this->theta = 0.0;

	//setting the origin for the particles
	this->x_orig_old = 0.0;
	this->y_orig_old = 0.0;
	this->theta_old = 0.0;
	
	//setting initial bool
	this->show_IR = false;
	this->show_Hist = false;
	this->show_sonar = false;
	this->show_particle_sonar = false;
	this->draw_particles = false;
	
	
	//setting the left IR origin  (based on the location of our left IR relative to center of rotation)
	this->ir_x[0] = 16.0*2.54; // inches *2.54 cm/inch
	this->ir_y[0] = 3.5*2.54; // inches *2.54 cm/inch
	this->ir_th[0] = 15.0;

	//setting the center IR origin (not used, ours is setup for stairs detection)
	this->ir_x[1] = 15.0*2.54;
	this->ir_y[1] = 0.0; 
	this->ir_th[1] = 0.0;

	//setting the right IR origin (based on the location of our right IR relative to center of rotation)
	this->ir_x[2] = 16.0*2.54; // inches *2.54 cm/inch
	this->ir_y[2] = -3.5*2.54; // inches *2.54 cm/inch
	this->ir_th[2] = -15.0;

	//setting the camera origin
	this->cam_x = 12.0*2.54; // inches *2.54 cm/inch
	this->cam_y = 2.5*-2.54;
	this->cam_th = 0.0;
	this->cam_red = 0.0;

	//setting the sonar origin
	this->son_x = 2.54*14.0; // convert from inches to cm
	this->son_y = 2.0*2.54;
	this->son_th = 0.0;    // this, of course, comes from the client
	this->son_dist = 50.0;  // this too should be sent from the real robot
	
	//pushing the origin into list
	x_hist.push_back(x_orig);
	y_hist.push_back(y_orig);
	th_hist.push_back(theta);

	filterIsInUse = 0; // ERIK put this in.  Not sure if it should be here, but its the semaphore, and it seems like it should start unlocked.

	totalParticles = 50;

	// initialize our random number generator.
	srand ( time(NULL) );

	//calls upon set particles
	this->setupParticles(totalParticles,Robot::AT_POSE);
	
}

Robot::~Robot(void)
{
}

void Robot::clearParticleFilter()
{
	// need to get each item out and delete it!
	while (!Filter.empty())
	{
		Particle* d = Filter.front();   // get a particle
		Filter.pop_front();             // remove it from the list
        delete d;                       // reclaim the memory
	}
}

// sets up particles in a variety of ways... also clears the
// particles beforehand...
void Robot::setupParticles(int N, int particleSetupType)
{
	while (filterIsInUse != 0) ; // busy waiting is probably not so good...

	filterIsInUse = 1; // lock the particle filter
	// clear the filter
	clearParticleFilter();

	// which kind of particle setup do we want?
	switch (particleSetupType)
	{
	// at the Robot's position (not including orientation)
	case Robot::AT_POSITION:
		for (int i=0; i<N ; ++i) {
			int theta = rand() % 360;
			Particle* par = new Particle(x_orig, y_orig, theta, 1.0);
			Filter.push_back(par);	
		}
		break;

	// anywhere within the map
	case Robot::ANYWHERE:
		{
		// Retrieve the current map
		Map* map = Program::Get().map;
		int i=0;
		while (i<N) {
			// not very efficient, but it'll do for now
			int negative1 = (rand() % 2)*2-1;  // -1 or 1
			int negative2 = (rand() % 2)*2-1;  // -1 or 1
			int x = negative1 * (rand()%mapMaxX); // rand()/13;
			int y = negative2 * (rand()%mapMaxY); // rand()/7;

			int numberOfIntersections = 0; 
			map->getIntersectionNumber(x, y, &numberOfIntersections);
			if (numberOfIntersections % 2 == 0)
				continue;

			int theta = rand() % 360;
			Particle* par = new Particle(x, y, theta, 1.0);
			Filter.push_back(par);	
			++i;
			if (i%100 == 0) cout << "Placed particle " << i << endl;
		}
		break;
		}
		
	// back some distance from the map...
	case Robot::WALL_RELATIVE:
		{
		// Retrieve the current map
		Map* map = Program::Get().map;
		int i=0;
		while (i<N) {
			// not very efficient, but it'll do for now
			int negative1 = (rand() % 2)*2-1;  // -1 or 1
			int negative2 = (rand() % 2)*2-1;  // -1 or 1
			int x = negative1 * (rand()%mapMaxX); // rand()/13;
			int y = negative2 * (rand()%mapMaxY); // rand()/7;
			int theta = rand() % 360;

			double gx,gy,gth;
			double dist; // distance to the wall
			int onseg;
			int numberOfIntersections = 0; 
			double ix,iy; // not used, but required for the next function

			// get the global coordinates of the particle's sonar device
			this->getGlobalCoord(x,y,theta,son_x,son_y,son_th,&gx,&gy,&gth);
	
			// calculate its distance
			map->obstacleDist(gx,gy,gth, &ix, &iy, &dist, &onseg);

			// check if its inside of a wall
			map->getIntersectionNumber(x, y, &numberOfIntersections);

			// if its inside of a wall, try again
			if (numberOfIntersections % 2 == 0)
				continue;

			// if its distance to a wall is too different
			if (abs(dist - son_dist) > sonarParticlePlaceUncertainty)
				continue;
			
			Particle* par = new Particle(x, y, theta, 1.0);
			Filter.push_back(par);	
			++i;
			if (i%100 == 0) cout << "Placed particle " << i << endl;
		}
		break;
		}
	// at the Robot's pose (including orientation)
	case Robot::AT_POSE:
	default:
		for (int i=0; i<N ; ++i) {
			Particle* par = new Particle(x_orig, y_orig, theta, 1.0);
			Filter.push_back(par);	
		}
		break;
	}
	filterIsInUse = 0;
}

//This updates the position and probabilities of the list of
// particles
void Robot::UpdateParticles() // Monte Carlo Localization
{
	bool wasDrawing = draw_particles;
	while (filterIsInUse != 0) ;  // semaphore
	filterIsInUse = 1;

	if(draw_particles == true)
		draw_particles = false;

	cout << "Start moving particles" << endl;
	MoveParticles();
	cout << "End moving particles" << endl;
	cout << "Start updating probabilities" << endl;
	UpdateParticleProbabilities();
	cout << "End updating probabilites" << endl;
	cout << "Start resampling particles" << endl;
	ResampleParticles();
	cout << "End resampling particles" << endl;

	if(wasDrawing)
		draw_particles = true;

	filterIsInUse = 0;
}

//Moves the particles based on original position and the new position
// of the robot. Noise is added to the particles movement based on 
// where the robot is moved from the last particle movement
// If the particles move outside of the map, set their probabilities to 0
void Robot::MoveParticles()
{
	// Calculate the differences between our previous position and where we are right now
	double deltaX;
	double deltaY;
	double deltaTheta;
	double distanceAlongTheta;
	double distancePerpendicularToTheta;

	// calculate the deltas: = current - previous
	deltaX = x_orig - x_orig_old;
	deltaY = y_orig - y_orig_old;
	deltaTheta = theta - theta_old;

	// calclate the distance that the robot moved along its old theta
	distanceAlongTheta = DotWithAngle(deltaX, deltaY, theta_old);

	// and the distance it moved perpendicular to its old theta
	distancePerpendicularToTheta = DotWithAngle(deltaX, deltaY, theta_old+90);


	list<Particle*>::iterator particleIterator;
	for(particleIterator = Filter.begin(); particleIterator != Filter.end(); particleIterator++)
	{
		// now move the particles the distances we just calculated, but now with respect to their own thetas
		MoveParticleStraight(*particleIterator, distanceAlongTheta);
		MoveParticlePerpendicular(*particleIterator, distancePerpendicularToTheta);

		// and then turn them
		TurnParticle(*particleIterator, deltaTheta);
	}

	// update the old values
	x_orig_old = x_orig;
	y_orig_old = y_orig;
	theta_old = theta;
}


// adds uncertainty
void Robot::MoveParticleStraight(Particle* particle, double distance)
{
	double deltaX = distance * cos(particle->th * PI / 180);
	double deltaY = distance * sin(particle->th * PI / 180);

	deltaX *= linearCalibrationFactor
			+ Particle::getUniformRandom(-linearUncertaintyFraction, linearUncertaintyFraction);
	deltaY *= linearCalibrationFactor
			+ Particle::getUniformRandom(-linearUncertaintyFraction, linearUncertaintyFraction);

	particle->x = particle->x + deltaX;
	particle->y = particle->y + deltaY;
}

// adds uncertainty
void Robot::MoveParticlePerpendicular(Particle* particle, double distance)
{
	double deltaX = distance * cos((PI/ 2.0) + particle->th * PI / 180);
	double deltaY = distance * sin((PI/ 2.0) + particle->th * PI / 180);

	deltaX *= linearCalibrationFactor
			+ Particle::getUniformRandom(-linearUncertaintyFraction, linearUncertaintyFraction);
	deltaY *= linearCalibrationFactor
			+ Particle::getUniformRandom(-linearUncertaintyFraction, linearUncertaintyFraction);

	particle->x = particle->x + deltaX;
	particle->y = particle->y + deltaY;
}

// adds uncertainty
void Robot::TurnParticle(Particle* particle, double angle)
{
	double deltaTheta = angle * (angularCalibrationFactor
								+ Particle::getUniformRandom(angularUncertaintyFraction, angularUncertaintyFraction));

	particle->th = particle->th + deltaTheta;

	if(particle->th >= 360)
		particle->th -= 360;
	else if(particle->th < 0)
		particle->th += 360;

}

double Robot::DotWithAngle(double x, double y, double angle)
{
	double angleXComponent = cos(angle * PI / 180);
	double angleYComponent = sin(angle * PI / 180);

	return x * angleXComponent + y * angleYComponent;
}

/*
 * these functions are examples of how to get 
 * the robot's simulated sensor readings
 */
void Robot::computeSonarDistance()
{
	// sets son_dist

	// first, get global coordinates
	double gx,gy,gth;
	this->getGlobalCoord(son_x,son_y,son_th,&gx,&gy,&gth);

//	cout << "Coords of the robot " << x_orig << "," << y_orig << "," << theta << endl;
//	cout << "Global coords of the sonar: " << gx << ","  << gy << "," << gth << endl;

	// Retrieve the current map
	Map* map = Program::Get().map;
	// next, find the distance
	double ix,iy,dist;
	int onSegment;
	map->obstacleDist(gx,gy,gth, &ix, &iy, &dist, &onSegment);

	//cout << "int is " << ix << "," << iy << "  and onSeg is " << onSegment << endl;
	// set the sonar distance...
	this->son_dist = dist;
}

/*
 * here is one to get the particles' simulated readings
 * similar things are possible with the camera...
 * the *onseg value will be 2 for red and 1 for white (I think)
 */
void Robot::computeSonarDistance(double x, double y, double th, double* dist, int* onseg)
{
	// sets son_dist

	// first, get global coordinates
	double gx,gy,gth;
	this->getGlobalCoord(x,y,th,son_x,son_y,son_th,&gx,&gy,&gth);

	//cout << "Coords of the robot " << x_orig << "," << y_orig << "," << theta << endl;
	//cout << "Global coords of the particle: " << x << ","  << y << "," << th << endl;
	//cout << "Global coords of the sonar: " << gx << ","  << gy << "," << gth << endl;

	// Retrieve the current map
	Map* map = Program::Get().map;
	// next, find the distance
	double ix,iy;
	map->obstacleDist(gx,gy,gth, &ix, &iy, dist, onseg);

	// is it valid?
	if (*onseg < 1)
		*dist = 0;
}

/* 
 * some code for testing if things are inbounds and for handling
 * sonar values has been left here, in case you'd like to use it...
 */
void Robot::UpdateParticleProbabilities()
{
	Map* map = Program::Get().map;

	if(!realRobot) // if we are using the simulated robot, then calculate its sensor readings
		findSensorReadings();

	// go through each particle
	list<Particle*>::iterator startPar = Filter.begin();
	list<Particle*>::iterator endPar = Filter.end();	

	totalProbability = 0; // reset this so we can sum up the new particles

	Particle* par;
	int pcount = 0;
	for ( ; startPar != endPar; ++startPar)
	{
		// grab a particle
		par = *startPar;
		
		// update the particle's sensors
		findParticleSensorReadings(par);

		// calculate the probability that this particle would have the same sensor readings as the ones the robot
		// reported
		// ie p(sensorReadings | particlePosition, map)
		par->p = calculateParticleProbability(par);

		totalProbability += par->p;
	}
}

double Robot::calculateParticleProbability(Particle* par)
{
	double particleFactor = 1.0;
	double effectiveSensorReading; // chops ideal infinite length sensor readings down to the range actual sensors give
									// ie real world IR only gives a max of 1024

	// Retrieve the current map
	Map* map = Program::Get().map;


	// Sonar
	if(usingSonar)
	{	
		effectiveSensorReading = par->son_dist;
		if(realRobot && par->son_dist > sonarMax) // correct for ideal distance into sonar sensor range
			effectiveSensorReading = sonarMax;

		particleFactor *= gaussianDistribution(effectiveSensorReading, son_dist, sonarUncertaintyFraction*son_dist);
			// THIS WAS WHAT DODDS HAD, SHOULD WE KEEP IT?
			//particleFactor *= 1/(1 + 0.1*abs(this->son_dist - par->son_dist));	
	}

	// IR
	if(usingIR)
	{
		for(int i = 0; i < 2; i++)
		{
			if(i!= 1 || !ignoreCenterIR)
			{
			if(par->ir[i] < IRBumpThreshold && ir[i] < IRBumpThreshold) // both particle and real IR agree theres a bump
					particleFactor *= 1.0;
				else if(par->ir[i] > IRMaxThreshold && ir[i] > IRMaxThreshold) // both particle and real IR agree theres not a bump
					particleFactor *= 1.0;
				else // the particle and real IR disagree
					particleFactor *= .2;
			}
		}
	}		

	// Camera
	if(usingCamera)
	{
		if(par->is_red == 1 && is_red == 1) // both particle and real camera think the red stripe is in front
			particleFactor *= 1.0;
		else if(par->is_red == 0 && is_red == 0) // both particle and real camera think there's no red stripe in front
			particleFactor *= 1.0;
		else if(par->is_red == 1 && is_red == 0) // particle thinks red, and real camera thinks not
			particleFactor *= .8;
		else if(par->is_red == 0 && is_red == 1) // particle thinks not red, camera thinks red stripe is there
			particleFactor *= .3;
	}

	// Inside of a wall
	int numberOfIntersections;
	// if the particle is now inside of a wall, give it really low probability
	map->getIntersectionNumber(par->x, par->y, &numberOfIntersections);
	if (numberOfIntersections % 2 == 0)
		particleFactor *= 0.0001;


	// if any of the IR sensor locations are in a wall, give it a really low probability
	if(usingIR)
	{
		double gx, gy, gth; // global coordinates
		int numberOfIntersections;

		for(int i = 0; i < 3; i++)
		{
			Robot::getGlobalCoord(par->x, par->y, par->th, ir_x[i],ir_y[i],ir_th[i],&gx,&gy,&gth);
			map->getIntersectionNumber(gx, gy, &numberOfIntersections);
			
			if(numberOfIntersections % 2 == 0)
				particleFactor *= 0.0001;
		}
	}

	return particleFactor;
}

// calculate the probability for getting value x,
// using a normal distribution with avg = mean and standard deviation = stdDev
double Robot::gaussianDistribution(double x, double mean, double stdDev)
{
//	double coefficient = 1.0/(stdDev*sqrt(2*PI));
	double exponent = -1.0*(x - mean)*(x-mean)/(2.0*stdDev*stdDev);
//	return coefficient * exp(exponent);
	return exp(exponent);
}

// Function that goes through each particle and makes copies of it and pushes
// it into the Filter list.  If the probability is too low, it will not get copied.
// After copying the needed number of particles, the old particles (the first elements
// in the list) will be deleted.  
void Robot::ResampleParticles()
{		
	list<Particle*> newFilter;

	// this isn't a good enough test for lostitude
/*	if(totalProbability / totalParticles < 0.01)// we're lost, just start over with new random points
	{
		// unlock the semaphore which should be locked right now
		Robot::filterIsInUse = 0;
		setupParticles(totalParticles, Robot::ANYWHERE);
		Robot::filterIsInUse = 1;
		return;
	}*/

	Particle* newPar;
	Particle* oldPar;


/*	double maxProb = .2;
	bool maxValid = false;
	double maxProbX, maxProbY, maxProbTheta;
*/
	double probabilityWidth = totalProbability / totalParticles;
	double probabilityStart = Particle::getUniformRandom(0.0, probabilityWidth);
	
	Map* map = Program::Get().map;
	int numberOfIntersections;
	double probSeenSoFar = 0.0;
	list<Particle*>::iterator oldParIter = Filter.begin();

	for(int newParticleCount = 0; newParticleCount < totalParticles; newParticleCount++)
	{
		while(probSeenSoFar <= newParticleCount * probabilityWidth + probabilityStart)
		{	
			oldPar = (*oldParIter);
			probSeenSoFar += oldPar->p;
/*
			if(oldPar->p > maxProb)
			{
				maxProb = oldPar->p;
				maxProbX = oldPar->x;
				maxProbY = oldPar->y;
				maxProbTheta = oldPar->th;
				maxValid = true;
			}*/

			++oldParIter;
		}

		// 1 in 50 particles will just be randomly spawned, or if we somehow ran out of oldParticles
		if(/*rand() % 25 == 1 ||*/ oldParIter == Filter.end())
		{
			int negative1;
			int negative2;
			int x = 400;
			int y = 400;
/*
			numberOfIntersections = 0;

			while(numberOfIntersections % 2 == 0)// make sure we get a point which is in the map
			{
				negative1 = (rand() % 2)*2-1;  // -1 or 1
				negative2 = (rand() % 2)*2-1;  // -1 or 1
				x = negative1 * (rand()%mapMaxX); 
				y = negative2 * (rand()%mapMaxY); 
	
				map->getIntersectionNumber(x, y, &numberOfIntersections);
			}
*/
			int theta = rand() % 360;
			newPar = new Particle(x, y, theta, 1.0);
		}
		else
		{
			oldPar = (*oldParIter);
/*			map->getIntersectionNumber(	oldPar->x, oldPar->y, &numberOfIntersections);
			// if we copied a point which is out of bounds, instead put it where the most recent maxProb was
			if(maxValid && numberOfIntersections % 2 == 0)
			{
				newPar = new Particle(maxProbX, maxProbY, maxProbTheta, 1.0);
			}
			else
			{ */
				newPar = new Particle(oldPar->x, oldPar->y, oldPar->th, 1.0);
/*			}*/
				
			// give the new particle a bit of noise
			newPar->x += Particle::getUniformRandom(-linearSpawnSpread, linearSpawnSpread);
			newPar->y += Particle::getUniformRandom(-linearSpawnSpread, linearSpawnSpread);
			newPar->th += Particle::getUniformRandom(-angularSpawnSpread, angularSpawnSpread);

		}

		newFilter.push_front(newPar);
		
	}

	clearParticleFilter();

	Filter = newFilter;
}


//set the coordinates based on the values that were passed through
void Robot::deltaCoordinates(double dx, double dy, double dth)
{
	this->x_orig += dx;
	this->y_orig += dy;
	this->theta += dth;

	if(theta >= 360)
		theta -= 360;
	else if(theta < 0)
			theta += 360;
	
	// stores the values where the origin moves to
	x_hist.push_back(x_orig);
	y_hist.push_back(y_orig);
	th_hist.push_back(theta);
}

void Robot::setCoordinates(double x, double y, double th)
{
	this->x_orig = x;
	this->y_orig = y;
	this->theta = th;
	
	// stores the values where the origin moves to
	x_hist.push_back(x_orig);
	y_hist.push_back(y_orig);
	th_hist.push_back(theta);
}

//sets the values of the IR sensors
void Robot::setIR(double L, double C, double R)
{
	this->ir[0] = L;
	this->ir[1] = C;
	this->ir[2] = R;
}

void Robot::Draw()
{
	glPushMatrix();
	glTranslatef(x_orig,y_orig,0);
	glRotatef(theta,0.0,0.0,1.0);
	
	//draws the camera
	glBegin(GL_LINES);
	 glColor3f(1.0,0.0,0.0);
	 glVertex2f(cam_x,cam_y);
	 glVertex2f(cam_x + cam_red*cos((cam_th)*(PI/180)),
				cam_red*sin((cam_th)*(PI/180)));
	glEnd();

	//draws the robot
	glBegin(GL_LINE_LOOP);
	  glColor3f(1.0,1.0,1.0);
	  /* old
	  glVertex2f(-35.0,-20.0);
	  glVertex2f(0.0,-20.0);
	  glVertex2f(15.0,0.0);
	  glVertex2f(0.0,20.0);
	  glVertex2f(-35.0,20.0);
	  */
	  // convert to cm?
	  double cnv = 2.54;
	  glVertex2f(-3*cnv, -7.5*cnv);
	  glVertex2f(8*cnv, -7.5*cnv);
	  glVertex2f(15*cnv, 0.0*cnv);
	  glVertex2f(8*cnv, 7.5*cnv);
	  glVertex2f(-3*cnv, 7.5*cnv);
	  glVertex2f(-3*cnv, -7.5*cnv);
	glEnd();

	//draws the sonar
	if (show_sonar == true) {
		glBegin(GL_LINES);
		glColor3f(1.0,1.0,0.0);
		glVertex2f(son_x,son_y);
		glVertex2f(son_x + son_dist*cos((son_th)*(PI/180)),
				   son_y + son_dist*sin((son_th)*(PI/180)));
		glEnd();
	}


	//show the IR range sensors
	if (show_IR == true) {
		//get the IR values from the bot (changing y value)
		 glBegin(GL_LINES);
		  glColor3f(0.0,1.0,1.0);
		  
		  glVertex2f(ir_x[0], ir_y[0]);
	      glVertex2f(ir_x[0] + ir[0]*cos((PI/180)*ir_th[0]),
			         ir_y[0] + ir[0]*sin((PI/180)*ir_th[0]));
		  
		  glVertex2f(ir_x[1],ir_y[1]);
		  glVertex2f(ir_x[1] + ir[1]*cos((PI/180)*ir_th[1]),
			         ir_y[1] + ir[1]*sin((PI/180)*ir_th[1]));  
		  
		  glVertex2f(ir_x[2],ir_y[2]);
	      glVertex2f(ir_x[2] + ir[2]*cos((PI/180)*ir_th[2]),
			         ir_y[2] + ir[2]*sin((PI/180)*ir_th[2]));
		
		 glEnd();
	}
	glPopMatrix();
	
	//show the path taken by robot
	glPushMatrix();
	if (show_Hist == true) {
		list<double>::iterator x_end = x_hist.end();
		list<double>::iterator x_iter = x_hist.begin();
		list<double>::iterator y_iter = y_hist.begin();
		
		glBegin(GL_LINE_STRIP);
   		 glColor3f(1.0,1.0,0.5);
		 for( ; x_iter != x_end; ++x_iter, ++y_iter)
		 {
			 glVertex2f(*x_iter,*y_iter);
		 }
		glEnd();
	}
	glPopMatrix();	


	//draws the particles of the robot
	float red=1.0, green=0, blue=0;
	glPushMatrix();
	int count = 0;

	if (draw_particles == true && filterIsInUse == 0) // is semaphore unlocked?
	{
		filterIsInUse = 1; // semaphore lock
		Particle* par;
		list<Particle*>::iterator par_iter = Filter.begin();
		list<Particle*>::iterator par_end = Filter.end();

		list< pair<double,double> >::iterator land_iter = landmarks.begin();
		list< pair<double,double> >::iterator land_end = landmarks.end();

		glColor3f(0,0,1.0);
		for( ; land_iter != land_end; ++land_iter)
		{
			int x1 = (*land_iter).first - 5;
			int y1 = (*land_iter).second - 5;
			int x2 = (*land_iter).first + 5;
			int y2 = (*land_iter).second + 5;

			glRecti(x1, y1, x2, y2);
		}
			  		
		glPointSize(4);					//makes the points the size of n pixels 
		glEnable(GL_POINT_SMOOTH);		//makes the points rounded

		glColor3f(0.0,1.0,0.0);
				
		for( ; par_iter != par_end; ++par_iter)
		{
			par = *par_iter;
			count++;
			//obtains the color of the particle based on probability
			probToColor(par->p,&red,&green,&blue);
				
			glBegin(GL_POINTS);
				glColor3f(red,green,blue);
				glVertex2f(par->x,par->y);
			glEnd();

			double gx,gy,gth;
			if (show_particle_sonar) {  // draw the sonar reading
				glBegin(GL_LINES);
					glColor3f(0.8,0.8,0.0);
					// get global coords!
					getGlobalCoord(par->x,par->y,par->th,son_x,son_y,son_th,&gx,&gy,&gth);
					//cout << "Particle #" << pcount++ << endl;
					//cout << "particle pose: " << par->x << "," << par->y << ","<< par->th << endl;
					//cout << "sonar pose: " << gx << "," << gy << ","<< gth << endl;
					//cout << "particle pose: " << par->x << "," << par->y << ","<< par->th << endl;
					glVertex2f(gx,gy);
					double S = par->son_dist;
					glVertex2f(gx + S*cos((PI/180)*(gth)), gy + S*sin((PI/180)*(gth)));
				glEnd();
			}
			// in either case draw the direction it's facing
			glBegin(GL_LINES);
				glColor3f(red,green,blue);
				glVertex2f(par->x,par->y);
				double S = 10.0;
				glVertex2f(par->x + S*cos((PI/180)*(par->th)), par->y + S*sin((PI/180)*(par->th)));
			glEnd();
			
		}

		filterIsInUse = 0;  // semaphore unlock
		//cout << "Number of particles drawn is " << count << endl;
	 }

	glPopMatrix();
	
}



//Changes the color of the particles based on the probability
void Robot::probToColor(double p, float *r, float *g, float *b)
{
/*	if(p <= 0.0)
	{
		*r = 1.0f;	*g = 0.0f;	*b = 0.0f;
	}
	else if( p >= 1.0)
	{
		*r = 0.0f;	*g = 1.0f;	*b = 0.0f;
	}
	else if(p > .5)
	{
		*r = 0.0f;	*g = float(p);	*b = 0.0f;	
	}
	else // p <= .5
	{
		*r = float(1.0 - p);	*g = 0.0f;	*b = 0.0f;
	}*/
	
	if (p > 0.5)
	{
		*r = 0.0f;	*g = 1.0f;	*b = 0.0f;
	}
	else if(p > 0.2)
	{
		*r = 0.1f;	*g = 0.4f;	*b = 0.1f;
	}
	else
	{
		*r = 1.0f;	*g = 0.0f;	*b = 0.0f;
	}
	
}

//Function used to convert the sensor's coordinates to global coordinates for the ROBOT
void Robot::getGlobalCoord(double x, double y, double th, double *gx, double *gy, double *gth)
{
	double r = sqrt((x*x)+(y*y));
	double sensor_th = 180*atan2(y,x)/PI;
	
	*gx = x_orig + r*cos(PI*(theta+sensor_th)/180);
	*gy = y_orig + r*sin(PI*(theta+sensor_th)/180);
	*gth = theta + th;
}

//Function used to convert the sensor's coordinates to global coordinates for a given position (ie of a particle)
void Robot::getGlobalCoord(double x_orig, double y_orig, double theta, 
						   double x, double y, double th, 
						   double *gx, double *gy, double *gth)
{
	double r = sqrt((x*x)+(y*y));
	double sensor_th = 180*atan2(y,x)/PI;
	
	*gx = x_orig + r*cos(PI*(theta+sensor_th)/180);
	*gy = y_orig + r*sin(PI*(theta+sensor_th)/180);
	*gth = theta + th;
}

//Function that finds the sensor readings for each sensor on the robot
// as well as the camera which senses only the red walls
void Robot::findSensorReadings()
{
	// THIS FUNCTION SHOULD NOT BE CALLED IF USING THE REAL ROBOT

	double gx;		//variable for the global x coordinate
	double gy;		//variable for the global y coordinate
	double gth;		//variable for the global theta coordinate
	
	double ix;		//variable for the x coordinate of intersection
	double iy;		//variable for the y coordinate of intersection
	double dist;	//variable for the distance from sensor to intersetion
	int onSegment;	//variable to test if intersection is on the wall 


	Map* map = Program::Get().map;

	// do the robot's sonar
	computeSonarDistance();

	for (int i = 0; i < 3; i++)
	{
		dist = 10000000000000;		//setting and resetting distance

		Robot::getGlobalCoord(ir_x[i],ir_y[i],ir_th[i],&gx,&gy,&gth);
		map->obstacleDist(gx, gy, gth, &ix, &iy, &dist, &onSegment);

		// now model the funky behavior of IR
		if(dist > IRBumpRange)
            ir[i] = IRMax;	// 1024
		else
			ir[i] = Particle::getUniformRandom(0.0, IRBumpThreshold); // doesn't really need to be random,
																	// just needs to be less than IRBumpThreshold
	}	

	//finding the distance value for the camera
	dist = 10000000000000;

	Robot::getGlobalCoord(cam_x, cam_y, cam_th, &gx, &gy, &gth);
	map->obstacleDist(gx,gy,gth,&ix,&iy,&dist, &onSegment);
	
	// setting whether the 
	if(onSegment == 2 && dist > 60 && dist < 250 ) // red wall (distance check is cuz camera can't see it too close or far)
	{
		is_red = 1; // sees a red wall
		cam_red = dist;
	}
	else
	{
		is_red = 0; // doesn't see a red wall
		cam_red = dist;
	}

}

void Robot::addLandmark(double distance, double angle)
{
	cout << "Trying to add landmark at\n";
	cout << "distance: " << distance << endl;
	cout << "angle: " << angle << endl;

	UpdateParticleProbabilities();

	double x,y,theta;
	findBestParticle(&x,&y,&theta);

	ResetParticleProbabilities();

	int landX = x + (distance*cos((theta-angle)*(PI/180)));
	int landY = y + (distance*sin((theta-angle)*(PI/180)));
	landmarks.push_back(pair<double, double>(landX,landY));
}

void Robot::findParticleSensorReadings(Particle* par)
{
	double gx;		//variable for the global x coordinate
	double gy;		//variable for the global y coordinate
	double gth;		//variable for the global theta coordinate
	
	double ix;		//variable for the x coordinate of intersection
	double iy;		//variable for the y coordinate of intersection
	double dist;	//variable for the distance from sensor to intersetion
	int onSegment;	//variable to test if intersection is on the wall 


	Map* map = Program::Get().map;
	// do the sonar
	if(usingSonar)
	{
	dist = 10000000000000;		//setting and resetting distance

	Robot::getGlobalCoord(par->x, par->y, par->th, son_x,son_y,son_th,&gx,&gy,&gth);
	map->obstacleDist(gx, gy, gth, &ix, &iy, &dist, &onSegment);

	par->son_dist = dist;
	}

	// do the IR
	if(usingIR)
	{
		for (int i = 0; i < 3; i++)
		{
			dist = 10000000000000;		//setting and resetting distance

			Robot::getGlobalCoord(par->x, par->y, par->th, ir_x[i],ir_y[i],ir_th[i],&gx,&gy,&gth);
			map->obstacleDist(gx, gy, gth, &ix, &iy, &dist, &onSegment);

			// now model the funky behavior of IR
			if(dist > IRBumpRange)
				par->ir[i] = IRMax;	// 1024
			else
				par->ir[i] = IRBumpThreshold/2.0; // just needs to be less than IRBumpThreshold
		}	
	}

	//finding the distance value for the camera
	if(usingCamera)
	{
		dist = 10000000000000;

		Robot::getGlobalCoord(par->x, par->y, par->th, cam_x, cam_y, cam_th, &gx, &gy, &gth);
		map->obstacleDist(gx,gy,gth,&ix,&iy,&dist, &onSegment);
		
		// setting whether the camera sees a red wall or not
		if(onSegment == 2 && dist > 60 && dist < 250 ) // red wall (distance check is cuz camera can't see it too close or far)
		{
			par->is_red = 1; // sees a red wall
		}
		else
		{
			par->is_red = 0; // doesn't see a red wall		
		}
	}

}

void Robot::findBestParticle(double* x, double* y, double* theta)
{
	list<Particle*>::iterator par_iter = Filter.begin();
	list<Particle*>::iterator par_end = Filter.end();

	Particle* best = *par_iter;

	for(; par_iter != par_end; ++par_iter)
	{
		if((*par_iter)->p > best->p)
			best = *par_iter;
	}

	*x = best->x;
	*y = best->y;
	*theta = best->th;
}

void Robot::ResetParticleProbabilities()
{
	list<Particle*>::iterator par_iter = Filter.begin();
	list<Particle*>::iterator par_end = Filter.end();

	for(; par_iter != par_end; ++par_iter)
	{
		(*par_iter)->p = 1.0;
	}
}