// Processing source code : Braitenberg's Vehicles
// Based on program written by william ngan <contact@metaphorical.net>
// SensoryField class stub coded by Prof. Mateas and Mayhew Seavey in 2004...
// ...and slightly-further hackery added by Jason Alderman in 2005.

// Press UP and DOWN arrows to increase number of Braitenberg vehicles.
// Press number keys 1-5 to change the number of sources on the sensory field.
// Press SPACE to toggle the PImage ground visible or invisible.

Vehicle[] robots;
SensoryField lightsGround;  
boolean mouseDown;
PImage ground;
boolean bounded = true; // vehicles run into world boundary (if true), wrap around (if false)
int numOfLights = 2;
int numOfRobots = 2;
float move_speed = PI/6;
boolean groundIsDrawn;

void setup() {

  size( 300, 300 );
  framerate( 30 );
  ellipseMode( CENTER );
  rectMode( CENTER );

  noStroke();

  groundIsDrawn = false;                 // Don't draw the sensoryfield grounds on the background.
  ground = new PImage( width, height );

  robots = new Vehicle[10];               // Create an array of the Braitenberg vehicles.
  for (int i=0; i<robots.length; i++) {
    robots[i] = new Vehicle( random(width), random(height), 0, 10, i );
  }

  lightsGround = new SensoryField( width, height, 5);  // Create a SensoryField...
  // Now that SensoryField is a class, you can make as many different types as you wish.

  for (int i=0; i<numOfLights; i++) {
    lightsGround.addSource( new Source( random(10,width-10), random(10,height-10), 1.0, 100, i));
    println("Adding source "+ i + " to lights at "+lightsGround.getSource(i).getLocation());
  }
  println("The size of the lightsGround arraylist is "+lightsGround.sources.size());

  smooth();

  updateGround();
}

void draw() {

  background( 0 );

  if(groundIsDrawn) image( ground, 0, 0 );

  fill( 255 );
  for (int i=0; i<numOfLights; i++) {
    lightsGround.getSource(i).drawMe();

  }

  for (int i=0; i<numOfRobots; i++) {
    robots[i].moveMe();
    robots[i].drawMe();
  }

}

void keyPressed() {

  if (key>='1' && key<='5') {
    numOfLights = 5-('5'-key);
    updateGround();  // updateGround so that it adds or removes the lights from the ArrayList!
  }

  if (key==' '){
    groundIsDrawn= (!groundIsDrawn);
    println("ground-drawn boolean is now: "+groundIsDrawn);
    updateGround();  // updateGround so that it draws the PImage! (or doesn't, when it shouldn't!)
  }

  if (key==CODED) {
    if (keyCode==UP) {
      numOfRobots++;
      numOfRobots = min( numOfRobots, robots.length-1 );
    } 
    else if (keyCode==DOWN) {
      numOfRobots--;
      numOfRobots = max( numOfRobots, 1 );
    }
  }
}

void updateGround() {

  float sum;
  int c;
  int px = 5;
  color cc;
  int temp;

  // if a keypress has changed the number of lights, then add or remove sources from the arraylist of lightsGround
  if(numOfLights!=lightsGround.sources.size()){
    if(numOfLights < lightsGround.sources.size()){
      // if the number of light sources has been decreased...
      temp = lightsGround.sources.size() - numOfLights;
      for(int i=0; i<temp; i++){
        println("deleting source id "+(lightsGround.sources.size()-1));
        lightsGround.deleteSource(lightsGround.sources.size()-1);
      }
    } 
    else {
      // if the number of light sources has been increased...
      temp = numOfLights - lightsGround.sources.size();
      for(int i=0; i<temp; i++){
        println("adding source id "+lightsGround.sources.size());
        lightsGround.addSource(new Source( random(10, width-10), random(10, height-10), 1.0, 100, lightsGround.sources.size()));
      }
    }
  }

  lightsGround.update();

  if(groundIsDrawn){

    ground = new PImage( width, height);
    for (int i=0; i<width; i++ ) {
      for (int k=0; k<height; k++ ) {

        c = int(lightsGround.get(i,k));
        ground.set( i, k, color( c, min(200,c), c/8) );
      }  // end for-loop (k)
    }    // end for-loop (i)

  } // end if(groundIsDrawn)

}

// Vehicle class
class Vehicle {

