timestampManager.hpp

#include <iostream>
#include <iomanip>

#define AVGSIZE 3

#define PHI 1.618033989 // golden ratio
/*
struct Timestamp
{
  double sensorDate;
  double rawhostDate;
  double fixedhostDate;
  double arrivalDate;
};
*/

class TimestampSensor
{
 private:
  double clockScale;
  double clockShiftS;
  double clockShiftH;
  double latency;
  double averagingLength;

  bool first;
  unsigned sector;
  double next_sector_date;
  double delay_min;
  double currentAvgLength;
  unsigned nAvgLength;

  double sectorBestS[AVGSIZE];
  double sectorBestH[AVGSIZE];

 public:
  TimestampSensor(double clockScale, double latency, double averagingLength = 30):
    clockScale(clockScale), latency(latency), averagingLength(averagingLength), first(true), sector(0), currentAvgLength(0.1), nAvgLength(0)
  {}

  double filter(double sensorDate, double hostDate)
  {
    // first
    if (first)
    {
      clockShiftS = sensorDate;
      clockShiftH = hostDate - latency;
      delay_min = 1e9;
      first = false;
      next_sector_date = hostDate + currentAvgLength;
      return hostDate;
    }

    // new sector
    if (hostDate > next_sector_date)
    {
      if (sector >= AVGSIZE-1) // if we have at least AVGSIZE sectors
      {
        unsigned best1, best2;
        double max_scale = 0.;
        for(int i = 0; i < AVGSIZE-1; ++i)
          for(int j = i+1; j < AVGSIZE; ++j)
          {
            // compute scale hypothesis
            unsigned s1 = (sector+1+i) % AVGSIZE;
            unsigned s2 = (sector+1+j) % AVGSIZE;
            double scale = (sectorBestH[s2]-sectorBestH[s1])/(sectorBestS[s2]-sectorBestS[s1]);

            // check it is comptatible with all the other sectors
            bool compatible = true;
            for(int k = 0; k < AVGSIZE; ++k) if (k != s1 && k != s2)
              if (sectorBestH[k] - (sectorBestH[s2] + (sectorBestS[k]-sectorBestS[s2])*scale) < 0)
                { compatible = false; break; }

            // if so check if it is the best so far
            if (compatible && scale > max_scale) { max_scale = scale; best1 = s1; best2 = s2; }
          }

        // compute final scale
        clockScale = (sectorBestH[best2]-sectorBestH[best1])/(sectorBestS[best2]-sectorBestS[best1]);
        clockShiftS = sectorBestS[best2];
        clockShiftH = sectorBestH[best2]-latency;
      }

      if (nAvgLength == 0)
      {
        currentAvgLength *= PHI; // using this ratio so that when we merge two sectors, the new size is the same than the next one
        if (currentAvgLength > averagingLength) { currentAvgLength = averagingLength; nAvgLength++; }
      } else
      if (nAvgLength <= AVGSIZE) nAvgLength++;

      // merge two first sectors
      if (nAvgLength <= AVGSIZE)
      {
        unsigned s1 = (sector+1) % AVGSIZE;
        unsigned s2 = (sector+2) % AVGSIZE;

        if ((sectorBestH[s1] - (clockShiftH + (sectorBestS[s1]-clockShiftS)*clockScale + latency))
          < (sectorBestH[s2] - (clockShiftH + (sectorBestS[s2]-clockShiftS)*clockScale + latency)))
        {
          sectorBestS[s2] = sectorBestS[s1];
          sectorBestH[s2] = sectorBestH[s1];
        }
      }

      ++sector;
      next_sector_date += currentAvgLength;
      delay_min = 1e9;
    }

    // update
    double fixedDate = clockShiftH + (sensorDate-clockShiftS)*clockScale;
    double delay = hostDate - (fixedDate + latency);

    // check shift
    if (delay < 0)
    {
      clockShiftH += delay;
      fixedDate += delay;
      sectorBestS[sector % AVGSIZE] = sensorDate;
      sectorBestH[sector % AVGSIZE] = hostDate;
      delay_min = 0;
    }

    if (delay < delay_min)
    {
      sectorBestS[sector % AVGSIZE] = sensorDate;
      sectorBestH[sector % AVGSIZE] = hostDate;
      delay_min = delay;
    }

    return fixedDate;
  }


  //bool isExpectingDataBefore(double date); // NOT IMPLEMENTED YET

  #if 0 // SYNC
  private:
    TimestampSensor *syncSensor;
    double syncShift;

  public:
    void syncTo(TimestampSensor *sensor, double shift);
  #endif
};



class TimestampManager
{












};