// Pathfinding w/ Genetic Algorithms
// Daniel Shiffman <http://www.shiffman.net>

// This example produces an obstacle course with a start and finish
// Virtual "creatures" are rewarded for making it closer to the finish
// a simple "pathfinding" AI program

// For ever creature, we grid out the screen and their DNA offers
// a steering vector for each cell on the screen

import noc.*;  // The Vector3D class is now in a library: http://www.shiffman.net/teaching/the-nature-of-code/library/

// To store new people

Flock flock;


ArrayList newPeople = new ArrayList();

final int W = 640; 
final int H = 480;  //screen size
final int gridscale = 5;              //scale of grid is 1/24 of screen size

// DNA needs one vector for every spot on the grid 
// (it's like a pixel array, but with vectors instead of colors)
final int DNASIZE = (W / gridscale) * (H / gridscale); 

int lifetime;// = 200;  // How long should each generation live

// Global maxforce and maxspeed (hmmm, could make this part of DNA??)
float maxspeed = 4.0f;
float maxforce = 1.0f;

Population popul;  //population
int lifecycle;     //timer for cycle of generation
int recordtime;    //fastest time to target
//Obstacle target;   //target location
//Obstacle start;    //start location
int diam = 24;//size of target
PFont f;           //font for display

ArrayList obstacles;  //an array list to keep track of all the obstacles!

void setup() {
  size(640,480);
  //size(W,H);
  //framerate(60);
colorMode(RGB,255,255,255,100);

 flock = new Flock();
  // Add an initial set of boids into the system
  for (int i = 0; i < 0; i++) {//this number could be 50 or 100
    flock.addBoid(new Boid(new Vector3D(random(0, width),random(0,height)),50f,0.05f));
  }


  f = loadFont("GillSans-12.vlw");
  smooth();
  
  lifetime = 10;//width/2;

  //*initialize the globals*//
  lifecycle = 0;
  recordtime = lifetime;
  //target = new Obstacle(width-diam-diam/2,height/2-diam/2,diam,diam);
  //start = new Obstacle(diam/2,height/2-diam/2,diam,diam);

  //*create a population with a mutation rate, and population max*//
  int popmax = 50;
  float mutationRate = 0.05f;
  popul = new Population(mutationRate,popmax);

  //*Create the obstacle course!*//
  
  obstacles = new ArrayList();
  obstacles.add(new Obstacle(0,-150,150,-150));
   obstacles.add(new Obstacle(200,-150,150,-150));
   obstacles.add(new Obstacle(400,-150,150,-150));
 obstacles.add(new Obstacle(600,-150,150,-150));
   
   obstacles.add(new Obstacle(0,25,150,100));
   obstacles.add(new Obstacle(200,25,150,100));
   obstacles.add(new Obstacle(400,25,150,100));
   obstacles.add(new Obstacle(600,25,150,100));
   
     obstacles.add(new Obstacle(0,175,150,100));
   obstacles.add(new Obstacle(200,175,150,100));
   obstacles.add(new Obstacle(400,175,150,100));
   obstacles.add(new Obstacle(600,175,150,100));
   
    obstacles.add(new Obstacle(0,325,150,100));
   obstacles.add(new Obstacle(200,325,150,100));
   obstacles.add(new Obstacle(400,325,150,100));
   obstacles.add(new Obstacle(600,325,150,100));
   
    // obstacles.add(new Obstacle(0,-200,600,200)); //make invisible boxes to keep them contained
 // obstacles.add(new Obstacle(-200,0,200,400));
  // obstacles.add(new Obstacle(600,400,600,200));
  // obstacles.add(new Obstacle(650,0,200,400));
   
   
  //obstacles.add(new Obstacle(10,20,600,1));
 // obstacles.add(new Obstacle(width/2,0,1,height/2-10));
  //obstacles.add(new Obstacle(width/2,height-height/2+10,1,height/2-10));
 // obstacles.add(new Obstacle(10,460,600,1));
}

