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

#ifndef SLINPARBUF_H_SEEN
#define SLINPARBUF_H_SEEN

namespace SOPHYA {

//////////////////////////////////////////////////////////////////////////////////
class SLinParBuff {
public:
  friend class SLinParBuffMerger;

  //***************************************************************** 
  SLinParBuff(uint_4 lenbuf,uint_4 nresynch=0
             ,r_8 x0=0.,r_8 y0=0.,bool autoxy0=false)
  // ---- Createur
  {
    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();
  }

  SLinParBuff(SLinParBuff& slp)
  // ---- Createur par copie
  {
    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;
  }

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

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

  //***************************************************************** 
  inline void GetX0Y0(r_8& x0,r_8& y0) {x0 = mX0; y0 = mY0;}
  inline bool GetAutoX0Y0() {return mAutoXY0;}

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

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

  inline void Reset(void)
  // ---- Reset des sommes et des compteurs
  {
    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.;
  }

  //***************************************************************** 
  inline uint_4 Pop(void)
  // ---- Pour enlever la donnee la plus ancienne
  {
    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;
  }

  inline uint_4 Push(r_8 x,r_8 y)
  // ---- Pour ajouter une donnee, la donnee la plus ancienne
  //      est enlevee si le buffer est deja plein.
  {
    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;
  }

  //***************************************************************** 
  inline uint_4 ReComputeSum(void)
  // ---- Pour re-synchroniser (eviter les derives numeriques).
  {
    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;
  }

  //***************************************************************** 
  // ---- Retourne le nombre de points
  inline uint_4 NPoints(void) {return mNCur;}

  inline r_8 Compute(r_8& mean,bool recomputeXi2=false)
  // ---- Calcul <y>, Var(y)
  //   recomputeXi2=true : recalcule le xi2 (sigma) avec la courbe et les points
  {
    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;
  }

  inline r_8 Compute(r_8& a0,r_8 &a1,bool recomputeXi2=false)
  // ---- 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
  {
    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;
  }

  inline r_8 Compute(r_8& a0,r_8 &a1,r_8 &a2,bool recomputeXi2=false)
  // ---- 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
  {
    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;
  }

  //***************************************************************** 
  void Print(int lp=0)
  // ---- Print de l'etat de la classe
  //      lp = 0 : parametres
  //           1 :            + sommes
  //           2 :                     + tableaux
  {
    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;
    }
  }

  void PrintCompute(int lp=0)
  // ---- Print des valeurs numeriques calculees
  //      lp = 0 : valeurs calculees
  //           1 :             + sommes
  {
    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;
  }

protected:
  uint_4 mLenBuf,mNResynch;
  r_8 *mX,*mY, mX0, mY0;
  bool mAutoXY0;
  uint_4 mNCur,mIDeb,mIResynch;
  r_8 mSx,mSy,mSx2,mSy2,mSxy, mSx3,mSx2y, mSx4;
  uint_4 mNResynchEff, mNPush, mNPop;
};

///////////////////////////////////////////////////////////////////
// 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
class SLinParBuffMerger {
public:
  SLinParBuffMerger(void) {mFirst=true; mSlp.Reset();}
  SLinParBuffMerger(SLinParBuff& s,bool recompute=false)
                   {mFirst=true; mSlp.Reset(); Add(s,recompute);}
  virtual ~SLinParBuffMerger(void) {}
  inline uint_4  NPoints(void) {return mSlp.NPoints();}
  inline void Reset(void) {mSlp.Reset(); mFirst=true;}
  inline void Add(SLinParBuff& s,bool recompute=false)
  {
    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;
    }
  }
  inline r_8 Compute(r_8& mean)
              {return mSlp.Compute(mean);}
  inline r_8 Compute(r_8& a0,r_8 &a1)
              {return mSlp.Compute(a0,a1);}
  inline r_8 Compute(r_8& a0,r_8 &a1,r_8 &a2)
              {return mSlp.Compute(a0,a1,a2);}
  inline void Print(int lp=0) {mSlp.Print(lp);}
  inline void PrintCompute(int lp=0) {mSlp.PrintCompute(lp);}

protected:
  bool mFirst;
  SLinParBuff mSlp;
};

} // Fin du namespace

#endif
