// starmatcher.cc
// Eric Aubourg         CEA/DAPNIA/SPP   novembre 1999

#include "starmatcher.h"
#include "sststarfinder.h"
#include "toimanager.h"
#include "archexc.h"
#include "archparam.h"
#include "gondolageom.h"
#include "polfitclip.h"

#define STARDUMP

#define TEchan TFin

#include <math.h>

extern "C" {
#include "aa_hadec.h"
#define NRANSI
#include "nrutil.h"

#ifndef M_PI
#define M_PI 3.1415926535
#endif

void lfit(double x[], double y[], double sig[], int ndat, double a[], int ia[],
	int ma, double **covar, double *chisq, void (*funcs)(double, double [], int));

void polfunc(double x, double afunc[], int ma);
void sinfunc(double x, double afunc[], int ma);
}

void polfunc(double x, double afunc[], int ma) {
  afunc[1] = 1;
  for (int i=2; i<=ma; i++)
    afunc[i] = afunc[i-1]*x;
}

void sinfunc(double x, double afunc[], int /*ma*/) {
  afunc[1] = cos(x);
  afunc[2] = sin(x);
  afunc[3] = 1;
}


double polval(double x, double a[], int ma);

double polval(double x, double a[], int ma) {
  double r = a[ma];
  for (int i=ma-1; i>0; i--) {
    r = r*x+a[i];
  }
  return r;
}

#include <stdio.h>

#ifdef __DECCXX
#define SWAP
#endif
#if defined(Linux)  || defined(linux)
#define SWAP
#endif

typedef unsigned int4 uint_4;
typedef unsigned short uint_2;

static inline void bswap4(void* p)
{
  uint_4 tmp = *(uint_4*)p;
  *(uint_4*)p = ((tmp >> 24) & 0x000000FF) | 
                ((tmp >> 8)  & 0x0000FF00) |
                ((tmp & 0x0000FF00) << 8)  |
                ((tmp & 0x000000FF) << 24);
}

static inline void bswap2(void* p)
{
  uint_2 tmp = *(uint_2*)p;
  *(uint_2*)p = ((tmp >> 8) & 0x00FF) | 
                ((tmp & 0x00FF) << 8);
}


#define azimuthPendul   "azimuthPendul"
#define anglePendul     "anglePendul"
#define azimuthAxis     "azimuthAxis"
#define elvAxis         "deltaZenith"
#define alphaAxis       "alphaZenith"
#define deltaAxis       "deltaZenith"
#define azimuthFPC      "azimuthFPC"
#define elvFPC          "elvFPC"
#define alphaFPC        "alphaFPC"
#define deltaFPC        "deltaFPC"
#define azimuthBolo     "azimuthBolo"
#define elvBolo         "elvBolo"
#define alphaBolo       "alphaBolo"
#define deltaBolo       "deltaBolo"
#define azimuthSST      "azimuthSST"
#define elvSST          "elvSST"
#define alphaSST        "alphaSST"
#define deltaSST        "deltaSST"


