float L_THRESHOLD = 10;
float DELTA_TIME = 1f / 60f;
float BOX_SIZE = 2;
float FORCE = 15;
float C_TIME = 1.2;

float time;
Vector sourcePosition;
Derivatives sourceDerivatives;
State lastStateSent;
State remotePreviousState;
State remoteCurrentState;

void setup()
{
  size(400, 400);
  background(51);

  time = 0;
  sourcePosition = new Vector(200, 200);
  sourceDerivatives = new Derivatives(new Vector(0, 0), new Vector(0, 0));
  lastStateSent = new State(sourcePosition.clone(), time, sourceDerivatives.clone());
  remotePreviousState = lastStateSent.clone();
  remoteCurrentState = lastStateSent.clone();
}

void draw()
{  
  // Process input
  Vector accel = new Vector();
  if (keyPressed) {
    switch(keyCode) {
    case UP:
      accel.y = -FORCE;
      break;
    case DOWN:
      accel.y = FORCE;
      break;
    case LEFT:
      accel.x = -FORCE;
      break;
    case RIGHT:
      accel.x = FORCE;
      break;
    }
  }

  // Update source state
  sourceDerivatives.accel = accel;
  integrate(sourcePosition, sourceDerivatives, DELTA_TIME);

  // Send an update if DR threshold exceeded
  State update = deadReckoningMessage(sourcePosition, time, sourceDerivatives, lastStateSent);
  if (update != null) {
    println(time + " SEND UPDATE");
    send(update);
  }

  // Predict remote position
  Vector remotePosition = deadReckoningValue(remotePreviousState, remoteCurrentState, time);

  time += DELTA_TIME;

  // Render view
  noStroke();
  smooth();
  rectMode(CENTER);

  fill(102, 204, 0, 50);
  rect(sourcePosition.x, sourcePosition.y, BOX_SIZE, BOX_SIZE);

  fill(204, 102, 0, 50);
  rect(remotePosition.x, remotePosition.y, BOX_SIZE, BOX_SIZE);
}

/////////////////////////////////////////////////////////

void send(State update)
{
  lastStateSent = update;
  remotePreviousState = new State(prediction(remoteCurrentState, time), time, remoteCurrentState.deriv.clone());
  remoteCurrentState = update.clone();
}

Vector deadReckoningValue(State previous, State current, float t)
{
  // Zero-order convergence (or snap)
  //return prediction(current, t);

  // Linear convergence
  float startTime = previous.time;
  float endTime = startTime + C_TIME;
  if (t < endTime) {
    Vector startPos = previous.pos;
    Vector endPos = prediction(current, endTime);
    return convergenceLinear(startPos, startTime, endPos, endTime, t);
  } 
  else {
    return prediction(current, t);
  }
}

void integrate(Vector pos, Derivatives deriv, float d)
{
  deriv.vel.addLocal(deriv.accel.mult(d));
  pos.addLocal(deriv.vel.mult(d));
}

State deadReckoningMessage(Vector pos, float t, Derivatives deriv, State last)
{
  if (pos.subtract(prediction(last, t)).length() < L_THRESHOLD) {
    return null;
  }
  State u = new State(pos.clone(), t, deriv.clone());
  return u;
}

Vector convergenceLinear(Vector p0, float t0, Vector p1, float t1, float t)
{
  float d = (t - t0) / (t1 - t0);
  // d * (p1 - p0) + p0
  return p1.subtract(p0).mult(d).add(p0);
}

Vector prediction(State state, float t)
{
  float d = t - state.time;
  Vector v = state.deriv.vel;
  // pos + vd
  return state.pos.add(v.mult(d));
}

class State
{
  Vector pos;
  float time;
  Derivatives deriv;

  State(Vector pos, float time, Derivatives deriv)
  {
    this.pos = pos;
    this.time = time;
    this.deriv = deriv;
  }

  State clone()
  {
    return new State(pos.clone(), time, deriv.clone());
  }
}

class Derivatives
{
  Vector vel;

  // Source side only
  Vector accel;

  Derivatives(Vector vel, Vector accel)
  {
    this.vel = vel;
    this.accel = accel;
  }

  Derivatives clone()
  {
    return new Derivatives(vel.clone(), accel.clone());
  }
}