/*
 * Daniel Lowd
 * CS154: Robotics
 * mcl.cc
 *
 * Feel free to use the makefile in this directory also.
 *   use 
 *
 *   make mcl
 *
 */

#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include <string>
#include <fstream.h>
#include <iostream.h>
#include <stdio.h>
#include <vector>
#include "Timing.hh"
#include "KbdInput.hh"

// Debugging defines
#define FILEOUT
#define THREE_DIM
#define DRAW_TO_SCREEN

#define PI 3.14159

extern "C" {
#include "Nclient.h"
}

#define MIN(a, b) (((a) < (b)) ? (a) : (b)) 
#define MAX(a, b) (((a) > (b)) ? (a) : (b)) 
#define DEGREES(a) ((a)*180.0/PI)

/*
 * global parameters (read from "parameters" file)
 */

int    COLOR = 10;               // color of particles before they move
int    COLOR2 = 16;               // color of particles after they move
double PARTICLE_RADIUS = 50.0;   // size of drawn particles
double NOISE = 10.0;             // mean of the sensor noise
double STDEV = 10.0;             // st. deviation of the sensor noise
int    NPART = 100;              // number of particles used
double POS_STDEV_FRAC = 0.2;     // fraction of robot moves used as
                                 //   the st. dev. of the distribution
                                 //   on where the robot actually goes
double DUAL_FRAC = 0.1;
int    DUAL_NPART = 10;

/*
 * a global timing object and forward declarations
 */

Timing T;
double gaussrand();

/*
 * the Wall class
 */

class Wall
{
  public:

    double x1,y1,x2,y2;

    Wall(double x1_, double y1_, double x2_, double y2_):
      x1(x1_), y1(y1_), x2(x2_), y2(y2_) { ; }

    Wall():
      x1(.0), y1(.0), x2(.0), y2(.0) { ; }

};

/*
 * the Particle class
 */

class Particle
{
  public:
    double x,y,a,w; // x, y, angle, and weight

    Particle(double x_, double y_, double a_, double w_):
      x(x_), y(y_), a(a_), w(w_) { ; }

    Particle():
      x(.0), y(.0), a(.0), w(.0) { ; }
};

ostream& operator<<(ostream& out, const Particle& part)
{
    out << "(" << part.x << "," << part.y << "," << part.a << "," << part.w << ")";
    return out;
}

/*
 * functions provided -- feel free to use
 *
 * loadMap: reads in a number of walls from a map file
 *          created in the Nomad simulator
 */

void
loadMap(vector<Wall>& w, char* mapfile)
{
  ifstream map(mapfile);

  int nvertices;
  int i=0;
  double x0, y0, lastx, lasty, x, y;

  while (map >> nvertices)
  {
      map >> x0 >> y0;
      lastx = x0;
      lasty = y0;

      while (--nvertices)
      {
          map >> x >> y;
          w.push_back(Wall(lastx, lasty, x, y));
          lastx = x;
          lasty = y;
      }

      w.push_back(Wall(lastx, lasty, x0, y0));
  }
}

/*
 * drawParticles: displays the individual pose guesses
 *                in the simulator
 */

void
drawParticles(vector<Particle>& pFilt, int radius, int color)
{
  int NPART = pFilt.size();

#ifdef FILEOUT
  static int filenum = 0;
  char filename[1024];
  FILE* outfile;
  
  sprintf(filename, "of.%02d", filenum++);
  outfile = fopen(filename, "w+");
#endif

  for (int i = 0; i < NPART; i++)
  {
    double x = pFilt[i].x;
    double y = pFilt[i].y;
    double a = pFilt[i].a;
    double w = pFilt[i].w;
#ifdef DRAW_TO_SCREEN
    //draw_arc(x-(radius/2),y+(radius/2),radius,radius,0,3600,color);
    draw_line(x,y,x,y,color);
#endif

#ifdef FILEOUT
    fprintf(outfile, "%.2f\t%.2f\t%.2f\t%.2f\n", x, y, a, NPART*w);
#endif
  }

#ifdef FILEOUT
  fclose(outfile);
#endif
}


