#include ".\robot.h"
#include <windows.h>

Robot::Robot(void)
{
	//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;
	this->realRobot = false;
	this->filterIsInUse = 0;
	this->drawRobot = true;
	this->sonarIsSafe = true;
	this->solvingKidnappingProblem = false;

	this->is_red = 3;

	//setting the IR lengths
	this->ir[0] = 25.0;
	this->ir[1] = 25.0;
	this->ir[2] = 25.0;
	
	//setting the left sensor origin
	this->ir_x[0] = 7.5*2.54 + 7.5;
	this->ir_y[0] = 11*2.54 + 2;
	this->ir_th[0] = 45;

	//setting the center sensor origin
	this->ir_x[1] = 8*2.54 + 9.5;
	this->ir_y[1] = 0.0;
	this->ir_th[1] = 0.0;

	//setting the right sensor origin
	this->ir_x[2] = 7.5*2.54 + 7.5;
	this->ir_y[2] = -11*2.54 - 2;
	this->ir_th[2] = -45.0;

	//setting the camera origin
	this->cam_x = -20.0;
	this->cam_y = 0.0;
	this->cam_th = -90.0;
	this->cam_red = 0.0;

	//setting the sonar origin
	this->son_x = 2.54*8.0; // convert from inches to cm
	this->son_y = 0.0;
	this->son_th = 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);

	//calls upon set particles
	this->setupParticles(1000,Robot::AT_POSITION);