StarMatcher::StarMatcher() {
  possibleTOIs.insert(TOI(azimuthSST,       TOI::unspec, "interp", "degrees","sstmatch"));  
  possibleTOIs.insert(TOI(elvSST,           TOI::unspec, "interp", "degrees","sstmatch"));  
  possibleTOIs.insert(TOI(alphaSST,         TOI::unspec, "interp", "hours","sstmatch"));  
  possibleTOIs.insert(TOI(deltaSST,         TOI::unspec, "interp", "degrees","sstmatch"));  
  possibleTOIs.insert(TOI(azimuthAxis,      TOI::unspec, "interp", "degrees","sstmatch"));  
  possibleTOIs.insert(TOI(elvAxis,          TOI::unspec, "interp", "degrees","sstmatch"));  
  possibleTOIs.insert(TOI(alphaAxis,        TOI::unspec, "interp", "hours","sstmatch"));  
  possibleTOIs.insert(TOI(deltaAxis,        TOI::unspec, "interp", "degrees","sstmatch"));  
  possibleTOIs.insert(TOI(azimuthPendul,    TOI::unspec, "interp", "degrees","sstmatch"));  
  possibleTOIs.insert(TOI(anglePendul,      TOI::unspec, "interp", "degrees","sstmatch"));  
  possibleTOIs.insert(TOI(azimuthFPC,       TOI::unspec, "interp", "degrees", "sstmatch"));  
  possibleTOIs.insert(TOI(elvFPC,           TOI::unspec, "interp", "degrees", "sstmatch"));  
  possibleTOIs.insert(TOI(alphaFPC,         TOI::unspec, "interp", "hours",   "sstmatch"));  
  possibleTOIs.insert(TOI(deltaFPC,         TOI::unspec, "interp", "degrees", "sstmatch"));  
  possibleTOIs.insert(TOI(azimuthBolo,      TOI::all,    "interp", "degrees", "sstmatch"));  
  possibleTOIs.insert(TOI(elvBolo,          TOI::all,    "interp", "degrees", "sstmatch"));  
  possibleTOIs.insert(TOI(alphaBolo,        TOI::all,    "interp", "hours",   "sstmatch"));  
  possibleTOIs.insert(TOI(deltaBolo,        TOI::all,    "interp", "degrees", "sstmatch"));  

  FILE* f;
  
  f = fopen("gsc7.dat","r");
  if (!f) throw ArchExc("Error opening gsc7.dat");
  
  int4 n4;
  fread(&n4, sizeof(int4), 1 , f);

#ifdef SWAP
  bswap4(&n4);
#endif
  nstars = n4;

  stars = new gscStar[nstars];
  char* compdata = new char[10*nstars];
  fread(compdata, 10, nstars, f);
  fclose(f);
  
  for (int i=0; i<nstars; i++) {
#ifdef SWAP
     ((char*)&(stars[i].ra))[0]  = compdata[10*i+3];
     ((char*)&(stars[i].ra))[1]  = compdata[10*i+2];
     ((char*)&(stars[i].ra))[2]  = compdata[10*i+1];
     ((char*)&(stars[i].ra))[3]  = compdata[10*i+0];
     ((char*)&(stars[i].dec))[0] = compdata[10*i+7];
     ((char*)&(stars[i].dec))[1] = compdata[10*i+6];
     ((char*)&(stars[i].dec))[2] = compdata[10*i+5];
     ((char*)&(stars[i].dec))[3] = compdata[10*i+4];
     ((char*)&(stars[i].mag))[0] = compdata[10*i+9];
     ((char*)&(stars[i].mag))[1] = compdata[10*i+8];
#else
     ((char*)&(stars[i].ra))[0]  = compdata[10*i+0];
     ((char*)&(stars[i].ra))[1]  = compdata[10*i+1];
     ((char*)&(stars[i].ra))[2]  = compdata[10*i+2];
     ((char*)&(stars[i].ra))[3]  = compdata[10*i+3];
     ((char*)&(stars[i].dec))[0] = compdata[10*i+4];
     ((char*)&(stars[i].dec))[1] = compdata[10*i+5];
     ((char*)&(stars[i].dec))[2] = compdata[10*i+6];
     ((char*)&(stars[i].dec))[3] = compdata[10*i+7];
     ((char*)&(stars[i].mag))[0] = compdata[10*i+8];
     ((char*)&(stars[i].mag))[1] = compdata[10*i+9];
#endif
  }
  
  delete[] compdata;

  TOIProducer* prod = TOIManager::findTOIProducer(TOI("sstStarCount"));
  if (!prod) {
    cerr << "StarMatcher : cannot find producer for sstStarCount" << endl;
    exit(-1);
  }
  
  SSTStarFinder* sprod = dynamic_cast<SSTStarFinder*>(prod);
  if (!sprod) {
    cerr << "StarMatcher : producer for sstStarCount is not a SSTStarFinder" << endl;
    exit(-1);
  }
  
  lastSeq = 0;
  
  sprod->registerProcessor(this);

}

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

#ifdef STARDUMP
static ofstream starstream("stars.dat");
static ofstream cstarstream("cstars.dat");
static ofstream pendstream("pendul.dat");
#endif
static ofstream logstream("starmatch.log");

void StarMatcher::dataFeed(SSTEtoile const& x) {
  lastStars.push_back(x);
}

