#include "array.h"
#include "simtoipr.h"
#include <math.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 = out_range_nscount = 0;
  deglitchdone = false;
  SetRange(-9.e39, 9.e39);
}

SimpleDeglitcher::~SimpleDeglitcher()
{
}


void SimpleDeglitcher::PrintStatus(ostream & os)
{
  os << "\n ------------------------------------------------------ \n" 
     << " SimpleDeglitcher::PrintStatus() - WindowSize=" << WSize() 
     << "  NbSigmas=" << NbSigmas() << " MaxPoints=" << MaxPoints() << endl;
  os << "  Range_Min= " << range_min << " Range_Max= " << range_max << endl;
  TOIProcessor::PrintStatus(os);
  if (deglitchdone) os << " Deglitching performed " << endl;
  else os << " NO deglitching done " << endl;
  double nst = (ProcessedSampleCount() > 0) ? ProcessedSampleCount() : 1.;
  os << " ProcessedSampleCount=" << ProcessedSampleCount() 
     << " OutOfRangeSampleCount=" << OutOfRangeSampleCount() << endl;
  os << " 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 
    Vector vin(wsize);
    TVector<int_8> vfg(wsize);
    int k;
    for(k=0; k<wsize; k++) {
      vin(k) = getData(0, k+snb);
      vfg(k) = getFlag(0, k+snb);
    }
    double s = vin.Sum();
    double mean = s/wsize;
    for(k=0; k<wsize; k++) {
      if ( (vin(k) < range_min) || (vin(k) > range_max) ) {
	vin(k) = mean;
	vfg(k) |= 2;
	out_range_nscount++;
      }
    }
    s = vin.Sum();
    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;
    uint_8 fgcur;

    // Boucle sur les sampleNum 

    for(k=0;k<=sne-snb;k++) {
      totnscount++;
//       if (k%10000 == 0) cout << " DBG: K=" << k << endl;
      // Calcul mean-sigma
      if (k>=wsize)  {
	valsub = vin(k%wsize);
	vin(k%wsize) = valadd = getData(0, k+snb);
	vfg(k%wsize) = getFlag(0, k+snb);
	if ( (valadd < range_min) || (valadd > range_max) ) 
	  valadd = mean;
	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);
      if (fgincopie) 
	putData(3, k+snb, vin(k%wsize), vfg(k%wsize));

      valcur = vin(k%wsize);
      if ( (valcur < range_min) || (valcur > range_max) ) {
	vin(k%wsize) = valcur = mean;
	vfg(k%wsize) |= 2;
	out_range_nscount++;
      }
	
      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, vfg(ii%wsize)|5);
	      glnscount++; 
	    }
	    lastput = snb+k-1;
	  }
	  else {
	    for(ii=kgl; ii<k; ii++) {
	      putData(0, ii+snb, vin(ii%wsize), vfg(ii%wsize));
	    }
	    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), vfg(ii%wsize));
	    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, fgcur);
	    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 " 
    << " ProcessedSampleCount=" << ProcessedSampleCount() 
    <<  " 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;
  }                                                                             
}


// -------------------------------------------------------------------
//   Classe SimpleFilter : Filtre simple ds le domaine temporel
// -------------------------------------------------------------------

string SimpleFilter::FilterKind2String(FilterKind fk)
{
  switch (fk) {
    case UserFilter :
      return ("UserFilter");
      break;
    case MeanFilter :
      return ("MeanFilter");
      break;
    case SumFilter :
      return ("SumFilter");
      break;
    case GaussFilter :
      return ("GaussFilter");
      break;
    case DiffFilter :
      return ("DiffFilter");
      break;
    default :
      return ("ErrorFilterKind");
      break;
  }
  return("");
}

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 
       << " FilterKind=" << FilterKind2String(fk) << endl; 
  wsize = wsz;
  totnscount = 0;
  coef = new double[wsz];
  switch (fk) {
    case UserFilter :
      throw ParmError("SimpleFilter: Error in filter Kind (UserFilter)!");
      break;
    case MeanFilter :
      for(int kk=0; kk<wsz; kk++) 
	coef[kk] = a/wsize;
      break;
    case SumFilter :
      for(int kk=0; kk<wsz; kk++) 
	coef[kk] = a;
      break;
    case GaussFilter :
      for(int kk=-(wsz/2); kk<=(wsz/2); kk++) 
	coef[kk] = a*exp((double)(kk*kk)/(s*s));
      break;
    case DiffFilter :
      for(int kk=0; kk<wsz; kk++) 
	coef[kk] = -a/wsize;
      coef[wsz/2+1] += 1.;
      break;
    default :
      throw ParmError("SimpleFilter: Error in filter Kind (UnknownFilter)!");
      break;
  }
}

