class SteeredVehicle extends Vehicle
{
  // _maxForce best values
  // seek 0.05 - 0.1, flee 0.01 - 0.05
  float _maxForce = 0.1; //1.0;
  Vector2D _steeringForce;
  
  float _arrivalThreshold = 100;
  
  float _wanderAngle = 0.0; //0
  float _wanderDistance = 10.0; //10
  float _wanderRadius = 5.0; //5
  float _wanderRange = 1.0; //1
  
  float _avoidDistance = 50;
  float _avoidBuffer = 10;
  
  int _pathIndex = 0;
  float _pathThreshold = 20;
  Boolean _loopRound = false;
  
  float _inSightDist = 200;
  float _tooCloseDist = 60;
  
  
  SteeredVehicle()
  {
    super();
    _steeringForce = new Vector2D();
  }
  
  SteeredVehicle(Vector2D _position, Vector2D _velocity)
  {
    super(_position, _velocity);
    _steeringForce = new Vector2D();
  }

  // set/get maxForce
  void setMaxForce(float value)
  {
    _maxForce = value;
  }
  float getMaxForce()
  {
    return _maxForce;
  }
  
  void update()
  {
    _steeringForce.limit(_maxForce);
    _steeringForce.div(_mass);
    _velocity.add(_steeringForce);
    _steeringForce = new Vector2D();
//          if (_position.x < 0 ) exit();
    super.update();
  }
  
  //*** seek and flee
  // differ only by addition or subtraction of force
  void seek(Vector2D target)
  {
    Vector2D desiredVelocity = target.clone();
    desiredVelocity.sub(_position);
    desiredVelocity.normalize();
    desiredVelocity.mult(_maxSpeed);
    Vector2D force = desiredVelocity.clone();
    force.sub(_velocity);
    _steeringForce.add(force);
  }
  void flee(Vector2D target)
  {
    Vector2D desiredVelocity = target.clone();
    desiredVelocity.sub(_position);
    desiredVelocity.normalize();
    desiredVelocity.mult(_maxSpeed);
    Vector2D force = desiredVelocity.clone();
    force.sub(_velocity);
    _steeringForce.sub(force);
  }

  //*** arrive
  void arrive(Vector2D target)
  {
    Vector2D desiredVelocity = target.clone();
    desiredVelocity.sub(_position);
    desiredVelocity.normalize();
    float distV = _position.distV(target);
    if(distV > _arrivalThreshold)
    {
      desiredVelocity.mult(_maxSpeed);
    }
    else
    {
      desiredVelocity.mult(_maxSpeed * distV / _arrivalThreshold);
    }
    
    Vector2D force = desiredVelocity.clone();
    force.sub(_velocity);
    _steeringForce.add(force);
  }
  
  //*** persue and evade
  void persue(Vehicle target)
  {
    float lookAheadTime = _position.distV(target._position) / _maxSpeed;
    Vector2D predictedPosition = target._position.clone();
    Vector2D predictedVelocity = target._velocity.clone();
    predictedVelocity.mult(lookAheadTime);
    predictedPosition.add(predictedVelocity);
    seek(predictedPosition);
  }
  void evade(Vehicle target)
  {
    float lookAheadTime = _position.distV(target._position) / _maxSpeed;
    Vector2D predictedPosition = target._position.clone();
    Vector2D predictedVelocity = target._velocity.clone();
    predictedVelocity.mult(lookAheadTime);
    predictedPosition.sub(predictedVelocity);
    flee(predictedPosition);
  }

  //*** wander
  void wander()
  {
    Vector2D centre = _velocity.clone();
    centre.normalize();
    centre.mult(_wanderDistance);
    Vector2D offset = new Vector2D(0,0);
    offset.setLength(_wanderRadius);
    offset.setAngle(_wanderAngle);
    _wanderAngle += random(-_wanderRange, _wanderRange);
    Vector2D force = centre.clone();
    force.add(offset);
    _steeringForce.add(force);
  }

  
  //*** flocking
  void flock(Vehicle[]Vehicles)
  {
    Vector2D averageVelocity = _velocity.clone();
    Vector2D averagePosition = new Vector2D();
    int inSightCount = 0;
    for(int v = 0; v < Vehicles.length; v++)
    {
      if(Vehicles[v] != this && inSight(Vehicles[v]))
      {
        averageVelocity.add(Vehicles[v]._velocity);
        averagePosition.add(Vehicles[v]._position);
        if(tooClose(Vehicles[v])) flee(Vehicles[v]._position);
        inSightCount++;
      }
    }
    
    if(inSightCount > 0)
    {
      averageVelocity.div(inSightCount);
      averagePosition.div(inSightCount);
      seek(averagePosition);
      averageVelocity.sub(_velocity);
      _steeringForce.add(averageVelocity);
    }
    
  }
  Boolean inSight(Vehicle vehicle)
  {
    if(_position.distV(vehicle._position) > _inSightDist) return false;
    Vector2D heading = _velocity.clone();
    heading.normalize();
    Vector2D difference = vehicle._position.clone();
    difference.sub(_position);
    float dotProd = difference.dot(heading);
    if(dotProd < 0) return false;
    
    return true;
  }
  Boolean tooClose(Vehicle vehicle)
  {
    return _position.distV(vehicle._position) < _tooCloseDist;
  }
  void setInSightDist(float value)
  {
    _inSightDist = value;
  }
  float getInSightDist()
  {
    return _inSightDist;
  }
  void setTooCloseDist(float value)
  {
    _tooCloseDist = value;
  }
  float getTooCloseDist()
  {
    return _tooCloseDist;
  }
}