static long lastCleanSave=0;

void nrerror(char * error_text) {
  throw(string(error_text));
}


void StarMatcher::processStars() {

  if (lastStars.empty()) return;

  map<TOI, TOIProducer*> & m = (*neededTOIs.begin()).second;
  while (!lastStars.empty()) {
    SSTEtoile lastStar = lastStars.front();
    lastStars.pop_front();

    double lat, lon, ts, alpha, delta, az, rspeed;
    
    long snstar = (long) lastStar.TEchan;
  
    for (map<TOI, TOIProducer*>::iterator i = m.begin(); i != m.end(); i++) {
      TOI const& inToi = (*i).first;
      TOIProducer* prod =  (*i).second;
      if (inToi.name == "latitude")    lat = prod->getValue(snstar, inToi);
      if (inToi.name == "longitude")   lon = prod->getValue(snstar, inToi);
      if (inToi.name == "tsid")         ts = prod->getValue(snstar, inToi);
      if (inToi.name == "alphaSST")  alpha = prod->getValue(snstar, inToi);
      if (inToi.name == "deltaSST")  delta = prod->getValue(snstar, inToi);
      if (inToi.name == "azimuthSST")   az = prod->getValue(snstar, inToi);
      if (inToi.name == "rotSpeed") rspeed = prod->getValue(snstar, inToi);
    }
    
    // correct azimuth using fractional value of TEchan
    
    az -= rspeed * archParam.acq.perEch * (lastStar.TEchan-snstar);
    
    // find all stars +- 2 deg boresight
    double dist = 2;
    double dmin = delta - dist; if (dmin<-90) dmin=-90;
    double dmax = delta + dist; if (dmax> 90) dmax= 90;
    double amin = alpha - dist / cos(delta * M_PI/180) / 15.;
    if (amin<0) amin += 24;
    double amax = alpha + dist / cos(delta * M_PI/180) / 15.;
    if (amax>24) amax -= 24;
  
    int a,b,c;
    a=0; c=nstars-1;
    while (a+1<c) {
      b = (a+c)/2;
      if (stars[b].dec < dmin) a=b; else c=b;
    }
    int imin = a;
    a=0; c=nstars;
    while (a+1<c) {
      b = (a+c)/2;
      if (stars[b].dec < dmax) a=b; else c=b;
    }
    int imax = c;

    for (int i=imin; i<=imax; i++) {
      if (stars[i].ra >= amin && stars[i].ra <= amax) {
        double ha = (ts/3600. - stars[i].ra) * 15. * M_PI/180.;
        double elv, azim;
        hadec_aa(lat * M_PI/180., ha, stars[i].dec * M_PI/180.,
                 &elv, &azim);
        elv  *= 180/M_PI;
        azim *= 180/M_PI;
        if (azim<0) azim += 360;
      
        double da = azim-az; if (da>360) da -= 360;
       // if (da < -0.6 || da > 0.4) continue; // appropriate for TEchan
        if (da < -0.7 || da > 0.3) continue; // appropriate for TFin
        double elv0 = elv - GondolaGeom::sstPixelHeight * lastStar.NoDiode;
        if (fabs(elv0-GondolaGeom::elevSST0) > 0.25) continue; // Might be too strong
        
#ifdef STARDUMP     
        starstream << setprecision(10)  << lastStar.TEchan << " " << 
          lastStar.NoDiode << " " <<
          alpha << " " << delta << " " <<
          az << " " << 
          stars[i].ra << " " << stars[i].dec << " " <<
          elv << " " << azim << " " <<
          lastStar.InpCurrent << " " << stars[i].mag << "\n";
#endif
          
        matchStar s;
        lastSeq++;
        s.SN       = lastStar.TEchan;
        s.raGSC    = stars[i].ra;
        s.decGSC   = stars[i].dec;
        s.azGSC    = azim;
        s.elvGSC   = elv;
        s.nDiode   = lastStar.NoDiode;
        s.ok       = true;
        s.seq      = lastSeq;
        s.lon      = lon;
        s.lat      = lat;
        s.ts       = ts;
        
        matchStars.push_back(s);
      }
    }
  }
  
  // new set of matched stars... Clean, and get parameters...
  // We don't want more than 30 seconds of data
  
  if (matchStars.empty()) return;
  
  
  double snEnd = matchStars.back().SN;
  deque<matchStar>::iterator it;
  for (it = matchStars.begin();  it!=matchStars.end(); it++) {
     if ((snEnd - (*it).SN)*archParam.acq.perEch < 30 ||
         (*it).seq > lastCleanSave) 
       break;
  }
  if (it != matchStars.begin()) {
    it--;
  }
  if (it != matchStars.begin()) {
    matchStars.erase(matchStars.begin(), it);
  }
  
  // we want to clean on the last 5 seconds of data.
  
  int nskip=0;
  for (it = matchStars.begin();  it!=matchStars.end(); it++) {
     if ((snEnd - (*it).SN)*archParam.acq.perEch < 5) 
       break;
     nskip++;
  }
  
  if (matchStars.size()-nskip < 30) return; // pas assez d'etoiles
  
  // we remove "bursts" of stars, ie more than 4 stars in the same samplenum
  
  double lastSN = 0;
  deque<matchStar>::iterator lastIt = it;
  long burstLen = 0;
  for (deque<matchStar>::iterator it1 = it ;  it1!=matchStars.end(); it1++) {
    matchStar s = (*it1);
    if ((long) s.SN == lastSN) {
      burstLen++;
      continue;
    }
    if (burstLen >= 4) {
      for (deque<matchStar>::iterator it2=lastIt; it2 != it1; it2++) {
        //if ((*it2).ok) 
        //  logstream << "  kill " << (*it2).seq << " " << setprecision(11) << (*it2).SN << " burst" << '\n';
        (*it2).ok=false;
      }
    }
    burstLen = 1;
    lastIt = it1;
    lastSN = s.SN;
  }
  // we fit the data to a polynomial, with clipping...
  
  //double* sn   =  ::dvector(1, matchStars.size());
  double* elv0 =  ::dvector(1, matchStars.size());
  double* azi  =  ::dvector(1, matchStars.size());
  double* sig  =  ::dvector(1, matchStars.size());
  //double* ae   =  ::dvector(1, 3);
  double* aa   =  ::dvector(1, 3);
  int*  ia    = ivector(1, 3);
  double** cov =  matrix(1, 3, 1, 3);
  int ndata;
  
  //long sn0 = matchStars.front().SN;
  double sn0 = (*it).SN;
  PolFitClip2 fitElvAz(matchStars.size(), 2); fitElvAz.setClip(0.1,0,2,3);
  ndata = 0;

  double oldAz = -1;
  for (deque<matchStar>::iterator it1 = it ;  it1!=matchStars.end(); it1++) {
    matchStar s1 = (*it1);
    if (!s1.ok) continue;
    double az = s1.azGSC;
    if (ndata > 0 && az - oldAz >  180) az -= 360;
    if (ndata > 0 && az - oldAz < -180) az += 360;
    fitElvAz.addData(s1.SN-sn0, s1.elvGSC - s1.nDiode*GondolaGeom::sstPixelHeight, az);
    oldAz = az;
    ndata++;
  }
   
  double celv[3], caz[3];
  if (fitElvAz.doFit(celv,caz)) return;
  //if (fitElvAz.doFit()) return;
  
  //logstream << "*** Fit sig=" << fitElvAz.getSigmaY() << " " << fitElvAz.getSigmaZ()
  //          << " n  =" << fitElvAz.getNData() << " " << fitElvAz.getNDataUsed() 
  //          << " SN :" << fitElvAz.getXMin() + sn0 << " - " << fitElvAz.getXMax() + sn0 << '\n';
  //logstream << " sn0 = " << sn0 << "; snmin =" << fitElvAz.getXMin() + sn0 << "; snmax =" 
  //          << fitElvAz.getXMax() + sn0  << '\n';
  //logstream << " fitelv[x_] := " << celv[2] << " x^2 + " << celv[1] << " x + " << celv[0] << '\n';
  //logstream << " fitaz[x_]  := " <<  caz[2] << " x^2 + " <<  caz[1] << " x + " <<  caz[0] << '\n';
  
  //if (fitElvAz.getSigmaY() > 0.05 || fitElvAz.getSigmaZ() > 0.05) return;
  if (fitElvAz.getNDataUsed() < 5 ||
      (double)fitElvAz.getNDataUsed()/fitElvAz.getNData() < .5) return;
      
  double dcutElv = fitElvAz.getSigmaY()*3;
  double dcutAz  = fitElvAz.getSigmaZ()*3;
  
  if (dcutElv < 0.05) dcutElv = 0.05;
  if (dcutAz  < 0.05) dcutAz  = 0.05;

  // don't kill borders of fit....
  //if (matchStars.end() - it > 6)
  //  for (deque<matchStar>::iterator it1 = it+3 ;  it1!=matchStars.end()-3; it1++) {
  for (deque<matchStar>::iterator it1 = it ;  it1!=matchStars.end(); it1++) {
      matchStar sss = (*it1);
      if (!sss.ok) continue;
      if (fabs(fitElvAz.valueY(sss.SN-sn0)-
	       (sss.elvGSC - sss.nDiode*GondolaGeom::sstPixelHeight)) > dcutElv) {
        (*it1).ok = false;
        //logstream << "  kill " << sss.seq << " " << setprecision(11) << sss.SN << " "
        //          << fitElvAz.valueY(sss.SN-sn0)-(sss.elvGSC - sss.nDiode*1.41/45.) << '\n';
        continue;
      }
      double daz = fitElvAz.valueZ(sss.SN-sn0) - sss.azGSC;
      if (daz>=180) daz -= 360;
      if (daz<-180) daz += 360;
      if (fabs(daz) > dcutAz) (*it1).ok = false;
      if (!(*it1).ok) {
        //logstream << "  kill " << sss.seq << " " << setprecision(11) << sss.SN << " "
        //          << fitElvAz.valueY(sss.SN-sn0)-(sss.elvGSC - sss.nDiode*1.41/45.) << " "
        //          << daz << '\n';
      }
  }
    
  bool gotNewStars = false;
  for (deque<matchStar>::iterator it1 = matchStars.begin() ;  it1!=it; it1++) {
    if ((*it1).ok && (*it1).seq > lastCleanSave) {
      gotNewStars = true;
      lastCleanSave = (*it1).seq;
#ifdef STARDUMP
      cstarstream <<  (*it1).seq << "\n";
#endif
      posInfo info;
      info.SN      =   (*it1).SN;
      info.azStar  =   (*it1).azGSC;
      info.elvStar =   (*it1).elvGSC;
      info.diodStar=   (*it1).nDiode;
      info.lon     =   (*it1).lon;
      info.lat     =   (*it1).lat;
      info.ts      =   (*it1).ts;
      posInfos[info.SN] = info;
    }
  }
  
  if (!gotNewStars) return;
  
  // On a des etoiles nettoyees, on va trouver amplitude et phase du
  // signal en elevation, ce qui va nous donner les deux angles d'Euler
  // de la pendulation (au premier ordre en theta)
  
  // Il faut avoir une periode entiere ou pas loin, sinon on ne peut
  // rien dire simplement.... -> we want to run on the last 18 seconds of
  // data before the last fully cleaned star (it).
  
  deque<matchStar>::iterator itstart;
  
  for (itstart = matchStars.begin();  itstart != it; itstart++) {
     if (((*it).SN - (*itstart).SN)*archParam.acq.perEch < 19) 
       break;
  }
  
  if (((*it).SN - (*itstart).SN)*archParam.acq.perEch < 15) return;

  
 // it = matchStars.end(); it--;
 // if (((*it).SN - matchStars.front().SN)*archParam.acq.perEch < 17) return;
  
  // $CHECK$ utiliser plutot le SN moyen/median de tous les points effectivement utilises.
  double snmid = ((*it).SN + (*itstart).SN)/2;
    
  ndata=0;  
  double snmean = 0;

  logstream << "PendFit : " <<  setprecision(11) << (*itstart).SN << '-' << (*it).SN <<  " "
            << setprecision(4)
            << ((*it).SN - (*itstart).SN)*archParam.acq.perEch << " " ;
              
  for (deque<matchStar>::iterator it1 = itstart ;  it1!=it; it1++) {
    matchStar st = *it1;
    if (!st.ok) continue;
    ndata++;
    snmean += st.SN;
    azi[ndata]  = st.azGSC * M_PI/180;
    elv0[ndata] = st.elvGSC - st.nDiode*GondolaGeom::sstPixelHeight;
    sig[ndata]  = 0.01;
  }
  if (ndata) snmean /= ndata;
  
  ia[1]   =  ia[2] = 1;
  ia[3] = 0;
  aa[3] = GondolaGeom::elevSST0;// do not fit elv0

  if (ndata<5) return;
  double chi2;
  try {
    lfit(azi, elv0,  sig, ndata, aa, ia, 3, cov, &chi2, sinfunc);
  } catch(string st) {
    return;
  }

  double cc = aa[1];
  double ss = aa[2];
  
  logstream << setprecision(11) << snmean << setprecision(4)
            << " cs=" << cc << " " << ss << " chi2r=" << chi2/ndata 
            << " cov " << cov[1][1] << " " << cov[2][2] << '\n';
  
  // Get rid of bad fits. The cuts are rather ad hoc
  
  //if (aa[3] < 39.64 || aa[3] > 39.68) return;
  if (chi2/ndata > 9) return;
  if (cov[1][1] > 0.0001) return;
  if (cov[2][2] > 0.0001) return;

  double ampl = sqrt(cc*cc+ss*ss);
  double phase = atan2(cc,ss)/(M_PI/180);
  
  
  pendulInfo info;
  info.SN          =   snmean;
  info.azPendul    =   180-phase;
  if (info.azPendul > 360) info.azPendul -= 360;
  if (info.azPendul <   0) info.azPendul += 360;
  info.angPendul   =   ampl;
  pendulInfos[info.SN] = info;

#ifdef STARDUMP
  pendstream << setprecision(11) << snmean << " " 
             << setprecision(4) << ampl << " " << info.azPendul << " " << ndata << " " << chi2/ndata <<  " "
             << cov[1][1] << " " << cov[2][2] << '\n';
#endif
  /*
  double snum = (matchStars.front().SN + matchStars.back().SN)/2-sn0;
  if (snmin > snum || snmax < snum) return;
  double elsst = polval(snum, ae, 3);
  double azsst = polval(snum, aa, 3);
  
  if (azsst > 360) azsst -= 360;
  if (azsst < 0  ) azsst += 360;
  */
  
//  for (set<TOI>::iterator i = producedTOIs.begin(); i!=producedTOIs.end(); i++) {
//    if ((*i).name == "azimuthSST")  computedValue((*i), snum+sn0, azsst);
//    if ((*i).name == "elvSST")      computedValue((*i), snum+sn0, elsst);
//  }
  
  //free_vector(sn, 1, matchStars.size());
  free_vector(elv0, 1, matchStars.size());
  free_vector(azi, 1, matchStars.size());
  free_vector(sig, 1, matchStars.size());
  //free_vector(ae, 1, 3);
  free_vector(aa, 1, 3);
  free_ivector(ia, 1, matchStars.size());
  free_matrix(cov, 1, 3, 1, 3);
}