  float x, y, axle, half_axle, axleSq;
  float angle; // direction of the Vehicle
  float wheel_diff, wheel_average; // the difference and average of the wheels' rotating speed
  Wheel wA, wB; // two wheels
  Sensor sA, sB; // two sensors
  int id;

  Vehicle( float x, float y, float angle, float axle_length, int id ) {

    this.x = x;
    this.y = y;
    this.angle = angle;
    this.id = id;
    axle = axle_length;
    half_axle = axle/2;
    axleSq = axle*axle;

    wA = new Wheel( x+half_axle*cos(angle-HALF_PI), y+half_axle*sin(angle-HALF_PI), 10, 0 );
    wB = new Wheel( x+half_axle*cos(angle+HALF_PI), y+half_axle*sin(angle+HALF_PI), 10, 0 );

    sA = new Sensor( x+axle*cos(angle-HALF_PI/1.5), y+axle*sin(angle-HALF_PI/1.5) );
    sB = new Sensor( x+axle*cos(angle+HALF_PI/1.5), y+axle*sin(angle+HALF_PI/1.5) );

  }

  void drawMe() {

    fill(255,0,0);
    triangle( wA.x, wA.y, wB.x, wB.y, x+cos(angle)*axle*2.5, y+sin(angle)*axle*2.5 );

    //fill(200,54,130);

    fill(102,102,51);
    wA.drawMe(angle, 1);
    wB.drawMe(angle-PI, -1);

    ellipse( x, y, axle*3, axle*3 );

    sA.drawMe();
    sB.drawMe();

  }

  // Computes the sensory logic to set the wheel velocity.

  // Sensor output goes directly to wheel on same side
  void doSenseLogic() {
    setASpeed(sA.getSense());
    setBSpeed(sB.getSense());
  }

  // Sensor output crossed to wheel on opposite side
  /* void doSenseLogic() {
   setASpeed(sB.getSense());
   setBSpeed(sA.getSense());
   }*/

  // Each sensor goes to wheel on same side with an inhibitory connection
  /* void doSenseLogic() {
   setASpeed(sA.getInverseSense());
   setBSpeed(sB.getInverseSense());
   } */

  // Each sensor goes to wheel on opposite side with an inhibitory connection
  /* void doSenseLogic() {
   setASpeed(sB.getInverseSense());
   setBSpeed(sA.getInverseSense());
   } */

  // Sensors are hooked up to opposite motors, with threshhold sensing.
  /* void doSenseLogic() {
   setASpeed(sB.getNonlinearSense());
   setBSpeed(sA.getNonlinearSense());
   } */

  void moveMe() {

    checkBounds();

    doSenseLogic();

    // move

    wheel_diff = wA.d - wB.d;
    wheel_average = ( wA.d + wB.d ) / 2;
    angle += wheel_diff / axle;

    x += cos( angle ) * wheel_average;
    y += sin( angle ) * wheel_average;

    checkCollision();

    // wheels move
    float ang = angle - HALF_PI;

    wA.x = x + half_axle * cos( ang );
    wA.y = y + half_axle * sin( ang );

    ang =  angle + HALF_PI;

    wB.x = x + half_axle * cos( ang );
    wB.y = y + half_axle * sin( ang );

    // sensors move
    ang = angle - HALF_PI/2;

    sA.x = x + axle * cos( ang );
    sA.y = y + axle * sin( ang );

    ang =  angle + HALF_PI/2;

    sB.x = x + axle * cos( ang );
    sB.y = y + axle * sin( ang );

  }

  void checkBounds() {
    if (bounded) {
      x = max( axle+5, min( width-axle-5, x ) );
      y = max( axle+5, min( height-axle-5, y ) );
    } 
    else {
      x = ( x<0 ) ? width+x : ((x>width) ? x-width : x );
      y = ( y<0 ) ? height+y : ((y>height) ? y-height : y );
      // If the above looks completely confusing to you, check out
      //  http://processing.org/reference/conditional_.html
    }
  }