class Vector2D extends PVector
{
  Vector2D()
  {
  }
 
  Vector2D(float x, float y)
  {
    super(x, y);
  }

  
  Vector2D clone()
  {
    return new Vector2D(x, y);
  }
  
  //*** zero and isZero
  Vector2D zero()
  {
    x = 0;
    y = 0;
    return this;
  }
  Boolean isZero()
  {
    return x == 0 && y == 0;
  }
  
  //*** sets/gets length
  void setLength(float someLength)
  {
    float a = getAngle();
    x = cos(a) * someLength;
    y = sin(a) * someLength;
  }
  float getLength()
  {
    return sqrt(x*x + y*y);
  }
  
  //*** sets/gets angle
  void setAngle(float someAngle)
  {
    float len = getLength();
    x = cos(someAngle) * len;
    y = sin(someAngle) * len;
  }
  float getAngle()
  {
    return atan2(y, x);
  }
  
  //*** nomalise and isNormalised
  // normalise = PVector.normalize()
  Boolean isNormalised()
  {
    float diff = getLength() - 1.0;
    if ((diff < 0.00001) && (diff >= 0.0)) return true;
    if ((diff > - 0.00001) && (diff <= 0.0)) return true;
    return false;
  }
  
  //*** sign
  //is second vector to the right (+1) or left (-1)
  //of current vector
  int sign(Vector2D v2)
  {
    return perp().dot(v2) < 0 ? -1 : 1;
  }
  
  
  //*** perpendicular vector
  Vector2D perp()
  {
    return new Vector2D(-y, x);
  }
  
  //*** isEqual
  Boolean equal(Vector2D v2)
  {
    return (x == v2.x) && (y == v2.y);
  }
  
  //*** distance to second vector2d
  float distV(Vector2D v2)
  {
    float dx = x - v2.x;
    float dy = y - v2.y;
    return sqrt(dx*dx + dy*dy);
  }
}

class Vehicle
{
  Vector2D _position;
  Vector2D _velocity;
  float _x, _y, _rotation;
  
  float _maxSpeed = 2;
  float _mass = 1.0;
  float _bounce = -1;
  
  String WRAP = new String("wrap");
  String BOUNCE = new String("bounce");
  String _edgeBehaviour = BOUNCE;  
  
  Vehicle()
  {
  }
  
  
  Vehicle(Vector2D _position, Vector2D _velocity)
  {
    this._position = _position;
    this._velocity = _velocity;
  }
  
  void update()
  {
    _velocity.limit(_maxSpeed);
    _position.add(_velocity);

    if(_edgeBehaviour.equals(WRAP)) wrapWalls();
    else if(_edgeBehaviour.equals(BOUNCE)) bounceWalls();

    display();
  }
  
  void display()
  {
//    stroke(255);
    noStroke();
    pushMatrix();
    translate(_position.x, _position.y);
    rotate(_velocity.getAngle()); 
    fill(255, 120, 0);
    ellipse(0, 0, 20, 10);
    beginShape(TRIANGLES);
    vertex(5, 0);
    vertex(-5, 10);
    vertex(-5, -10);
    endShape();
    beginShape(TRIANGLES);
    vertex(-5, 0);
    vertex(-15, 5);
    vertex(-15, -5);
    endShape();
    fill(0);
    ellipse(5, 0, 3, 3);
    popMatrix();
  }
  
  // set/get edgeBehaviour wrap or bounce
  void setEdgeBehaviour(String value)
  {
    _edgeBehaviour = value;
  }
  String getEdgeBehaviour()
  {
    return _edgeBehaviour;
  }  
  
  void wrapWalls()
  {
    if(_position.x > width)  _position.x = 0;
    else if(_position.x < 0) _position.x = width;
    if(_position.y > height) _position.y = 0;
    else if(_position.y < 0) _position.y = height;
  }
  
  void bounceWalls()
  {
    if(_position.x > width)
    {
      _position.x = width;
      _velocity.x *= _bounce;
    }
    else if(_position.x < 0)
    {
      _position.x = 0;
      _velocity.x *= _bounce;
    }
    if(_position.y > height)
    {
      _position.y = height;
      _velocity.y *= _bounce;
    }
    else if(_position.y < 0)
    {
      _position.y = 0;
      _velocity.y *= _bounce;
    }
  }
}

// steering behaviour
// Keith Peters - Advanced ActionScript Animation
// converted to processing - allonestring Jan 2010
// converted to fish August 2011

int numVehicles = 37;
SteeredVehicle[]Vehicles = new SteeredVehicle[numVehicles];


void setup()
{
  size(800, 500);
  
  for(int v = 0; v < numVehicles; v++)
  {
    Vector2D startPosition = new Vector2D(random(width), random(height));
    Vector2D startVelocity = new Vector2D(random(-2, 2), random(-2, 2));
    Vehicles[v] = new SteeredVehicle(startPosition, startVelocity);
  }
}


void draw()
{
  background(0, 16, 64);
  
  for(int v = 0; v < numVehicles; v++)
  {
    
    Vehicles[v].flock(Vehicles);
    Vehicles[v].setEdgeBehaviour("wrap");
    Vehicles[v].update();
  }
}