void
userDrawParticles(vector<Particle>& pFilt, int radius, int color)
{
  int NPART = pFilt.size();

  for (int i = 0; i < NPART; i++)
  {
    double x = pFilt[i].x;
    double y = pFilt[i].y;
    double a = pFilt[i].a;
    double w = pFilt[i].w;
    draw_line(x,y,x,y,color);
  }
}

/*
 * moveParticles: moves each particle in a particle filter
 *                by dx and dy
 *
 *                It also adds some Gaussian noise
 *                with a stdeviation of POS_STDEV_FRAC
 *                of the original move, i.e., the error
 *                is proportional to the size of the move.
 */

void
moveParticles(vector<Particle>& pFilt, double dx, double dy, double dangle)
{
  int NPART = pFilt.size();

  for (int i=0 ; i<NPART ; i++)
  {
    double xdiff = dx*cos(pFilt[i].a) + dy*sin(pFilt[i].a);
    double ydiff = dy*cos(pFilt[i].a) + dx*sin(pFilt[i].a);

    pFilt[i].x += xdiff + POS_STDEV_FRAC*xdiff*gaussrand();
    pFilt[i].y += ydiff + POS_STDEV_FRAC*ydiff*gaussrand();
    pFilt[i].a += dangle + POS_STDEV_FRAC*dangle*gaussrand();
  }
}


/*
 * clearance: determine the distance to the nearest
 *            wall in the specified direction
 */

double
clearance(vector<Wall>& w, double x, double y, double a, int dir)
{
    double result = 1.0e9;
    double diff = 0.0;

    // Convert robot angle and sensor direction to an absolute angle
    // in radians, in the range -PI to PI.
    a += dir * PI/8;
    while (a > PI)
        a -= 2*PI;

    for (int i=0 ; i<w.size() ; i++)
    {
        // Calculate angles to each wall endpoint, normalized so
        // that 0 degrees is the robot's angle.
        double theta1 = atan2(w[i].y1 - y, w[i].x1 - x) - a;
        double theta2 = atan2(w[i].y2 - y, w[i].x2 - x) - a;

        // Shift angles by 2PI until they're in the range PI to -PI
        while (theta1 > PI) theta1 -= 2*PI;
        while (theta2 > PI) theta2 -= 2*PI;
        while (theta1 < -PI) theta1 += 2*PI;
        while (theta2 < -PI) theta2 += 2*PI;
        
        // If the endpoints are both on one side of the robot's sonar
        // or behind it, then skip this wall.
        if ((theta1 > 0 && theta2 > 0) ||
            (theta1 < 0 && theta2 < 0) ||
            (fabs(theta1 - theta2) > PI))
        {
            continue;
        }

        // Calculate angle between two wall endpoints
        double other = atan2(w[i].y2 - w[i].y1, w[i].x2 - w[i].x1) - a;
        while (other < 0)  other += PI;
        while (other > PI) other -= PI;

        double angle3 = MAX(theta1, theta2);
        double angle2 = PI - other;
        double angle1 = PI - angle3 - angle2;

        double xdiff2, ydiff2;
        if (theta1 > theta2)
        {
            xdiff2 = w[i].x1 - x;
            ydiff2 = w[i].y1 - y;
        }
        else
        {
            xdiff2 = w[i].x2 - x;
            ydiff2 = w[i].y2 - y;
        }

        // Law of sines
        double dist2 = sqrt(xdiff2*xdiff2 + ydiff2*ydiff2);
        double diff = dist2*sin(angle1)/sin(angle2);

        if (diff < result) result = diff;
    }

    return result;
}


/*
 * gaussianPDF: return the (relative) probability that x
 *              was drawn from a Normal distribution with
 *              mean m and standard deviation stdev
 */

double
gaussianPDF(double x, double m, double stdev)
{
  // don't use the constant in front, as everything will
  // get normalized anyway...

  double diff = x-m;
  double exponent = -1.0*diff*diff/(2.0*stdev*stdev);

  return exp(exponent);
}