SimpleFilter::SimpleFilter(Vector const & vc)
{
  int wsz = vc.Size();
  if (wsz < 3) wsz = 3;
  if (wsz%2 == 0) wsz++;
  FilterKind fk = UserFilter;
  cout << "SimpleFilter::SimpleFilter(Vector & vc) vc.Size()= " 
       << vc.Size() << " WSize=" << wsz 
       << " FilterKind=" << FilterKind2String(fk) << endl; 
  wsize = wsz;
  totnscount = 0;
  coef = new double[wsz];
  for(int kk=0; kk<vc.Size(); kk++) 
    coef[kk] = vc(kk);
  for(int kk=vc.Size(); kk<wsz; kk++) 
    coef[kk] = 0.;
}

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 << " ProcessedSampleCount=" << ProcessedSampleCount() << 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);
    TVector<int_8> vfg(wsize);
    int k;
    for(k=0; k<wsize; k++) {
      vin(k%wsize) = getData(0, k+snb);
      vfg(k%wsize) = getFlag(0, k+snb);
    }
    double mean = vin.Sum()/wsize;
    for(k=wsz2+1; k<wsize; k++) { 
      vin(k) = mean; 
      vfg(k) = 0;
    }
    int knext;
    bool fgfin = false;
    // Boucle sur les sampleNum
    for(k=0;k<=sne-snb;k++) {
      double sortie = 0;
      for(int ii=0; ii<wsize; ii++) {
	sortie += vin(ii)*coef[(ii+wsz2)%wsize];
      }
      putData(0,k+snb,sortie,vfg(k%wsize));
      if (fgincopie) 
	putData(1, k+snb, vin(k%wsize), vfg(k%wsize));
      knext = k+wsz2+1;
      if (knext<=(sne-snb)) { 
	vin(knext%wsize) = getData(0, knext+snb);
	vfg(knext%wsize) = getFlag(0, knext+snb);
      }
      else {
	if (!fgfin) { 
	  mean = vin.Sum()/wsize;
	  fgfin = true;
	}
	vin(knext%wsize) = mean;
	vfg(knext%wsize) = 0;
      }
      totnscount++;
    }  // 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;
  }                                                                             
}

// ---------------------------------------------------------------
// -------------------- Classe SimpleAdder -----------------------
// ---------------------------------------------------------------

SimpleAdder::SimpleAdder(int nbinput)
  : gains(nbinput)
{
  if (nbinput < 1) 
    throw ParmError("SimpleAdder::SimpleAdder() NbInput < 1 !");
  nb_input = nbinput;
  for(int k=0; k<nb_input; k++) gains(k) = 1.;
  totnscount = 0;
}

SimpleAdder::~SimpleAdder()
{
}

void SimpleAdder::SetGain(int num, double g)
{
  if ((num < 0) || (num >= nb_input))
    throw RangeCheckError("SimpleAdder::SetGain() Out of range input number!");
  gains(num) = g;
  return;
}

double SimpleAdder::Gain(int num)
{
  if ((num < 0) || (num >= nb_input))
    throw RangeCheckError("SimpleAdder::Gain() Out of range input number!");
  return gains(num);
}

void SimpleAdder::PrintStatus(ostream & os)
{
  os << "\n ------------------------------------------------------ \n" 
     << " SimpleAdder::PrintStatus() - NbInput=" << NbInput() << endl; 
  TOIProcessor::PrintStatus(os);
  os << " Gains= " ; 
  for(int k=0; k<nb_input; k++) os << gains(k) << " " ;
  os << endl;
  os << " ProcessedSampleCount=" << ProcessedSampleCount() << endl;
  os << " ------------------------------------------------------ " << endl;
}

void SimpleAdder::init() {
  cout << "SimpleAdder::init NbInput=" << nb_input << endl;
  char buff[32];
  for(int k=0; k<nb_input; k++) {
    sprintf(buff,"in%d", k);
    declareInput(buff);
  }

  declareOutput("out");
  name = "SimpleAdder";
  //  upExtra = 1;
}

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

  bool fgout = checkOutputTOIIndex(0);
  string msg_err;
  for(int ki=0;ki<nb_input;ki++) {
    if (!checkInputTOIIndex(ki)) {
      msg_err = "SimpleAdder::run() - Input TOI (" + getInName(ki) +
	        " not connected!";
      cerr << msg_err << endl;
      throw ParmError(msg_err);
    }
  }
  if (!fgout) {
    cerr << " SimpleAdder::run() - No Output TOI connected! "
	 << endl;
    throw ParmError("SimpleAdder::run() No output TOI connected!");
  }

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

  try {
    Timer tm("SimpleAdder::run()");
    int k,i;
    double out = 0.;
    unsigned long fgout = 0;
    for(k=snb;k<=sne;k++) {
      out = 0;
      fgout = 0;
      for(i=0; i<nb_input; i++) {
	out += gains(i)*getData(i, k);
	fgout = fgout | (unsigned int)getFlag(i,k);
      }
    putData(0,k,out,fgout);
    totnscount++;
    }  // Boucle sur les num-sample
    cout << " SimpleAdder::run() - End of processing " << endl;
  }  // Bloc try 

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