// 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.
//
//
//Thanks, Timmy

ArrayList robots;
SensoryField lightsGround;  
boolean mouseDown;
PImage ground;
boolean bounded = true; // vehicles run into world boundary (if true), wrap around (if false)
float move_speed = PI/6;

PImage ground1;
PImage bee;
PImage mybackground;
PImage flower;
PImage littlewing;
PImage wing;
PImage littlewingright;
PImage wingright;
int sw;

int numOfLights = 2;
int numOfRobots = 2;
boolean groundIsDrawn;

//=========================================================================================================
void setup() {
  size( 500, 500 );
  frameRate( 30 );
  ellipseMode( CENTER );
  rectMode( CENTER );
  bee = loadImage ("bee.png");
  mybackground = loadImage ("mybackground.png");
  flower = loadImage ("flower.png");
  littlewing = loadImage ("littlewing.png");
  wing = loadImage ("wing.png");
  littlewingright = loadImage ("littlewingright.png");
  wingright = loadImage ("wingright.png");
  noStroke();
  groundIsDrawn = false;                 // Don't draw the sensoryfield grounds on the background.
  ground = new PImage( width, height );
  robots = new ArrayList();
  lightsGround = new SensoryField( width, height, 5);  
  for (int i = 0; i < 2; i++) {
    robots.add(new Vehicle( random(width), random(height), 0, 10, robots.size()));
    lightsGround.addSource( new Source( random(10,width-10), random(10,height-10), 1.0, 100, lightsGround.sources.size()));
  }
  smooth();
  updateGround();
}


//==========================================================================================================
void draw() {
//  background(0);
  image (mybackground, 0,0);
//  if(groundIsDrawn) {  
//  image( ground, 0, 0 );
//  image(ground1, 0, 0 );
//  }

//  image(ground, 0, 0);
//  fill( 255 );
//  for (int i=0; i < robots.size(); i++) {
//    Vehicle v = (Vehicle)robots.get(i);
//    v.moveMe();
//  }
//  updateGround(); 
  for (int i=0; i<lightsGround.sources.size(); i++) {
    lightsGround.getSource(i).drawMe();
  }
  for (int i=0; i < robots.size(); i++) {
    Vehicle v = (Vehicle)robots.get(i);
    v.moveMe();    
    v.drawMe();
  }
  updateGround();    
}


//==========================================================================================================
void keyPressed() {
//  println(keyCode);
  if (keyCode >=LEFT && keyCode <= DOWN) {
    if (keyCode==UP) {
      if (robots.size() < 10){
        robots.add(new Vehicle( random(width), random(height), 0, 10, robots.size()));
      }
    } 
    else if (keyCode==DOWN) {
      if (robots.size() > 0){
        robots.remove(robots.size() - 1);
      }
    }
    else if (keyCode==LEFT) {
      if (lightsGround.sources.size() > 0){
        lightsGround.deleteSource(lightsGround.sources.size()-1);
      }
    }
    else if (keyCode==RIGHT) {
      if (lightsGround.sources.size() < 10){
        lightsGround.addSource(new Source( random(10, width-10), random(10, height-10), 1.0, 100,lightsGround.sources.size()));
      }
    }
  }
}


//==========================================================================================================
void updateGround() {
  lightsGround.update();
  ground = new PImage( width, height);
  for (int i=0; i<width; i++ ) {
    for (int k=0; k<height; k++ ) {
      int 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)
}

// 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;
  Source light;
  Vehicle( float x, float y, float angle, float axle_length, int id) {

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

    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) );
    light =new Source( x, y, 1.0, 30,lightsGround.sources.size());
    lightsGround.addSource(light);
  }

  void drawMe() {

    wA.drawMe(angle, 1);
    wB.drawMe(angle, -1);

    pushMatrix();
    translate(x,y);
    rotate (angle+PI/2);
    image (bee, -axle,-axle); 
    popMatrix();


    if(sw == 40) sw = 0; 
    sw = sw+5;
    pushMatrix();      
    translate(x,y);      
    rotate (radians (sw)+angle+PI/2+radians(-15));
    image (wingright, 10,0); 
    popMatrix();      
    pushMatrix();
    translate (x,y);
    rotate (radians (sw)+angle+PI/2+radians(-23));
    image (wing, -27,0); 
    popMatrix();

    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 );
    if(light != null)
    {
      if (light.x != x || light.y != y){
        light.setLocation(x,y);
      }
    }
  }

  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<robots.size(); i++) {
      if (i!=id) {  // for all of the OTHER robots...
        Vehicle v = (Vehicle)robots.get(i);
        // start calculating the distance between me and the other robot
        dx = x-v.x;  
        dy = y-v.y;
        // if the distance sqrt(dx^2 + dy^2) is less than the axle size
        
        if (dx*dx + dy*dy<axleSq*7 ) {
        //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;
  //        v.x = v.x + cos(da+PI)*half_axle;
    //      v.y = v.y + sin(da+PI)*half_axle;
    // source removal logic
     //     println("collision!");
          lightsGround.deleteSource(light);
          light = null;

        }
      }
    }
  }

  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;

    if (angle>HALF_PI/2) angle=0;
    if(angle<(0-HALF_PI/2)) angle=0;
    float tmpang=angle+ainc;
    float temp = sin(angle)*HALF_PI+ainc;
    if (dir ==1){
      angle+=ang_speed;
      pushMatrix();
      translate (x,y);
      rotate (tmpang-PI);
      image(littlewingright,(cos(temp-PI)+littlewingright.width/4), (sin(temp-PI)+littlewingright.height/4));
      popMatrix();
    }
    else {
      angle-=ang_speed;
      pushMatrix();
      translate (x,y);
      rotate (tmpang);
      image (littlewing, -22,6);
      popMatrix();
    }
  }
}


//===========================================================================================================
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.id = id;
    this.strength = strength;
    this.max_radius = max_radius;
  }

  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 );
    // aqui hay que preguntar de que tipo de fuente se trata
    image (flower, x-flower.width/2, y-flower.height/2);    
  }

  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<lightsGround.sources.size(); 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);
  }
  
  void deleteSource(Source s)
  {
    sources.remove(s);
  }

  // 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();
}