float maxDistance = 400;

int numVehicles = 104;
int numSources = 4;
Vehicle [] v = new Vehicle[numVehicles];
Source [] s = new Source[numSources];
//Source s2;

void setup() {
  size( 650,650 ); // sets the size of the environment
  frameRate(50);   // sets the number of draw loops per second 
  noStroke();      // sets the stroke on ellipses to none
  
  // this type of loop lets us perform some action to each vehicle in our array
  for( int i = 0; i<numVehicles; i=i+2 ) {
    v[i] = new Vehicle2( random(0,width), random(0,height) );
    v[i+1] = new Vehicle2( random(0,width), random(0,height) );
  }
  
  for( int i = 0; i<numSources; i++ ) {
    s[i] = new Source( random(0,width), random(0,height)  );
  }
}

void draw() {
  fill( 255, 255, 255, 10 );
  rect( -10, -10, width+30, height+30 );
  for( int i = 0; i<numVehicles; i++ ) {
    v[i].drawMyself();
    v[i].updatePosition();
    v[i].update( s );
  }
  for( int i = 0; i<numSources; i++ ) {
    s[i].drawMyself();
  }
}


void keyPressed() {
  for( int i = 0; i<numVehicles; i=i+2 ) {
    v[i] = new Vehicle2( random(0,width), random(0,height) );
    v[i+1] = new Vehicle2( random(0,width), random(0,height) );
  }
  
  for( int i = 0; i<numSources; i++ ) {
    s[i] = new Source( random(0,width), random(0,height)  );
  }
}















public interface Vehicle {
  float turnSpeed = 3;
  float sensorDist = 9;
  float maxSpeed = 13;
  
  public void updatePosition();
  public void update( Source [] s );  
  public void drawMyself();
}


class Vehicle2 implements Vehicle {
 
  
  // every vehicle will have these things:
  float xPos;
  float yPos;
  float speed;
  float direction;
  int myColor;
  
  
  Sensor s1;
  Sensor s2;
  
  // CONSTRUCTOR
  Vehicle2( float x, float y ) { 
    xPos = x;    
    yPos = y;  
    speed = 2;
    direction = random(0,360);    // the direction and 
    myColor = color(0,0,255); // the color of every new 
                                  // vehicle will be random
    s1 = new Sensor( x + sensorDist * cos( radians(direction+90)) , 
                     y - sensorDist * sin( radians(direction+90)) );                         
    s2 = new Sensor( x - sensorDist * cos( radians(direction+90)) , 
                     y + sensorDist * sin( radians(direction+90)) );                                   
  }
  
  
  
  
  // METHODS
  
  void drawMyself() {
   fill( myColor );
   ellipse( xPos, yPos, 6, 6 ); 
   
   s1.drawMyself();
   s2.drawMyself();
  }
  
  
  void updatePosition() {
   xPos = xPos + speed * cos( radians(direction) );
   yPos = yPos - speed * sin( radians(direction) );
   
   s1.updatePosition( xPos + sensorDist * cos( radians(direction+90)) , 
                      yPos - sensorDist * sin( radians(direction+90)) );                            
   s2.updatePosition( xPos - sensorDist * cos( radians(direction+90)) , 
                      yPos + sensorDist * sin( radians(direction+90)) ); 
  }
  
  
  void update( Source [] s ) {
    
    float minDistance = 3000;
    
    for(int i=0; i<s.length; i++ ) {
      float dx = s[i].xPos - s1.xPos; 
      float dy = s[i].yPos - s1.yPos;
      float distance1 = sqrt( dx*dx + dy*dy );

      dx = s[i].xPos - s2.xPos; 
      dy = s[i].yPos - s2.yPos;
      float distance2 = sqrt( dx*dx + dy*dy );
      //System.out.println( "d1: " + distance1 + " / d2: " + distance2 );
    
      float averageDistance = (distance1 + distance2) / 2.0;
      if ( minDistance > averageDistance ) {
         minDistance = averageDistance;
      }
  
      direction = direction + distance1/turnSpeed * (1-distance1/maxDistance);
      direction = direction - distance2/turnSpeed * (1-distance2/maxDistance);
    } 
    
    speed = (maxSpeed - ( maxSpeed * minDistance/maxDistance));
    
  }
 
}



class Vehicle1 implements Vehicle{

  float xPos;
  float yPos;
  float speed;
  float direction;
  color myColor;
  
  Vehicle1( float x, float y ) { 
    xPos = x;    
    yPos = y;  
    speed = 2;
    direction = random(0,360);    // the direction and 
    myColor = color(0,0,0); // the color of every new 
                                  // vehicle will be rando
  }
  
  void drawMyself( ) {
    fill ( myColor );
    ellipse( xPos, yPos, 10, 10 ); 
  }
  
  void update( Source [] s) {
    float dx = s[0].xPos - xPos; 
    float dy = s[0].yPos - yPos;
    float distance1 = sqrt( dx*dx + dy*dy );
        
    speed = maxSpeed - ( maxSpeed * distance1/maxDistance);
  }
  
  void updatePosition() {
    xPos = xPos + speed * cos( radians(direction) );
    yPos = yPos - speed * sin( radians(direction) );
  }

  
  
}










class Source {
 
  float xPos;
  float yPos; 
  
  Source( float x, float y  ) {
    xPos = x;
    yPos = y;
    
  }
  
  void drawMyself() {
    fill( 255, 0, 0 );
    ellipse( xPos, yPos, 10, 10 );
    
  }
}







class Sensor {
 
  float xPos;
  float yPos;
  
  Sensor( float x, float y ) {
    xPos = x;
    yPos = y;
  }
  
  void drawMyself() {
    fill( 240, 120, 0 );
    ellipse( xPos, yPos, 2, 2 ); 
  }
  
  void updatePosition( float x, float y ) {
    xPos = x;
    yPos = y;
  }
}