/*
 * particle_filter.cpp
 *
 *  Created on: Dec 12, 2016
 *      Author: Tiffany Huang
 */

#include <random>
#include <algorithm>
#include <iostream>
#include <numeric>
#include <math.h>
#include <iostream>
#include <sstream>
#include <string>
#include <iterator>
#include "particle_filter.h"

using namespace std;

default_random_engine gen;

void ParticleFilter::init(double x, double y, double theta, double std[]) {
	// TODO: Set the number of particles. Initialize all particles to first position (based on estimates of
	//   x, y, theta and their uncertainties from GPS) and all weights to 1.
	// Add random Gaussian noise to each particle.
	// NOTE: Consult particle_filter.h for more information about this method (and others in this file).
    ParticleFilter::num_particles = 100;

    normal_distribution<double> dist_x(x, std[0]);
    normal_distribution<double> dist_y(y, std[1]);
    normal_distribution<double> dist_theta(theta, std[2]);

    for (int i=0; i < num_particles; i++)
    {
        Particle sample;
        sample.x = dist_x(gen);
        sample.y = dist_y(gen);
        sample.theta = dist_theta(gen);
        sample.weight = 1.0;

        particles.push_back(sample);
        ParticleFilter::weights.push_back(1.0);
    }

    ParticleFilter::is_initialized = true;

}

void ParticleFilter::prediction(double delta_t, double std_pos[], double velocity, double yaw_rate) {
	// TODO: Add measurements to each particle and add random Gaussian noise.
	// NOTE: When adding noise you may find std::normal_distribution and std::default_random_engine useful.
	//  http://en.cppreference.com/w/cpp/numeric/random/normal_distribution
	//  http://www.cplusplus.com/reference/random/default_random_engine
	    //    default_random_engine gen;

    for (int i=0; i < num_particles; i++)
    {
        normal_distribution<double> dist_x(particles[i].x, std_pos[0]);
        normal_distribution<double> dist_y(particles[i].y, std_pos[1]);
        normal_distribution<double> dist_theta(particles[i].theta, std_pos[2]);

        double x_gauss = dist_x(gen);
        double y_gauss = dist_y(gen);
        double theta_gauss = dist_theta(gen);

        if (yaw_rate != 0)
        {
            particles[i].x = x_gauss + ((velocity/yaw_rate)*
            (sin(theta_gauss + (yaw_rate * delta_t)) - sin(theta_gauss)));

            particles[i].y = y_gauss + ((velocity/yaw_rate)*
            (cos(theta_gauss) - cos(theta_gauss+(yaw_rate*delta_t))));
        }
        else
        {
            particles[i].x = x_gauss + (velocity * cos(theta_gauss) * delta_t);
            particles[i].y = y_gauss + (velocity * sin(theta_gauss) * delta_t);

        }
        particles[i].theta = theta_gauss + yaw_rate * delta_t;

    }

}

void ParticleFilter::dataAssociation(std::vector<LandmarkObs> predicted, std::vector<LandmarkObs>& observations) {
	// TODO: Find the predicted measurement that is closest to each observed measurement and assign the
	//   observed measurement to this particular landmark.
	// NOTE: this method will NOT be called by the grading code. But you will probably find it useful to
	//   implement this method and use it as a helper during the updateWeights phase.

	for (int i = 0; i < int(observations.size()); i++) {

		double closest_dist = 50;
		int closest_landmark_id = -1;
		double obs_x = observations[i].x;
		double obs_y = observations[i].y;

		for (int j = 0; j < int(predicted.size()); j++) {
		  double pred_x = predicted[j].x;
		  double pred_y = predicted[j].y;
		  int pred_id = predicted[j].id;
		  double current_dist = dist(obs_x, obs_y, pred_x, pred_y);

		  if (current_dist < closest_dist) {
		    closest_dist = current_dist;
		    closest_landmark_id = pred_id;
		  }
		}
		observations[i].id = closest_landmark_id;
	}
}