  void checkCollision() {

    float dx, dy, da;
    for (int i=0; i<numOfRobots; i++) {
      if (i!=id) {  // for all of the OTHER robots...
        // start calculating the distance between me and the other robot
        dx = x-robots[i].x;  
        dy = y-robots[i].y;
        // if the distance sqrt(dx^2 + dy^2) is less than the axle size
        if (dx*dx + dy*dy<axleSq ) {
          da = atan2( dy, dx ); // calculate the angle from me to the other robot
          // then push me through t
          x = x + cos(da)*half_axle;
          y = y + sin(da)*half_axle;
          robots[i].x = robots[i].x + cos(da+PI)*half_axle;
          robots[i].y = robots[i].y + sin(da+PI)*half_axle;
        }

        //}
      }
    }
  }

  void setASpeed( float ang_speed ) {
    wA.setSpeed( ang_speed*move_speed );
  }

  void setBSpeed( float ang_speed ) {
    wB.setSpeed( ang_speed*move_speed );
  }

  void changeASpeed( float inc ) {
    wA.setSpeedChange( inc );
  }

  void changeBSpeed( float inc ) {
    wB.setSpeedChange( inc );
  }

}

class Wheel {

  float x, y;
  float ang_speed, radius;
  float angle;
  float d;

  Wheel( float x, float y, float radius, float ang_speed ) {

    this.x = x;
    this.y = y;
    this.radius = radius;
    this.ang_speed = ang_speed;
    angle = 0;

    d = ang_speed * radius;
  }

  void setSpeed( float ang_speed ) {

    this.ang_speed = ang_speed;
    d = ang_speed * radius;
  }

  void setSpeedChange( float inc ) {
    ang_speed += inc;
    d = ang_speed * radius;
  }

  void drawMe(float ainc, int dir) {
    angle += ang_speed;
    if (angle>TWO_PI) angle -= TWO_PI;
    float temp = sin(angle)*HALF_PI+ainc;

    ellipse( x+cos(temp)*10*dir, y+sin(temp)*10*dir, 20, 20 );
  }
}

class Sensor {

  float x, y;
  float maxReading;
  float sense;

  Sensor( float x, float y ) {
    this.x = x;
    this.y = y;

    maxReading = 1;

  }

  void setLocation( float x, float y ) {
    this.x = x;
    this.y = y;
  }

  /*
  float getSense(boolean plus) {
   
   float sum = red( ground.get( (int)x, (int)y ) )/255.0;
   sum = (plus) ? sum : 1-sum;
   sense = (specialSense) ? nonlinear( sum, maxReading ) : 1-sum;
   
   return sense;
   }*/

  // This is an example of non-linear threshhold sensing. Returns 0
  // until the normalized sensory value = 0.5, then returns a linear value
  // between 0.5 and 1.
  float getNonlinearSense() {
    // old code, reading from PImage ground, was:
    // float val = red( ground.get( (int)x, (int)y ) )/255.0;
    float val = lightsGround.get( (int)x, (int)y )/255.0;
    if (val < 0.5)
      return 0;
    else
      return val;
  }

  // Returns 0 when sensory value is maximum, 1 when it's minimum
  float getInverseSense() {
    float val = lightsGround.get( (int)x, (int)y )/255.0;
    sense = 1 - val;

    return sense;
  }

  // Returns 1 when sensory value is maximum, 0 when it's minimum
  float getSense() {
    sense = lightsGround.get( (int)x, (int)y )/255.0;

    return sense;
  }

  void drawMe() {
    fill(255);
    ellipse( x, y, 16, 16 );
    fill(200*sense,0,0);
    ellipse( x, y, 7, 7 );
  }

}

class Source {

  float x, y;
  float strength; // between 0 to 1
  float max_radius;
  boolean dragging = true;
  int id;

  Source( float x, float y, float strength, float max_radius, int id ) {
    this.x = x;
    this.y = y;
    this.strength = strength;
    this.max_radius = max_radius;
    this.id = id;
  }

  void setLocation( float x, float y ) {
    this.x = x;
    this.y = y;
  }

  String getLocation(){
    return ""+this.x+","+this.y;
  }

  void drawMe() {

    checkCollision();

    // dragging?
    if (mouseDown && mouseX>x-10 && mouseX<x+10 && mouseY>y-10 && mouseY<y+10) {
      dragging = true;
    }

    if (!mouseDown) dragging = false;

    if (dragging) {
      x = mouseX;
      y = mouseY;
    }

    ellipse(x, y, 10, 10 );
    if(groundIsDrawn == false){
      //-- draw a circle of max radius around the source --
      stroke(255,255,255,45);
      strokeWeight(2);
      noFill();
      ellipse(x, y, max_radius*2, max_radius*2);
      noStroke();
      fill(255);
      //--
    }
  }

