class Creature
{
  int id;
  
  /*** MOVEMENT ***/
  Position p;
  Position d;
  float mass;
  Vector v;
  Vector a;
  Vector s;
  float max_force = .5;
  float max_speed = .25;
  float max_turn = radians(10);
  
  float wander = 0;
  float wanderAmt = radians(10);
  float wanderStr = 30;
  
  /*** ATTRIBUTES ***/
  float diameter;
  color c;
  
  /*** STATE ***/
  boolean alive = true;
  boolean selected = false;
  boolean matched = false;

  Creature(int $id)
  {
    id = $id;
  }
  
  void init()
  {
    p = new Position(random(width), random(height));
    mass = 10;
    v = new Vector(random(TWO_PI), .5);
    a = new Vector(v);
    s = new Vector(v);
    diameter = 10;
    c = colors[(int)random(colors.length)];
  }
  
  void set(Position $p, float $d, color $c)
  {
    p = $p;
    mass = $d;
    diameter = $d;
    v = new Vector(random(TWO_PI), .5);
    a = new Vector(v);
    s = new Vector(v);
    c = $c;
  }
  
  void be()
  {
    if (alive){
      look();
      move();
      draw();
    }
  }
  
  /*** OBSERVATION ***/
  void look()
  {
    selected = color_match();
  }
  
  boolean color_match()
  {
    //color cp = get((int)p.x, round(height-p.y));
    color cp = get((int)p.x, (int)p.y);
    //return (red(cp) == red(shapeColor) && green(cp) == green(shapeColor) && blue(cp) == blue(shapeColor)+1);
    return (red(cp) == red(shapeColor) && green(cp) == green(shapeColor) && blue(cp) == blue(shapeColor));
  }
  
  void matched(float x, float y)
  {
    println("matched!");
    matched = true;
    d = new Position(x, y);
  }
  
  /*** MOVEMENT***/
  void move()
  {
    boundaries();
    steer();
    go();
  }
  
  void boundaries()
  {
    if (p.x < 0-diameter) { p.x = width+diameter; } else
    if (p.x > width+diameter) { p.x = 0-diameter; } else
    if (p.y < 0-diameter) { p.y = height+diameter; } else
    if (p.y > height+diameter) { p.y = 0-diameter; }
  }
  
  void steer()
  {
    s.set(v);
    if (!matched) {
      s.add(wander());
    } else {
      s.add(seek());
    }
  }
  
  Vector wander()
  {
    wander += random(-1*wanderAmt, wanderAmt);
    wander = normalize(wander);
    
    Position center = new Position(p.x + cos(v.a)*wanderStr*1.3, p.y + sin(v.a)*wanderStr*1.3);
    Position dest = new Position(center.x + cos(wander)*wanderStr, center.y + sin(wander)*wanderStr);
    Vector steer = new Vector();
    steer.set(p, dest);
    return steer;
  }
  
  Vector seek()
  {
    max_force = 3; 
    max_speed = 3;
    Vector desired = new Vector();
    desired.set(p, d);
    if (dist(p, d) < 3) { joinCreatures = true; }
    return desired;
  }
  
  boolean collision()
  {
    int friend = find_closest_neighbor();
    Position coll = find_collision_point(friend);
    if (dist(p, coll) < diameter*5 && is_in_front(coll)) {
      return true;
    } else {
      return false;
    }
  }
  
  Vector avoid()
  {
    int friend = find_closest_neighbor();
    Position coll = find_collision_point(friend);
    float turn = (atan2(p, crtrs[friend].p) > atan2(p, coll)) ? -radians(5) : radians(5);
    line(p.x, p.y, p.x + cos(turn)*40, p.y + sin(turn)*40);
    return new Vector(turn, 1);
  }
  
  int find_closest_neighbor()
  {
    float distance = width*height;
    int friend = -1;
    for (int i = 0; i < numCrtrs; i++) {
      if (crtrs[i] != this && dist(p, crtrs[i].p) < distance) {
        friend = i;
        distance = dist(p, crtrs[i].p);
      }
    }
    return friend;
  }
  
  Position find_collision_point(int f)
  {
    Position future = new Position(p.x + cos(v.a)*10, p.y + sin(v.a)*10);
    float m1 = (p.y - future.y) / (p.x - future.x);
    float b1 = p.y - m1 * p.x;
    
    future.set(crtrs[f].p.x + cos(crtrs[f].v.a)*10, crtrs[f].p.y + sin(crtrs[f].v.a)*10);
    float m2 = (crtrs[f].p.y - future.y) / (crtrs[f].p.x - future.x);
    float b2 = crtrs[f].p.y - m2 * crtrs[f].p.x;
    
    return new Position((b2-b1)/(m1-m2), m1*(b2-b1)/(m1-m2)+b1);
  }
  
  boolean is_in_front(Position p1)
  {
    return ( abs(atan2(sin(v.a), cos(v.a)) - atan2(p, p1)) < HALF_PI );
  }
  
  void go()
  {
    s.m = constrain(s.m, 0, max_force);
    a.m = s.m/mass;
    v.m = constrain(v.m+a.m, 0, max_speed);
    v.a = s.a;
    p.add(v);
  }
  
  /*** DRAWING ***/
  void draw()
  {
    // body
    if (selected) {
      strokeWeight(3);
      stroke(255, 0, 92);
    } else if (matched) {
      strokeWeight(3);
      stroke(0, 255, 92);
    } else {
      noStroke();
    }
    fill(c);
    pushMatrix();
    translate(p.x, p.y);
    rotate(v.a - HALF_PI);
    beginShape(POLYGON);
    vertex(diameter/2, -diameter/2);
    vertex(0, diameter/2);
    vertex(-diameter/2, -diameter/2);
    endShape();
    popMatrix();
    
    // orientation
    strokeWeight(1);
    stroke(0);
    line(p.x, p.y, p.x+cos(v.a)*diameter, p.y+sin(v.a)*diameter);
    
    // steering
    stroke(0,0,0);
    line(p.x, p.y, p.x+cos(s.a+PI)*s.m, p.y+sin(s.a+PI)*s.m);
  }
  
  void kill()
  {
    alive = false;
    p.set(-1000, -1000);
    v.set(0,0);
    matched = false;
    selected = false;
  }
}