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

#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()");
    Vector vin(wsize);

    int wrec = maxpoints*2;
    Vector vrec(wrec);

    TVector<int_8> vfg(wsize);
    int wsz2 = wsize/2;
    // Le debut 
    int k;
    for(k=0; k<wsz2; k++) 
      getData(0, k+snb, vin(k), vfg(k));

    int nokdebut = 0.;
    double s = 0.;    
    double mean = 0.;
    double s2 = 0.;
    double sigma = 0.;
    for(k=0; k<wsz2; k++) {
      if ( vfg(k) != 0) continue;
      if ( (vin(k) < range_min) || (vin(k) > range_max) ) continue;
      s += vin(k);
      s2 += vin(k)*vin(k);
      nokdebut++;
    }
    if (nokdebut > 0) {
      mean = s/nokdebut;
      if (nokdebut > 1)  sigma = sqrt(s2/nokdebut-mean*mean);
    }
    for(k=wsz2; k<wsize; k++) {
      vin(k) = mean;
      vfg(k) = 0;
    } 

    for(k=0; k<wrec; k++) {
      if ( (vin(k) < range_min) || (vin(k) > range_max) ) vrec(k)=mean;
      else vrec(k)=vin(k);    
    }

    bool fgokdebut = false;

    int kgl = -1;
    int ii,lastput;
    bool fgglitch = false;
    double valcur,valsub,valadd;
    double lastvalok = mean;
    uint_8 fgcur;
    bool fgokcur=false;
    // Boucle sur les sampleNum 

    int knext;
    int kfin = sne-snb;
    for(k=0;k<=kfin;k++) {
      totnscount++;
//       if (k%10000 == 0) cout << " DBG: K=" << k << endl;
      knext = k+wsz2;
      // Calcul mean-sigma
      if (knext<=kfin)  {
        valsub = vin(knext%wsize);
        getData(0, knext+snb, vin(knext%wsize), vfg(knext%wsize));
        valadd = vin(knext%wsize);
        if ( (valadd < range_min) || (valadd > range_max) ) {
          valadd = mean;
          fgokcur = false;
        }
        else fgokcur = true;
        if ( (valsub < range_min) || (valsub > range_max) ) 
          valsub = mean;
        if (!fgokdebut && fgokcur) {
          s += valadd;
          s2 += valadd*valadd;
          nokdebut++;
          mean = s/nokdebut;
          if (nokdebut > 1)  sigma = sqrt(s2/nokdebut-mean*mean);
          if (nokdebut >= wsize)  { 
            fgokdebut = true;
            cout << " SimpleDeglitcher::DebugInfo -  nokdebut=" << nokdebut 
                 << " k=" << k << " knext=" << knext 
                 << "\n ...DebugInfo mean=" << mean 
                 << " sigma=" << sigma << " s=" << s << " s2=" << s2 << endl;
          }
        } 
        else {
          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));

      if ( (valcur < range_min) || (valcur > range_max) ) {
        vin(k%wsize) = valcur = mean;
        vfg(k%wsize) |= 2;
        out_range_nscount++;
      }
      valcur = vin(k%wsize);
      fgcur = vfg(k%wsize);
        
      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;
//            }

      double curnsig = nsig;
      if (fgglitch)  curnsig /= 2.;
        
      if (valcur < mean+curnsig*sigma) {  // inferieur au seuil
        if (fgglitch) {  
          if (k-kgl < maxpoints) {   // On vient de detecter un glitch
            glcount++;
            double recval = vrec.Sum()/wrec;
            for(ii=kgl; ii<k; ii++) {
              putData(0, ii+snb, recval, 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;
        vrec(k%wrec) = lastvalok = valcur;
      }
      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;  
            vrec(k%wrec) = lastvalok = valcur;      
          }
        }
        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;
          }
          vrec(k%wrec) = lastvalok;         
        }
      }

//           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];
  for(int k=0; k<wsz; k++) coef[k] = 0.;
  int kk;
  switch (fk) {
    case UserFilter :
      throw ParmError("SimpleFilter: Error in filter Kind (UserFilter)!");
      //      break;
    case MeanFilter :
      for(kk=0; kk<wsz; kk++) 
        coef[kk] = a/wsize;
      break;
    case SumFilter :
      for(kk=0; kk<wsz; kk++) 
        coef[kk] = a;
      break;
    case GaussFilter :
      for(kk=-(wsz/2); kk<=(wsz/2); kk++) 
        coef[kk+(wsz/2)] = a*exp(-(double)(kk*kk)/(s*s));
      break;
    case DiffFilter :
      for(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];
  int kk;
  for(kk=0; kk<vc.Size(); kk++) 
    coef[kk] = vc(kk);
  for(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++) 
      getData(0, k+snb, vin(k%wsize), vfg(k%wsize));
    
    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=-wsz2; ii<=wsz2; ii++) {
        sortie += vin((ii+k+wsize)%wsize)*coef[ii+wsz2];
      }
      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)) 
        getData(0, knext+snb, vin(knext%wsize), vfg(knext%wsize));

      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.;
    double valin = 0.;
    int_8 fgin = 0;
    int_8 fgout = 0;
    for(k=snb;k<=sne;k++) {
      out = 0;
      fgout = 0;
      for(i=0; i<nb_input; i++) {
        getData(i, k, valin, fgin);
        out += gains(i)*valin;
        fgout = fgout | fgin;
      }
    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;
  }                                                                             
}


