/*
  Our Boid objects build on our work with Particle objects. We need to add
  a few methods to create the larger effects we see in a Boid system.
*/

public class Boid extends Particle {

    // Keep track of the direction we wander
    float wanderTheta;
    
    // Maximum speed to move
    float maxSpeed;
    
    // Maximum acceleration force
    float maxForce;
    
    public Boid(Vector3D loc_, float maxSpeed_, float maxForce_) {
    
        // call the Particle constructor
        super(loc_);
        
        // the size of our boid
        r = 4.0f;
        
        // The start direction to wander in
        wanderTheta = 0.0f;
        
        // Store our maximum values
        maxSpeed = maxSpeed_;
        maxForce = maxForce_;
    }
    
    /*
      Our familiar simulate method, with one change; the borders() call
      allows our Boid to wrap around the edges of the screen.
      
      In addition, to allow for a flocking simulation we run the
      flock() method
    */
    public void simulate(ArrayList boids) {
    
        flock(boids);
        update();
        borders();
        render();
    }
    
    /*
        With the flock method we apply the three principles
        of flocking to derive the forces affecting this boid.
        
        Separation - We want to maintain distance between ourself and our neighbors
        Alignment - We want to try and move in the same direction as everyone else
        Cohension - We want to remain close to all of our neighbors
    */
    public void flock(ArrayList boids) {
    
        Vector3D sep = separate(boids);
        Vector3D ali = align(boids);
        Vector3D coh = cohesion(boids);
        
        // We want to be able to control how much
        // each of these forces affect us, so we can
        // fine tune our simulation. To do this we'll
        // multiply each force by a certain amount
        sep = sep.multiply(10.0f);
        ali = ali.multiply(1.0f);
        coh = coh.multiply(2.0f);
        
        // add the forces to accelaration
        accel = accel.add(sep);
        accel = accel.add(ali);
        accel = accel.add(coh);
    }
    
    /*
        The separation step of flocking. Check for nearby boids,
        and try and move away from them
    */
    public Vector3D separate(ArrayList boids) {
    
        // how far apart do we want our boids
        float desiredSpacing = 50.0f;
        
        // The positions and directional force of our neighbors
        Vector3D sum = new Vector3D(0, 0, 0);
        
        // how many neighbors are too close
        int count = 0;
        
        for (int i = 0; i < boids.size(); i++) {
            
            // Figure out if each boid is too close to us
            Boid other = (Boid)boids.get(i);
            float d = Vector3D.distance(loc, other.loc);
            
            // Is this boid within range of us
            if ( (d > 0) && (d < desiredSpacing) ) {
            
                // get the vector difference between our two locations
                Vector3D diff = Vector3D.subtract(loc, other.loc);
                
                // Get a directional force based upon how close we are to this
                // neighbor boid
                diff.normalize();
                diff = diff.divide(d);
                sum = sum.add(diff);
                count++;
            }
        }
        
        // Return a separation force based on the average separation
        // directional force of all of our neighbors
        if (count > 0) {
            sum = sum.divide((float)count);
        }
        
        return sum;
    }
    
    /*
      Go through all of the boids and attempt to align our direction
      and speed to that of our close neighbors.
    */
    public Vector3D align(ArrayList boids) {
    
        float neighborRange = 100.0f;
        Vector3D sum = new Vector3D(0, 0, 0);
        
        int count = 0;
        for (int i = 0; i < boids.size(); i++) {
            Boid other = (Boid)boids.get(i);
            float d = Vector3D.distance(loc, other.loc);
            
            if ( (d > 0) && (d < neighborRange) ) {
            
                sum = sum.add(other.vel);
                count++;
            }
        }
        
        if (count > 0) {
            
            sum = sum.divide((float)count);
            sum.limit(maxForce);
        }
        
        return sum;
    }
    
    /*
      Figure out the acceleration force towards the center point of
      all of our neighbors.
    */
    public Vector3D cohesion(ArrayList boids) {
    
        float neighborRange = 100.0f;
        Vector3D sum = new Vector3D(0, 0, 0);
        
        int count = 0;
        for (int i = 0; i < boids.size(); i++) {
        
            Boid other = (Boid)boids.get(i);
            float d = Vector3D.distance(loc, other.loc);
            
            if ( (d > 0) && (d < neighborRange) ) {
            
                sum = sum.add(other.loc);
                count++;
            }
        }
        
        if (count > 0) {
            sum = sum.divide((float)count);
            return steer(sum, false);
        }
        
        return sum;
    }
    
