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

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

// Warning, current implementation can only output ONE calibration.
// workaround : clone the object for different options.


#include "gyrocalibrator.h"
#include "archexc.h"
#include "archparam.h"

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

GyroCalibrator::GyroCalibrator() {
  possibleTOIs.insert(TOI(gyroCal,      TOI::all,    "linearCal", "deg/s/V"));  
  possibleTOIs.insert(TOI(gyroOffset,   TOI::all,    "linearCal", "deg/s/V"));  
  possibleTOIs.insert(TOI(gyroSpeed,    TOI::all   , "linearCal", "deg/s"));  
  
  needAfter = 3600;  // about one turn...
  startSample = -1;
  lastRotSpeed = -1;
  lastFence1 = lastFence2 = -1;
  for (int i=0; i<3; i++) {
    lastCalib[i] = -99999;
    lastOffset[i] = 0;
  }
  gyroProducer = NULL;
}

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* , TOI const& toi, long sampleNum, double value) {
 // if (toi.name != "gyroV" || toi.index != 2) return;
  if (toi.name != "gyroV" ) return;
  
  if (gyro[2].empty()) {
    startSample = sampleNum;
  }
  
  gyro[toi.index].push_back(value);
}

void GyroCalibrator::recomputeCalib() {
  // Integral of gyro signal between the two fences.
  
  // can we do it ?
  for (int i=0; i<3; i++) {
    lastCalib[i] = -99999;
  }
  if (startSample > lastFence1) return;
  if ((long)gyro[2].size()+startSample < lastFence2) return;
        
  double sum[3];
  for (int i=0; i<3; i++) {
    sum[i] = 0;
  }
  vector<double>::iterator i0     = gyro[0].begin() + (lastFence1-startSample);
  vector<double>::iterator i1     = gyro[1].begin() + (lastFence1-startSample);
  vector<double>::iterator i2     = gyro[2].begin() + (lastFence1-startSample);
  vector<double>::iterator stop2  = gyro[2].begin() + (lastFence2-startSample);
  for (; i2 != stop2; i0++,i1++,i2++) {
    sum[0] += *i0;
    sum[1] += *i1;
    sum[2] += *i2;
  }
    
  // average of signal
  sum[0] /= (lastFence2-lastFence1);
  sum[1] /= (lastFence2-lastFence1);
  sum[2] /= (lastFence2-lastFence1);
  
  lastOffset[0] = sum[0];
  lastOffset[1] = sum[1];
  lastOffset[2] = (lastOffset[0] + lastOffset[1])/2.;  // We don't have a better estimate...
  
  lastCalib[2]  = lastRotSpeed / (sum[2] - lastOffset[2]);
  lastCalib[0] = lastCalib[1] = lastCalib[2];          // We don't have a better estimate...
}

bool GyroCalibrator::fetchFences(long sampleNum) {
  map<TOI, TOIProducer*> & m = (*(neededTOIs.begin())).second;
  
  long oldf2 = lastFence2;
  for (map<TOI, TOIProducer*>::iterator i = m.begin(); i != m.end(); i++) {
    TOI const& inToi = (*i).first;
    TOIProducer* prod =  (*i).second;
    if (inToi.name == "rotSpeed")        lastRotSpeed = prod->getValue(sampleNum, inToi);
    if (inToi.name == "rotSpeedSample1") lastFence1   = long(prod->getValue(sampleNum, inToi));
    if (inToi.name == "rotSpeedSample2") lastFence2   = long(prod->getValue(sampleNum, inToi));
    if (inToi.name == "gyroV")           gyroProducer = prod;
  }
  
  return (oldf2 != lastFence2);
}


bool GyroCalibrator::canGetValue(long sampleNum, TOI const& ) {
  // We can get value if sampleNum is between the two fences, or
  // if a new fence later than sampleNum is now available.
  // In any case, we must have gyro data up to the highest fence.
  
  if (startSample > lastFence1) return false; // will never work...
  if (sampleNum >= lastFence1 && sampleNum < lastFence2 && (gyro[2].size()+startSample >= lastFence2)) return true;
  if ((long)gyro[2].size()+startSample < lastFence2) return false; // We have to wait for more gyro data
  if (fetchFences(sampleNum)) {
    recomputeCalib(); // do it now to keep a consistent state
  }
  if (sampleNum >= lastFence1 && sampleNum < lastFence2) {
    return true;
  } else {
    return false;
  }  
}

bool GyroCalibrator::canGetValueLater(long sampleNum, TOI const& ) {
  if (sampleNum >= lastFence1 && sampleNum < lastFence2 && ((long)gyro[2].size()+startSample >= lastFence2)) return false;
      // because can get now
  if (sampleNum >= lastFence2) { // check if new fence...
    if (fetchFences(sampleNum)) {
      recomputeCalib(); // do it now to keep a consistent state
    }
  }
  
  // Si nos fournisseurs ne peuvent pas, nous non plus...
  map<TOI, TOIProducer*> & m = (*(neededTOIs.begin())).second;
  for (map<TOI, TOIProducer*>::iterator i = m.begin(); i != m.end(); i++) {
    TOI const& inToi = (*i).first;
    TOIProducer* prod =  (*i).second;
    if (!prod->canGetValue(sampleNum, inToi) && !prod->canGetValueLater(sampleNum, inToi)) return false;

  }
  
  if (lastFence1<0 || lastFence2<0) return true;
  if (sampleNum > lastFence2) return true;
  return (startSample <= lastFence1 && (long)gyro[2].size()+startSample < lastFence2 && sampleNum > lastFence1);
}

double GyroCalibrator::getValue(long sampleNum, TOI const& toi) {
  if (startSample > lastFence1) return -1; // will never work...
  if ((long)gyro[2].size()+startSample < lastFence2) return -1; // We have to wait for more gyro data
  if (!(sampleNum >= lastFence1 && sampleNum < lastFence2)) {
    if (fetchFences(sampleNum)) {
      if ((long)gyro[2].size()+startSample < lastFence2) return -1; // We have to wait for more gyro data
      recomputeCalib(); // do it now to keep a consistent state
    }
  }
  
  if (!(sampleNum >= lastFence1 && sampleNum < lastFence2)) 
    return -1;
    
  if (lastCalib[0] < 0) recomputeCalib();
    
  if (toi.name == gyroCal)    return lastCalib[toi.index];
  if (toi.name == gyroOffset) return lastOffset[toi.index];
  if (toi.name == gyroSpeed) {
    TOI toi2 = TOI("gyroV", toi.index); // $CHECK$ maybe should get from reqtoi ?
    double gv = gyroProducer->getValue(sampleNum, toi2);
    return (gv-lastOffset[toi.index])*lastCalib[toi.index];
  }
  throw ArchExc("Cannot produce "+toi.fullName());
}


void GyroCalibrator::propagateLowBound(TOI const& toi, long sampleNum) {
   if (startSample < lastFence1) {
     if (gyro[0].size() > lastFence1 - startSample) {
       for (int i=0; i<3; i++) {
         vector<double>::iterator x = gyro[i].begin() + lastFence1 - startSample;
         gyro[i].erase(gyro[i].begin(), x);
       }
       startSample = lastFence1;
     }
   }
   TOIPullProducer::propagateLowBound(toi, sampleNum);
}