// Path Following
// Daniel Shiffman <http://www.shiffman.net>
// The Nature of Code, Spring 2009

// Boid class

class Boid {

  // All the usual stuff
  PVector loc;
  PVector vel;
  PVector acc;
  
  
  float r;
  float maxforce;    // Maximum steering force
  float maxspeed;    // Maximum speed

    // Constructor initialize all values
  Boid( PVector l, float ms, float mf) {
    loc = l.get();
    r = 4.0f;
    maxspeed = ms;
    maxforce = mf;
    acc = new PVector(0,0);
    vel = new PVector(maxspeed,0);
  }

  // Main "run" function
  public void run() {
    update();
    borders();
    render();
  }

  /*
  void applyForce(PVector f) {
   acc.add(f);
   }
   */

  // This function implements Craig Reynolds' path following algorithm
  // http://www.red3d.com/cwr/steer/PathFollow.html
  void follow(Path p) {


    // Predict location 25 (arbitrary choice) frames ahead
    PVector predict = vel.get();
    predict.normalize();
    predict.mult(25);
    PVector predictLoc = PVector.add(loc, predict);

    // Draw the predicted location
    if (debug) {
      fill(0);
      stroke(0);
      //stroke(255,255,0);
      line(loc.x,loc.y,predictLoc.x, predictLoc.y);
      fill(0,255,0);
      stroke(0);
      ellipse(predictLoc.x, predictLoc.y,4,4);
    }

    // Now we must find the normal to the path from the predicted location
    // We look at the normal for each line segment and pick out the closest one
    PVector target = null;
    PVector dir = null;
    float record = 1000000;  // Start with a very high record distance that can easily be beaten

    // Loop through all points of the path
    for (int i = 0; i < p.points.size()-1; i++) {

      // Look at a line segment
      PVector a = (PVector) p.points.get(i);
      PVector b = (PVector) p.points.get(i+1);

      // Get the normal point to that line
      PVector normal = getNormalPoint(predictLoc,a,b);
      // Check if normal is on line segment
      float da = PVector.dist(normal,a);
      float db = PVector.dist(normal,b);
      PVector line = PVector.sub(b,a);
      // If it's not within the line segment, consider the normal to just be the end of the line segment (point b)
      if (da + db > line.mag()+1) {
        normal = b.get();
      }
      //println(normal.x + "\t" + normal.y);
      

      // How far away are we from the path?
      float d = PVector.dist(predictLoc,normal);
      //PVector normalLine = PVector.sub(predictLoc, normal); #MLF, 10-13-10
      //line(normal.x, normal.y, normalLine.x, normalLine.y);  #MLF, 10-13-10
      //println(normal.x+ "\t" + normal.y); //#MLF, 10-13-10
      // Did we beat the record and find the closest line segment?
      if (d < record) {
        record = d;
        // If so the target we want to steer towards is the normal
        target = normal;

        // Look at the direction of the line segment so we can seek a little bit ahead of the normal
        dir = line;
        dir.normalize();
        // This is an oversimplification
        // Should be based on distance to path & velocity
        dir.mult(10);
        getNormalX(normal.x);
      }
    }

    // Draw the debugging stuff
    if (debug) {
      // Draw normal location
      //fill(0);
      fill(0,0,255);
      //noStroke();
      stroke(255,0,0);
      line(predictLoc.x,predictLoc.y,target.x,target.y);  //Normal to Path at Predicted Location
      noStroke();
      ellipse(target.x,target.y,4,4);
      //println(target.x + "\t" + target.y);

      stroke(0);
      fill(0);
      // Draw actual target (red if steering towards it)
      // MF: I think it's red if outside of boundaries [10/13/10].
      //line(predictLoc.x,predictLoc.y,target.x,target.y);
      if (record > p.radius) fill(255,0,0);
      noStroke();
      ellipse(target.x+dir.x, target.y+dir.y, 8, 8);
    }

    // Only if the distance is greater than the path's radius do we bother to steer
    if (record > p.radius) {
      target.add(dir);
      seek(target);
    }
  }


  float getNormalX(float x) {
    println(x);
    return x;
  }

  // A function to get the normal point from a point (p) to a line segment (a-b)
  // This function could be optimized to make fewer new Vector objects
  PVector getNormalPoint(PVector p, PVector a, PVector b) {
    // Vector from a to p
    PVector ap = PVector.sub(p,a);
    // Vector from a to b
    PVector ab = PVector.sub(b,a);
    ab.normalize(); // Normalize the line
    // Project vector "diff" onto line by using the dot product
    ab.mult(ap.dot(ab));
    PVector normalPoint = PVector.add(a,ab);
    return normalPoint;
  }


  // Method to update location
  void update() {
    // Update velocity
    vel.add(acc);
    // Limit speed
    vel.limit(maxspeed);
    loc.add(vel);
    // Reset accelertion to 0 each cycle
    acc.mult(0);
  }

  void seek(PVector target) {
    acc.add(steer(target,false));
  }

  void arrive(PVector target) {
    acc.add(steer(target,true));
  }

  // A method that calculates a steering vector towards a target
  // Takes a second argument, if true, it slows down as it approaches the target
  PVector steer(PVector target, boolean slowdown) {
    PVector steer;  // The steering vector
    PVector desired = PVector.sub(target,loc);  // A vector pointing from the location to the target
    float d = desired.mag(); // Distance from the target is the magnitude of the vector
    // If the distance is greater than 0, calc steering (otherwise return zero vector)
    if (d > 0) {
      // Normalize desired
      desired.normalize();
      // Two options for desired vector magnitude (1 -- based on distance, 2 -- maxspeed)
      if ((slowdown) && (d < 100.0f)) desired.mult(maxspeed*(d/100.0f)); // This damping is somewhat arbitrary
      else desired.mult(maxspeed);
      // Steering = Desired minus Velocity
      steer = PVector.sub(desired,vel);
      steer.limit(maxforce);  // Limit to maximum steering force
    } 
    else {
      steer = new PVector(0,0);
    }
    return steer;
  }

  void render() {
    // Draw a triangle rotated in the direction of velocity
    float theta = vel.heading2D() + PApplet.radians(90);
    int a = 20; //alpha
    //fill(175);

    //MF: FIND A SIMPLER WAY TO CODE THESE COLORS
    fill(236,233,154,a);  //yellow

    int count = millis()/1000;

    if (count>7) {
      fill(167,208,204,a);  //blue
    }  

    if (count>14) {
      fill(237,93,93,a);  //red
    } 

    if (count>21) {
      fill(180,208,159,a);  //green
    } 

    if (count>28) {
      fill(255,255,255,a);  //green
    } 


    //println(count);
    //stroke(0);
    noStroke();
    pushMatrix();
    translate(loc.x,loc.y);
    rotate(theta);
    /* beginShape(PConstants.TRIANGLES);
     vertex(0, -r*2);
     vertex(-r, r*2);
     vertex(r, r*2);
     endShape();
     */
    ellipse(0,0,10,10);
    //ellipse(0,0,5,5);

    popMatrix();
  }

  // Wraparound
  void borders() {
    if (loc.x < -r) loc.x = width+r;
    //if (loc.y < -r) loc.y = height+r;
    if (loc.x > width+r) loc.x = -r;
    //if (loc.y > height+r) loc.y = -r;
  }
}