// Swarming Robots

Robot robots[];
boolean running = true;
float ROBOT_SIZE = 10.0;
int NUM_ROBOTS = 100;

void setup() {
  size(600, 600);
  robots = new Robot[NUM_ROBOTS];
  for (int i = 0; i < NUM_ROBOTS; i++) {
    robots[i] = new Robot(robots, i);
  }
  smooth();
}

void draw() {
  if (running) {
    background(255);
    for (int i = 0; i < NUM_ROBOTS; i++) {
      robots[i].paint();
      robots[i].update();
    }
  }
}

void keyPressed() {
  if (key == ' ') {
    running = !running;
  }
}

class Robot {
  float x;
  float y;
  Vector2d vec;
  Point2d pos;
  Robot[] otherRobots;
  int index;
  color fillColor;
  
  Robot(Robot[] otherRobots, int index) {
    pos = new Point2d();
    vec = new Vector2d(2.0);
    this.otherRobots = otherRobots;
    this.index = index;
    fillColor = color(random(255), random(255), random(255));
  }
  
  void paint() {
    pushMatrix();
    translate(pos.x, pos.y);
    rotate(vec.dir());
    fill(fillColor);
    triangle(0.0, 0.0, ROBOT_SIZE, ROBOT_SIZE / 2.0, 0.0, ROBOT_SIZE);
    popMatrix();
  }
  
  void update() {
    pos.add(vec);
    if (mousePressed) {
      Vector2d mouseVec = new Vector2d((float) mouseX - pos.x, (float) mouseY - pos.y);
      float dir = mouseVec.dir();
      if (dir < vec.dir()) {
        vec.setAngle(vec.dir() - PI / 180.0);
      } else if (dir > vec.dir()) {
        vec.setAngle(vec.dir() + PI / 180.0);
      }
    }
  }
}

class Vector2d {
  public float dx;
  public float dy;
  
  Vector2d(float range) {
    dx = random(-range, range);
    dy = random(-range, range);
  }
  
  Vector2d(float x, float y) {
    dx = x;
    dy = y;
  }
  
  void reverseCourse() {
    dx = -dx;
    dy = -dy;
  }
  
  void bounceX() {
    dx = -dx;
  }
  
  void bounceY() {
    dy = -dy;
  }
  
  float dir() {
    float angle = (float) Math.atan2(-dy, dx);
    return -1*angle;
  }
  
  void add(Vector2d other) {
    dx += other.dx;
    dy += other.dy;
  }
  
  float magnitude() {
    return sqrt(dx * dx + dy * dy);
  }
  
  void normalize() {
    float m = magnitude();
    dx /= m;
    dy /= m;
  }
  
  void setAngle(float rad) {
    float m = magnitude();
    dx = cos(rad) * m;
    dy = sin(rad) * m;
  }
}

class Point2d {
  float x, y;
  
  Point2d() {
    x = random(width);
    y = random(height);
  }
  
  void add(Vector2d vec) {
    x += vec.dx;
    y += vec.dy;
    if (x < 0.0) {
      x = width;
    } else if (x > width) {
      x = 0.0;
    }
    if (y < 0.0) {
      y = height;
    } else if (y > height) {
      y = 0.0;
    }
  }
}