/*
 * gaussrand: A function to generate a normal random variable 
 *            with mean 0 and standard deviation of 1.  
 *
 * To adjust this normal distribution, multiply by the new standard
 * deviation and add the mean.  This uses the so-called Box-Muller method.
 *
 * note: drand48() is a function that returns a uniformly distributed random
 * number from 0.0 to 1.0
 *
 * from: http://remus.rutgers.edu/~rhoads/Code/code.html
 */

double 
gaussrand()
{
  static double V2, fac;
  static int phase = 0;
  double S, Z, U1, U2, V1;

  if (phase)
    Z = V2 * fac;
  else
  {
    do {
      U1 = drand48();
      U2 = drand48();

      V1 = 2 * U1 - 1;
      V2 = 2 * U2 - 1;
      S = V1 * V1 + V2 * V2;
    } while(S >= 1);

    fac = sqrt (-2 * log(S) / S);
    Z = V1 * fac;
  }

  phase = 1 - phase;

  return Z;
}

/*
 * noisify -- add Gaussian noise to a sensor reading.
 */
#define MAX_SENSOR_VALUE 255.0
double noisify(double x)
{
    double ret = x + gaussrand()*NOISE;

    if (ret > MAX_SENSOR_VALUE)
        return MAX_SENSOR_VALUE;
    else
        return ret;
}

/* ****** BEGIN DUAL MCL STUFF ****** */

#define MAX_X 3000
#define MAX_Y 3000
#define MIN_X -3000
#define MIN_Y -3000
#define X_VAR 0
#define Y_VAR 1
#define A_VAR 2

struct s_kd_data
{
    double* vars;
};

// A single node in a kd-tree
struct s_kd_node
{
    int split_var;
    double split_val;
    struct s_kd_node* left;
    struct s_kd_node* right;

    // Holds final child/children in leaf nodes
    struct s_kd_data* data;
};

typedef struct s_kd_data kd_data;
typedef struct s_kd_node kd_node;

int kd_var = 0;

int kd_comp(const void* first, const void* second)
{
    double first_val = ((kd_data*)first)->vars[kd_var];
    double second_val = ((kd_data*)second)->vars[kd_var];

    if (first_val < second_val)
        return -1;
    else if (first_val > second_val)
        return 1;
    else
        return 0;
}

kd_node* build_tree(int first_var, int max_var, int num_data, 
        kd_data* data)
{
    kd_node* ret = new kd_node;
    
    // It could happen.  Maybe.  But we hope it doesn't.
    if (num_data == 0)
    {
        cout << "Returning NULL tree node!\n";
        return NULL;
    }
    // Degenerate case -- 1 data point left.  End recursion.
    else if (num_data == 1)
    {
        ret->split_var = 1;
        ret->split_val = data[0].vars[first_var];
        ret->left      = NULL;
        ret->right     = NULL;
        ret->data      = data;
        return ret;
    }
    else
    {
        // Test to see if we have all identical data points.  (They could cause a
        // problem.)
        int i,j;
        for (i = 1; i < num_data; i++)
            for (j = 0; j <= max_var; j++)
                if (data[i].vars[j] != data[0].vars[j])
                    goto not_equal;

        // All the data are identical!
        ret->split_var = num_data;
        ret->split_val = 0.0;
        ret->left = NULL;
        ret->right = NULL;
        ret->data = data;
        return ret;

        // Normal case...
not_equal:
        // Prepare to recurse with half the data
        int next_var = (first_var + 1) % (max_var+1);
        int num_left = num_data/2;

        // Sort by specified variable (required to find median)
        // We use a global var to tell the comparison routine (kd_comp) which
        // of the k variables we're using for comparison.
        kd_var = first_var;
        qsort(data, num_data, sizeof(kd_data), kd_comp);

        ret->split_var = first_var;
        ret->split_val = data[num_left].vars[first_var];

        // If the median is a duplicate, ensure that all end up on one side.
        while (num_left > 0 && (data[num_left-1].vars[first_var] == ret->split_val))
            num_left--;

        // However, in the case of something like: 1 1 1 3, we'd want the 1's
        // on one side and the 3 on another.  Therefore, we may have to switch
        // split values to avoid having one empty subtree.
        if (num_left == 0)
        {
            while (num_left < num_data
                    && (data[num_left].vars[first_var] == ret->split_val))
                num_left++;

            if (num_left < num_data)
                ret->split_val = data[num_left].vars[first_var];
            else
                return build_tree(next_var, max_var, num_data, data);
        }

        // Recurse
        ret->left = build_tree(next_var, max_var, num_left, data);
        ret->right = build_tree(next_var, max_var, num_data - num_left, data + num_left);
        return ret;
    }
}

