#include "simtoipr.h"
#include "toimanager.h"
#include "pexceptions.h"
#include "ctimer.h"

SimpleDeglitcher::SimpleDeglitcher(int wsz, double ns, int maxnpt)
{
  if (wsz < 5) wsz = 5;
  if (wsz%2 == 0) wsz++;
  if (maxnpt > wsz) maxnpt = wsz;

  cout << "SimpleDeglitcher::SimpleDeglitcher() WSize= " << wsz 
       << " nSig=" << ns << " maxNPt=" << maxnpt << endl;
  
  wsize = wsz;
  nsig = ns;
  maxpoints = maxnpt;
  totnscount = glnscount = glcount = 0;
  deglitchdone = false;
}

SimpleDeglitcher::~SimpleDeglitcher()
{
}

void SimpleDeglitcher::PrintStatus(ostream & os)
{
  os << "\n ------------------------------------------------------ \n" 
     << " SimpleDeglitcher::PrintStatus() - WindowSize=" << WSize() 
     << "  NbSigmas=" << NbSigmas() << " MaxPoints=" << MaxPoints() << endl;
  TOIProcessor::PrintStatus(os);
  if (deglitchdone) os << " Deglitching performed " << endl;
  else os << " NO deglitching done " << endl;
  double nst = (TotalSampleCount() > 0) ? TotalSampleCount() : 1.;
  os << " TotalSampleCount=" << TotalSampleCount() << " GlitchCount= " 
     << GlitchCount() << " GlitchSampleCount=" << GlitchSampleCount() 
     << "( " << (double)GlitchSampleCount()*100./nst << " % )" << endl;
  os << " ------------------------------------------------------ " << endl;
}

void SimpleDeglitcher::init() {
  cout << "SimpleDeglitcher::init" << endl;
  declareInput("in");
  declareOutput("out");
  declareOutput("mean");
  declareOutput("sigma");
  declareOutput("incopie");
  name = "SimpleDeglitcher";
  //  upExtra = 1;   A quoi ca sert ?
}

void SimpleDeglitcher::run() {

  //  TOIManager* mgr = TOIManager::getManager();
  int snb = getMinIn();
  int sne = getMaxIn();

  bool fgout = checkOutputTOIIndex(0);
  bool fgmean = checkOutputTOIIndex(1);
  bool fgsigma = checkOutputTOIIndex(2);
  bool fgincopie = checkOutputTOIIndex(3);
  
  if (!checkInputTOIIndex(0)) {
    cerr << " SimpleDeglitcher::run() - Input TOI (in) not connected! "
	 << endl;
    throw ParmError("SimpleDeglitcher::run() Input TOI (in) not connected!");
  }
  if (!fgout && !fgmean && !fgsigma &&!fgincopie) {
    cerr << " SimpleDeglitcher::run() - No Output TOI connected! "
	 << endl;
    throw ParmError("SimpleDeglitcher::run() No output TOI connected!");
  }
  
  if (!fgout) {
    cout << "Warning:  SimpleDeglitcher::run() - No TOI connected to out \n"
	 << " No deglitching would be performed !" << endl;
  }

  cout << " SimpleDeglitcher::run() SNRange=" << snb << " - " << sne << endl; 
  try {
    Timer tm("SimpleDeglitcher::run()");
    // Le debut 
    int wsz2 = wsize/2;
    Vector vin(wsize);
    int k;
    for(k=0; k<wsize; k++) 
      vin(k) = getData(0, k+snb);
    double s = vin.Sum();
    double mean = s/wsize;
    double s2 = vin.SumX2();
    double sigma = sqrt(s2/wsize-mean*mean);
    int kgl = -1;
    int ii,lastput;
    bool fgglitch = false;
    double valcur,valsub,valadd;
    for(k=0;k<=sne-snb;k++) {
      totnscount++;
//       if (k%10000 == 0) cout << " DBG: K=" << k << endl;
      // Calcul mean-sigma
      if ((k>wsz2) && (k<sne-snb-wsz2)) {
	valsub = vin((k+wsz2)%wsize);
	vin((k+wsz2)%wsize) = valadd = getData(0, k+wsz2+snb);
	s += (valadd-valsub);
	s2 += (valadd*valadd-valsub*valsub);
	mean = s/wsize;
	sigma = sqrt(s2/wsize-mean*mean);
      }

      //  On gere les sorties Mean et Sigma
      if (fgmean) 
	putData(1, k+snb, mean, 0);
      if (fgsigma) 
	putData(2, k+snb, sigma, 0);

      valcur = vin(k%wsize);
      if (fgincopie) 
	putData(3, k+snb, valcur, 0);
	
      if (!fgout)   continue;  // Pas de sortie out (deglitche)

      
//             if (k<100) { 
//       	cout << "DBG-A-Deglitch[" << k << "] mean=" 
//       	     << mean << " sigma=" << sigma << " valcur=" 
//       	     << valcur << " Kgl=" << kgl ;
//       	if (fgglitch) cout << " In Glitch" ;
//       	cout << endl;
//            }

      if (valcur < mean+nsig*sigma) {  // inferieur au seuil
	if (fgglitch) {  
	  if (k-kgl < maxpoints) {   // On vient de detecter un glitch
	    glcount++;
	    for(ii=kgl; ii<k; ii++) {
	      putData(0, ii+snb, mean, 7);
	      glnscount++; 
	    }
	    lastput = snb+k-1;
	  }
	  else {
	    for(ii=kgl; ii<k; ii++) {
	      putData(0, ii+snb, vin(ii%wsize), 0);
	    }
	    lastput = snb+k-1;  
	  } 
	}
	putData(0, k+snb, valcur, 0);
	lastput = snb+k;
	kgl = -1;  fgglitch = false;
      }
      else {  // Superieur au seuil
	if (fgglitch) {
	  if (k-kgl+1 >= maxpoints) {  // serie de points > seuil
	    for(ii=kgl; ii<=k; ii++)   // -> Donc pas glitch
	      putData(0, ii+snb, vin(ii%wsize), 0);
	    lastput = snb+k;
	    fgglitch = false;  
	  }
	}
	else {
	  if (kgl < 0) {  // debut possible de glitch
	    fgglitch = true;  kgl = k;
	  }
	  else { // On est toujours dans une serie > seuil
	    putData(0, k+snb, valcur, 0);
	    lastput = snb+k;
	  }
	}
      }

//           if (k%5000 == 0) cout << " ---DBG2: K=" << k << " glcount=" 
// 				<< glcount << " LastPut= " << lastput << endl;
    }  // Fin de Boucle sur les num-sample

    //DBG    cout << " La fin lastput=" << lastput << " SNE=" << sne; 
    //DBG    for(k=lastput-snb+1; k<sne-snb; k++) 
    //DBG      putData(0, k+snb, vin(k%wsize), 0);
    //    cout << " DBG3- OUT of try bloc ! " << endl;
    cout << " SimpleDeglitcher::run() - End of processing " 
    << " TotalSampleCount=" << TotalSampleCount() 
    <<  " GlitchCount= " << GlitchCount() << endl;
    if (fgout) deglitchdone = true;
  }  // Bloc try 
  catch (PException & exc) {
    cerr << "SimpleDeglitcher: Catched Exception " << (string)typeid(exc).name()
         << "\n .... Msg= " << exc.Msg() << endl;
  }                                                                             
}



