/**
  <h3>Simulation of position estimation by succesive distance measurements<br/>
  to known beacons, using an Extended Kalman Filter.</h3>

  <p>The blue dots are <a href="http://www.flickr.com/photos/marcboon/4606331929/in/pool-marcboon">Cricket beacons</a>.
  They are transmitting ultrasound blips in turn at 10 blips/s, synchronized by a radio signal.
  The transmissions are represented by red lines.<br/>
  The white sphere represents the blimp. It listens to the beacons while moving in the space, and tries to
  estimate its current position using an Extend Kalman Filter.<br/>
  The red dot represents the estimated position.</p>
  
  <p>See <a href="../SCAAT.pdf" type="application/pdf">SCAAT.pdf</a> for a description of the used algorithm.<br/>
  See <a href="../BeaconRing.pdf" type="application/pdf">BeaconRing.pdf</a> for a description of the Cricket system.</p>

  <p>Change view: left button mouse drag<br/>
  Zoom: mouse wheel or right button mouse drag<br/>
  Pan: middle button mouse drag<br/>
  Top view: double click<br/>
  Step: space<br/>
  Run: enter<br/>
  </p>
 */

import peasy.*; // http://mrfeinberg.com/peasycam/

final static int SCALE = 20;
final static int GRID = 20;
final static int FPS = 20;
final static int TBEEP = 100; // ms
final static int RMS_AVG = 10;

Beacon[] beacons;
Blimp blimp;
int numBeacons = 6;
int beacon = 0;
long frame = 0;
int step = 0;
boolean run = true;
boolean randomBeacon = false;
boolean showGraph = false;
float range = 0;
float z = 10;
float speed = 0;
float var = 10;
float eta = 0.1;
float mu = 0.1;
float zeta = 0.1;

PeasyCam cam;
GraphXT graph;

void showGraph(boolean show)
{
  showGraph = show;
}

void setBeacons(int n)
{
  println("Beacons: " + n);
  numBeacons = n;
  setup();
}

void setRange(float val)
{
  println("Beacon range: " + val);
  range = val;
  setup();
}

void setRandomBeacon(boolean random)
{
  println("Random beacon: " + random);
  randomBeacon = random;
  setup();
}

void setSpeed(float val)
{
  println("Speed: " + val);
  speed = val;
  setup();
}

void setHeight(float val)
{
  println("Height: " + val);
  z = val;
  setup();
}

void setInitialVariance(float val)
{
  println("Initial variance: " + val);
  var = val;
  setup();
}

void setProcessNoise(float val)
{
  println("Process noise: " + val);
  eta = val;
  setup();
}

void setMeasurementNoise(float val)
{
  println("Measurement noise: " + val);
  mu = val;
  setup();
}

void setActualNoise(float val)
{
  println("Actual noise: " + val);
  zeta = val;
  setup();
}

void setup()
{
  size(600, 600, P3D);
  frameRate(FPS);
  textMode(SCREEN);
  cam = new PeasyCam(this, width);
  cam.setMinimumDistance(width);  
  cam.setMaximumDistance(2*width);
  cam.rotateX(-1.6);
  cam.pan(0, -width/3);
  createBeacons(numBeacons, GRID / 2);
  blimp = new Blimp(-3, 5, z, 1, color(255, 255, 255, 200), var, eta, mu);
  frame = beacon = step = 0;
  run = true;
  graph = new GraphXT(width, 101, 40, 80, 10);
  graph.add("residual", -5, 5, color(0, 200, 0));
  graph.add("rmserror", 0, 10, color(200, 0, 0));
}

void draw()
{
  background(40);
  lights();
  scale(SCALE);
  drawGrid();
  if(run)
  {
    ++frame;
    blimp.move();
  }
  if(frame % (FPS * TBEEP / 1000) == 0)
  {
    if(run)
    {
      if(randomBeacon && numBeacons > 2)
      {
        int previous = beacon;
        do beacon = (int)random(numBeacons);
        while(beacon == previous);
      }
      else
      {
        beacon = ++beacon % numBeacons;
      }
    }
    if(range == 0 || blimp.dist(beacons[beacon]) < range)
    {
      beacons[beacon].lineTo(blimp, color(200, 0, 0));
      if(run)
      {
        blimp.track(beacons[beacon]);
      }
      beacons[beacon].lineTo(blimp.pos, color(100));
    }
    if(run)
    {
      graph.scroll();
    }
  }
  if(run && step > 0 && frame % step == 0)
  {
    run = false;
    blimp.ekf.trace();
  }
  drawBeacons();
  blimp.draw();
  if(showGraph)
  {
    graph.draw(0, 0);
  }
  else
  {
    blimp.stats();
  }
}

