text: #include "matplotlibcpp.h" //Graph Library
  #include <iostream>
  #include <string>
  #include <math.h>
  #include <vector>
  #include <stdexcept> // throw errors
  #include <random> //C++ 11 Random Numbers
  
  
  #define PARTICLES 1000
  #define STEPS 50
  #define SENSE_NOISE 3
  #define TURN_NOISE 0.1 //0.1 RAD ~ 5 DEG
  #define FORWARD_NOISE 0.5
  
  namespace plt = matplotlibcpp;
  using namespace std;
  
  // Landmarks
  double landmarks[8][2] = { { 20.0, 20.0 }, { 20.0, 80.0 }, { 20.0, 50.0 },
  	{ 50.0, 20.0 }, { 50.0, 80.0 }, { 80.0, 80.0 },
  	{ 80.0, 20.0 }, { 80.0, 50.0 } };
  
  // Map size in meters
  double world_size = 100.0;
  
  // Random Generators
  random_device rd;
  mt19937 gen(rd());
  
  // Global Functions
  double mod(double first_term, double second_term);
  double gen_real_random();
  
  class Robot {
  	public:
  		Robot()
  		{
  			// Constructor
  			x = gen_real_random() * world_size; // robot's x coordinate
  			y = gen_real_random() * world_size; // robot's y coordinate
  			orient = gen_real_random() * 2.0 * M_PI; // robot's orientation
  
  			forward_noise = 0.0; //noise of the forward movement
  			turn_noise = 0.0; //noise of the turn
  			sense_noise = 0.0; //noise of the sensing
  		}
  
  		void set(double new_x, double new_y, double new_orient)
  		{
  			// Set robot new position and orientation
  			if (new_x < 0 || new_x >= world_size)
  				throw std::invalid_argument("X coordinate out of bound");
  			if (new_y < 0 || new_y >= world_size)
  				throw std::invalid_argument("Y coordinate out of bound");
  			if (new_orient < 0 || new_orient >= 2 * M_PI)
  				throw std::invalid_argument("Orientation must be in [0..2pi]");
  
  			x = new_x;
  			y = new_y;
  			orient = new_orient;
  		}
  
  		void set_noise(double new_forward_noise, double new_turn_noise, double new_sense_noise)
  		{
  			// Simulate noise, often useful in particle filters
  			forward_noise = new_forward_noise;
  			turn_noise = new_turn_noise;
  			sense_noise = new_sense_noise;
  		}
  
  		vector<double> sense()
  		{
  			// Measure the distances from the robot toward the landmarks
  			vector<double> z(sizeof(landmarks) / sizeof(landmarks[0]));
  			double dist;
  
  			for (int i = 0; i < sizeof(landmarks) / sizeof(landmarks[0]); i++) {
  				dist = sqrt(pow((x - landmarks[i][0]), 2) + pow((y - landmarks[i][1]), 2));
  				dist += gen_gauss_random(0.0, sense_noise);
  				z[i] = dist;
  			}
  			return z;
  		}
  
  		Robot move(double turn, double forward)
  		{
  			if (forward < 0)
  				throw std::invalid_argument("Robot cannot move backward");
  
  			// turn, and add randomness to the turning command
  			orient = orient + turn + gen_gauss_random(0.0, turn_noise);
  			orient = mod(orient, 2 * M_PI);
  
  			// move, and add randomness to the motion command
  			double dist = forward + gen_gauss_random(0.0, forward_noise);
  			x = x + (cos(orient) * dist);
  			y = y + (sin(orient) * dist);
  
  			// cyclic truncate
  			x = mod(x, world_size);
  			y = mod(y, world_size);
  
  			// set particle
  			Robot res;
  			res.set(x, y, orient);
  			res.set_noise(forward_noise, turn_noise, sense_noise);
  
  			return res;
  		}
  
  		string show_pose()
  		{
  			// Returns the robot current position and orientation in a string format
  			return "[x=" + to_string(x) + " y=" + to_string(y) + " orient=" + to_string(orient) + "]";
  		}
  
  		string read_sensors()
  		{
  			// Returns all the distances from the robot toward the landmarks
  			vector<double> z = sense();
  			string readings = "[";
  			for (int i = 0; i < z.size(); i++) {
  				readings += to_string(z[i]) + " ";
  			}
  			readings[readings.size() - 1] = ']';
  
  			return readings;
  		}
  
  		double measurement_prob(vector<double> measurement)
  		{
  			// Calculates how likely a measurement should be
  			double prob = 1.0;
  			double dist;
  
  			for (int i = 0; i < sizeof(landmarks) / sizeof(landmarks[0]); i++) {
  				dist = sqrt(pow((x - landmarks[i][0]), 2) + pow((y - landmarks[i][1]), 2));
  				prob *= gaussian(dist, sense_noise, measurement[i]);
  			}
  
  			return prob;
  		}
  
  		double x, y, orient; //robot poses
  		double forward_noise, turn_noise, sense_noise; //robot noises
  
  		double gen_gauss_random(double mean, double variance)
  		{
  			// Gaussian random
  			normal_distribution<double> gauss_dist(mean, variance);
  			return gauss_dist(gen);
  		}
  
  		double gaussian(double mu, double sigma, double x)
  		{
  			// Probability of x for 1-dim Gaussian with mean mu and var. sigma
  			return exp(-(pow((mu - x), 2)) / (pow(sigma, 2)) / 2.0) / sqrt(2.0 * M_PI * (pow(sigma, 2)));
  		}
  };
  
  // Functions
  double gen_real_random()
  {
  	// Generate real random between 0 and 1
  	uniform_real_distribution<double> real_dist(0.0, 1.0); //Real
  	return real_dist(gen);
  }
  
  double mod(double first_term, double second_term)
  {
  	// Compute the modulus
  	return first_term - (second_term)*floor(first_term / (second_term));
  }
  
  double evaluation(Robot r, Robot p[], int n)
  {
  	//Calculate the mean error of the system
  	double sum = 0.0;
  	for (int i = 0; i < n; i++) {
  		//the second part is because of world's cyclicity
  		double dx = mod((p[i].x - r.x + (world_size / 2.0)), world_size) - (world_size / 2.0);
  		double dy = mod((p[i].y - r.y + (world_size / 2.0)), world_size) - (world_size / 2.0);
  		double err = sqrt(pow(dx, 2) + pow(dy, 2));
  		sum += err;
  	}
  	return sum / n;
  }
  double max(double arr[], int n)
  {
  	// Identify the max element in an array
  	double max = 0;
  	for (int i = 0; i < n; i++) {
  		if (arr[i] > max)
  			max = arr[i];
  	}
  	return max;
  }
  
  void visualization(int n, Robot robot, int step, Robot p[], Robot pr[])
  {
  	//Draw the robot, landmarks, particles and resampled particles on a graph
  
  	//Graph Format
  	plt::title("MCL, step " + to_string(step));
  	plt::xlim(0, 100);
  	plt::ylim(0, 100);
  
  	//Draw particles in green
  	for (int i = 0; i < n; i++) {
  		plt::plot({ p[i].x }, { p[i].y }, "go");
  	}
  
  	//Draw resampled particles in yellow
  	for (int i = 0; i < n; i++) {
  		plt::plot({ pr[i].x }, { pr[i].y }, "yo");
  	}
  
  	//Draw landmarks in red
  	for (int i = 0; i < sizeof(landmarks) / sizeof(landmarks[0]); i++) {
  		plt::plot({ landmarks[i][0] }, { landmarks[i][1] }, "ro");
  	}
  
  	//Draw robot position in blue
  	plt::plot({ robot.x }, { robot.y }, "bo");
  
  	//Save the image and close the plot
  	plt::save("./img/step_" + to_string(step) + ".png");
  	plt::clf();
  }
  
  int main()
  {
  	// Instantiating a robot object from the Robot class
  	Robot myrobot;
  
  	// Position is random if not set
  	myrobot.set(50, 50, 0);
  	myrobot.set_noise(FORWARD_NOISE , TURN_NOISE, SENSE_NOISE);
  
  
  	Robot p[PARTICLES];
  	for (int i=0; i<PARTICLES; i++) {
  		p[i].set_noise(FORWARD_NOISE, TURN_NOISE, SENSE_NOISE);
  	}
  	// Used for moving and plotting
  	Robot p2[PARTICLES];
  	// Selected particles
  	Robot p3[PARTICLES];
  
  	for (int t=0; t<STEPS; t++) {
  		double move_dir = 0.2;
  		double move_dist = 3.0;
  		// Generate randomic path
  		//dir += myrobot.gen_gauss_random(0,0.1);
  		//dist = myrobot.gen_gauss_random(2,1);
  
  		// Move robot and sense distance from landmarks
  		myrobot = myrobot.move(move_dir, move_dist);
  		vector<double> z;
  		z = myrobot.sense();
  
  		// Move particles
  		for (int i=0; i<PARTICLES; i++) {
  			p2[i] = p[i].move(move_dir, move_dist);
  			p[i] = p2[i];
  			//cout << p[i].show_pose() << endl;
  		}
  
  		// Measure probability of each particle
  		double w[PARTICLES];
  		for (int i=0; i<PARTICLES; i++) {
  			w[i] = p[i].measurement_prob(z);
  			//cout << w[i] << endl;
  		}
  
  		// Resampling Wheel
  		int index = gen_real_random() *PARTICLES;
  		double beta = 0;
  		double maxw = max(w, PARTICLES);
  
  		for (int i=0; i<PARTICLES; i++) {
  			beta =+ 2.0 * maxw * gen_real_random();
  			while (w[index] < beta) {
  				beta -= w[index];
  				index = mod((index+ 1 ),PARTICLES);
  			}
  			p3[i] = p[index];
  		}
  		for (int i=0; i<PARTICLES; i++) {
  			p[i] = p3[i];
  		}
  		
  		double error;
  		error = evaluation(myrobot, p, PARTICLES);
  		cout << "Step:   " << t << "\terror: " << error << endl;
  		//Generate images
  		visualization(PARTICLES, myrobot, t, p2, p3);
  	}
  
  	return 0;
  }
  
  