source: Sophya/trunk/SophyaLib/NTools/slinparbuff.cc@ 3664

#include "machdefs.h"
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <algorithm>

#include "pexceptions.h"

#include "slinparbuff.h"

namespace SOPHYA {

//================================================================
// SLinParBuff
//================================================================

// ---- Createur
SLinParBuff::SLinParBuff(uint_4 lenbuf,uint_4 nresynch,r_8 x0,r_8 y0,bool autoxy0)
{
  if(lenbuf==0) throw RangeCheckError("SLinParBuff: lenbuf==0 !");
  mLenBuf = lenbuf;
  mNResynch = nresynch;
  mX = new r_8[mLenBuf]; mY = new r_8[mLenBuf];
  mX0 = x0; mY0 = y0; mAutoXY0 = autoxy0;
  Reset();
}

// ---- Createur par copie
SLinParBuff::SLinParBuff(SLinParBuff& slp)
{
  mLenBuf = slp.mLenBuf;
  mNResynch = slp.mNResynch;

  mX = mY = NULL;
  if(mLenBuf) {
    mX = new r_8[mLenBuf]; mY = new r_8[mLenBuf];
    for(uint_4 i=0;i<mLenBuf;i++) {mX[i]=slp.mX[i]; mY[i]=slp.mY[i];}
  }

  mX0 = slp.mX0; mY0 = slp.mY0; mAutoXY0 = slp.mAutoXY0;
  mNCur = slp.mNCur; mIDeb = slp.mIDeb; mIResynch = slp.mIResynch;
  mNResynchEff = slp.mNResynchEff; mNPush = slp.mNPush; mNPop = slp.mNPop;

  mSx = slp.mSx; mSy = slp.mSy;
  mSx2 = slp.mSx2; mSy2 = slp.mSy2; mSxy = slp.mSxy;
  mSx3 = slp.mSx3; mSx2y = slp.mSx2y;
  mSx4 = slp.mSx4;
}

// ---- Createur par defaut
SLinParBuff::SLinParBuff(void)
{
  mLenBuf = 0; mNResynch = 0;
  mX = mY = NULL;
  mX0 = mY0 = 0.; mAutoXY0 = false;
  Reset();
}

// ---- Destructeur
SLinParBuff::~SLinParBuff()
{
  if(mX) delete [] mX; if(mY) delete [] mY;
}

void SLinParBuff::GetX0Y0(r_8& x0,r_8& y0)
{
  x0 = mX0;
  y0 = mY0;
}

bool SLinParBuff::GetAutoX0Y0()
{
  return mAutoXY0;
}

// ---- Stabilite numerique, centrage des x,y en mX0,mY0
void SLinParBuff::SetX0Y0(r_8 x0,r_8 y0)
{
  mX0 = x0; mY0 = y0; mAutoXY0 = false;
  ReComputeSum();
}

// ---- Stabilite numerique, centrage automatique a <x> et <y>
//      a chaque ReComputeSum
void SLinParBuff::SetAutoX0Y0(bool autoxy0)
{
  mAutoXY0 = autoxy0;
  ReComputeSum();
}

// ---- Reset des sommes et des compteurs
void SLinParBuff::Reset(void)
{
  mSx=mSy=mSx2=mSy2=mSxy=mSx3=mSx2y=mSx4=0.;
  mNCur = mIDeb = mIResynch = mNResynchEff = mNPush = mNPop = 0;
  //if(mX) for(uint_4 i=0;i<mLenBuf;i++) mX[i] = 0.;
  //if(mY) for(uint_4 i=0;i<mLenBuf;i++) mY[i] = 0.;
}

// ---- Pour enlever la donnee la plus ancienne
uint_4 SLinParBuff::Pop(void)
{
  if(mNCur==0) return mNCur;
  r_8 x=mX[mIDeb]-mX0, y=mY[mIDeb]-mY0; r_8 x2 = x*x;
  mSx  -= x;     mSy   -= y;
  mSx2 -= x2;    mSy2  -= y*y;   mSxy -= x*y;
  mSx3 -= x2*x;  mSx2y -= x2*y;
  mSx4 -= x2*x2;
  mNCur--; mNPop++;
  if(mNCur==0) {Reset(); return mNCur;}
  mIDeb++; if(mIDeb==mLenBuf) mIDeb=0;
  return mNCur;
}

// ---- Pour ajouter une donnee, la donnee la plus ancienne
//      est enlevee si le buffer est deja plein.
uint_4 SLinParBuff::Push(r_8 x,r_8 y)
{
  uint_4 ifill;
  if(mNCur==mLenBuf) {
    r_8 X=mX[mIDeb]-mX0, Y=mY[mIDeb]-mY0; r_8 X2 = X*X;
    mSx  -= X;     mSy   -=Y;
    mSx2 -= X2;    mSy2  -=Y*Y;   mSxy -= X*Y;
    mSx3 -= X2*X;  mSx2y -=X*X*Y;
    mSx4 -= X2*X2;
    ifill = mIDeb;
    mIDeb++; if(mIDeb==mLenBuf) mIDeb=0;
  } else {
    ifill = (mIDeb+mNCur)%mLenBuf;
    mNCur++;
  }
  mX[ifill] = x; mY[ifill] = y;
  x -= mX0; y -= mY0; r_8 x2 = x*x;
  mSx += x; mSy += y;
  mSx2 += x2; mSy2 += y*y; mSxy += x*y;
  mSx3 += x2*x; mSx2y += x2*y;
  mSx4 += x2*x2;
  mNPush++;
  // Il faut re-synchroniser pour eviter les derives numeriques
  mIResynch++; if(mIResynch == mNResynch) ReComputeSum();
  return mNCur;
}

// ---- Pour re-synchroniser (eviter les derives numeriques).
uint_4 SLinParBuff::ReComputeSum(void)
{
  if(mNCur==0) return 0;
  // Re-centrage automatique a la moyenne demande:
  // Attention, mSx = sum(x-mX0) ==> nouvel mX0 = mSx/N + mX0(ancien)
  if(mAutoXY0) {mX0 = mSx/(r_8)mNCur + mX0; mY0 = mSy/(r_8)mNCur + mY0;}
  mSx=mSy=mSx2=mSy2=mSxy=mSx3=mSx2y=mSx4=0.;
  for(uint_4 i=mIDeb;i<mIDeb+mNCur;i++) {
    uint_4 ii = i%mLenBuf;
    r_8 x=mX[ii]-mX0, y=mY[ii]-mY0; r_8 x2 = x*x;
    mSx  += x;     mSy   += y;
    mSx2 += x2;    mSy2  += y*y;   mSxy += x*y;
    mSx3 += x2*x;  mSx2y += x2*y;
    mSx4 += x2*x2;
  }
  mIResynch=0;
  mNResynchEff++;
  return mNCur;
}

// ---- Calcul <y>, Var(y)
//   recomputeXi2=true : recalcule le xi2 (sigma) avec la courbe et les points
r_8 SLinParBuff::Compute(r_8& mean,bool recomputeXi2)
{
  mean=0.;
  if(mNCur==0) return -1.;
  // Moyenne
  mean  = mSy/(r_8) mNCur;
  // Sigma
  r_8 sigma;
  if(recomputeXi2) {
    sigma=0.;
    for(uint_4 i=mIDeb;i<mIDeb+mNCur;i++) {
      uint_4 ii = i%mLenBuf;
      r_8 s = mean - (mY[ii]-mY0);
      sigma += s*s;
    }
    sigma /= (r_8) mNCur;
  } else {
    sigma = mSy2/(r_8) mNCur - mean*mean;
  }
  // gestion du decalage
  mean += mY0;

  if(sigma>0.) return sqrt(sigma);
  if(sigma<0.) return -sqrt(-sigma);
  return sigma;
}

// ---- Calcul y=a0+a1*x et slin=Var(y-(a0+a1*x))=sqrt(<dy^2>)
//   recomputeXi2=true : recalcule le xi2 avec la courbe et les points
r_8 SLinParBuff::Compute(r_8& a0,r_8 &a1,bool recomputeXi2)
{
  a0=a1=0.;
  if(mNCur==0) return -1.;
  // Fit lineaire
  r_8 w = mNCur*mSx2 - mSx*mSx;
  if(w==0. || mNCur==1) return -2.;
  a0 = (mSx2*mSy - mSx*mSxy)/w;
  a1 = (mNCur*mSxy - mSx*mSy )/w;
  // Sigma par rapport Fit lineaire 
  // (On a XI2=mNCur*slin**2 dans notre cas ou les erreurs=1)
  r_8 slin;
  if(recomputeXi2) {
    slin=0.;
    for(uint_4 i=mIDeb;i<mIDeb+mNCur;i++) {
      uint_4 ii = i%mLenBuf;
      r_8 s = a0+a1*(mX[ii]-mX0) - (mY[ii]-mY0);
      slin += s*s;
    }
    slin /= (r_8) mNCur;
  } else {
    slin = (mSy2 +a0*a0*mNCur +a1*a1*mSx2 -2.*a0*mSy -2.*a1*mSxy +2.*a0*a1*mSx)
           / (r_8)mNCur;
  }
  // gestion du decalage y-y0 = a0 + a1*(x-x0)
  a0 = mY0 + a0 - a1*mX0;

  if(slin>0.) return sqrt(slin);
  if(slin<0.) return -sqrt(-slin);
  return slin;
}

// ---- Calcul y=a0+a1*x+a2*x^2 et spar=Var(y-(a0+a1*x+a2*x^2))=sqrt(<dy^2>)
//   recomputeXi2=true : recalcule le xi2 avec la courbe et les points
r_8 SLinParBuff::Compute(r_8& a0,r_8 &a1,r_8 &a2,bool recomputeXi2)
{
  a0=a1=a2=0.;
  if(mNCur==0) return -1.;
  // Fit parabolique
  r_8 w = mSx2*(mSx2*mSx2-mSx3*mSx) -mSx*(mSx3*mSx2-mSx4*mSx) +mNCur*(mSx3*mSx3-mSx4*mSx2);
  if(w==0. || mNCur<=2) return -2.;
  a2 =  (mSy*(mSx2*mSx2-mSx3*mSx)  - mSxy*(mSx*mSx2-mSx3*mNCur)  + mSx2y*(mSx*mSx-mSx2*mNCur) )/w;
  a1 = -(mSy*(mSx3*mSx2-mSx4*mSx)  - mSxy*(mSx2*mSx2-mSx4*mNCur) + mSx2y*(mSx2*mSx-mSx3*mNCur))/w;
  a0 =  (mSy*(mSx3*mSx3-mSx4*mSx2) - mSxy*(mSx2*mSx3-mSx4*mSx)   + mSx2y*(mSx2*mSx2-mSx3*mSx) )/w;
  // Sigma par rapport Fit parabolique
  // (On a XI2=mNCur*spar**2 dans notre cas ou les erreurs=1)
  // Le calcul direct du Xi2 n'est pas precis
  r_8 spar;
  if(recomputeXi2) {
    spar=0.;
    for(uint_4 i=mIDeb;i<mIDeb+mNCur;i++) {
      uint_4 ii = i%mLenBuf;
      r_8 s = a0+(a1+a2*(mX[ii]-mX0))*(mX[ii]-mX0) - (mY[ii]-mY0);
      spar += s*s;
    }
    spar /= (r_8) mNCur;
  } else {
    spar = (mSy2 +a0*a0*mNCur +a1*a1*mSx2 +a2*a2*mSx4 -2.*mSy*a0 -2.*a1*mSxy
            -2.*a2*mSx2y +2.*a0*a1*mSx +2.*a0*a2*mSx2 +2.*a1*a2*mSx3)
           / (r_8) mNCur;
  }
  // gestion du decalage y-y0 = a0 + a1*(x-x0) + a2*(x-x0)^2
  a0 = mY0 + a0 - a1*mX0 + a2*mX0*mX0;
  a1 = a1 - 2.*a2*mX0;

  if(spar>0.) return sqrt(spar);
  if(spar<0.) return -sqrt(-spar);
  return spar;
}

// ---- Print de l'etat de la classe
//      lp = 0 : parametres
//           1 :            + sommes
//           2 :                     + tableaux
void SLinParBuff::Print(int lp)
{
  cout<<"SLinParBuff(LenBuf="<<mLenBuf<<",NResynch="<<mNResynch
      <<",auto_xy0="<<mAutoXY0
      <<"): mX0="<<mX0<<" mY0="<<mY0<<" mNCur="<<mNCur<<endl
      <<"           NPush="<<mNPush<<" NPop="<<mNPop
      <<" NSynchro="<<mNResynchEff<<endl;
  if(mNCur==0) return;
  if(lp>=2) {
    cout<<"X:";
    if(mX) for(uint_4 i=0;i<mNCur;i++) {
      uint_4 ii = (mIDeb+i)%mLenBuf;
      cout<<" "<<mX[ii]; if(i%10==9 || i==mNCur-1) cout<<endl;
      }
    if(mY) cout<<"Y:";
    for(uint_4 i=0;i<mNCur;i++) {
      uint_4 ii = (mIDeb+i)%mLenBuf;
      cout<<" "<<mY[ii]; if(i%10==9 || i==mNCur-1) cout<<endl;
    }
  }
  if(lp>=1) {
    cout<<"...IDeb="<<mIDeb<<" IResynch="<<mIResynch<<endl;
    cout<<"   Sx="<<mSx<<" Sx2="<<mSx2<<" Sx3="<<mSx3<<" Sx4="<<mSx4<<endl
        <<"   Sy="<<mSy<<" Sy2="<<mSy2<<" Sxy="<<mSxy<<" Sx2y="<<mSx2y<<endl;
  }
}

// ---- Print des valeurs numeriques calculees
//      lp = 0 : valeurs calculees
//           1 :             + sommes
void SLinParBuff::PrintCompute(int lp)
{
  bool recompute = (mX==NULL || mY==NULL) ? false: true;
  r_8 mean;
  r_8 sigma = Compute(mean,recompute), sigmar = Compute(mean,false);
  cout<<"SLinParBuff: n="<<NPoints()<<" mean="<<mean
      <<" sigma="<<sigma<<" (raw="<<sigmar<<")"<<endl;
  r_8 a0,a1,a2;
  r_8 slin  = Compute(a0,a1,recompute), slinr  = Compute(a0,a1,false);
  cout<<"             a0="<<a0<<" a1="<<a1
      <<" slin="<<slin<<" (raw="<<slinr<<")"<<endl;
  r_8 spar = Compute(a0,a1,a2,recompute), sparr = Compute(a0,a1,a2,false);
  cout<<"             a0="<<a0<<" a1="<<a1<<" a2="<<a2
      <<" spar="<<spar<<" (raw="<<sparr<<")"<<endl;
  if(lp<1) return;
  cout<<"...Sx="<<mSx<<" Sx2="<<mSx2<<" Sx3="<<mSx3<<" Sx4="<<mSx4<<endl
      <<"   Sy="<<mSy<<" Sy2="<<mSy2<<" Sxy="<<mSxy<<" Sx2y="<<mSx2y<<endl;
}




//================================================================
// SLinParBuffMerger
//================================================================

///////////////////////////////////////////////////////////////////
// That Class allows merging of SLinParBuff Class for computing  //
// parameters.
// ex:
// SLinParBuff s1;  -> Fill s1
// SLinParBuff s2;  -> Fill s2
// SLinParBuff s3;  -> Fill s3
// SLinParBuffMerger smerge(s1);
//   sig = smerge.Compute(mean); -> same as sig = s1.Compute(mean);
// smerge.Add(s2);
//   sig = smerge.Compute(mean); -> sig and mean are those for


SLinParBuffMerger::SLinParBuffMerger(void)
{
  mFirst=true;
  mSlp.Reset();
}

SLinParBuffMerger::SLinParBuffMerger(SLinParBuff& s,bool recompute)
{
  mFirst=true;
  mSlp.Reset();
  Add(s,recompute);
}

SLinParBuffMerger::~SLinParBuffMerger(void)
{
}

void SLinParBuffMerger::Add(SLinParBuff& s,bool recompute)
{
  bool changex0y0=false, AutoXY0_Save; r_8 X0_Save,Y0_Save;
  if(mFirst) {
    // Cas ou c'est le premier SLinParBuff additionne.
    mSlp.mX0=s.mX0; mSlp.mY0=s.mY0; mFirst=false;
  } else if(mSlp.mX0!=s.mX0 || mSlp.mY0!=s.mY0) {
    // Attention: pour merger il faut avoir les memes mX0,mY0
    changex0y0=true;
    X0_Save=s.mX0; Y0_Save=s.mY0; AutoXY0_Save=s.mAutoXY0;
    s.mX0=mSlp.mX0; s.mY0=mSlp.mY0; s.mAutoXY0=false;
    recompute=true;
  }
  if(recompute) s.ReComputeSum();
  mSlp.mNCur += s.mNCur;
  mSlp.mSx   += s.mSx;
  mSlp.mSy   += s.mSy;
  mSlp.mSx2  += s.mSx2;
  mSlp.mSy2  += s.mSy2;
  mSlp.mSxy  += s.mSxy;
  mSlp.mSx3  += s.mSx3;
  mSlp.mSx2y += s.mSx2y;
  mSlp.mSx4  += s.mSx4;
  // Dans le cas ou on a change les X0,Y0, on remet en etat
  if(changex0y0) {
    s.mX0=X0_Save; s.mY0=Y0_Save;
    s.ReComputeSum();
    s.mAutoXY0=AutoXY0_Save;
  }
}


} // Fin du namespace