void draw() {
  background(255);
 flock.run();
  //*DRAW THE START NAND TARGET*//
 //start.render();
// target.render();
  
  //*DRAW THE OBSTACLES*//
  for (int i = 0; i < obstacles.size(); i++) {
    Obstacle obs = (Obstacle) obstacles.get(i);
    obs.render();
  }

  //**IF WE HAVEN'T REACHED THE END OF OUR LIVES, KEEP GOING!*//
  //if (lifecycle < lifetime) {
    popul.live(obstacles);
    if ((popul.targetReached()) && (lifecycle < recordtime)) {
      recordtime = lifecycle;
    }
    lifecycle++;
    //**OTHERWISE, LET'S MAKE A NEW GENERATION*//
  //} 
  //else {// THIS IS STARTING OVER
    //lifecycle = 0;
    //popul.calcFitness();
    //popul.naturalSelection();
    //popul.generate();
    
  //}

  //DISPLAY SOME INFO
 // textFont(f);
 //  textAlign(RIGHT);
//   fill(255);
//   text("Generation #:" + popul.getGenerations(),width-10,18);
 //  text("Time left:" + ((lifetime-lifecycle)/10),width-10,36);
 //  text("Record time: " + recordtime,width-10,54);
   for(int i = 0; i < newPeople.size(); i++)
   {
     MyShape aPerson = (MyShape)newPeople.get(i);
     aPerson.render();
   }
}

//**MOVE THE TARGET IF WE CLICK THE MOUSE**//

  
  

// Add a new boid into the System
void mousePressed() {
    recordtime = lifetime;
  int acolor;
  int asize;
  
   float s = random(100);
  if (s > 50){
    //make it gray
    acolor=100;
  }
  else{
    acolor=0;
  }

  float r = random(100);
  if (r > 100){
    //make big man
    asize=1;
  }
  else{
    //make little man
    asize=0;
  }

  
  
  flock.addBoid(new Boid(new Vector3D(mouseX,mouseY),2.0f,0.05f,acolor,asize));
}

class Flock {
  ArrayList boids; // An arraylist for all the boids

  Flock() {
    boids = new ArrayList(); // Initialize the arraylist
  }

  void run() {
    for (int i = 0; i < boids.size(); i++) {
      Boid b = (Boid) boids.get(i);  
      b.run(boids);  // Passing the entire list of boids to each boid individually
    }
  }

  void addBoid(Boid b) {
    boids.add(b);
  }

}


class Boid {

  Vector3D loc;
  Vector3D vel;
  Vector3D acc;
  float r;
  float maxforce;    // Maximum steering force
  float maxspeed;    // Maximum speed
  MyShape aMan;

 
  int asize;
  int acolor =0;

  Boid(Vector3D l, float ms, float mf) {
    acc = new Vector3D(0,0);
    vel = new Vector3D(random(-1,1),random(-1,1));
    loc = l.copy();
    r = 2.0f;
    maxspeed = ms;
    maxforce = mf;
    aMan = new Man();

  }
  
  Boid(Vector3D l, float ms, float mf, int ac, int as) {
    acc = new Vector3D(0,0);
    vel = new Vector3D(random(-1,1),random(-1,1));
    loc = l.copy();
    r = 2.0f;
    maxspeed = ms;
    maxforce = mf;
    acolor=ac;
    if (as==0)
    {
      aMan = new Man();
    }
    else
    {
      aMan = new Man2();
    }
  }
  
  void run(ArrayList boids) {
    flock(boids);
    update();
    borders();
    render();
  }

  // We accumulate a new acceleration each time based on three rules
  void flock(ArrayList boids) {
    Vector3D sep = separate(boids);   // Separation
    Vector3D ali = align(boids);      // Alignment
    Vector3D coh = cohesion(boids);   // Cohesion
    // Arbitrarily weight these forces
    sep.mult(2.0f);
    ali.mult(1.0f);
    coh.mult(1.0f);
    // Add the force vectors to acceleration
    acc.add(sep);
    acc.add(ali);
    acc.add(coh);
  }
  
  // Method to update location
  void update() {
    // Update velocity
    vel.add(acc);
    // Limit speed
    vel.limit(maxspeed);
    loc.add(vel);
    // Reset accelertion to 0 each cycle
    acc.setXYZ(0,0,0);
  }

  void seek(Vector3D target) {
    acc.add(steer(target,false));
  }
 
  void arrive(Vector3D target) {
    acc.add(steer(target,true));
  }

