//MODIFIED FROM:

/**Copyright (c) 2009, Ioannis (Yiannis) Chatzikonstantinou, All rights reserved.
 **http://prototy.blogspot.com
 * 
 * Redistribution and use in source and binary forms, with or without modification, 
 * are permitted provided that the following conditions are met:
 * 	- Redistributions of source code must retain the above copyright 
 * notice, this list of conditions and the following disclaimer.
 * 	- Redistributions in binary form must reproduce the above copyright 
 * notice, this list of conditions and the following disclaimer in the documentation 
 * and/or other materials provided with the distribution.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY 
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 
 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 
 * OF SUCH DAMAGE.
 */

//This software uses code portions of Daniel Shiffman's Flocking example, included
//as an example in the original distribution of Processing
//http://www.shiffman.net/
//http://www.processing.org

//Control Parameters
//Affect behavior
float accNorm=1.20f;
float F=0.85f;
float separationDist = 20.00f;
float cohesionDist = 210.00f;
float cohMag=0.16f;
float atrLim=1.10f;
float sepMag=38.56f;

// The Boid class

class Boid {

  PVector loc;
  PVector vel;
  PVector pre;
  PVector temp;
  PVector acc;
  float r;
  float maxforce;    // Maximum steering force
  float maxspeed;    // Maximum speed

  node3 targetNode;
  node3 sourceNode;

  ArrayList course = new ArrayList(100); //Holds last positions for plotting course

  Boid(PVector l, float ms, float mf, node3 sourceNodein, node3 targetNodein) 
{
    loc = l.get();
    pre = l.get();
    vel = new PVector(random(-1,1),random(-1,1));
    temp = new PVector(0,0);
    acc = new PVector(0,0);
    r = 2.0;
    maxspeed = ms;
    maxforce = mf;
    sourceNode = sourceNodein;
    targetNode = targetNodein;
  }

  void run() {
    acc.add(separate(boids)); // Short-Range Separation & Long-Range Cohesion
    acc.add(align(boids));
    acc.add(attraction());    // Attraction
    acc.normalize();
    //acc.add(new PVector(random(.8)-.4,random(.8)-.4,random(.8)-.4));
    acc.mult(accNorm);        // Acceleration Limiter
    flock(boids);
    update();
  }
  
    // We accumulate a new acceleration each time based on three rules
  void flock(ArrayList boids) {
    PVector sep = separate(boids);   // Separation
    PVector ali = align(boids);      // Alignment
    PVector coh = cohesion(boids);   // Cohesion
    // Arbitrarily weight these forces
    sep.mult(1.5);
    ali.mult(1.0);
    coh.mult(1.0);
    // Add the force vectors to acceleration
    acc.add(sep);
    acc.add(ali);
    acc.add(coh);
  }

  // Method to update location
  // Derived from the Verlet Integration Method
  void update() {
    temp=loc.get();
    loc.mult(1+F);
    pre.mult(F);
    loc.sub(pre);
    loc.add(acc);
    pre=temp;
    acc.mult(0);
  }

  // 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 trail of the course the boid has followed.
    course.add(loc.get());
    if (course.size()>400) {
      course.remove((int)random(25));
    }
    if (boids.size()>600) {
      boids.remove((int)random(25));
    }
    stroke(255,50);
    strokeWeight(1);
    noFill();
    PVector p;
    beginShape();
    for (int i=0,j=course.size();i<j;i++) {
      p=(PVector)course.get(i);
      vertex(p.x,p.y,p.z);
    }
    endShape();
  }

  // Separation & Cohesion
  PVector separate (ArrayList boids) {
    float d;
    PVector dV;
    PVector sum=new PVector(0,0,0);
    for (int i = 0 ; i < boids.size(); i++) {
      Boid b=(Boid)boids.get(i);
      dV=b.loc.get();
      dV.sub(loc);
      d=dV.mag();
      if (d<separationDist) {
        PVector dV2 = dV.get();
        dV2.mult((separationDist-d)*sepMag/separationDist+random(1));
        sum.sub(dV2);
      }
      if (d<cohesionDist && b.sourceNode!=sourceNode) {
        PVector dV2 = dV.get();
        dV2.mult((cohesionDist-d)*cohMag/cohesionDist);
        sum.add(dV2);
      }
    }
    sum.normalize();
    return(sum);
  }
  
    // Alignment
  // For every nearby boid in the system, calculate the average velocity
  PVector align (ArrayList boids) {
    float neighbordist = 50.0;
    PVector steer = new PVector(0,0,0);
    int count = 0;
    for (int i = 0 ; i < boids.size(); i++) {
      Boid other = (Boid) boids.get(i);
      float d = PVector.dist(loc,other.loc);
      if ((d > 0) && (d < neighbordist)) {
        steer.add(other.vel);
        count++;
      }
    }
    if (count > 0) {
      steer.div((float)count);
    }

    // As long as the vector is greater than 0
    if (steer.mag() > 0) {
      // Implement Reynolds: Steering = Desired - Velocity
      steer.normalize();
      steer.mult(maxspeed);
      steer.sub(vel);
      steer.limit(maxforce);
    }
    return steer;
  }
  
  // Cohesion
  // For the average location (i.e. center) of all nearby boids, calculate steering vector towards that location
  PVector cohesion (ArrayList boids) {
    float neighbordist = 50.0;
    PVector sum = new PVector(0,0,0);   // Start with empty vector to accumulate all locations
    int count = 0;
    for (int i = 0 ; i < boids.size(); i++) {
      Boid other = (Boid) boids.get(i);
      float d = PVector.dist(loc,other.loc);
      if ((d > 0) && (d < neighbordist)) {
        sum.add(other.loc); // Add location
        count++;
      }
    }
    if (count > 0) {
      sum.div((float)count);
      return steer(sum,false);  // Steer towards the location
    }
    return sum;
  }

  // Attraction
  PVector attraction () {
    PVector target=PVector.sub(targetNode.loc,loc);
    target.limit(atrLim);
    return target;
  }
}