#pragma once


#include <windows.h>

#include <stdlib.h>

#include <string>

#include <GL/glut.h>

#include <fstream>

#include <iostream>

#include <list>

#include <GL/glut.h>

#include "drawable.h"

#include "program.h"

#include "map.h"

#include <cmath>


using namespace std;

class Robot : public Drawable
{
public:

	// constants for where to put particles
	static const int AT_POSE = 0;
	static const int AT_POSITION = 1;
	static const int ANYWHERE = 2;
	static const int WALL_RELATIVE = 3;

	double PI;

	// robot's origin (and old origin for creating the path it takes)
	double x_orig, y_orig, theta;
	double x_orig_old, y_orig_old, theta_old;
	
	list<double> x_hist;
	list<double> y_hist;
	list<double> th_hist;

	list<double> mcl_x_hist;
	list<double> mcl_y_hist;
	list<double> mcl_th_hist;
	
	bool show_IR;
	bool show_Hist;
	bool show_sonar;
	bool show_particle_sonar;
	bool draw_particles;

	double ir_x[3];		// the x coordinate of the irs 
	double ir_y[3];		// the y coordinate of the irs
	double ir_th[3];	// the angle of the irs in degrees 
	double ir[3];		// dist to the nearest wall (red or white)

	double cam_x;		// the x coordinate of the camera
	double cam_y;		// the y coordinate of the camera
	double cam_th;		// the angle of the camera in degrees
	double cam_red;		// dist to nearest red wall

	double son_x;		// the x coordinate of the camera
	double son_y;		// the y coordinate of the camera
	double son_th;		// the angle of the camera in degrees
	double son_dist;    // the distance from sonar to the nearest wall

	double trans_adj;   // adjustment of reported distance
	double trans_noise; // noise on reported distance
	double angle_adj;   // adjustment of reported angle
	double angle_noise; // noise on reported angle

	int is_red;         // I forget what this is for...

	int totalParticles; // number of MCL particles
	double percentNewParticles; // percentage of new random particles to generate
	                            // (on average) per iteration

	Robot(void);
	virtual ~Robot(void);
	
	void Draw();
	void setCoordinates(double x, double y, double th);
	void deltaCoordinates(double dx, double dy, double dth);
	void setIR(double L, double C, double R);
	
	void setupParticles(int N, int particleSetupType);
	void UpdateParticles();
	void MoveParticles();
	void UpdateParticleProbabilities();
	void ResampleParticles();
	void clearParticleFilter();
	
	void probToColor(double p, float *r, float *g, float *b);
	void getGlobalCoord(double x, double y, double th, double *gx, double *gy, double *gth);
	void getGlobalCoord(double x_orig, double y_orig, double theta,
		                double x, double y, double th, 
					    double *gx, double *gy, double *gth);
	void findSensorReadings();

	void computeSonarDistance();
	void computeSonarDistance(double x, double y, double th, double* dist, int* onsegment);

        void setSonarStatus(double _dist, double _theta);      // This is the function to set the sonar stuff

	// Sorry. This should be moved, but I'm getting pissed off at this thing.
		double noise(double amt, int flag)
		{
			amt = fabs(amt); // absolute value
			if (flag == Particle::ANGLE_NOISE)
				return Particle::getUniformRandom(angle_noise*amt*-1,
				                                  angle_noise*amt);
			if (flag == Particle::TRANS_NOISE)
				return Particle::getUniformRandom(trans_noise*amt*-1,
				                                  trans_noise*amt);
			return 0.0;
		};

		double applyMotionModel(double amt, int flag)
		{
			if (flag == Particle::ANGLE_NOISE)
				return amt + amt * angle_adj + noise(amt, flag);
			if (flag == Particle::TRANS_NOISE)
				return amt + amt * trans_adj + noise(amt, flag);
			return amt;
		};

	/*
	 * a Particle class with somesimple noise models...
	 * 
	 * Well, not anymore... moved them out. -- Paul
	 */
	class Particle
	{
	public: 
		static const int ANGLE_NOISE=0;
		static const int TRANS_NOISE=1;

		double x,y,th,p;  // theta is in degrees
		double son_dist;  // the sonar distance for this particle

		Particle(double x2, double y2, double th2, double p2)
		{
			x = x2; y = y2; th = th2 ; p = p2;
			son_dist = -1;
		}

		static double getUniformRandom(double L, double H)
		{
			if (H <= L) return 0.0;
			double diff = H-L;
			return diff*(((double)rand())/RAND_MAX)+L;
		}



	};

	int filterIsInUse;          // our not-so-effective semaphore...
	list<Particle*> Filter;		// list of particles

	Particle* generateRandomParticle();
		
};