// gyrocalibrator.cc
// Eric Aubourg         CEA/DAPNIA/SPP   octobre 1999

// assumption : same calibration for all gyros...


#include "gyrocalibrator.h"

#define gyroCal    "gyroCal"
#define gyroSpeed  "gyroSpeed"

GyroCalibrator::GyroCalibrator() {
  possibleTOIs.insert(TOI(gyroCal,      TOI::unspec, "linearCal", "deg/s/V"));  
  possibleTOIs.insert(TOI(gyroSpeed,    TOI::all   , "linearCal", "deg/s"));  
  
  //needAfter = 3600;  // about one turn...
  startSample = -1;
  lastFence1 = lastFence2 = -1;
}

string GyroCalibrator::getName() {
  return("GyroCalibrator 1.0");
}


set<TOI> GyroCalibrator::reqTOIFor(TOI const&) {
  set<TOI> t; 
  t.insert(TOI("rotSpeed",        TOI::unspec)); // pull only
  t.insert(TOI("rotSpeedSample1", TOI::unspec)); // pull only
  t.insert(TOI("rotSpeedSample2", TOI::unspec)); // pull only
  t.insert(TOI("gyroV", 0));  // push
  t.insert(TOI("gyroV", 1));  // push
  t.insert(TOI("gyroV", 2));  // push
  return t;
}

void GyroCalibrator::dataFeed(TOIProducer* source, TOI const& toi, long sampleNum, double value) {
  if (toi.name != "gyroV" || toi.index != 2) return;
  
  if (gyro2.empty()) {
    startSample = sampleNum;
  }
  
  gyro2.push_back(value);
}