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

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

void setup() {

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

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

  background( 0 );

  image( ground, 0, 0 );

  fill( 255 );
  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() {

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

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

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

      ellipse(x, y, 10, 10 );

    }
  }

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