void keyPressed()
{
  if(key == ENTER || key == RETURN)
  {
    step = 0;
    run = true;
  }
  else if(key == ' ')
  {
    step = TBEEP * FPS / 1000;
    run = true;
  }
  else if(!run && key == CODED && keyCode == RIGHT)
  {
    step = 1;
    run = true;
  }
}

void drawGrid()
{
  stroke(60);
  for(int x = -GRID/2; x <= GRID/2; x++)
  {
    line(x, -GRID/2, 0, x, GRID/2, 0);
  }
  for(int y = -GRID/2; y <= GRID/2; y++)
  {
    line(-GRID/2, y, 0, GRID/2, y, 0); 
  }
}

void createBeacons(int n, float r)
{
  beacons = new Beacon[n];
  float theta = 2 * PI / n;
  float phi = 0;
  for(int i = 0 ; i < beacons.length; i++, phi += theta)
  {
    float x = cos(phi) * r;
    float y = sin(phi) * r;
    beacons[i] = new Beacon(x, y, 0.5, 0.5, color(0, 50, 200));
  }
}

void drawBeacons()
{
  for(int i = 0; i < beacons.length; i++)
  {
    beacons[i].draw();
  }

}

// generate gaussian white noise sample
// see http://en.wikipedia.org/wiki/Marsaglia_polar_method
float gaussianNoise(float mean, float variance)
{
  float x, y, s;
  do
  {
    x = random(2) - 1;
    y = random(2) - 1;
    s = x * x + y * y;
  }
  while(s >= 1);
  x *= sqrt(-2 * log(s) / s);
  return (mean + sqrt(variance) * x);
}

class Sphere extends PVector
{
  float r;
  color c;
  
  Sphere(float x, float y, float z, float r, color c)
  {
    super(x, y, z);
    this.r = r;
    this.c = c;
  }
  
  void draw()
  {
    fill(c);
    noStroke();
    pushMatrix();
    translate(x, y, z);
    sphere(r);
    popMatrix();
  }
}

class Beacon extends Sphere
{
  Beacon(float x, float y, float z, float r, color c)
  {
    super(x, y, z, r, c);
  }
  
  void lineTo(PVector p, color c)
  {
    stroke(c);
    line(x, y, z, p.x, p.y, p.z);
  }
}

class Blimp extends Sphere
{
  Sphere pos;
  float dt, d;
  EKF ekf;
  float error = 0;
  int avg = 0;
  
  Blimp(float x, float y, float z, float r, color c, float var, float eta, float mu)
  {
    super(x, y, z, r, c);
    ekf = new EKF(var, eta, mu);
    pos = new Sphere(ekf.getX(), ekf.getY(), ekf.getZ(), r / 4, color(240, 0, 0));
    ekf.trace();
  }

  void draw()
  {
    stroke(color(0, 240, 0));
    line(pos.x, pos.y, pos.z, pos.x + ekf.getdX()/4, pos.y + ekf.getdY()/4, pos.z + ekf.getdZ()/4);
    super.draw();
    pos.draw();
  }  

  void move()
  {
    float now = frame / float(FPS);
    // update blimp position
    if(speed > 0)
    {
      float a = 2 * speed * now / GRID;
      x = GRID/2 * sin(a);
      y = GRID/2 * cos(a * 1.6);
    }
    // update estimated position
    pos.x = ekf.getX(now);
    pos.y = ekf.getY(now);
    pos.z = ekf.getZ(now);
  }
  
  void track(Beacon beacon)
  {
    // calculate dt
    float now = frame / float(FPS);
    dt = now - ekf.t;
    // estimate current position based on previous position and dt
    ekf.predict(dt);
    // simulate a distance measurement (actual distance to beacon plus noise)
    d = dist(beacon) + gaussianNoise(0, zeta);
    // kalman filter innovation
    ekf.innovate(d, beacon.x, beacon.y, beacon.z);
    // update rms error
    if(avg < RMS_AVG) avg++;
    error = (error * (avg - 1) + ekf.getResidual() * ekf.getResidual()) / avg;
    // plot residual and rms error
    graph.plot("residual", ekf.getResidual());
    graph.plot("rmserror", sqrt(error));
    // adjust position
    pos.x = ekf.getX();
    pos.y = ekf.getY();
    pos.z = ekf.getZ();
  }
  
  void stats()
  {
    float now = frame / float(FPS);
    fill(255);
    text("t", 10, 20);
    text(nf(now, 1, 2), 30, 20);
    text("dt", 10, 40);
    text(nf(dt, 1, 2), 30, 40);
    text("d", 10, 60);
    text(beacon, 16, 60);
    text(nf(d, 1, 2), 30, 60);
    text("e", 10, 80);
    text(nf(ekf.getResidual(), 1, 2), 30, 80);
    text("v", 10, 100);
    text(nf(ekf.getVelocity(), 1, 2), 30, 100);
  }

}