#include <stdio.h>
#include <stdlib.h>
#include <string.h>



//! Interpolator can interpolate values for you over time, based on an 
//! incoming stream of position/velocity/time triples. To avoid sharp 
//! snapping, it interpolates along a line that's extrapolated from the 
//! extrapolated positions of the last two updates. Each time a new 
//! update is received, it additionally updates the interpolation line.
//!
//! View illustration.
//!
//! \param Val The data type of the value being interpolated -- typically, 
//! a Vector3 in whatever library you're using.
//! \param Time The data type for times. Typically, seconds measured as 
//! a float.
//!
//! \note Your Val type needs to support addition and subtraction with 
//! itself, as well as multiplication by Time (which returns a scaled Val).
template<typename Val, typename Time = float>
class Interpolator {
  public:

    //! Initialize the behavior of the interpolator. You should typically 
    //! set calcAhead and displayBehind to an estimate of the latency of 
    //! your connection, and slopTime to the maximum time without updates 
    //! that you are willing to go before stopping the interpolation.
    struct Settings {
      Time calcAhead;       //!<  How far forward to extrapolate
      Time displayBehind;   //!<  How far back to interpolate
      Time slopTime;        //!<  How long without updates is OK
    };

    //! Construct the interpolator, giving it some initial values. Typically, 
    //! these will be taken from the first position/velocity/time packet received 
    //! for the entity.
    //! \param init The parameters for interpolation (see struct Settings).
    //! \param initPos The position that the entity starts out at.
    //! \param initVel The velocity that the entity starts out having.
    //! \param initTime The game time at which the pos/vel reading was correct.
    //! Typically, this is the timestamp at which the sender sent the data.
    Interpolator(Settings const & init, Val const & initPos, Val const & initVel, Time const & initTime) {
      settings_ = init;
      lastPos_ = initPos;
      lastVel_ = initVel;
      lastTime_ = initTime;
      lastReturnTime_ = initTime;
      lastReturnPos_ = initPos;
    }

    //! Update the interpolation with newer values. This is typically data 
    //! received from other players.
    //! \param newPos The position of the player at game time newTime.
    //! \param newVel The velocity of the player at game time newTime.
    //! \param newTime The game time at which the readings were taken.
    //! \note Interpolator will discard the values if newTime is earlier than the
    //! last received game time.
    //! \return TRUE if the data was not discarded.
    bool update(Val const & newPos, Val const & newVel, Time const & newTime) {
      if (newTime < lastTime_) {
        // Discard updates that are in the past.
        return false;
      }
      // The last returned datapoint was at lastReturnTime_. Use that to 
      // figure out where to start from. Note: if I receive an update that, 
      // when extrapolated, is behind the last returned time, the returned 
      // value starts getting very jittery. That means you aren't compensating
      // enough!
      if (settings_.displayBehind < 0.0001) {
        // Avoid a divide by zero, but snap the player
        lastPos_ = newPos;
        lastVel_ = newVel;
      }
      else {
        lastPos_ = lastReturnPos_;
        lastVel_ = (newPos + newVel * settings_.calcAhead - lastPos_)
          * (1/settings_.displayBehind);
      }
      lastTime_ = newTime;
      return true;
    }
    //! Provide estimates of the position of the entity at the given game time.
    //! \param outPos If not NULL, receives the estimated position at the given time.
    //! \param outVel If not NULL, receives the estimated velocity at the given time.
    //! \param atTime The game time at which to estimate the position (typically 
    //! later than the last received packet).
    bool read(Val * outPos, Val * outVel, Time const & atTime) {
      bool ret = false;
      if (atTime < lastTime_ + settings_.slopTime) {
        lastReturnTime_ = atTime;
        lastReturnPos_ = lastPos_ + lastVel_ * (atTime - lastTime_);
        ret = true;
      }
      if (outPos) {
        *outPos = lastReturnPos_;
      }
      if (outVel) {
        *outVel = lastVel_;
      }
      return ret;
    }

  private:
    Settings            settings_;            //!< \internal Settings from initialization.
    Val                 lastPos_;             //!< \internal Last received packet pos.
    Val                 lastVel_;             //!< \internal Last received packet velocity.
    Time                lastTime_;            //!< \internal Last received packet time.
    Time                lastReturnTime_;      //!< \internal Last time returned at.
    Val                 lastReturnPos_;       //!< \internal Last position returned.
};


//! \internal Sample Vector3 used for testing
class Vector3 {
  public:
    float x, y, z;
    Vector3() { x = y = z = 0; }
    Vector3(float ix, float iy, float iz) { x = ix; y = iy; z = iz; }
    Vector3 operator+(Vector3 const & o) const {
      return Vector3(x+o.x, y+o.y, z+o.z);
    }
    Vector3 operator-(Vector3 const & o) const {
      return Vector3(x-o.x, y-o.y, z-o.z);
    }
    Vector3 operator*(float f) const {
      return Vector3(x*f, y*f, z*f);
    }
};

//! \internal Read one data point (a simulated packet) from stdin.
bool read_line(Vector3 * op, Vector3 * ov, float * ot)
{
  char line[1024];
again:
  fprintf(stderr, "pos/vel/time> ");
  fflush(stderr);
  line[0] = 0;
  fgets(line, 1024, stdin);
  if (!line[0] || line[0] == 'q') {
    return false;
  }
  if (7 != sscanf(line, "%f , %f , %f / %f , %f , %f / %f",
        &op->x, &op->y, &op->z, &ov->x, &ov->y, &ov->z, ot)) {
      fprintf(stderr, "error: input format x,y,z / x,y,z / t\n");
    goto again;
  }
  return true;
}

int main() {
  Interpolator<Vector3, float>::Settings setting;
  setting.calcAhead = 0.2f;
  setting.displayBehind = 0.2f;
  setting.slopTime = 0.5f;
  Interpolator<Vector3, float> terp(setting, Vector3(0,0,0), Vector3(0,0,0), -1);
  float curTime = 0;
  fprintf(stderr, "Input successively reveived data packets. Type 'q' to quit.\n");
  Vector3 p, v;
  float t;
  while (read_line(&p, &v, &t)) {
    terp.update(p, v, t);
    while (curTime < t+0.15f) {
      curTime += 0.05f;
      terp.read(&p, &v, curTime);
      printf("pos: %.2f,%.2f,%.2f  vel: %.3f,%.3f,%.3f  time: %.3f\n",
          p.x, p.y, p.z, v.x, v.y, v.z, curTime);
    }
  }
  return 0;
}