  // A method that calculates a steering vector towards a target
  // Takes a second argument, if true, it slows down as it approaches the target
  Vector3D steer(Vector3D target, boolean slowdown) {
    Vector3D steer;  // The steering vector
    Vector3D desired = target.sub(target,loc);  // A vector pointing from the location to the target
    float d = desired.magnitude(); // 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 = target.sub(desired,vel);
      steer.limit(maxforce);  // Limit to maximum steering force
    } else {
      steer = new Vector3D(0,0);
    }
    return steer;
  }
  
  void render() {
    // Draw a triangle rotated in the direction of velocity
    float theta = vel.heading2D() + radians(90);
    stroke(acolor);
    pushMatrix();
    translate(loc.x,loc.y);
    //rotate(theta);
    /*
    beginShape(TRIANGLES);
    vertex(0, -r*2);
    vertex(-r, r*2);
    vertex(r, r*2);
    endShape();
    */
    aMan.render();
    popMatrix();
  }
  
  // Wraparound
  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;
  }

  // Separation
  // Method checks for nearby boids and steers away
  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's too close
    for (int i = 0 ; i < boids.size(); i++) {
      Boid other = (Boid) boids.get(i);
      float d = loc.distance(loc,other.loc);
      // If the distance is greater than 0 and less than an arbitrary amount (0 when you are yourself)
      if ((d > 0) && (d < desiredseparation)) {
        // Calculate vector pointing away from neighbor
        Vector3D diff = loc.sub(loc,other.loc);
        diff.normalize();
        diff.div(d);        // Weight by distance
        sum.add(diff);
        count++;            // Keep track of how many
      }
    }
    // Average -- divide by how many
    if (count > 0) {
      sum.div((float)count);
    }
    return sum;
  }
  
  // Alignment
  // For every nearby boid in the system, calculate the average velocity
  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 = loc.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;
  }

  // Cohesion
  // For the average location (i.e. center) of all nearby boids, calculate steering vector towards that location
  Vector3D cohesion (ArrayList boids) {
    float neighbordist = 50.0f;
    Vector3D sum = new Vector3D(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 = loc.distance(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;
  }
}

// Simple Vector3D Class 

static class Vector3D {
  float x;
  float y;
  float z;

  Vector3D(float x_, float y_, float z_) {
    x = x_; y = y_; z = z_;
  }

  Vector3D(float x_, float y_) {
    x = x_; y = y_; z = 0f;
  }
  
  Vector3D() {
    x = 0f; y = 0f; z = 0f;
  }

  void setX(float x_) {
    x = x_;
  }

  void setY(float y_) {
    y = y_;
  }

  void setZ(float z_) {
    z = z_;
  }
  
  void setXY(float x_, float y_) {
    x = x_;
    y = y_;
  }
  
  void setXYZ(float x_, float y_, float z_) {
    x = x_;
    y = y_;
    z = z_;
  }

  void setXYZ(Vector3D v) {
    x = v.x;
    y = v.y;
    z = v.z;
  }
  
  float magnitude() {
    return (float) Math.sqrt(x*x + y*y + z*z);
  }

  Vector3D copy() {
    return new Vector3D(x,y,z);
  }

  Vector3D copy(Vector3D v) {
    return new Vector3D(v.x, v.y,v.z);
  }
  
  void add(Vector3D v) {
    x += v.x;
    y += v.y;
    z += v.z;
  }

  void sub(Vector3D v) {
    x -= v.x;
    y -= v.y;
    z -= v.z;
  }

  void mult(float n) {
    x *= n;
    y *= n;
    z *= n;
  }

  void div(float n) {
    x /= n;
    y /= n;
    z /= n;
  }

  void normalize() {
    float m = magnitude();
    if (m > 0) {
       div(m);
    }
  }

  void limit(float max) {
    if (magnitude() > max) {
      normalize();
      mult(max);
    }
  }

  float heading2D() {
    float angle = (float) Math.atan2(-y, x);
    return -1*angle;
  }

  Vector3D add(Vector3D v1, Vector3D v2) {
    Vector3D v = new Vector3D(v1.x + v2.x,v1.y + v2.y, v1.z + v2.z);
    return v;
  }

  Vector3D sub(Vector3D v1, Vector3D v2) {
    Vector3D v = new Vector3D(v1.x - v2.x,v1.y - v2.y,v1.z - v2.z);
    return v;
  }

  Vector3D div(Vector3D v1, float n) {
    Vector3D v = new Vector3D(v1.x/n,v1.y/n,v1.z/n);
    return v;
  }

  Vector3D mult(Vector3D v1, float n) {
    Vector3D v = new Vector3D(v1.x*n,v1.y*n,v1.z*n);
    return v;
  }

  float distance (Vector3D v1, Vector3D v2) {
    float dx = v1.x - v2.x;
    float dy = v1.y - v2.y;
    float dz = v1.z - v2.z;
    return (float) Math.sqrt(dx*dx + dy*dy + dz*dz);
  }

}