void free_tree(kd_node* tree)
{
    if (tree == NULL)
        return;

    free_tree(tree->left);
    delete(tree->left);
    free_tree(tree->right);
    delete(tree->right);
    return;
}

void print_tabs(int numtabs, ostream& out)
{
    while (numtabs--) {
        out << '\t';
    }
}

void print_tree(kd_node* curr_node, int curr_level, int max_var, ostream& out)
{
    if (curr_node->left == NULL && curr_node->right == NULL)
    {
        print_tabs(curr_level, out);
        out << '(';
        for (int i = 0; i <= max_var; i++) { 
            if (i != 0)
                out << ',';
            out << curr_node->data->vars[i];
        }
        out << ')' << endl;
        return;
    }

    print_tabs(curr_level, out);
    out << "var: " << curr_node->split_var << "; split: " << curr_node->split_val << endl;
    if (curr_node->left != NULL)
    {
        print_tabs(curr_level, out);
        out << "LEFT CHILD\n";
        print_tree(curr_node->left, curr_level+1, max_var, out);
    }
    if (curr_node->right != NULL)
    {
        print_tabs(curr_level, out);
        out << "RIGHT CHILD\n";
        print_tree(curr_node->right, curr_level+1, max_var, out);
    }
}


kd_node* build_uniform_dual_tree(vector<Wall> walls)
{
    // Uniformly generate data points.
    int index = 0;
    kd_data* uniform_data = new kd_data[81000];
    for (double x = MIN_X; x < MAX_X; x += (MAX_X - MIN_X)/100)
    {
        printf("x = %f; index = %d\n", x, index);

        for (double y = MIN_Y; y < MAX_Y; y += (MAX_Y - MIN_Y)/100)
            for (double a = 0; a < 2*PI; a += PI/4)
            {
                kd_data curr;
                Particle p(x, y, a, 1.0);
                curr.vars = new double[19];

                if (curr.vars == 0)
                {
                    printf("ERROR: out of memory!\n");
                    return 0;
                }

                //bool all_maxed = true;
                for (int sensor = 0; sensor < 16; sensor++)
                {
                    double clear = clearance(walls,x,y,a,sensor);
                    curr.vars[sensor] = MIN(clear/10.0 - 9.0, MAX_SENSOR_VALUE);

                    /*
                    if (curr.vars[sensor] != MAX_SENSOR_VALUE)
                        all_maxed = false;
                        */
                }

                curr.vars[16] = x;
                curr.vars[17] = y;
                curr.vars[18] = a;

                /*
                if (all_maxed)
                    maxed_data[maxed_index++] = curr;
                else
                */
                    uniform_data[index++] = curr;
            }
    }

    return build_tree(0, 15, index, uniform_data);
}

// DEBUG
ofstream plog("newparts.log");

Particle guessPoint(double sonarReadings[16], kd_node* root)
{
    // Add noise here so we sample a point from the distribution
    double noisyReadings[16];
    for (int i = 0; i < 16; i++)
        noisyReadings[i] = noisify(sonarReadings[i]);

    kd_node* curr_node = root;
    while (curr_node->left != NULL || curr_node->right != NULL)
    {
        if (noisyReadings[curr_node->split_var] < curr_node->split_val 
                || curr_node->left == NULL)
            curr_node = curr_node->left;
        else
            curr_node = curr_node->right;
    }

    Particle ret;
    // If there are multiple data points at this final node, select one of
    // them at random.
    int index = drand48()*curr_node->split_var;
    ret.x = curr_node->data[index].vars[16];
    ret.y = curr_node->data[index].vars[17];
    ret.a = curr_node->data[index].vars[18];
    ret.w = 1.0;

    // DEBUG
    plog << ret << endl;

    return ret;
}