SimpleFilter::SimpleFilter(int wsz, FilterKind fk, double a, double s)
{
  if (wsz < 3) wsz = 3;
  if (wsz%2 == 0) wsz++;
  cout << "SimpleFilter::SimpleFilter() wsz= " << wsz 
       << " fk=" << fk << endl; 
  wsize = wsz;
  totnscount = 0;
  coef = new double[wsz];
  for(int kk=0; kk<wsz; kk++) 
    coef[kk] = 1./wsize;
}

SimpleFilter::~SimpleFilter()
{
  delete[] coef;
}

void SimpleFilter::PrintStatus(ostream & os)
{
  os << "\n ------------------------------------------------------ \n" 
     << " SimpleFilter::PrintStatus() - WindowSize=" << WSize() 
     << " FilterKind= " << Type() << endl;
  TOIProcessor::PrintStatus(os);
  os << " Coeff= " ; 
  for(int k=0; k<wsize; k++) os << coef[k] << " " ;
  os << endl;
  os << " TotalSampleCount=" << TotalSampleCount() << endl;
  os << " ------------------------------------------------------ " << endl;
}

void SimpleFilter::init() {
  cout << "SimpleFilter::init" << endl;
  declareInput("in");
  declareOutput("out");
  declareOutput("incopie");
  name = "SimpleFilter";
  //  upExtra = 1;
}

void SimpleFilter::run() {
  //  TOIManager* mgr = TOIManager::getManager();
  int snb = getMinIn();
  int sne = getMaxIn();

  bool fgout = checkOutputTOIIndex(0);
  bool fgincopie = checkOutputTOIIndex(1);

  if (!checkInputTOIIndex(0)) {
    cerr << " SimpleFilter::run() - Input TOI (in) not connected! "
	 << endl;
    throw ParmError("SimpleFilter::run() Input TOI (in) not connected!");
  }
  if (!fgout) {
    cerr << " SimpleFilter::run() - No Output TOI connected! "
	 << endl;
    throw ParmError("SimpleFilter::run() No output TOI connected!");
  }

  cout << " SimpleFilter::run() SNRange=" << snb << " - " << sne << endl; 
  

  try {
    Timer tm("SimpleFilter::run()");
    // Le debut
    int wsz2 = wsize/2;
    Vector vin(wsize);
    int k;
    for(k=0; k<wsize; k++) 
      vin(k) = getData(0, k+snb);
    double mean = vin.Sum()/wsize;
    for(k=0; k<wsize/2; k++) vin(k) = mean; 
    for(k=0;k<=sne-snb;k++) {
      totnscount++;
      // Calcul mean-sigma
      if ((k>wsz2) && (k<sne-snb-wsz2)) 
	vin((k+wsz2)%wsize) = getData(0, k+snb);
      //      if (k == sne-snb-wsz2) {
      //      }
      double sortie = 0;
      for(int ii=0; ii<wsize; ii++) {
	sortie += vin((ii+k)%wsize)*coef[ii];
      }
      putData(0,k+snb,sortie,0);
      if (fgincopie) putData(1,k+snb,vin(k%wsize),0);
    }  // Boucle sur les num-sample
    cout << " SimpleFilter::run() - End of processing " << endl;
  }  // Bloc try 

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


}