source: Sophya/trunk/AddOn/TAcq/brproc.cc@ 3645

#include "racquproc.h"

#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fstream>
#include <signal.h>

#include "pexceptions.h"
#include "tvector.h"
#include "fioarr.h"
#include "timestamp.h"
#include "ctimer.h"
#include "fftpserver.h"
#include "fftwserver.h"

#include "FFTW/fftw3.h"


#include "pciewrap.h"
#include "brpaqu.h"
#include "brproc.h"


//---------------------------------------------------------------
// Classe thread de traitement donnees ADC avec 2 voies par frame
//---------------------------------------------------------------

/* --Methode-- */
BRProcARaw2C::BRProcARaw2C(RAcqMemZoneMgr& mem, string& path, uint_4 nmean, 
                           uint_4 nmax, bool fgnotrl, int card)
  :  memgr(mem) 
{
  nmax_ = nmax; 
  nmean_ = nmean;
  stop_ = false;        
  path_ = path; 
  fgnotrl_ = fgnotrl;
  card_ = card;
}

/* --Methode-- */
void BRProcARaw2C::Stop()
{
 stop_=true;
 // cout <<" BRProcARaw2C::Stop ... > STOP " << endl;
}


static inline r_4 Zmod2(complex<r_4> z) 
{ return (z.real()*z.real()+z.imag()*z.imag()); }

