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

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

#include "flagtoidef.h"

SimpleCleaner::SimpleCleaner(int wsz, int nbw)

{
  SetMeanSigWindow(wsz, nbw);
  SetCleanForMeanThrNSig();
  SetFlaggingThrNSig();
  
  SetRange();
  SetFlagMask();

  FillMeanSigNTuple();
  char* noms[] = {"sn","mean","sigma"};
  meansignt = new NTuple(3,noms);

  totnscount = 0;
  totnbblock = 0;
  ns_outofrange = ns_flag1p = ns_flag2p = ns_flag1m = ns_flag2m = 0;
  ns_inflagged = 0;
  ns_cleannsig = 0;
  gl_sum = gl_sum2 = 0.;  ns_glms = 0;
}

SimpleCleaner::~SimpleCleaner()
{
}

void SimpleCleaner::SetMeanSigWindow(int wsz, int nbw)
{
  mWSz = (wsz > 8) ? wsz : 8;
  mNbW = (nbw > 3) ? nbw : 3;  
}

void SimpleCleaner::PrintStatus(::ostream & os)
{
  os << "\n ------------------------------------------------------ \n" 
     << " SimpleCleaner::PrintStatus() - MeanWSize= " << mWSz << " mNbW=" 
     << mNbW << " Range/Min=" << range_min << " Max=" << range_max << endl;
  os << " CleanForMeanThrNSig = " << nsigclean << " NSig1= " << nsig1 
     << " NSig2 (" << min_npt2 << "pts)= " << nsig2 << endl;
  TOIProcessor::PrintStatus(os);
  os << " ProcessedSampleCount=" << ProcessedSampleCount() 
     << " NS_InFlagged= " << ns_inflagged
     << " NS_OutOfRange= " << ns_outofrange 
     << " NS_CleanNSig= " << ns_cleannsig << endl 
     << " NS_Flag1(+)= " << ns_flag1p << " NS_Flag2(+)= " << ns_flag2p
     << " NS_Flag1(-)= " << ns_flag1m << " NS_Flag2(-)= " << ns_flag2m << endl;
  os << " NS_GlobMeanSig= " << ns_glms << " GlMean()= " << GetGlMean() 
     << " GlSigma2() " << GetGlSigma2() << endl;
  os << " ------------------------------------------------------ " << endl;  
}

