// ArchTOIPipe           (C)     CEA/DAPNIA/SPP IN2P3/LAL
//                               Eric Aubourg
//                               Christophe Magneville
//                               Reza Ansari
#include "config.h"

#include "array.h"
#include "simoffset.h"
#include <math.h>
#include "toimanager.h"
#include "pexceptions.h"
#include "ctimer.h"
#include "xntuple.h"

#include "flagtoidef.h"

SimpleOffsetEstimator::SimpleOffsetEstimator(int mwsz, int nptfit, int degpol)
  : poly((degpol > 1)?degpol:1)
{
  mWSz = (mwsz > 8) ? mwsz : 8;
  nPtFit = (nptfit > degpol+2) ? nptfit : degpol+2;
  degPol = (degpol > 1)?degpol:1;
  totnscount = 0;
  totnbblock = 0;
  SavePolyNTuple();
}

SimpleOffsetEstimator::~SimpleOffsetEstimator()
{
}

void SimpleOffsetEstimator::PrintStatus(::ostream & os)
{
  os << "\n ------------------------------------------------------ \n" 
     << " SimpleOffsetEstimator::PrintStatus() - MeanWSize= " << mWSz << " NPtFit=" 
     << nPtFit << " DegPoly=" << degPol << " poly.Degre()=" << poly.Degre() << endl;
  TOIProcessor::PrintStatus(os);
  os << " ProcessedSampleCount=" << ProcessedSampleCount() << endl;
  os << " ------------------------------------------------------ " << endl;  
}

void SimpleOffsetEstimator::init()
{
  cout << "SimpleOffsetEstimator::init" << endl;
  declareInput("in");
  declareOutput("offset");
  declareOutput("out");
  declareOutput("incopie");
  declareOutput("poly_a0");
  declareOutput("poly_a1");
  declareOutput("poly_a2");
  declareOutput("poly_sn0");
  declareOutput("mean_y");
  declareOutput("sig_y");
  declareOutput("mean_x");
  name = "SimpleOffsetEstimator";
}

