class Boid {
  Vector3D loc,vel,acc;
  float r,maxspeed,maxforce;
  float wandertheta;
  colBox boundingBox;

  Boid(Vector3D loc_, float maxspeed_, float maxforce_) {
    acc = new Vector3D(0,0);
    vel = new Vector3D(random(-1,1),random(-1,1));
    loc = loc_.copy();
    r = 10.0f;
    wandertheta = 0.0f;  
    maxspeed = maxspeed_;
    maxforce = maxforce_;
    boundingBox = new colBox();
  }

  void run(ArrayList boids) {
   
    calcBox();
    if(drawBoundBox) drawBox();
    if (checkObstacles() != null) {
     // println("not flocking");
      Vector3D target = obsAvoid();
      steerAway(target);
    } else {
     flock(boids);
     wander();
    // println("flocking");
}
    vel.add(acc);
    vel.limit(maxspeed);
    loc.add(vel);
    acc.setXYZ(0,0,0);
    borders();

    render();
  }


  Vector3D checkObstacles() {
    Vector3D critical = null;
    for (int i=0; i < obstacles.length; i++) {

      if(boundingBox.intersect(obstacles[i])) {
       // println("critical");
        critical = obstacles[i].loc;

      }
    }
    return critical;     
  }

  Vector3D obsAvoid() {
    Vector3D target = checkObstacles();
    return target;
  }  

  void calcBox() {
    float theta = vel.heading2D() + radians(90);
    // boundingBox.setBox(theta, loc);
    boundingBox.setLine(theta, loc, vel);
    // maybe separate setValues and draw function in colbox
  } 
  
   void drawBox() {
    float theta = vel.heading2D() + radians(90);
    // boundingBox.setBox(theta, loc);
    boundingBox.drawLine(theta, loc, vel);
    // maybe separate setValues and draw function in colbox
  } 

  void flock(ArrayList boids) {

    Vector3D separation = separate(boids);
    Vector3D align      = align(boids);
    Vector3D coherence  = cohesion(boids);

    separation.mult(0.7f);
     align.mult(0.8f);
     coherence.mult(1.7f);
    
    acc.add(separation);
    acc.add(align);
    acc.add(coherence);
  }


  void wander() {
    float wanderRadius    = 16.0f;
    float wanderDistance  = 60.0f;
    float change          = 1.0f;
    wandertheta += random(-change,change);

    Vector3D circleloc = vel.copy();
    circleloc.normalize();
    circleloc.mult(wanderDistance);
    circleloc.add(loc);

    Vector3D circleOffset = new Vector3D(wanderRadius*cos(wandertheta),wanderRadius*sin(wandertheta));
    Vector3D target       = Vector3D.add(circleloc,circleOffset);
    acc.add(steer(target));

  }

  Vector3D steer(Vector3D target) {
    Vector3D steer;
    Vector3D desired = Vector3D.sub(target,loc);
    float d = desired.magnitude();
    desired.normalize();
    desired.mult(maxspeed);
    steer = Vector3D.sub(desired,vel);
    steer.limit(maxforce);
    return steer;
  }

  void steerAway(Vector3D target) {
    Vector3D steer;
    Vector3D desired = Vector3D.sub(loc,target);
    desired.normalize();
     float d = Vector3D.distance(loc,target);
     d = d/100;
     d = 1-d;
     d = d*5;
     //println(d);
     desired.mult(maxspeed*20*d);
    
    steer = Vector3D.sub(desired,vel);
   

    steer.limit(maxforce*2*d);
    //  println(steer.x);
    acc.add(steer);
  }

  Vector3D separate (ArrayList boids) {
    float desiredseparation = 25.0f;
    Vector3D sum = new Vector3D(0,0,0);
    int count = 0;
    // for every boid in the system, check if it is too close
    for (int i = 0; i<boids.size();i++) {
      Boid other = (Boid)boids.get(i);
      float d = Vector3D.distance(loc,other.loc);
      // if the distance is greater than 0 and less than an arbitrary amount
      if((d>0) && (d < desiredseparation)) {
        // calculate vector pointing away from neighbour
        Vector3D diff = Vector3D.sub(loc,other.loc);
        diff.normalize();
        diff.div(d);
        sum.add(diff);
        count++;
      }
    }
    if(count>0) {
      sum.div((float)count);
    }
    return sum;
  }

  // alignment
  Vector3D align(ArrayList boids) {
    float neighbordist = 50.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 < neighbordist)) {
        sum.add(other.vel);
        count++;
      }
    }
    if(count > 0) {
      sum.div((float)count);
      sum.limit(maxforce);
    }
    return sum;
  }

  Vector3D cohesion (ArrayList boids) {
    float neighbordist = 50.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 < neighbordist)) {
        sum.add(other.loc);
        count++;
      }
    }
    if(count > 0) {
      sum.div((float)count);
      return steer(sum);
    }
    return sum;
  }



  void render() {

    fill(255);
    stroke(255);
    Vector3D lineh =  vel.copy();
    lineh.normalize();
    lineh.mult(30);
    lineh.add(loc);
    pushMatrix();
   // line(loc.x,loc.y,lineh.x,lineh.y);
    ellipseMode(CENTER);
    ellipse(loc.x,loc.y,r,r);
    popMatrix();
  }

  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;
  }




}