// $CHECK$ do a polynomial fit with several points...
int StarMatcher::getPendulInfo(double sampleNum, pendulInfo& info) {

  static double lastSN = -1;
  static pendulInfo lastPendul;
  
  if (sampleNum == lastSN) {
    info = lastPendul;
    return 0;
  }

  PolFitClip2 fitPendul(30,2);

  map<double, pendulInfo>::iterator i = pendulInfos.lower_bound(sampleNum);
  if (i == pendulInfos.begin() && (*i).second.SN >= sampleNum) return -1;
  if (i == pendulInfos.end()) return -1;
  map<double, pendulInfo>::iterator last = pendulInfos.end();
  if (last == pendulInfos.begin()) return -1;
  last--;
  if (i == last   && (*i).second.SN <= sampleNum) return -1;
  
  if ((*i).second.SN > sampleNum) i--; // i just before us...
  
  //$CHECK$ reject if too large a gap...
  if (sampleNum - (*i).second.SN > 1000) return -1;
  last = i; last++;
  if ((*last).second.SN - sampleNum > 1000) return -1;

  int nn=0;
  double aziprev=0, azicur=0, azi0=0;
  for (map<double, pendulInfo>::iterator ii=i; ii != pendulInfos.begin(); ii--) {
    pendulInfo inf1 = (*ii).second;
    if (fabs(inf1.SN - sampleNum) > 1000) continue;
    aziprev = azicur;
    azicur = inf1.azPendul;
    nn++;
    if (nn==1) azi0 = azicur;
    if (nn>1 && azicur - aziprev > 180)  azicur -= 360;
    if (nn>1 && azicur - aziprev < -180) azicur += 360;
    fitPendul.addData(inf1.SN, inf1.angPendul, azicur);
    if (nn>=5) break;
  }
  
  azicur = azi0;
  if (i != pendulInfos.end()) i++;
  for (map<double, pendulInfo>::iterator ii=i; ii != pendulInfos.end(); ii++) {
    pendulInfo inf1 = (*ii).second;
    if (fabs(inf1.SN - sampleNum) > 1000) continue;
    aziprev = azicur;
    azicur = inf1.azPendul;
    nn++;
    if (nn>1 && azicur - aziprev > 180)  azicur -= 360;
    if (nn>1 && azicur - aziprev < -180) azicur += 360;
    fitPendul.addData(inf1.SN, inf1.angPendul, azicur);
    if (nn>=10) break;
  }
  
  if (fitPendul.doFit()) return -1;
  
  info.SN = sampleNum;
  info.azPendul   = fitPendul.valueZ(sampleNum);
  if (info.azPendul > 360) info.azPendul -= 360;
  if (info.azPendul <   0) info.azPendul += 360;
  info.angPendul  = fitPendul.valueY(sampleNum);
  
  lastSN = sampleNum;
  lastPendul = info;

  return 0;
}