void SimpleCleaner::init()
{
  cout << "SimpleCleaner::init" << endl;
  declareInput("in");
  declareOutput("out");
  declareOutput("mean");
  declareOutput("sigma");
  name = "SimpleCleaner";
}

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

  bool fgout = checkOutputTOIIndex(0);
  bool fgmean = checkOutputTOIIndex(1);
  bool fgsig = checkOutputTOIIndex(2);

  if (!checkInputTOIIndex(0)) {
    cerr << " SimpleCleaner::run() - Input TOI (in) not connected! "
	 << endl;
    throw ParmError("SimpleCleaner::run() Input TOI (in) not connected!");
  }
  
  cout << " SimpleCleaner::run() SNRange=" << snb << " - " << sne << endl; 

  try {

    // Vecteurs pour les donnees et les sorties
    int wsize = mWSz;
    int hisblk = mNbW;
    int vecsize = mWSz*mNbW;
    Vector vinhist(vecsize);
    TVector<uint_8> vfghist(vecsize);

    Vector sumx(mNbW);
    Vector sumx2(mNbW);
    TVector<int_4> nbx(mNbW);

    // Variables diverses 
    int k,kb,kbm,j,klast;
    klast = snb-1;
    totnbblock = 0;
    

    int ns_flg2p_last = 0;
    int ns_flg2m_last = 0;

    // Moyenne et sigma courant
    double cur_mean = 0.;
    double cur_sig = 0.;

    double last_sum = 0.;
    double last_sum2 = 0.;
 
    // Boucle sur les sampleNum
    // 1ere partie, on traite par paquets de wsize
    int nblk = (sne-snb+1)/wsize; 
    for(kb=0; kb<nblk; kb++) {
      k = kb*wsize+snb;
      kbm = kb%hisblk;
      // Lecture d'un bloc de donnees
      Vector vin( vinhist(Range(kbm*wsize, 0, wsize) ) );
      TVector<uint_8> vfg( vfghist(Range(kbm*wsize, 0, wsize) ) );

      getData(0, k, wsize, vin.Data(), vfg.Data());
      double nok = 0.;
      double sum = 0.;
      double sum2 = 0.;
      double cleanthrmin = range_min;
      double cleanthrmax = range_max;
      if ((kb > mNbW/2) && ( ns_glms > 3*wsize) ) {
        cleanthrmin = cur_mean-nsigclean*cur_sig;
	cleanthrmax = cur_mean+nsigclean*cur_sig;
      }
      for(j=0; j<wsize; j++) {
	double x = vin(j);
	if ((vfg(j)&flag_mask)) { ns_inflagged++; continue; }
	if ((x > range_max) || (x < range_min) ) continue;
	if ((x > cleanthrmax) || (x < cleanthrmin) )  { 
	  ns_cleannsig++;
	  continue;	  
	}   
	sum += x;  sum2 += x*x;   nok++;
      }
      if (nok > 0) {
	gl_sum += (sum - last_sum);
	gl_sum2 += (sum2 - last_sum2);
	ns_glms += nok;
	last_sum = sum;  last_sum2 = sum2;
      }
      else {
	sum = cur_mean;  sum2 = cur_mean*cur_mean;
	nok = 1.;
      }
      sumx(kbm) = sum;
      sumx2(kbm) = sum2;
      nbx(kbm) = nok;
      sum = sumx.Sum();
      sum2 = sumx2.Sum();
      nok = nbx.Sum();
      if (nok > 0) {
	cur_mean = sum/(double)nok;
	cur_sig = sum2/(double)nok-cur_mean*cur_mean;
	cur_sig = (cur_sig > 0.) ? sqrt(cur_sig) : 0.;
      }
      
      if (kb < mNbW/2) continue;

      Vector vinc;
      TVector<uint_8> vfgc;

      int kbs = kb-mNbW/2;
      k = kbs*wsize+snb;
      if (kb == nblk-1) {
	int wszt = wsize*(kb+1-kbs);
	vinc.ReSize(wszt); 
	vfgc.ReSize(wszt);
	int jb = 0;
	for(int kbsc=kbs; kbsc<nblk; kbsc++) {
	  vinc(Range(jb*wsize, 0, wsize)) = 
	       vinhist(Range((kbsc%hisblk)*wsize, 0, wsize) ) ;
	  vfgc(Range(jb*wsize, 0, wsize)) = 
	       vfghist(Range((kbsc%hisblk)*wsize, 0, wsize) ) ;
	  jb++;     
	}
	wsize = wszt;
      }
      else {
	vinc.Share( vinhist(Range((kbs%hisblk)*wsize, 0, wsize)) ) ;
	vfgc.Share( vfghist(Range((kbs%hisblk)*wsize, 0, wsize)) ) ;
      }

      Vector vout(wsize);

      if (fgmean) {  // Sortie valeur moyenne courante
	vout = cur_mean;
	putData(1, k, wsize, vout.Data());
      }
      if (fgsig) {   // Sortie sigma courante
	vout = cur_sig;
	putData(2, k, wsize, vout.Data());
      }

      if (fg_meansignt) {  // Remplissage du NTuple mean-sigma  
	float xnt[5];
	xnt[0] = k;
	xnt[1] = cur_mean;
	xnt[2] = cur_sig;
	meansignt->Fill(xnt);
      }

      if (!fgout) { 
	klast+=wsize;
	totnscount+=wsize;
	totnbblock++;
	continue;
      }

      // Calcul des flags en sortie
      for(j=0; j<wsize; j++) { 
	double x = vinc(j);
	if ((x > range_max) || (x < range_min) ) {
	  ns_outofrange++; vfgc(j) |= FlgToiOut;
	}
	if (x > cur_mean) {   // Superieur  a mean
	  ns_flg2m_last = 0;
	  if (x > cur_mean+nsig2*cur_sig) ns_flg2p_last++;
	  else ns_flg2p_last = 0;
	  if (x > cur_mean+nsig1*cur_sig) {
	    ns_flag1p++; vfgc(j) |= FlgToiSpike;
	  }
	  if (ns_flg2p_last == min_npt2) {
	    int jj0 = j-ns_flg2p_last+1;
	    if (jj0 < 0) jj0 = 0;
	    for(int jj = jj0; jj<=j; jj++) {
	      ns_flag2p++;  vfgc(jj) |= FlgToiSpike;
	    }
	  }
	  else if (ns_flg2p_last > min_npt2) {
	    ns_flag2p++;  vfgc(j) |= FlgToiSpike;
	  }
	}
	else {   // Inferieur a mean 
	  ns_flg2p_last = 0;
	  if (x < cur_mean-nsig2*cur_sig) ns_flg2m_last++;
	  else ns_flg2m_last = 0;
	  if (x < cur_mean-nsig1*cur_sig) {
	    ns_flag1m++; vfgc(j) |= FlgToiSpike;
	  }
	  if (ns_flg2m_last == min_npt2) {
	    int jj0 = j-ns_flg2m_last+1;
	    if (jj0 < 0) jj0 = 0;
	    for(int jj = jj0; jj<=j; jj++) {
	      ns_flag2m++;  vfgc(jj) |= FlgToiSpike;
	    }
	  }
	  else if (ns_flg2m_last > min_npt2) {
	    ns_flag2m++;  vfgc(j) |= FlgToiSpike;
	  }
	}
      }

      putData(0, k, wsize, vinc.Data(), vfgc.Data());

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

    // 3eme partie, on traite la fin du bloc d'echantillons si necessaire
    if (klast < sne) {
      wsize = sne-klast;
      Vector vinc(wsize);
      Vector vout(wsize);
      TVector<uint_8> vfgc(wsize);
      k = klast+1;
      getData(0, k, wsize, vinc.Data(), vfgc.Data());

      if (fgmean) {  // Sortie valeur moyenne courante
	vout = cur_mean;
	putData(1, k, wsize, vout.Data());
      }
      if (fgsig) {   // Sortie sigma courante
	vout = cur_sig;
	putData(2, k, wsize, vout.Data());
      }

      if (fg_meansignt) {  // Remplissage du NTuple mean-sigma  
	float xnt[5];
	xnt[0] = k;
	xnt[1] = cur_mean;
	xnt[2] = cur_sig;
	meansignt->Fill(xnt);
      }

      if (fgout)  for(j=0; j<wsize; j++) { 
	double x = vinc(j);
	if ((vfgc(j)&flag_mask))  ns_inflagged++; 
	if ((x > range_max) || (x < range_min) ) {
	  ns_outofrange++; vfgc(j) |= FlgToiOut;
	}
	if (x > cur_mean) {   // Superieur  a mean
	  ns_flg2m_last = 0;
	  if (x > cur_mean+nsig2*cur_sig) ns_flg2p_last++;
	  else ns_flg2p_last = 0;
	  if (x > cur_mean+nsig1*cur_sig) {
	    ns_flag1p++; vfgc(j) |= FlgToiSpike;
	  }
	  if (ns_flg2p_last == min_npt2) {
	    int jj0 = j-ns_flg2p_last+1;
	    if (jj0 < 0) jj0 = 0;
	    for(int jj = jj0; jj<=j; jj++) {
	      ns_flag2p++;  vfgc(jj) |= FlgToiSpike;
	    }
	  }
	  else if (ns_flg2p_last > min_npt2) {
	    ns_flag2p++;  vfgc(j) |= FlgToiSpike;
	  }
	}
	else {   // Inferieur a mean 
	  ns_flg2p_last = 0;
	  if (x < cur_mean-nsig2*cur_sig) ns_flg2m_last++;
	  else ns_flg2m_last = 0;
	  if (x < cur_mean-nsig1*cur_sig) {
	    ns_flag1m++; vfgc(j) |= FlgToiSpike;
	  }
	  if (ns_flg2m_last == min_npt2) {
	    int jj0 = j-ns_flg2m_last+1;
	    if (jj0 < 0) jj0 = 0;
	    for(int jj = jj0; jj<=j; jj++) {
	      ns_flag2m++;  vfgc(jj) |= FlgToiSpike;
	    }
	  }
	  else if (ns_flg2m_last > min_npt2) {
	    ns_flag2m++;  vfgc(j) |= FlgToiSpike;
	  }
	}
	putData(0, k, wsize, vinc.Data(), vfgc.Data());
      }

      
      klast+=wsize;
      totnscount+=wsize;
      totnbblock++;      
    }

    meansignt->Info()["SampleCount"] = ProcessedSampleCount();
    meansignt->Info()["GlMean"] = GetGlMean();
    meansignt->Info()["GlSigma2"] = GetGlSigma2();
    meansignt->Info()["OutOfRange"] = ns_outofrange;
    meansignt->Info()["NSFlag1+"] = ns_flag1p;
    meansignt->Info()["NSFlag1-"] = ns_flag1m;
    meansignt->Info()["NSFlag2+"] = ns_flag2p;
    meansignt->Info()["NSFlag2-"] = ns_flag2m;

    cout << " SimpleCleaner::run() - End of processing " 
	 << " TotNbBlocks= " << totnbblock << " ProcSamples=" << totnscount << endl;
    
  }  // Bloc try 

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

}