#include ".\robot.h"

Robot::Robot(void)
{
	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;

	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] = -12.0;
	this->ir_y[0] = 20.0;
	this->ir_th[0] = 90.0;

	//setting the center sensor origin
	this->ir_x[1] = 15.0;
	this->ir_y[1] = 0.0;
	this->ir_th[1] = 0.0;

	//setting the right sensor origin
	this->ir_x[2] = -12.0;
	this->ir_y[2] = -20.0;
	this->ir_th[2] = -90.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 = 45.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(100,Robot::AT_POSITION);
	
}

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...
	if (N <= 1) N = 100;         // one hunderd particles by default
	this->totalParticles = N;

	filterIsInUse = 1;
	// 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(); // rand()/13;
			int y = negative2 * rand(); // 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
{
	while (filterIsInUse != 0) ;  // semaphore
	filterIsInUse = 1;
	MoveParticles();
	UpdateParticleProbabilities();
	ResampleParticles();
	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
void Robot::MoveParticles()
{
	// Print the values of the old and new positions.
	cout << "x_orig, y_orig, theta: " << x_orig << ", " << y_orig << ", " << theta << endl;
	cout << "x_orig_old, y_orig_old, theta_old: " << x_orig_old << ", " << y_orig_old << ", " << theta_old << endl;

	// Go through all the particles.
	list<Particle*>::iterator startPar = Filter.begin();
	list<Particle*>::iterator endPar = Filter.end();	

	Particle* par;

	// Changes in x, y, theta.
	double x = x_orig - x_orig_old;
	double y = y_orig - y_orig_old;
	double th = theta - theta_old;

	for ( ; startPar != endPar; ++startPar)
	{
		par = *startPar;

		// A bunch of trig for moving the particles in the appropriate direction.
		double al = atan2(y,x);
		double d = sqrt(pow(x,2) + pow(y,2));
		double al2 = par->th*PI/180 - theta_old*PI/180 + al;
		double x_d = d * cos(al2);
		double y_d = d * sin(al2);

		// Move particles with noise.
		par->x += x_d + par->noise(x_d, Particle::TRANS_NOISE);
		par->y += y_d + par->noise(y_d, Particle::TRANS_NOISE);
		par->th += th + par->noise(th, Particle::ANGLE_NOISE);
	}
	// Set old position to current position.
	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;
}

/* 
 * 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
	computeSonarDistance(); // now this->son_dist is the robot's sonar value

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

	Particle* par;
	int pcount = 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;
		computeSonarDistance(par->x, par->y, par->th, &(par->son_dist), &onsegment);

		// adjust for sonar closeness...
		if (onsegment > 0) // if it hit a wall at all
		{
			par->p *= 1/(1 + fabs(0.1*(this->son_dist - par->son_dist)));
		}
		else // out of bounds...
		{
			par->p *= 0.0001; // out of the map
		}

		// adjustments for staying in the map
		// obtains the number of intersections
		int i;
		map->getIntersectionNumber(par->x,par->y,&i);
		if (i%2 == 0) // decrease prob if it leaves the map
		{
			par->p*=.001; // why not 
		}
	}
}


// 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()
{
	// Go through all the particles.
	list<Particle*>::iterator startPar = Filter.begin();
	list<Particle*>::iterator endPar = Filter.end();	
	Particle* par;
	double psum = 0;
	int count = 0;
	for ( ; startPar != endPar; ++startPar)
	{
		par = *startPar;
		psum += par->p;
		count++;
	}

	startPar = Filter.begin();
	endPar = Filter.end();	
	double pinc = (double)psum/count;
	double pcutoff = 0;
	psum = 0;
	int newPars = 0;
	for ( ; startPar != endPar && newPars < count; ++startPar)
	{
		par = *startPar;
		psum += par->p;
		while (psum > pcutoff && newPars < count)
		{
			Particle* newpar = new Particle(par->x, par->y, par->th, 1.0);
			Filter.push_back(newpar);
			newPars++;
			pcutoff += pinc;
		}
	}
	
	// need to get each item out and delete it!
	for (int i = 0; i < count; i++)
	{
		Particle* d = Filter.front();   // get a particle
		Filter.pop_front();             // remove it from the list
        delete d;                       // reclaim the memory
	}

	cout << "done" << endl;
}


//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
	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) 
	{
		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;
			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;
		//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.5)
	{
		*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 = 10000000000000;

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

}