void ParticleFilter::updateWeights(double sensor_range, double std_landmark[],
		const std::vector<LandmarkObs> &observations, const Map &map_landmarks) {
	// TODO: Update the weights of each particle using a mult-variate Gaussian distribution. You can read
	//   more about this distribution here: https://en.wikipedia.org/wiki/Multivariate_normal_distribution
	// NOTE: The observations are given in the VEHICLE'S coordinate system. Your particles are located
	//   according to the MAP'S coordinate system. You will need to transform between the two systems.
	//   Keep in mind that this transformation requires both rotation AND translation (but no scaling).
	//   The following is a good resource for the theory:
	//   https://www.willamette.edu/~gorr/classes/GeneralGraphics/Transforms/transforms2d.htm
	//   and the following is a good resource for the actual equation to implement (look at equation
	//   3.33
	//   http://planning.cs.uiuc.edu/node99.html


  double weight_normalizer = 0.0;

  for (int i = 0; i < num_particles; i++) {
    double p_x = particles[i].x;
    double p_y = particles[i].y;
    double p_theta = particles[i].theta;

    //transform observations from vehicle coordinates to map cooordinates
    vector<LandmarkObs> transformed_observations;

    for (int j = 0; j < int(observations.size()); j++) {
      LandmarkObs transformed_obs;
      transformed_obs.id = j;
      transformed_obs.x = p_x + (cos(p_theta) * observations[j].x) - (sin(p_theta) * observations[j].y);
      transformed_obs.y = p_y + (sin(p_theta) * observations[j].x) + (cos(p_theta) * observations[j].y);
      transformed_observations.push_back(transformed_obs);
    }

    //filter map landmarks to keep only those which are in the current sensor_range
    vector<LandmarkObs> predicted_landmarks;
    for (int j = 0; j < int(map_landmarks.landmark_list.size()); j++) {
      Map::single_landmark_s current_landmark = map_landmarks.landmark_list[j];
      if (dist(p_x, p_y, current_landmark.x_f, current_landmark.y_f) <= sensor_range) {
        predicted_landmarks.push_back(LandmarkObs {current_landmark.id_i, current_landmark.x_f, current_landmark.y_f});
      }
    }

    dataAssociation(predicted_landmarks, transformed_observations);

    // calculate weights
    double sigma_x = std_landmark[0];
    double sigma_y = std_landmark[1];
    double sigma_x_2 = pow(sigma_x, 2);
    double sigma_y_2 = pow(sigma_y, 2);
    double gauss_normalizer = (1.0/(2.0 * M_PI * sigma_x * sigma_y));
    particles[i].weight = 1.0;

    for (int k = 0; k < int(transformed_observations.size()); k++) {
      double trans_obs_x = transformed_observations[k].x;
      double trans_obs_y = transformed_observations[k].y;
      double trans_obs_id = transformed_observations[k].id;
      double multi_dist = 1.0;

      for (int l = 0; l < int(predicted_landmarks.size()); l++) {
        double pred_landmark_x = predicted_landmarks[l].x;
        double pred_landmark_y = predicted_landmarks[l].y;
        double pred_landmark_id = predicted_landmarks[l].id;

        if (trans_obs_id == pred_landmark_id) {
          multi_dist = gauss_normalizer * exp(-1.0 * ((pow((trans_obs_x - pred_landmark_x), 2)/(2.0 * sigma_x_2)) + (pow((trans_obs_y - pred_landmark_y), 2)/(2.0 * sigma_y_2))));
          particles[i].weight *= multi_dist;
        }
      }
    }
    weight_normalizer += particles[i].weight;
  }

  // normalize the weights
  for (int i = 0; i < int(particles.size()); i++) {
    particles[i].weight /= weight_normalizer;
    weights[i] = particles[i].weight;
  }
}

void ParticleFilter::resample() {
	// TODO: Resample particles with replacement with probability proportional to their weight.
	// NOTE: You may find std::discrete_distribution helpful here.
	//   http://en.cppreference.com/w/cpp/numeric/random/discrete_distribution
    discrete_distribution<> disc_dist(ParticleFilter::weights.begin(), ParticleFilter::weights.end());
    vector<Particle> resampled_particles;
    for (int i=0; i < ParticleFilter::num_particles; i++)
    {
        resampled_particles.push_back(particles[disc_dist(gen)]);
    }
    particles = resampled_particles;

}


Particle ParticleFilter::SetAssociations(Particle& particle, const std::vector<int>& associations,
                                     const std::vector<double>& sense_x, const std::vector<double>& sense_y)
{
    //particle: the particle to assign each listed association, and association's (x,y) world coordinates mapping to
    // associations: The landmark id that goes along with each listed association
    // sense_x: the associations x mapping already converted to world coordinates
    // sense_y: the associations y mapping already converted to world coordinates

	particle.associations= associations;
 	particle.sense_x = sense_x;
 	particle.sense_y = sense_y;

  return particle;
}

string ParticleFilter::getAssociations(Particle best)
{
	vector<int> v = best.associations;
	stringstream ss;
    copy( v.begin(), v.end(), ostream_iterator<int>(ss, " "));
    string s = ss.str();
    s = s.substr(0, s.length()-1);  // get rid of the trailing space
    return s;
}
string ParticleFilter::getSenseX(Particle best)
{
	vector<double> v = best.sense_x;
	stringstream ss;
    copy( v.begin(), v.end(), ostream_iterator<float>(ss, " "));
    string s = ss.str();
    s = s.substr(0, s.length()-1);  // get rid of the trailing space
    return s;
}
string ParticleFilter::getSenseY(Particle best)
{
	vector<double> v = best.sense_y;
	stringstream ss;
    copy( v.begin(), v.end(), ostream_iterator<float>(ss, " "));
    string s = ss.str();
    s = s.substr(0, s.length()-1);  // get rid of the trailing space
    return s;
}

