// Processing source code : Braitenberg's Vehicles
// Based on program written by william ngan <contact@metaphorical.net>

Vehicle[] robots;
Source[] sources;
boolean mouseDown;
PImage ground;
PImage bee;
PImage mybackground;
PImage flower;
PImage littlewing;
PImage leg;
PImage wing;
PImage littlewingright;
PImage wingright;
boolean bounded = true; // vehicles run into world boundary (if true), wrap around (if false)
int numOfLights = 2;
int numOfRobots = 6;
float move_speed = PI/6;
int sw;

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");
  leg = loadImage ("leg.png");
  wing = loadImage ("wing.png");
  littlewingright = loadImage ("littlewingright.png");
  wingright = loadImage ("wingright.png");

  noStroke();

  // ground = new PImage( width, height );

  robots = new Vehicle[10];
  for (int i=0; i<robots.length; i++) {
    robots[i] = new Vehicle( random(width), random(height), 0, 10, i );
  }

  sources = new Source[5];
  for (int i=0; i<sources.length; i++) {
    sources[i] = new Source( random(10,width-10), random(10,height-10), 1.0, 100, i );

  }

  smooth();

  updateGround();
}

void draw() {
  image (mybackground, 0,0);
  for (int i=0; i<numOfLights; i++) {
    sources[i].drawMe();
  }

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

void keyPressed() {
  if (key>='1' && key<='5') {
    numOfLights = 5-('5'-key);
    updateGround();
  }
  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;
  ground = new PImage(width, height);

  for (int i=0; i<width; i+=px ) {
    for (int k=0; k<height; k+=px ) {
      sum = 0;
      for (int m=0; m<numOfLights; m++ ) {
        sum += sources[m].getReading( i, k, true );
        if (sum>=1) break;
      }
      c = (int)min(sum*255, 255);
      for (int p=0; p<px; p++) {
        for (int q=0; q<px; q++) {
          ground.set( i+p, k+q, color(  c, min(200,c), c/8) );
        }
      }

    }
  }

}

// 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() {

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

  void checkCollision() {

    float dx, dy, da;
    for (int i=0; i<numOfRobots; i++) {
      if (i!=id) {

        dx = x-robots[i].x;
        dy = y-robots[i].y;
        //if (abs(dx) <=axle && abs(dy)<=axle ) {
        if (dx*dx + dy*dy<axleSq ) {
          da = atan2( dy, dx );
          //angle = ;
          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) {
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() {
      float val = red( ground.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 = red( ground.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 = red( ground.get( (int)x, (int)y ) )/255.0;

      return sense;
    }


    
    // Antenas en su lugar?
      void drawMe() {/*
        fill(255);
        ellipse( x, y, 16, 16 );
        fill(200*sense,0,0);
        ellipse( x, y, 7, 7 );*/
      }

  }


// Source ==================================================================================================
  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;
    }

    void drawMe() {

      if (id < numOfLights ) {
        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;
        }
        image (flower, x-flower.width/2, y-flower.height/2);

      }
    }

    void checkCollision() {

      float dx, dy, da;
      for (int i=0; i<sources.length; i++) {
        if (i!=id) {

          dx = x-sources[i].x;
          dy = y-sources[i].y;
          //if (abs(dx) <=axle && abs(dy)<=axle ) {
          if (dx*dx + dy*dy<max_radius ) {
            da = atan2( dy, dx );
            sources[i].x = sources[i].x + cos(da+PI)*5;
            sources[i].y = sources[i].y + sin(da+PI)*5;
          }

          //}
        }
      }
    }

    float getReading( float tx, float ty, boolean plus ) {

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

  }

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

  }

  void mousePressed() {
    mouseDown = true;
  }

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