  void checkCollision() {
    // see vehicle's checkCollision for an explanation of the math here...
    float dx, dy, da;
    
    //-- keep the Source on the screen
    if(x > width) x = width;
    if(x < 0) x = 0;
    if(y > height) y = height;
    if(y < 0) y = 0;
    //--
    
    for (int i=0; i<numOfLights; i++) {
      if (i!=id) {

        dx = x-lightsGround.getSource(i).x;
        dy = y-lightsGround.getSource(i).y;

        if (dx*dx + dy*dy<max_radius ) {
          da = atan2( dy, dx );
          lightsGround.getSource(i).x = lightsGround.getSource(i).x + cos(da+PI)*5;
          lightsGround.getSource(i).y = lightsGround.getSource(i).y + sin(da+PI)*5;
        }

      }
    }
  }

  float getReading( float tx, float ty, boolean plus ) {
    // This method calculates the field strength by measuring the distance of an arbitrary point from this source.
    float d = dist( tx, ty, x, y );
    if (d >= max_radius) return ((plus) ? 0 : 1);

    // Strength of source falls of linearly in distance (up to max_radius) from source
    // d = strength*(d/max_radius);

    // Strength of source falls off as a function of the cosine of distance (up to max_radius) from source
    d = 1-nonlinear( d, max_radius );

    return ((plus) ? 1-d : d );
  }

}  // end Source

// The function nonlinear(r, rmax) is used by several bits of code above, so it falls inside the main program.
//   Returns a value between 0 and 1 (assume r & rmax >= 0)
//   Returns 0 if r is >= rmax
//   Returns 1 if r = 0
float nonlinear(float r, float rmax) {
  float f = (rmax - Math.min(r, rmax)) / rmax;
  return 0.5 - 0.5*cos(f*PI);
}

// SensoryField is a class that contains our 2D array of sensory values (instead of using a PImage).
class SensoryField {
  float[][] field;   // A 2D array of summed sensory values.  The 2D array should be the same size as the processing window.
  int w;             // The width of the field
  int h;             // The height of the field
  ArrayList sources; // The list of sources that produce values for this SensoryField
  int pixelSize;     // Determines the block size at which we sample the field.  Change the value (in the constructor argument) 
  //  to increase the sample size.  The bigger the value, the more pixelated the field, but the less 
  //  computational work it takes to update the field.

  SensoryField(int w, int h, int ps) {
    field = new float[w+100][h+100];  // ...making a sensory field with 50 elements-worth 
    //  of buffer on each side, so that when a vehicle or source goes to the edge of the 
    //  screen, there aren't negative index values for the 2D array (something that 
    //  PImages don't care about, but arrays do!).
    this.w = w;
    this.h = h;
    sources = new ArrayList();
    pixelSize = ps;
  }

  // Get the sensory value at a specific point.
  float get(int x, int y){
    return field[x+50][y+50];
  }

  // Add a source to the field.
  void addSource(Source s) {
    sources.add(s);
  }

  // Delete a source from the field.
  void deleteSource(int j) {
    sources.remove(j);
  }

  // Get a source from the sources arraylist.
  Source getSource(int i){
    return (Source)sources.get(i);
  }

  // Update the field.  For every point in the field, iterate over the sources to determine
  //  the sensory value at each point.
  void update() {
    float sum;
    // We're going to iterate through the all of the elements of the sensory field, 
    //  but increment by the pixelSize in the both directions...
    for(int i = 0; i < w; i += pixelSize){
      for(int k = 0; k < h; k += pixelSize){
        // Now sum up the sensory readings for each source in the field... 
        sum = 0;
        for(int m=0; m<sources.size(); m++){
          sum += this.getSource(m).getReading(i,k,true);
        }
        // ...and now write that sum (or a maximum of 255) in each element of the field within each big "pixel."
        for(int p=0; p<pixelSize; p++) {
          for(int q=0; q<pixelSize; q++){
            field[i+p+50][k+q+50] = min(sum*255, 255);
          }
        }

      }
    }
  }

  // 
}

void mousePressed() {
  mouseDown = true;
}

void mouseReleased() {
  mouseDown = false;
  updateGround();
}