static inline string card2name_(int card)
{
  if (card==2) return " (Chan3,4) ";
  else return " (Chan1,2) ";
}
/* --Methode-- */
void BRProcARaw2C::run()
{
  setRC(1);     
  try {
    Timer tm("BRProcARaw2C", false);
    TimeStamp ts; 
    BRPaqChecker pcheck(~fgnotrl_);  // Verification/comptage des paquets 
    
    size_t totnbytesout = 0;
    size_t totnbytesproc = 0;

    cout << " BRProcARaw2C::run() - Starting " << ts << " NMaxMemZones=" << nmax_ 
         << " NMean=" << nmean_ << card2name_(card_) << endl;   
    cout << " BRProcARaw2C::run()... - Output Data Path: " << path_ << endl;
    char fname[512];
//    sprintf(fname,"%s/proc.log",path_.c_str());
//    ofstream filog(fname);
//    filog << " BRProcARaw2C::run() - starting log file " << ts << endl;                      
//    filog << " ... NMaxMemZones=" << nmax_ << " NMean=" << nmean_ << " Step=" << step_ << endl;       

// Initialisation pour calcul FFT 
    TVector< complex<r_4> > cfour1;  // composant TF
    uint_4 paqsz = memgr.PaqSize();
    uint_4 procpaqsz = memgr.ProcPaqSize();
    BRPaquet pq(NULL, NULL, paqsz); 
    TVector<r_4> vx(pq.DataSize()/2);
    vx = (r_4)(0.);
    FFTPackServer ffts;
    ffts.FFTForward(vx, cfour1);
    TVector< complex<r_4> > cfour2(cfour1.Size());
    
    TVector<r_4> spectreV1(cfour1.Size());
    TVector<r_4> spectreV2(cfour1.Size());
    TVector< complex<r_4> > visiV12( cfour1.Size() );


    fftwf_plan plan1 = fftwf_plan_dft_r2c_1d(vx.Size(), vx.Data(), 
                          (fftwf_complex*)cfour1.Data(), FFTW_ESTIMATE); 
    fftwf_plan plan2 = fftwf_plan_dft_r2c_1d(vx.Size(), vx.Data(), 
                          (fftwf_complex*)cfour2.Data(), FFTW_ESTIMATE); 

    uint_4 ifile = 0;                           
    uint_4 nzm = 0;
    for (uint_4 kmz=0; kmz<nmax_; kmz++) {
      if (stop_) break;
      int mid = memgr.FindMemZoneId(MemZA_ProcA);
      Byte* buff = memgr.GetMemZone(mid);
      if (buff == NULL) {
         cout << " BRProcARaw2C::run()/ERROR memgr.GetMemZone(" << mid << ") -> NULL" << endl;
             break;             
      }
      Byte* procbuff = memgr.GetProcMemZone(mid);
      if (procbuff == NULL) {
            cout << " BRProcARaw2C::run()/ERROR memgr.GetProcMemZone(" << mid << ") -> NULL" << endl;
        break;  
      }

      for(uint_4 i=0; i<memgr.NbPaquets(); i++) {
            BRPaquet paq(NULL, buff+i*paqsz, paqsz); 
        if (!pcheck.Check(paq)) continue;   // on ne traite que les paquets OK

// Traitement voie 1        
          for(sa_size_t j=0; j<vx.Size(); j++) 
            vx(j) = (r_4)(*(paq.Data1()+j))-127.5;
//        fftwf_complex* coeff1 = (fftwf_complex*)(procbuff+i*procpaqsz);
          fftwf_execute(plan1); 
//        complex<r_4>* zp1 = (complex<r_4>*)(coeff1);
//        ffts.FFTForward(vx, cfour1);
        for(sa_size_t j=0; j<spectreV1.Size(); j++) 
          spectreV1(j) += Zmod2(cfour1(j));
        memcpy(procbuff+i*procpaqsz, cfour1.Data(), sizeof(complex<r_4>)*cfour1.Size());
// Traitement voie 2        
        for(sa_size_t j=0; j<vx.Size(); j++) 
          vx(j) = (r_4)(*(paq.Data2()+j))-127.5;

        fftwf_execute(plan2); 

        for(sa_size_t j=0; j<spectreV2.Size(); j++) 
          spectreV2(j) += Zmod2(cfour2(j));  // Zmod2(zp2[j]); 
        memcpy(procbuff+i*procpaqsz+procpaqsz/2, cfour2.Data(), sizeof(complex<r_4>)*cfour2.Size());

// Calcul correlation (visibilite V1 * V2)
        for(sa_size_t j=0; j<visiV12.Size(); j++) 
          visiV12(j)+=cfour1(j)*conj(cfour2(j));
//        for(sa_size_t j=0; j<visiV12.Size(); j++) visiV12(j)+=zp1[j]*zp2[j];
        nzm++;   
        totnbytesproc += paq.DataSize();
        totnbytesout += (2*sizeof(complex<r_4>)*cfour1.Size());

      } // Fin de boucle sur les paquets d'une zone
      if ((nzm >= nmean_) || ((kmz==(nmax_-1))&&(nzm>1))) {
        spectreV1 /= (r_4)(nzm);
        spectreV2 /= (r_4)(nzm);

        visiV12 /= complex<r_4>((r_4)nzm, 0.);

        spectreV1.Info()["NPaqMoy"] = nzm;
        spectreV2.Info()["NPaqMoy"] = nzm;
        visiV12.Info()["NPaqMoy"] = nzm;
            {
        sprintf(fname,"%s_%d.ppf",path_.c_str(),(int)ifile);
        POutPersist po(fname);
        string tag1="specV1";
        string tag2="specV2";
        string tag12="visiV12";
        if (card_==2) {
          tag1 = "specV3";
          tag2 = "specV4";
          tag12="visiV34";
        }
        po << PPFNameTag(tag1) << spectreV1; 
        po << PPFNameTag(tag2) << spectreV2; 
        po << PPFNameTag(tag12) << visiV12; 
            }
        spectreV1 = (r_4)(0.);
        spectreV2 = (r_4)(0.);
        visiV12 = complex<r_4>(0., 0.);
        nzm = 0;  ifile++; 
//        ts.SetNow();
//        filog << ts << " :  proc file  " << fname << endl;                   
        cout << " BRProcARaw2C::run() created file  " << fname << card2name_(card_) << endl;
      }   
      
      memgr.FreeMemZone(mid, MemZS_ProcA);
    }  // Fin de boucle sur les zones a traiter 
  cout << " ------------  BRProcARaw2C::run() END " << card2name_(card_) 
       << " ------------ " << endl;
  ts.SetNow();
  tm.SplitQ();
  cout << "  TotalProc= " << totnbytesproc/(1024*1024) << " MBytes, rate= " 
       << (double)(totnbytesproc)/1024./tm.PartialElapsedTimems() << " MB/s" 
       << " ProcDataOut=" <<  totnbytesout/(1024*1024) << " MB" << endl;    
  cout << pcheck;
  cout << " BRProcARaw2C::run()/Timing: " << card2name_(card_) << endl; 
  tm.Print();
  cout << " ---------------------------------------------------------- " << endl;
    
  }
  catch (PException& exc) {
    cout << " BRProcARaw2C::run()/catched PException " << exc.Msg() << endl;
    setRC(3);   
    return; 
  }
  catch(...) {
    cout << " BRProcARaw2C::run()/catched unknown ... exception " << endl;
    setRC(4);   
    return; 
  }
  setRC(0);
  return;
}   

//---------------------------------------------------------------------
// Classe thread de traitement 2 x 2 voies/frames (Apres BRProcARaw2C)
//---------------------------------------------------------------------

/* --Methode-- */
BRProcBRaw4C::BRProcBRaw4C(RAcqMemZoneMgr& mem1, RAcqMemZoneMgr& mem2, 
                           string& path, uint_4 nmean, uint_4 nmax, bool fgnotrl)
  :  memgr1(mem1), memgr2(mem2) 
{
  nmax_ = nmax; 
  nmean_ = nmean;
  stop_ = false;        
  path_ = path; 
  fgnotrl_ = fgnotrl;
}