double StarMatcher::getValue(long sampleNum, TOI const& toi) {
  processStars();
  
  // 1. Interpoler la valeur de pendulation
  // 2. Interpoler la position en azimuth avec les etoiles encadrant
  
  pendulInfo pendul;
  int rc = getPendulInfo(sampleNum, pendul);
  if (rc) return -99999;
  if (toi.name == azimuthPendul) return pendul.azPendul;
  if (toi.name == anglePendul)   return pendul.angPendul;
  
  // find nearest matched star
  map<double, posInfo>::iterator i = posInfos.lower_bound(sampleNum);
  if (i == posInfos.begin() && (*i).second.SN >= sampleNum) return -1;
  if (i == posInfos.end()   && (*i).second.SN <= sampleNum) return -1;
  if ((*i).second.SN > sampleNum) i--;
  
  // $CHECK$ if time spent here, can keep a GondolaGeom object for several
  // samples...
  GondolaGeom geom;
  geom.setEarthPos((*i).second.lon,(*i).second.lat);
  geom.setTSid((*i).second.ts);
  geom.setPendulation(pendul.azPendul, pendul.angPendul);
  
  int ns=0;
  for (map<double, posInfo>::iterator it=i; it != posInfos.end(); it++) {
    posInfo s = (*it).second;
    double delsn = s.SN - sampleNum;
    ns++;
    //if (delsn * archParam.acq.perEch > 1 && ns > 4) break;
    if (delsn * archParam.acq.perEch > 5) break;
    geom.addStar(delsn, s.azStar, s.elvStar, s.diodStar);
  }
  
  if (i != posInfos.begin()) i--;
  ns = 0;
  for (map<double, posInfo>::iterator it=i; it != posInfos.begin(); it--) {
    posInfo s = (*it).second;
    double delsn = s.SN - sampleNum;
    ns++;
    //if (-delsn * archParam.acq.perEch > 1 && ns > 4) break;
    if (-delsn * archParam.acq.perEch > 5) break;
    geom.addStar(delsn, s.azStar, s.elvStar, s.diodStar);
  }

  if (geom.solveStars()) return -99999;
  
  if (toi.name == azimuthAxis) return geom.getAzimutAxis();
  if (toi.name == elvAxis)     return geom.getElvAxis();
  if (toi.name == alphaAxis)   return geom.getAlphaAxis();
  if (toi.name == deltaAxis)   return geom.getDeltaAxis();
  
  if (toi.name == azimuthSST)  return geom.getAzimutSST();
  if (toi.name == elvSST)      return geom.getElvSST();
  if (toi.name == alphaSST)    return geom.getAlphaSST();
  if (toi.name == deltaSST)    return geom.getDeltaSST();
  
  if (toi.name == azimuthFPC)  return geom.getAzimutCenter();
  if (toi.name == elvFPC)      return geom.getElvCenter();
  if (toi.name == alphaFPC)    return geom.getAlphaCenter();
  if (toi.name == deltaFPC)    return geom.getDeltaCenter();

  if (toi.name == azimuthBolo) return geom.getAzimutBolo(toi.index);
  if (toi.name == elvBolo)     return geom.getElvBolo(toi.index);
  if (toi.name == alphaBolo)   return geom.getAlphaBolo(toi.index);
  if (toi.name == deltaBolo)   return geom.getDeltaBolo(toi.index);

  return -99999;
}

