class State{
  // Utility class 
  // Velocity is separated in heading (direction) and speed (magnitude).
  // This makes it easier to work with static particles, a 0-vector has no direction :-(

  Vec3D position;
  Vec3D heading;
  float speed;

  State(Vec3D pos, Vec3D hdg, float spd){
    position = pos.copy();
    heading = hdg.getNormalized().copy();
    speed = spd;
  }

  State copy(){
    return new State(position, heading, speed);

  }
}


class Tracker{
  State currentState;
  State previousState;
  State targetState;   // targetState contains the current target position, heading to the target and speed particle is trying to maintain.

  float maxSpeed; // maximum speed when lined up with target
  float maxTurnSpeed; // maximum speed while turning
  float maxAcc; // maximum increase of speed per update
  float maxDec; // maximum decrease of speed per update
  float maxTurn; // maxim change of heading per update


  Tracker(Vec3D pos, Vec3D hdg, float spd, float maxSpd, float maxA, float maxD, float maxTrn, Vec3D tgt){
    currentState = new State(pos,hdg,spd);
    previousState = currentState.copy();
    maxSpeed = maxSpd;
    maxTurnSpeed = maxSpd*0.5f;
    maxAcc = maxA;
    maxDec = maxD;
    maxTurn=maxTrn;

    // Initial heading is determined by target, if identical to initial position, use a random heading instead
    Vec3D tmpHdg;

    if(tgt.sub(pos).isZeroVector()){
      tmpHdg=Vec3D.randomVector();  
    }
    else{
      tmpHdg = tgt.sub(pos).getNormalized();
    }

    targetState = new State(tgt, tmpHdg, maxSpd);
  }


  void update(Vec3D tgt){

    previousState = currentState.copy();         

    // Determine new heading
    Vec3D tmpHdg = tgt.sub(currentState.position);

    // Reduce maximum speed if close to target otherwise high speeds result in orbiting particles never reaching the target
    float distanceFactor = min(tmpHdg.magSquared()/(maxSpeed*maxSpeed),1f);

    // If target is reached keep current heading
    if(tmpHdg.isZeroVector()){
      tmpHdg= targetState.heading.copy();
    }
    else{
      tmpHdg.normalize();
    }

    targetState.position = tgt.copy();
    targetState.heading = tmpHdg.copy();
    targetState.speed = maxSpeed*distanceFactor;


    // Turn heading towards target
    float angle = currentState.heading.angleBetween(targetState.heading);
    if (angle<maxTurn){
      // Target heading reached
      currentState.heading = targetState.heading.copy();

    }
    else{
      // Rotate current heading towards target heading
      // Rotation axis determined by cross product
      // * Disadvantage: sensitive to numerical errors: renomralization is critical...
      // * Advantage: works in arbitrary 3D
      // * Advantage: automatically rotates over smallest angle

        Vec3D axis = currentState.heading.cross(targetState.heading);
      axis.normalize();
      currentState.heading = currentState.heading.rotateAroundAxis(axis,maxTurn);
      currentState.heading.normalize();
      targetState.speed = targetState.speed*0.5f;
    }


    // Adjust speed, if too fast, brake, if too slow, accelerate
    if(currentState.speed > targetState.speed){
      if(currentState.speed > targetState.speed + maxDec){
        currentState.speed-=maxDec;
      }
      else{
        currentState.speed = targetState.speed;
      }
    }
    else{
      if(currentState.speed < targetState.speed - maxAcc){
        currentState.speed+=maxAcc;
      }
      else{
        currentState.speed = targetState.speed;
      }
    }

    // update position, velocity = speed * heading
    currentState.position.addSelf(currentState.heading.scale(currentState.speed));


  }


  void draw(){
    // Embossed lines for 3D illusion
    stroke(255,10);
    line(currentState.position.x,currentState.position.y-1,currentState.position.z,previousState.position.x,previousState.position.y-1,previousState.position.z);   
    stroke(220,170,124,10);
    line(currentState.position.x+1,currentState.position.y+1,currentState.position.z,previousState.position.x+1,previousState.position.y+1,previousState.position.z);
    stroke(0,10);
    line(currentState.position.x,currentState.position.y,currentState.position.z,previousState.position.x,previousState.position.y,previousState.position.z);




  }







}