double weightPoint(Particle part, kd_node* prior_root, double dx, double dy, double da)
{
    // Translate this point backwards (with noise)...
    double xdiff = dx*cos(part.a) + dy*sin(part.a);
    double ydiff = dy*cos(part.a) + dx*sin(part.a);
    part.x -= xdiff + POS_STDEV_FRAC*xdiff*gaussrand();
    part.y -= ydiff + POS_STDEV_FRAC*ydiff*gaussrand();
    part.a -= da    + POS_STDEV_FRAC*da*gaussrand();

    if (part.a < 0)
        part.a += 2*PI;

    double mins[3] = {MIN_X, MIN_Y, 0};
    double maxs[3] = {MAX_X, MAX_Y, 2*PI};
    kd_node* curr_node = prior_root;
    
    while (curr_node->left && curr_node->right)
    {
        double val;
        switch(curr_node->split_var) {
            case 0: val = part.x; break;
            case 1: val = part.y; break;
            case 2: val = part.a; break;
        }

        if (val < curr_node->split_val)
        {
            //if (curr_node->split_val < maxs[curr_node->split_var])
                maxs[curr_node->split_var] = curr_node->split_val;
            curr_node = curr_node->left;
        }
        else
        {
            //if (curr_node->split_val > mins[curr_node->split_var])
                mins[curr_node->split_var] = curr_node->split_val;
            curr_node = curr_node->right;
        }
    }

    if (curr_node->left || curr_node->right)
    {
        cout << "ERROR: one of two subtrees is NULL!\n";
        return 0.0;
    }

    // Weight is proportional to density, measured in weight/unit area.
    double local_weight = 0.0;
    for (int i = 0; i < curr_node->split_var; i++)
        local_weight += curr_node->data[i].vars[3];

    cout << "x: " << (maxs[0]-mins[0]) << "; y: " << (maxs[1]-mins[1]) 
        << "; a: " << (maxs[2]-mins[2]) << endl;
    cout << "local_weight: " << local_weight << endl;

    return (local_weight)/((maxs[0]-mins[0])*(maxs[1]-mins[1])*(maxs[2]-mins[2]));
}


////////////// END NEW DUAL MCL STUFF /////////////////////////

/****************************
 *
 *  The usual main method
 *
 ****************************/