bool StarMatcher::canGetValue(long sampleNum, TOI const& /*toi*/) {
  processStars();

  map<double, pendulInfo>::iterator i = pendulInfos.begin();
  if (i == pendulInfos.end()) return false;
  if (sampleNum < (*i).second.SN) return false;
  i = pendulInfos.end(); i--;
  if (sampleNum > (*i).second.SN) return false;
  
  return true;
}

bool StarMatcher::canGetValueLater(long sampleNum, TOI const& /*toi*/) {
  processStars();
  
  map<double, pendulInfo>::iterator i = pendulInfos.end();
  if (i == pendulInfos.begin()) return true;
  i--;
  return (sampleNum+4000> (*i).second.SN);
}



set<TOI> StarMatcher::reqTOIFor(TOI const&) {
  set<TOI> t; 
  t.insert(TOI("latitude",  TOI::unspec, "interp"));
  t.insert(TOI("longitude", TOI::unspec, "interp"));
  t.insert(TOI("tsid",      TOI::unspec));
  t.insert(TOI("alphaSST",  TOI::unspec, "galcross0"));
  t.insert(TOI("deltaSST",  TOI::unspec, "galcross0"));
  t.insert(TOI("azimuthSST",TOI::unspec, "galcross0"));
  t.insert(TOI("elvSST",    TOI::unspec, "galcross0"));
  t.insert(TOI("rotSpeed",  TOI::unspec, "galcross0"));
  return t;
}

