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

#include "particle_filter.h"

#include <math.h>
#include <algorithm>
#include <iostream>
#include <iterator>
#include <numeric>
#include <random>
#include <string>
#include <vector>

#include "helper_functions.h"

using namespace std;//

using std::string;
using std::vector;

static default_random_engine engine;//
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.
   * TODO: Add random Gaussian noise to each particle.
   * NOTE: Consult particle_filter.h for more information about this method
   *   (and others in this file).
   */
  num_particles = 100;  // TODO: Set the number of particles

  normal_distribution<double> x_dev(x,std[0]);
  normal_distribution<double> y_dev(y, std[1]);
  normal_distribution<double> theta_dev(theta, std[2]);

  for(int i=0;i<num_particles;i++){
    Particle pp;
    pp.id = i;
    pp.x = x_dev(engine);
    pp.y = y_dev(engine);   //注意不是sense_y 而是y，因为：1.y的偏差由main.cpp中landmark measurement提供,
        //2. sense_y为数组,y_dev为一个数
    pp.theta = theta_dev(engine);
    pp.weight = 1.0;
    particles.push_back(pp);
}
  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/
   */
//   int min_yaw_rate = 0.0001;  //第一次修改

  double min_yaw_rate = 0.0001;
  for (unsigned int i=0;i<particles.size();i++) {  //第三次提交后修改，变量i需要初始化为0
    if (abs(yaw_rate) > min_yaw_rate) {
//       particles[i].x += velocity / yaw_rate * (cos(particles[i].theta) - cos(particles[i].theta + yaw_rate * delta_t));
//       particles[i].y += velocity / yaw_rate * (sin(particles[i].theta + yaw_rate*delta_t) + sin(particles[i].theta));
      particles[i].x += velocity / yaw_rate * (sin(particles[i].theta + yaw_rate * delta_t)-sin(particles[i].theta));
      particles[i].y += velocity / yaw_rate * (cos(particles[i].theta)-cos(particles[i].theta + yaw_rate * delta_t));

      particles[i].theta += yaw_rate * delta_t;
    }
    else {
//       particles[i].x = particles[i].x + velocity * cos(particles[i].theta);
//       particles[i].y = particles[i].y + velocity * sin(particles[i].theta);
//       particles[i].x = particles[i].x + velocity * sin(particles[i].theta);   //第二次修改
//       particles[i].y = particles[i].y + velocity * cos(particles[i].theta);   //第二次修改

      particles[i].x = particles[i].x + velocity * cos(particles[i].theta);
      particles[i].y = particles[i].y + velocity * sin(particles[i].theta);
    }

    normal_distribution<double> x_dev(0,std_pos[0]);
    normal_distribution<double> y_dev(0,std_pos[1]);
    normal_distribution<double> theta_dev(0, std_pos[2]);

    particles[i].x += x_dev(engine);
    particles[i].y += y_dev(engine);
    particles[i].theta += theta_dev(engine);

//     std::cout<<"init"<<std::endl;
  }
}

void ParticleFilter::dataAssociation(vector<LandmarkObs> predicted,
                                     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 (unsigned int i=0;i< observations.size();i++) {  //第三次提交后修改，变量i需要初始化为0
//     double x_m;
//     double y_m;
    double cal;
    double set_val = numeric_limits<double>::max();
    int min_id;

    for (unsigned int j=0;j<predicted.size();j++){   //第三次提交后修改，变量i需要初始化为0
      //x_m = predicted[i].x + (cos(particles[i].theta)*observations[i].x) - (sin(particles[i].theta)*observations[i].y);
      //y_m = predicted[i].y + (sin(particles[i].theta)*observations[i].x) - (cos(particles[i].theta)*observations[i].y);
      //从 helper_functions.h中可知，输入参数predicted和observations都为LandmarkObs，所以不需要坐标转换，而且我认为predicted是
      //真实的地标？
      cal = dist(observations[i].x, observations[i].y, predicted[i].x, predicted[i].y);
      if (cal < set_val) {
        set_val = cal;
        min_id = observations[i].id;
      }
    }
    observations[i].id = min_id;
  }
//   std::cout<< "remain" << &predicted << std::endl;  //调试用
}

