/**
 Implementation of the RRT confguration-space exploration routine by Steve LaValle.
 http://msl.cs.uiuc.edu/rrt/
 
 Clicking restarts the tree construction, the height of the mouse changes the "delta q" value
 which (when close to 1) biases the algorithm towards growing new tree branches close to
 the existing tree.
*/

float interpolateAmount = 0.66;

PVector[] points;
int[] parents;


int numPoints = 2000;
int curPoint;

int fontHeight = 18;
void setup() {
  size(640,480);
  textFont(createFont("Helvetica", fontHeight), fontHeight);
  dataSetup();
}

void dataSetup() {
  points = new PVector[numPoints];
  points[0] = new PVector(width/2, height/2);
  curPoint = 1;
  parents = new int[numPoints];
  parents[0] = 0;
}

void draw() {
  background(255);
  stroke(0);
  for(int i = 0; i < curPoint; i++) {
    PVector a = points[i], b = points[parents[i]];
    line(a.x,a.y,b.x,b.y);
  }
  
  
  String text = "Interpolate from random point to nearest neighbor by "+interpolateAmount;
  int border = 5;
  noStroke(); fill(255, 255, 255, 200);
  rect(20-border, 30-border-fontHeight, textWidth(text)+border*2, fontHeight+border*2);
  fill(0);
  text(text, 20, 30);
  addPoints(2);
}

// here's the meat
void addPoints(int num) {
  while(curPoint < numPoints && num > 0) {
    // random point in configuration space
    PVector p = new PVector(random(0,width), random(0,height));
    float closestDist = 100000f;
    int closestIndex = -1;
    // find closest existing point
    for(int i = 0; i < curPoint; i++) {
      PVector other = points[i];
      float d = dist(other.x, other.y, p.x, p.y);
      if(d < closestDist) {
        closestIndex = i;
        closestDist = d;
      }
    }
    // move towards closest point by some amount
    p = lerp(p, points[closestIndex], interpolateAmount);
    
    // record an edge between the closest point and the inerpolated point
    parents[curPoint] = closestIndex;
    points[curPoint] = p;
    
    curPoint++;
    num--;
  }
}

PVector lerp(PVector a, PVector b, float amount) {
  return new PVector(lerp(a.x,b.x, amount), lerp(a.y,b.y, amount));
}

void mousePressed() {
  dataSetup();
  interpolateAmount = mouseY/(float)height;
}