void SimpleOffsetEstimator::run()
{
  int snb = getMinIn();
  int sne = getMaxIn();

  bool fgoffset = checkOutputTOIIndex(0);
  bool fgout = checkOutputTOIIndex(1);
  bool fgincopie = checkOutputTOIIndex(2);
  bool fga0 = checkOutputTOIIndex(3);
  bool fga1 = checkOutputTOIIndex(4);
  bool fga2 = checkOutputTOIIndex(5);
  bool fgsn0 = checkOutputTOIIndex(6);
  bool fgmeany = checkOutputTOIIndex(7);
  bool fgsigy = checkOutputTOIIndex(8);
  bool fgmeanx = checkOutputTOIIndex(9);
  
  if (!checkInputTOIIndex(0)) {
    cerr << " SimpleOffsetEstimator::run() - Input TOI (in) not connected! "
	 << endl;
    throw ParmError("SimpleOffsetEstimator::run() Input TOI (in) not connected!");
  }
  if (!fgoffset && !fgout) {
    cerr << " SimpleOffsetEstimator::run() - No Output TOI (offset/in-offset) connected! "
	 << endl;
    throw ParmError(" SimpleOffsetEstimator::run() No output TOI (offset/in-offset) connected!");
  }
  
  cout << " SimpleOffsetEstimator::run() SNRange=" << snb << " - " << sne << endl; 

  // NTuple pour sauvegarde des coeff de poly  
  char * nomsnt[] = {"sncur", "sn0", "meanx", "meany", "sigy", "a0", "a1", "a2", "ycur"};
  XNTuple xntp(0, 9, 0, 0, nomsnt);

  try {

    // Vecteurs pour les donnees et les sorties
    int wsize = mWSz;

    Vector vin(wsize);
    Vector voff(wsize);
    Vector vout(wsize);
    TVector<uint_8> vfg(wsize);
    
    // Pour le fit 
    Vector errCoef(3);
    
    Vector X(nPtFit);
    Vector X0(nPtFit);
    Vector Y(nPtFit);
    Vector YErr(nPtFit);

    // Variables diverses 
    int k,i,j,klast;
    int nks = 0;
    klast = snb-1;
    totnbblock = 0;
    

    int nbblkok = 0;

    bool fginiXYdone = false;

    double sn0 = 0.;
    double nok = 0.;
    double mean = 0.;
    double sig = 0.;
    double meanx = 0.;
    
    // Boucle sur les sampleNum
    // 1er partie, on traite par paquets de wsize

    for(k=snb;k<=sne-wsize+1;k+=wsize) {
      // Lecture d'un bloc de donnees
      getData(0, k, wsize, vin.Data(), vfg.Data());

      // Calcul moyenne et sigma du bloc 
      nok = 0.;  meanx = 0.;
      mean = 0.;  sig = 0.;
      
      for(j=0; j<wsize; j++) {
	if ( vfg(j) ) continue;
	mean += vin(j);
	sig += vin(j)*vin(j);
	meanx += k+j;
	nok++;
      }
      
      sn0 = (double)(k+wsize/2);
      
      if (!fginiXYdone) {
	if (nok > 0.5) {
	  mean /= nok;
	  meanx /= nok;
	  sig = sig/nok-mean*mean;	
	}
	X = RegularSequence(k+wsize*0.5, (double)wsize);
	Y = mean;
	YErr = (nok > 0.5) ? sqrt(mean) : 1.;
	fginiXYdone = true; 
      }

      if (nok > 3.) {
	mean /= nok;
	meanx /= nok;
	sig = sig/nok-mean*mean;
	int kk = nbblkok%nPtFit;
	nbblkok++;
	Y(kk) = mean; 
	YErr(kk) = sig;
	X(kk) = meanx;
      }
      
      X0 = X;
      X0 -= sn0;
      if (nbblkok > poly.Degre()+1) 
	poly.Fit(X0,Y,YErr,degPol,errCoef);
      else {
	poly[0] = mean;
	for(int jj=1; jj<=poly.Degre(); jj++) poly[jj] = 0.;
      }
      /*
      if (nbblkok < 8) {
	cout << "------ DBG-X " << nbblkok << "," << nok << " degre=" << poly.Degre() << endl;
	cout << "DBG-A X0=" << X0 << endl;
 	cout << "DBG-A Y=" << Y << endl;
 	cout << "DBG-A YErr=" << YErr << endl;
	cout << "DBG-A poly= " << poly << endl;
      }
      */
      
      // Calcul des valeurs d'offset en sortie
      for(j=0; j<wsize; j++) 
	voff(j) = poly(k+j-sn0);
      
      if (fgoffset) putData(0, k, wsize, voff.Data());
      if (fgincopie) putData(2, k, wsize, vin.Data(), vfg.Data());
      if (fgout) { 
	vin -= voff;
	putData(1, k, wsize, vin.Data(), vfg.Data());
      }
      
      if (fga0) {
	vout = poly[0];
	putData(3, k, wsize, vout.Data());
      }
      if (fga1) {
	vout = poly[1];
	putData(4, k, wsize, vout.Data());
      }
      if (fga2) {
	vout = poly[2];
	putData(5, k, wsize, vout.Data());
      }
      if (fgsn0) {
	vout = sn0;
	putData(6, k, wsize, vout.Data());
      }
      if (fgmeany) {
	vout = mean;
	putData(7, k, wsize, vout.Data());
      }
      if (fgsigy) {
	vout = sig;
	putData(8, k, wsize, vout.Data());
      }

      if (fgmeanx) {
	vout = meanx;
	putData(9, k, wsize, vout.Data());
      }

      if (ntpoly) {    // Remplissage du XNTuple de controle 
	char * nomsnt[] = {"sncur", "sn0", "meanx", "meany", "sigy", "a0", "a1", "a2", "ycur"};
	float xnt[10];
	xnt[0] = k;
	xnt[1] = sn0;
	xnt[2] = meanx;
	xnt[3] = mean;
	xnt[4] = sig;
	xnt[5] = poly[0];
	xnt[6] = poly[1];
	xnt[7] = poly[2];
	xnt[8] = poly(k-sn0);
	xntp.Fill(NULL, xnt, NULL, NULL);
      }

      klast+=wsize;
      totnscount+=wsize;
      totnbblock++;
      
    } // Fin boucle sur les samples, par pas de wsize

    // 2eme partie, on traite la fin du bloc d'echantillons si necessaire
    if (klast < sne) {
      wsize = sne-klast;
      vin.ReSize(wsize);
      voff.ReSize(wsize);
      vout.ReSize(wsize);
      vfg.ReSize(wsize);
      getData(0, k, wsize, vin.Data(), vfg.Data());
      for(j=0; j<wsize; j++) 
	voff(j) = poly(k+j-sn0);
      if (fgoffset) putData(0, k, wsize, voff.Data());
      if (fgincopie) putData(2, k, wsize, vin.Data(), vfg.Data());
      if (fgout) { 
	vin -= voff;
	putData(1, k, wsize, vin.Data(), vfg.Data());
      }
      
      if (fga0) {
	vout = poly[0];
	putData(3, k, wsize, vout.Data());
      }
      if (fga1) {
	vout = poly[1];
	putData(4, k, wsize, vout.Data());
      }
      if (fga2) {
	vout = poly[2];
	putData(5, k, wsize, vout.Data());
      }
      if (fgsn0) {
	vout = sn0;
	putData(6, k, wsize, vout.Data());
      }
      if (fgmeany) {
	vout = mean;
	putData(7, k, wsize, vout.Data());
      }
      if (fgsigy) {
	vout = sig;
	putData(8, k, wsize, vout.Data());
      }
      
      if (fgmeanx) {
	vout = meanx;
	putData(9, k, wsize, vout.Data());
      }
      
      klast+=wsize;
      totnscount+=wsize;
      totnbblock++;      
    }
    
    cout << " SimpleOffsetEstimator::run() - End of processing " 
	 << " TotNbBlocks= " << totnbblock << " ProcSamples=" << totnscount << endl;
    
  }  // Bloc try 

  catch (PException & exc) {
    cerr << "SimpleOffsetEstimator::run() Catched Exception " << (string)typeid(exc).name()
         << "\n .... Msg= " << exc.Msg() << endl;
  }

  if (ntpoly) {
    if (ntpolyname.length() < 1)  ntpolyname = "simoffset.ppf";
    POutPersist pos(ntpolyname);
    cout << " SimpleOffsetEstimator::run()/Info : Writing poly ntuple to PPF file " << ntpolyname << endl;
    pos << xntp;
  }
}