    /*
      The update method follows the basic model of Vector motion. We need
      to be careful to reset the acceleration each time.
    */
    public void update() {
    
        vel = vel.add(accel);
        vel.limit(maxSpeed);
        loc = loc.add(vel);
        
        accel.setXYZ(0, 0, 0);
    }
    
    /*
      Using the seek method the boid attempts to move towards a specific
      location.
    */
    public void seek(Vector3D target) {
        accel = accel.add(steer(target, false));
    }
    
    /*
      Arrive is similar to seek, except the boid will slow down and stop when
      it reaches its target destination.
    */
    public void arrive(Vector3D target) {
        accel = accel.add(steer(target, true));
    }
    
    /*
      Sometimes we want our boid to just wander around the screen. Instead of
      randomly choosing a new direction every time, it should attempt a 
      "directed" random wandering. Each time we call wander() we change our
      wanderTheta a small bit, creating random movement somewhat more like the
      random noise from Perlin noise.
    */
    public void wander() {
    
        /*
            The wander method projects a virtual circle in front of the boid, using
            the wanderTheta to determine the point towards which the boid should
            move.
        */
        
        // Radius of the circle
        float wanderRadius = 40.0f;
        
        // Distance the cirlce is offset from the boid
        float wanderDistance = 80.0f;
        
        // Range in which our wanderTheta can change each time
        float wanderChange = 0.5f;
        
        // Adjust our wanderTheta
        wanderTheta += random(-wanderChange, wanderChange);
        
        // start with a copy of the boid velocity
        Vector3D circleLoc = vel.copy();
        
        // normalize and scale to the wanderDistance offset
        circleLoc.normalize();
        circleLoc = circleLoc.multiply(wanderDistance);
        
        // position it relative to the boid location
        circleLoc.add(loc);
        
        // Figure out the wander circle heading
        float actualTheta = wanderTheta + vel.heading2D();
        
        // Determine the vector showing the offset of the point on the circle
        Vector3D circleOffset = new Vector3D(wanderRadius * cos(actualTheta), wanderRadius * sin(actualTheta), 0);

        // figure out the target position of the wander point
        Vector3D target = Vector3D.add(circleLoc, circleOffset);
        
        // get our acceleration towards our wander point
        accel = accel.add(steer(target, false));
    }
    
    /*
        Calculate a steering vector towards a target position. If the second
        argument is true, the acceleration will slowdown as the boid approaches
        the target. If the second argument is false, the boid will not slow down.
    */
    public Vector3D steer(Vector3D target, boolean slowdown) {
    
        // The steering vector
        Vector3D steer;
        
        // A vector pointing from the boid to the target
        Vector3D desiredLoc = Vector3D.subtract(target, loc);
        
        // get the distance to the target
        float d = desiredLoc.magnitude();
        
        // If the distance to the target is greater than zero, calculate a steering
        // force, otherwise the steering force is 0
        if (d > 0) {
        
            // normalize the desired location target
            desiredLoc.normalize();
            
            // figure out if we need to slow down
            if ( (slowdown) && (d < 100.0f)) {
            
                // apply a max speed inversely proportional to how close we are to the target
                desiredLoc = desiredLoc.multiply(maxSpeed * (d / 100.0f));
            } else {
            
                // apply our maximum speed
                desiredLoc = desiredLoc.multiply(maxSpeed);
            }
            
            // get a steering force towards our target
            steer = Vector3D.subtract(desiredLoc, vel);
            
            // limit our acceleration force
            steer.limit(maxForce);
        } else {
        
            // no acceleration
            steer = new Vector3D(0, 0, 0);
        }
        
        return steer;
    }
    
    /*
      Our render method uses popMatrix and pushMatrix to keep track of our
      screen location so we can adjust the screen to make drawing our Boid
      much easier.
    */
    public void render() {
    
        // figure out what direction our boid points in
        float theta = vel.heading2D() + radians(90);
        
        // set the fill color and drawing style
        fill(64, 128, 255);
        stroke(0);
        
        // save our screen position
        pushMatrix();
        
        // translate to the location of our boid
        translate(loc.getX(), loc.getY());
        
        // rotate so we point the screen in the direction our boid
        // points in
        rotate(theta);
        
        // draw a triangle around our boid
        beginShape(TRIANGLES);
        vertex(0, -r * 2);
        vertex(-r, r * 2);
        vertex(r, r * 2);
        endShape();
        
        // retrieve our screen position
        popMatrix();
    }
    
    /*
      Wrap our Boid's position around the screen.
    */
    public void borders() {
    
        if (loc.getX() < -r) loc.setX(width + r);
        if (loc.getY() < -r) loc.setY(height + r);
        if (loc.getX() > width + r) loc.setX(-r);
        if (loc.getY() > height + r) loc.setY(-r);
    }
}