// ---------------------------------------------------------------
// -------------------- Classe SimpleFanOut -----------------------
// ---------------------------------------------------------------

SimpleFanOut::SimpleFanOut(int nbinput, int mfanout)
{
  if (nbinput < 1) 
    throw ParmError("SimpleFanOut::SimpleFanOut() NbInput < 1 !");
  if (mfanout < 1) 
    throw ParmError("SimpleFanOut::SimpleFanOut() M_FanOut < 1 !");

  nb_input = nbinput;
  m_fanout = mfanout;
  totnscount = 0;
}

SimpleFanOut::~SimpleFanOut()
{
}


void SimpleFanOut::PrintStatus(ostream & os)
{
  os << "\n ------------------------------------------------------ \n" 
     << " SimpleFanOut::PrintStatus() - NbInput=" << NbInput() 
     << " M_FanOut=" << MFanOut() << endl; 
  TOIProcessor::PrintStatus(os);
  os << endl;
  os << " ProcessedSampleCount=" << ProcessedSampleCount() << endl;
  os << " ------------------------------------------------------ " << endl;
}

void SimpleFanOut::init() {
  cout << "SimpleFanOut::init NbInput=" << nb_input << endl;
  char buff[64];
  for(int k=0; k<nb_input; k++) {
    sprintf(buff,"in%d", k);
    declareInput(buff);
    for(int j=0; j<m_fanout; j++) {
      sprintf(buff,"out%d_%d", k, j);
      declareOutput(buff);
    }
  }

  name = "SimpleFanOut";
  //  upExtra = 1;
}

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

  TVector<int_4> in_index(nb_input);
  TMatrix<int_4> out_index(nb_input, m_fanout);
  in_index = -1;
  out_index = -1;
  int nbconin = 0;
  bool fggin = false;
  char buff[64];
  for(int ki=0;ki<nb_input;ki++) {
    sprintf(buff,"in%d", ki);
    int idx = getInputTOIIndex(buff);
    if (!checkInputTOIIndex(idx)) continue;
    nbconin++;
    in_index(ki) = idx;
    bool fgout = false;
    for(int jo=0; jo<m_fanout; jo++) {
      sprintf(buff,"out%d_%d", ki, jo);
      int odx = getOutputTOIIndex(buff);
      if (checkOutputTOIIndex(odx)) {
        out_index(ki, jo) = odx;
        fgout = true;
      }
    }
    if (!fgout) {
      string msg_err = 
        "SimpleFanOut::run() - No connected Output for Input TOI (" 
        + getInName(ki) + ") !";
      cerr << msg_err << endl;
      throw ParmError(msg_err);
    }
  }
  if (nbconin == 0) {
    cerr << " SimpleFanOut::run() - No Input TOI connected! "
         << endl;
    throw ParmError("SimpleFanOut::run() No Inout TOI connected!");
  }

  /*
  for(int ki=0;ki<nb_input;ki++) {
    cout << " SimpleFanOut::run() In(" << ki << ") Index=" << in_index(ki) 
         << " Name=" << getInName(in_index(ki)) << endl;
    for(int jo=0; jo<m_fanout; jo++) 
      cout << " .... Out(" << ki << "," << jo << ") Index=" << out_index(ki, jo)
           << " Name=" << getOutName(out_index(ki, jo)) << endl;
  }
  */
  cout << " SimpleFanOut::run() SNRange=" << snb << " - " << sne << endl; 
  

  try {
    Timer tm("SimpleFanOut::run()");
    double valin = 0.;
    int_8 fgin = 0;
    for(int k=snb;k<=sne;k++) {
      for(int i=0;i<nb_input;i++) {
        if (in_index(i) < 0) continue;
        valin = 0;
        fgin = 0;
        getData(in_index(i), k, valin, fgin);
        for(int j=0; j<m_fanout; j++) {
          if (out_index(i, j) < 0)  continue;
          putData(out_index(i, j), k, valin, fgin);
        }
      }
      totnscount++;
    }  // Boucle sur les num-sample
    cout << " SimpleFanOut::run() - End of processing " 
         << " ProcessedSampleCount=" << ProcessedSampleCount() << endl;
  }  // Bloc try 

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