void StarMatcher::propagateLowBound(TOI const& toi, long sampleNum) {
 // we want to keep some past information to interpolate...
 // keep 1000 sampleNums (easier than a number of posinfos...)
 
 sampleNum -= 4000;

  map<double, posInfo>::iterator i = posInfos.begin();
  while (i != posInfos.end() && (*i).first < sampleNum) i++;
  if (i != posInfos.begin()) {
    i--;
    posInfos.erase(posInfos.begin(), i);
  }

  map<double, pendulInfo>::iterator j = pendulInfos.begin();
  while (j != pendulInfos.end() && (*j).first < sampleNum) j++;
  if (j != pendulInfos.begin()) {
    j--;
    pendulInfos.erase(pendulInfos.begin(), j);
  }

  TOIDerivProducer::propagateLowBound(toi, sampleNum);
}


// 1. processStars seulement quand au moins 10 etoiles nouvelles
// 2. Nettoyer avec fit parabolique sur les 5 dernieres seconde de donnees
// 3. Garder periodeRotation secondes de donnees nettoyees
// 4. TF ordre 0 sur ces donnees, amplitude et phase -> theta et phi pendulation,
//     elevationSST = elv-theta Sin[azimut-phi]
//     azimutSST    = azimut+theta Cos[azimut-phi] Tan[elv] (+ OFFSET GALCROSS)
//   le signal le plus propre est l'elevation -> fit dessus, puis
//   correction azimut SST a partir seconde equation, sans utiliser azimut galcross