/* --Methode-- */
void BRProcBRaw4C::Stop()
{
 stop_=true;
 // cout <<" BRProcBRaw4C::Stop ... > STOP " << endl;
}


/* --Methode-- */
void BRProcBRaw4C::run()
{
  setRC(1);     
  try {
    Timer tm("BRProcBRaw4C", false);
    TimeStamp ts; 
    BRPaqChecker pcheck1(~fgnotrl_);  // Verification/comptage des paquets 
    BRPaqChecker pcheck2(~fgnotrl_);  // Verification/comptage des paquets 

    size_t totnbytesout = 0;
    size_t totnbytesproc = 0;

    cout << " BRProcBRaw4C::run() - Starting " << ts << " NMaxMemZones=" << nmax_ 
         << " NMean=" << nmean_ << endl;        
    cout << " BRProcBRaw4C::run()... - Output Data Path: " << path_ << endl;

    uint_4 paqsz = memgr1.PaqSize();
    uint_4 procpaqsz = memgr1.ProcPaqSize();
    if ((paqsz != memgr2.PaqSize())||(procpaqsz!= memgr2.ProcPaqSize())) {
      cout << "BRProcBRaw4C::run()/ERROR : different paquet size -> stop \n ...(PaqSz1="
           << paqsz << " Sz2=" << memgr2.PaqSize() << " ProcPaqSz1=" 
           << procpaqsz << " Sz2=" << memgr2.ProcPaqSize() << " )" << endl;
      setRC(9); 
      return;   
    }

    TVector< complex<r_4> > cfour;  // composant TF
    BRPaquet pq(NULL, NULL, paqsz); 
    TVector<r_4> vx(pq.DataSize()/2);
    vx = (r_4)(0.);
    FFTPackServer ffts;
    ffts.FFTForward(vx, cfour);
    
    TVector< complex<r_4> > visiV13( cfour.Size() );
    TVector< complex<r_4> > visiV14( cfour.Size() );
    TVector< complex<r_4> > visiV23( cfour.Size() );
    TVector< complex<r_4> > visiV24( cfour.Size() );

    uint_4 nzm = 0;
    uint_4 totnoksfc = 0;
    uint_4 totnokpaq = 0;
    uint_4 totnpaq = 0;
    uint_4 ifile = 0;
    for (uint_4 kmz=0; kmz<nmax_; kmz++) {
      uint_4 noksfc = 0;
      uint_4 nokpaq = 0;
      if (stop_) break;
      int mid1 = memgr1.FindMemZoneId(MemZA_ProcB);
      Byte* buff1 = memgr1.GetMemZone(mid1);
      if (buff1 == NULL) {
         cout << " BRProcBRaw4C::run()/ERROR memgr.GetMemZone(" << mid1 << ") -> NULL" << endl;
             break;             
      }
      Byte* procbuff1 = memgr1.GetProcMemZone(mid1);
      if (procbuff1 == NULL) {
            cout << " BRProcBRaw4C::run()/ERROR memgr.GetProcMemZone(" << mid1 << ") -> NULL" << endl;
        break;  
      }
      int mid2 = memgr2.FindMemZoneId(MemZA_ProcB);
      Byte* buff2 = memgr2.GetMemZone(mid2);
      if (buff1 == NULL) {
         cout << " BRProcBRaw4C::run()/ERROR memgr.GetMemZone(" << mid2 << ") -> NULL" << endl;
             break;             
      }
      Byte* procbuff2 = memgr2.GetProcMemZone(mid2);
      if (procbuff2 == NULL) {
            cout << " BRProcBRaw4C::run()/ERROR memgr.GetProcMemZone(" << mid2 << ") -> NULL" << endl;
        break;  
      }
      uint_4 i1,i2;
      i1=i2=0;
      while((i1<memgr1.NbPaquets())&&(i2<memgr2.NbPaquets())) {
        BRPaquet paq1(NULL, buff1+i1*paqsz, paqsz); 
        BRPaquet paq2(NULL, buff2+i2*paqsz, paqsz); 
        totnpaq++;
//        cout << " DBG["<<kmz<<"] i1,i2=" << i1 <<","<<i2<<" FC1,FC2=" <<paq1.FrameCounter()
//<<","<<paq2.FrameCounter()<<endl;
        // on ne traite que les paquets OK
        if (!pcheck1.Check(paq1)) { i1++; continue; } 
        if (!pcheck2.Check(paq2)) { i2++; continue; }
        nokpaq++;
        if (paq1.FrameCounter()<paq2.FrameCounter()) { i1++; continue; }  
        if (paq2.FrameCounter()<paq1.FrameCounter()) { i2++; continue; }  
//        cout << " DBG["<<kmz<<"]OKOK i1,i2=" << i1 <<","<<i2<<" FC1,FC2=" <<paq1.FrameCounter()
// <<","<<paq2.FrameCounter()<<endl;

        // Les deux framecounters sont identiques ...
        noksfc++;
        complex<r_4>* zp1 = (complex<r_4>*)(procbuff1+i1*procpaqsz);
        complex<r_4>* zp2 = (complex<r_4>*)(procbuff1+i1*procpaqsz+procpaqsz/2);
        complex<r_4>* zp3 = (complex<r_4>*)(procbuff2+i2*procpaqsz);
        complex<r_4>* zp4 = (complex<r_4>*)(procbuff2+i2*procpaqsz+procpaqsz/2);
        for(sa_size_t j=0; j<visiV13.Size(); j++) { 
          visiV13(j)+=zp1[j]*conj(zp3[j]);
          visiV14(j)+=zp1[j]*conj(zp4[j]);
          visiV23(j)+=zp2[j]*conj(zp3[j]);
          visiV24(j)+=zp2[j]*conj(zp4[j]);
        }
        nzm++;  i1++; i2++;
        totnbytesproc += 2*paq1.DataSize();
      } // Fin de boucle sur les paquets d'une zone
      if ((nzm >= nmean_) || ((kmz==(nmax_-1))&&(nzm>1))) {
        visiV13 /= complex<r_4>((r_4)nzm, 0.);
        visiV14 /= complex<r_4>((r_4)nzm, 0.);
        visiV23 /= complex<r_4>((r_4)nzm, 0.);
        visiV24 /= complex<r_4>((r_4)nzm, 0.);
        visiV13.Info()["NPaqMoy"] = nzm;
        visiV14.Info()["NPaqMoy"] = nzm;
        visiV23.Info()["NPaqMoy"] = nzm;
        visiV24.Info()["NPaqMoy"] = nzm;
        char fname[512];
        {
        sprintf(fname,"%s_%d.ppf",path_.c_str(),(int)ifile);
        POutPersist po(fname);
        po << PPFNameTag("visiV13") << visiV13; 
        po << PPFNameTag("visiV14") << visiV14; 
        po << PPFNameTag("visiV23") << visiV23; 
        po << PPFNameTag("visiV24") << visiV24; 
        }
        visiV13 = complex<r_4>(0., 0.);
        visiV14 = complex<r_4>(0., 0.);
        visiV23 = complex<r_4>(0., 0.);
        visiV24 = complex<r_4>(0., 0.);
        nzm = 0;  
//        ts.SetNow();
//        filog << ts << " :  proc file  " << fname << endl;                   
        cout << " BRProcBRaw4C::run() created file  " << fname << endl;
      }   
      memgr1.FreeMemZone(mid1, MemZS_ProcB);
      memgr2.FreeMemZone(mid2, MemZS_ProcB);
      double okfrac = (nokpaq>1)?((double)noksfc/(double)nokpaq*100.):0.;
      cout << "BRProcBRaw2C ["<<kmz<<"] NOKPaq=" << nokpaq << " NSameFC=" << noksfc 
           << " (" << okfrac << " %)" << endl;
      totnokpaq += nokpaq;
      totnoksfc += noksfc;
    }  // Fin de boucle sur les zones a traiter 
    cout << " ------------------  BRProcBRaw4C::run() END ----------------- " << endl;
    ts.SetNow();
    tm.SplitQ();
    cout << "  TotalProc= " << totnbytesproc/(1024*1024) << " MBytes, rate= " 
         << (double)(totnbytesproc)/1024./tm.PartialElapsedTimems() << " MB/s" << endl;
    double totokfrac = (totnokpaq>1)?((double)totnoksfc/(double)totnokpaq*100.):0.;
    cout << " NOkPaq1,2=" << totnokpaq << " /TotNPaq=" << totnpaq << " TotNSameFC=" 
         << totnoksfc << " (" << totokfrac << " %)" << endl;
//  cout << pcheck1;
//  cout << pcheck2;
    cout << " BRProcBRaw4C::run()/Timing: \n"; 
    tm.Print();
    cout << " ---------------------------------------------------------- " << endl;
}
  catch (PException& exc) {
    cout << " BRProcBRaw4C::run()/catched PException " << exc.Msg() << endl;
    setRC(3);   
    return; 
  }
  catch(...) {
    cout << " BRProcBRaw4C::run()/catched unknown ... exception " << endl;
    setRC(4);   
    return; 
  }
  setRC(0);
  return;
}