//	this->setupParticles(5000,Robot::ANYWHERE);
	
}

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) {
		cout << " waiting for filter in setup " << endl;
		Sleep(500);
	}	
	filterIsInUse = 1;
	if (N <= 1) N = 100;         // one hunderd particles by default
	this->totalParticles = N;

	cout << N << endl;


	// 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:
	{
		drawRobot = false;
		solvingKidnappingProblem = true;
		// 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() % 300 ; // rand()/13;
			int y = negative2 * rand() % 300; // 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:
		//break; not implemented yet!! instead we simply use the robot's pose

	// 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
{
	//Ensure that drawing can't lock us down
	bool temp = draw_particles;
	draw_particles = false;

	while (filterIsInUse != 0) //sleep
	{
		cout << " waiting in UpdateParticles "<< endl;
		Sleep(20);
	}
	filterIsInUse = 1;
	MoveParticles();
	UpdateParticleProbabilities();
	ResampleParticles();
	filterIsInUse = 0;

	//Reset drawing when we're done
	draw_particles = temp;
}

//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
void Robot::MoveParticles()
{
	// go through each particle
	list<Particle*>::iterator startPar = Filter.begin();
	list<Particle*>::iterator endPar = Filter.end();	

	Particle* par;
	//Find the offsets since we last did this update	
	double dx, dy, dth;
	dx = x_orig - x_orig_old;
	dy = y_orig - y_orig_old;
	dth = theta - theta_old;

	double radtheta = theta_old * PI / 180;
	//Convert into displacements straight-forward and to-the-left
	double forward = dot(dx, dy, cos(radtheta), sin(radtheta));
	double left = dot(dx, dy, cos(radtheta + PI / 2), sin(radtheta + PI / 2));

	//Iterate over all particles and move them
	for ( ; startPar != endPar; ++startPar)
	{
		par = *startPar;
		par->noisyMove(forward, left, dth);
	}

	cout << "OldRX: " << x_orig_old << " OldRY: " << y_orig_old << " OldRT: " << theta_old; 
	cout << "NewRX: " << x_orig << " NewRY: " << y_orig_old << " OldRT: " << theta_old; 

	//Update the origin data
	x_orig_old = x_orig;
	y_orig_old = y_orig;
	theta_old = theta;
}

/*
 * 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;
}

/*
 * Compute virtual IR readings for each IR sensor on a particle. Steal code from
 * computeSonarDistance. Slightly modified to use the IR offsets instead of the 
 * sonar offsets (for location of the physical sensor with respect to the center 
 * of the bot).
 */
void Robot::computeIRDistance(double x, double y, double theta, double* ir)
{
	for (int i = 0; i < 3; ++i)
	{
		// first, get global coordinates
		double gx,gy,gth;
		this->getGlobalCoord(x, y, theta, ir_x[i] ,ir_y[i] ,ir_th[i], &gx, &gy, &gth);

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

		// is it valid?
		if (onseg < 1)
			ir[i] = 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;

	// SONAR: get "real" sonar value
	//Accomplished for us already. See main.cpp's "y" input.
	cerr << "Entering update; sonar dist is: " << son_dist << endl;
	if (!realRobot)
		computeSonarDistance(); // now this->son_dist is the robot's sonar value

	cerr << "sonar distance is now: " << son_dist << endl;

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

	Particle* par;
	int pcount = 0;
	double avgIRDiff = 0;
	for ( ; startPar != endPar; ++startPar)
	{
		//maximizing to find the minimum
		par = *startPar;
		
		// particles at the moment are hypotheses for the robot
		// pose (including orientation), not the sensor's pose, so we
		// need to adjust both pose and orientation...

		// sonar reading for each particle
		// the particle represents the robot's position, so we need to
		// calculate the estimated sonar reading
		int onsegment;
		double d;
		computeSonarDistance(par->x, par->y, par->th, &(par->son_dist), &onsegment);
//		cout << pcount << ": Got distance " << d << endl;
//		pcount++;
//		par->son_dist = d;
		
		if (onsegment > 0) // if it hit a wall at all
		{
			//Values for a pseudo-Gaussian distribution
			double avg = this->son_dist;
			double stddev = this->son_dist / 10;
			//Adjust particle probability based on sonar reading
			par->p = exp(-pow(par->son_dist - avg, 2) / (2 * pow(stddev, 2)));

			//Old probability updater formula
//			par->p *= 1/(1 + 0.1*fabs(this->son_dist - par->son_dist));
		}
		else // out of bounds...
		{
			par->p *= 0.0001; // out of the map
		}
		
		// Check for wall collisions
		// obtains the number of intersections
		int intersections;
		map->getIntersectionNumber(par->x,par->y,&intersections);
		if (intersections % 2 == 0) // decrease prob if it leaves the map
		{
			par->p*=.001; // why not 
		}
	}
	avgIRDiff /= 3 * Filter.size();
	cout << "Average IR distance disparity is " << avgIRDiff << endl;
}


// 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()
{
	//If we don't have valid sonar data to work with, then bail.
	if (!sonarIsSafe)
		return;

	//Iterate over all particles creating new particles.
	//We know we need to generate N particles and we have a total S = the sum
	//of probabilities of particles. Take samples every S / N units (e.g. for
	//three particles of total probability 1, take samples at 0, .33, .66). If
	//we exceed the current particle's probability, go to the next one.

	//Find the sum of probabilities
	list<Particle*>::iterator startPar = Filter.begin();
	list<Particle*>::iterator endPar = Filter.end();	
	Particle* par;

	double sn = 0;
	for ( ; startPar != endPar; ++startPar)
	{
		par = *startPar;
		sn += par->p;
	}
	sn /= totalParticles;

	//Now iterate over all particles, spawning duplicates and removing old copies
	//newP increments by sn; oldP increments by the particle probabilities
	//newP starts randomly between 0 and sn because we don't necessarily want to grab
	//the first particle in the list
//	double newP = ((double) rand()) / RAND_MAX * sn;
	double newP = 0;
	double oldP = 0;

	//Use a fixed-size for loop to ensure that we don't try to work with particles that
	//we just created.
	size_t oldListSize = Filter.size();
	unsigned int numRobotClones = 0;
	for (size_t i = 0; i < oldListSize; ++i)
	{
		par = Filter.front();			//get a particle
		Filter.pop_front();             //remove it from the list
		oldP += par->p;					//Update our cumulative probability sum
		//Generate new particles until newP gets out of range
		while (newP < oldP)
		{
			Particle* newParticle = par->spawnNoisyCopy();
			//Sometimes, spawn a particle where the robot is. Always spawn MAX_ROBOT_CLONES
			//particles in such a fashion.
				if (!solvingKidnappingProblem && 
					(oldListSize - i <= (MAX_ROBOT_CLONES - numRobotClones) || 
					(((double) rand()) / RAND_MAX > .05 && numRobotClones < MAX_ROBOT_CLONES)))
				{
					newParticle->noisyMoveTo(x_orig, y_orig, theta);
					numRobotClones++;
				}
			Filter.push_back(newParticle);
			newP += sn;
		}
		delete par;
	}

	//Renormalize the particle probabilities so that they sum to 1.
	startPar = Filter.begin();
	endPar = Filter.end();	

	double totalProb = sn * totalParticles;
	for ( ; startPar != endPar; ++startPar)
		(*startPar)->p /= totalProb;
}


//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;
	
	// 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
	if (drawRobot)
	{
		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=1.0, blue=0;
	glPushMatrix();

	if (draw_particles == true && filterIsInUse == 0) 
	{
		filterIsInUse = 1;

		Particle* par;
		list<Particle*>::iterator par_iter = Filter.begin();
		list<Particle*>::iterator par_end = Filter.end();

		
			  		
		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;
			//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); // This line is breaking everything, independent
										   // of par->x and 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;
	 }
	glPopMatrix();
	
}



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

//Function used to convert the sensor's robot coordinates to global coordinates
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
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()
{
	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;

	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);

		ir[i] = dist;	//setting the sensor reading for distance
	}	

	//finding the distance value for the camera
	dist = 1e12;

	Robot::getGlobalCoord(cam_x, cam_y, cam_th, &gx, &gy, &gth);
	map->obstacleDist(gx,gy,gth,&ix,&iy,&dist, &onSegment);
	
	//setting the length of the camera
	if(onSegment == 2)
	{
		cam_red = dist;
	}
	else
	{
		//no red is detected
		cam_red = 0.0;
	}

}

double Robot::dot(double v1, double v2, double y1, double y2)
{
	return v1 * y1 + v2 * y2;
}