source: Sophya/trunk/ArchTOIPipe/ProcWSophya/simtoipr.cc@ 1442

#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;
}

SimpleDeglitcher::~SimpleDeglitcher()
{
}

void SimpleDeglitcher::init() {
  cout << "SimpleDeglitcher::init" << endl;
  declareInput("in");
  declareOutput("out");
  declareOutput("mean");
  declareOutput("sigma");
  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);
  
  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) {
    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() SN=" << snb << " - " << sne << endl; 
  try {
    // 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()/wsize;
    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+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);

      if (!fgout)   continue;  // Pas de sortie out (deglitche)

      valcur = vin(k%wsize);

      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;
    }  // Boucle sur les num-sample
    cout << " La fin lastput=" << lastput << " SNE=" << sne; 
    for(k=lastput-snb; k<sne-snb; k++) 
      putData(0, k+snb, vin(k%wsize), 0);
    cout << " DBG3- OUT of try bloc ! " << endl;
      
  }  // 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::init() {
  cout << "SimpleFilter::init" << endl;
  declareInput("in");
  declareOutput("out");
  name = "SimpleFilter";
  //  upExtra = 1;
}

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

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

  bool fgout = checkOutputTOIIndex(0);

  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!");
  }
  

  try {
    // 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);
    }  // Boucle sur les num-sample
  }  // Bloc try 

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


}