void ParticleFilter::updateWeights(double sensor_range, double std_landmark[],
                                   const 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
   */
//   std::cout<< "%%%%%%%%%%%%" << std::endl;  //调试用
//   std::cout<<num_particles<<std::endl; // 调试用
  for (int i = 0;i<num_particles;i++) {  //第一次修改，添加 i=0
//     std::cout<< "@@@@@@@@@@@@@@@" << std::endl;  // 调试用

    double p_x = particles[i].x;
    double p_y = particles[i].y;

    vector<LandmarkObs> remain_landmarks;


    for (unsigned int j=0;j<map_landmarks.landmark_list.size();j++) {   //第三次提交后修改，变量i需要初始化为0
      double m_x = map_landmarks.landmark_list[j].x_f;
      double m_y = map_landmarks.landmark_list[j].y_f;

      if ((fabs(p_x-m_x) <=sensor_range)&&(fabs(p_y-m_y)<=sensor_range)) {
        remain_landmarks.push_back(LandmarkObs{ map_landmarks.landmark_list[j].id_i,m_x,m_y });

      }
    }

    vector<LandmarkObs> objs;
    for (unsigned int t = 0; t < observations.size(); t++){   //第二次修改
//       double x_m = remain_landmarks[i].x + (cos(particles[i].theta)*observations[i].x) - (sin(particles[i].theta)*observations[i].y);  //第一次修改
//       double y_m = remain_landmarks[i].y + (sin(particles[i].theta)*observations[i].x) - (cos(particles[i].theta)*observations[i].y);  //第一次修改

//       double x_m = particles[i].x + (cos(particles[i].theta)*observations[i].x) - (sin(particles[i].theta)*observations[i].y);  //第二次修改
//       double y_m = particles[i].y + (sin(particles[i].theta)*observations[i].x) + (cos(particles[i].theta)*observations[i].y);
//   第二次修改
      double x_m = particles[i].x + (cos(particles[i].theta)*observations[t].x) - (sin(particles[i].theta)*observations[t].y);
      double y_m = particles[i].y + (sin(particles[i].theta)*observations[t].x) + (cos(particles[i].theta)*observations[t].y);

      objs.push_back(LandmarkObs{observations[i].id,x_m,y_m});
    }

    dataAssociation(remain_landmarks,objs);

    particles[i].weight = 1.0;

    for (unsigned int t = 0; t < objs.size(); t++){   //第二次修改
      double objs_id = objs[t].id;
      double objs_x = objs[t].x;
      double objs_y = objs[t].y;
      double r_l_x;   //添加
      double r_l_y;   //添加

//       std::cout<< "@@@@@@@@@@@@@@@" << std::endl;   //调试用
//       std::cout<< "%%%%%%%%%%"<<objs_id << std::endl;   //调试用

      for (unsigned int j = 0; j < remain_landmarks.size(); j++){
//         std::cout<< "#########"<<remain_landmarks[j].id << std::endl;   //调试用
        if (remain_landmarks[j].id == objs_id) {
//           std::cout<< "@@@@@@@@@@@@@@@" << objs_id << std::endl;   //调试用
//           particles[i].weight *= 1 / (2 * M_PI * std_landmark[0] * std_landmark[1]) * exp((pow(objs_x - remain_landmarks[j].x, 2) / (2 * pow(std_landmark[0], 2))) + (pow(objs_y - remain_landmarks[j].y, 2) / (2 * pow(std_landmark[1], 2)))); //第一次修改
//           particles[i].weight *= (1/(2 * M_PI * std_landmark[0] * std_landmark[1])) * exp(-(pow(objs_x - remain_landmarks[j].x, 2) / (2 * pow(std_landmark[0], 2)) + (pow(objs_y - remain_landmarks[j].y, 2)/(2 * pow(std_landmark[1], 2)))));
          r_l_x = remain_landmarks[j].x;    //添加
          r_l_y = remain_landmarks[j].y;    //添加
//           std::cout<< "@@@@@@@@@@@@@@@" << std::endl;   //调试用
        }
//          std::cout<< "$$$$$$$$$$$" << std::endl;    //调试用
      }
      particles[i].weight *= (1 / (2 * M_PI * std_landmark[0] * std_landmark[1])) * exp(-(pow(objs_x - r_l_x, 2) / (2 * pow(std_landmark[0], 2)) + (pow(objs_y - r_l_y, 2) / (2 * pow(std_landmark[1], 2)))));   //添加
    }
  }
}

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
   */
  vector<Particle> pp;

  uniform_int_distribution<int> particles_distribution(0, particles.size() - 1);  // 修改
  int index = particles_distribution(engine);
  //int index = int(engine * particles.size());
  double beta = 0.0;
  vector<double> weight_list;
  for (unsigned int i = 0; i < particles.size(); i++)
  {
    weight_list.push_back(particles[i].weight);
  }
  double ww = *max_element(weight_list.begin(),weight_list.end());

  uniform_real_distribution<double> random_engine(0.0,ww);

  for (unsigned int i = 0; i < particles.size(); i++)
  {
    beta += random_engine(engine) * 2.0;
//     while (beta > particles[i].weight)
//     {
//       beta -= particles[i].weight;
//       index = (index + 1) % particles.size();
//     }
    while (beta > weight_list[index])
    {
      beta -= particles[i].weight;
      index = (index + 1) % particles.size();
    }
    pp.push_back(particles[index]);
  }
  particles = pp;
}

void ParticleFilter::SetAssociations(Particle& particle,
                                     const vector<int>& associations,
                                     const vector<double>& sense_x,
                                     const vector<double>& sense_y) {
  // particle: the particle to which assign each listed association,
  //   and association's (x,y) world coordinates mapping
  // 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;
}

string ParticleFilter::getAssociations(Particle best) {
  vector<int> v = best.associations;
  std::stringstream ss;
  copy(v.begin(), v.end(), std::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::getSenseCoord(Particle best, string coord) {
  vector<double> v;

  if (coord == "X") {
    v = best.sense_x;
  } else {
    v = best.sense_y;
  }

  std::stringstream ss;
  copy(v.begin(), v.end(), std::ostream_iterator<float>(ss, " "));
  string s = ss.str();
  s = s.substr(0, s.length()-1);  // get rid of the trailing space
  return s;
}