int
main(int argc, char** argv)
{

    SERV_TCP_PORT = 7654; // My server port

  /*
   * initialize random number generator
   *   you can choose your own long int here to test repeatably
   */

  long int L = T.randomLongInt();
  srand48(L);

  /*
   * lots of variables that will be useful
   */

  int oldx = 0, x = 0;
  int oldy = 0, y = 0;
  int oldangle = 0, angle = 0;
  double dx = 0, dy = 0, dangle = 0;

  int sonar = 0;
  double noise = 0.0;
  double noisysonar = 0.0;

  /*
   * read in the file of parameters
   */

  char comment[100];
  ifstream inf("./parameters");
  inf >> COLOR >> comment;
  cout << "COLOR is " << COLOR << endl;
  inf >> COLOR2 >> comment;
  cout << "COLOR2 is " << COLOR2 << endl;
  inf >> PARTICLE_RADIUS  >> comment;
  cout << "PARTICLE_RADIUS is " << PARTICLE_RADIUS << endl;
  inf >> NOISE >> comment;
  cout << "NOISE is " << NOISE << endl;
  inf >> STDEV >> comment;
  cout << "STDEV is " << STDEV << endl;

  // HACK -- NPART is an int now
  double npart;
  inf >> npart >> comment;
  NPART = npart;

  cout << "NPART is " << NPART << endl;
  inf >> POS_STDEV_FRAC >> comment;
  cout << "POS_STDEV_FRAC is " << POS_STDEV_FRAC << endl;
  inf >> DUAL_FRAC >> comment;
  cout << "DUAL_FRAC is " << DUAL_FRAC << endl;

  DUAL_NPART = DUAL_FRAC*NPART;


  /*
   * get the environment
   */

  vector<Wall> walls; 
  loadMap(walls, "mclmap1");
  cout << "walls.size is " << walls.size() << endl;

  // Generate a kd-tree to help us probablistically sample points 
  // given sensor readings.
  kd_node* dual_root = build_uniform_dual_tree(walls);

  // DEBUG -- output the entire kd-tree generated
  ofstream tree_file("treefile.log");
  print_tree(dual_root, 0, 15, tree_file);

  x = 0; y = 0;


  /* 
   * connect to the simulator
   */

  strcpy(SERVER_MACHINE_NAME, "localhost");
  if (connect_robot(1)) 
  {
    printf("Connected to robot.\n");
    printf("Hit q to exit.\n");
  }
  else 
  {
    printf("Failed to connect to robot\n");
    exit(0);
  }

  /* 
   * zero the robot
   */

  zr();

  /* 
   * turn on all sensors 
   */

  for (int i=1 ; i<=42 ; ++i)  Smask[i] = 1;
  ct();

  gs();                       /* update the robot's state           */
  refresh_all();              /* clear the graphics                 */

  /*
   * the KbdInput object allows nonblocking input
   */

  KbdInput KI; 
  char c; 

  /*
   * initial set of particles
   */

  vector<Particle> pFilt(NPART);
  for (int i=0 ; i<NPART ; ++i)
  {
    // assume -3000 to 3000 for now
    double x = (drand48()-0.5)*2*3000;
    double y = (drand48()-0.5)*2*3000;
#ifdef THREE_DIM
    double a = (drand48()*2*PI) - PI;
#else
    double a = 0.0;
#endif
    double w = 1.0;              // equal weights
    pFilt[i] = Particle(x,y,a,w);
  }

  drawParticles(pFilt, PARTICLE_RADIUS, COLOR);

  /*
   * initial prior
   */ 
  kd_data prior_data[NPART];
  for (int i = 0; i < NPART; i++)
  {
      prior_data[i].vars = new double[4];
      prior_data[i].vars[0] = pFilt[i].x;
      prior_data[i].vars[1] = pFilt[i].y;
      prior_data[i].vars[2] = pFilt[i].a;
      prior_data[i].vars[3] = pFilt[i].w;
  }
  kd_node* prior_tree = build_tree(0, 2, NPART, prior_data);

  /*
   * main loop
   */

  bool localizing = true;
  while (localizing)
  {

    /*
     * move the robot with the keyboard
     */

    KI.On();                    /* turn on raw input/output            */

    bool done = false;
    while (!done) {
      gs();
      c = KI.nextChar();      /* c is 0 if there's no input ready     */

      switch(c) {             /* hit q in the terminal window to quit */
          case 'q': done = true; break;
          case 'l': localizing = !localizing;
                    cout << "localizing status = " << localizing << "\r\n";
                    break;
#ifdef THREE_DIM
          case 'f': vm(0,-100,-100); break;
          case 's': vm(0,100,100); break;
#else
          case 'f': vm(0,-100,0); break;
          case 's': vm(0,100,0); break;
#endif
          case 'e': vm(100,0,0); break;
          case 'd': vm(-100,0,0); break;
          case ' ': vm(0,0,0); break;
          case 'D': userDrawParticles(pFilt, 
                            PARTICLE_RADIUS, COLOR);
                    break;
      }

      /* Sleep for 100,000 microseconds = 0.1 seconds */
      usleep(100000);
    }

    KI.Off();

    /*
     * now the robot's move has ended
     * 
     * we obtain the actual location and newly achieved offset
     * of the robot with the following
     */

    gs();                       /* update the robot's state           */
    oldx = x;                   
    oldy = y;
    oldangle = angle;
    x = State[34]; 
    y = State[35];
    angle = State[37];

    dx = x-oldx;
    dy = y-oldy;
    dangle = PI*(angle-oldangle)/1800.0;

    refresh_all();              /* clear the graphics                 */

    vector<Particle> newFilt(NPART);

    NPART = pFilt.size();
    vector<Particle> dualParts(DUAL_NPART);

    double noisySonar[16];
    for (int sensor = 0; sensor < 16; sensor++)
        noisySonar[sensor] = noisify(State[17 + sensor]);


    // Total weight required for normalization
    double totalWeight = 0.0;

    // Create and weight DUAL_NPART points sampled from the configuration space,
    // conditioned on the sensor readings.
    for (int i = 0; i < DUAL_NPART; i++)
    {
        dualParts[i]   = guessPoint(noisySonar, dual_root);
        dualParts[i].w = weightPoint(dualParts[i], prior_tree, dx, dy, dangle);
        totalWeight += dualParts[i].w;
        cout << dualParts[i].w << endl;
    }

    // Normalize
    for (int i = 0; i < DUAL_NPART; i++)
        dualParts[i].w *= DUAL_FRAC/totalWeight;

    double currWeight = 0.0;
    double mult = 1.0;
    for (int j = 0; j < DUAL_NPART; mult *= 2.0) {
        for (int i = 0; i < DUAL_NPART && j < DUAL_NPART; i++) {
            currWeight += dualParts[i].w * mult;
            while (currWeight > 1.0/DUAL_NPART && j < DUAL_NPART) {
                newFilt[j] = dualParts[i];
                newFilt[j++].w = 1.0;
                currWeight -= 1.0/DUAL_NPART;
            }
        }
    }

    // Regular, forward MCL
    /*
     * move the particles appropriately and draw them
     */

    moveParticles(pFilt, dx, dy, dangle);
    drawParticles(pFilt,PARTICLE_RADIUS,COLOR2);

    /*
     * update the weight of each particle
     */ 
    gs();                       /* update state */

    NPART = pFilt.size();

    for (int sensor = 0; sensor < 16; sensor++) {
        double noisy_sonar = noisify(State[17 + sensor]);

        for (int i = 0; i < NPART; i++) {
            double min_dist = clearance(walls, pFilt[i].x, pFilt[i].y,
                    pFilt[i].a, sensor);
            min_dist = MIN(min_dist/10.0 - 9.0, MAX_SENSOR_VALUE);
            pFilt[i].w *= gaussianPDF(min_dist, noisy_sonar, STDEV);
        }
    }

    // Calculate total weight, so we can normalize
    totalWeight = 0.0;
    for (int i = 0; i < NPART; i++)
        totalWeight += pFilt[i].w;

    // Normalize
    for (int i = 0; i < NPART; i++)
        pFilt[i].w *= (1-DUAL_FRAC)/totalWeight;

    currWeight = 0.0;
    mult = 1.0;
    for (int j = DUAL_NPART; j < NPART; mult *= 2.0) {
        for (int i = 0; i < NPART && j < NPART; i++) {
            currWeight += pFilt[i].w * mult;
            while (currWeight > 1.0/(NPART-DUAL_NPART) && j < NPART) {
                newFilt[j] = pFilt[i];
                newFilt[j++].w = 1.0;
                currWeight -= 1.0/(NPART-DUAL_NPART);
            }
        }
    }

    pFilt = newFilt;
    drawParticles(pFilt,PARTICLE_RADIUS,COLOR);

    // Build new prior
    for (int i = 0; i < NPART; i++)
    {
        prior_data[i].vars[0] = pFilt[i].x;
        prior_data[i].vars[1] = pFilt[i].y;
        prior_data[i].vars[2] = pFilt[i].a;
        prior_data[i].vars[3] = pFilt[i].w;
    }
    prior_tree = build_tree(0, 2, NPART, prior_data);
    ofstream priorfile("./priortree.log");
    print_tree(prior_tree, 0, 3, priorfile);
  }  // end of while (localizing) loop



  KI.Off();                 /* reset terminal settings              */

  disconnect_robot(1);      /* gracefully disconnect from the robot */
}