#include "sopnamsp.h"
#include "machdefs.h"
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <math.h>
#include "fct1dfit.h"
#include "perrors.h"
#include "nbconst.h"
#include "tabmath.h"

//define EXPO exp
#define EXPO tabFExp
#define MINEXPM (100.)

//================================================================
// CLASSES DE FONCTIONS 1D AVEC PARAMETRES POUR LE FIT
//================================================================

/////////////////////////////////////////////////////////////////
/*!
  \class SOPHYA::Gauss1DPol
  \ingroup NTools
  \anchor Gauss1DPol
  \verbatim
     Gaussienne+polynome:
     Si polcenter=true: xc=(x-par[1]), sinon xc=x
     f(x) = par[0]*exp[-0.5*( (x-par[1]) / par[2] )**2 ]
           +par[3] + par[4]*xc + .... + par[3+NDegPol]*xc**NDegPol
     NDegPol = degre du polynome, si <0 pas de polynome
  \endverbatim
*/
Gauss1DPol::Gauss1DPol(unsigned int ndegpol,bool polcenter)
: GeneralFunction(1,ndegpol+4), NDegPol(ndegpol), PolCenter(polcenter)
{
}

Gauss1DPol::Gauss1DPol(bool polcenter)
: GeneralFunction(1,3), NDegPol(-1), PolCenter(polcenter)
{
}

Gauss1DPol::~Gauss1DPol()
{
}

double Gauss1DPol::Value(double const xp[], double const* Par)
{
double xc = (xp[0]-Par[1])/Par[2];
double e = 0.5*xc*xc;
if( e<MINEXPM ) e = EXPO(-e); else e = 0.;
double f = Par[0]*e;

if(NDegPol>=0) {
  double xcp = (PolCenter) ? Par[1] : 0.;
  double xpow = 1.;
  for(int i=0;i<=NDegPol;i++) {
    f += Par[3+i]*xpow;
    xpow *= xp[0]-xcp;
  }
}
return (f);
}

double Gauss1DPol::Val_Der(double const xp[],double const* Par
                          ,double *DgDpar)
{

 double xc = (xp[0]-Par[1])/Par[2];
 double xc2 = xc*xc;
 double e = 0.5*xc2;
 if( e<MINEXPM ) e = EXPO(-e); else e = 0.;
 double f = Par[0]*e;

 DgDpar[0] = e;
 DgDpar[1] = xc / Par[2] *f;
 DgDpar[2] = xc2/ Par[2] *f;

 if(NDegPol>=0) {
   double xcp = (PolCenter) ? Par[1] : 0.;
   double xpow = 1.;
   for(int i=0;i<=NDegPol;i++) {
     DgDpar[3+i] = xpow;
     f += Par[3+i]*xpow;
     if(PolCenter && i<NDegPol) DgDpar[2] += -(i+1)*xpow*Par[4+i];
     xpow *= xp[0]-xcp;
   }
 }
return f;
}


/////////////////////////////////////////////////////////////////
/*!
  \class SOPHYA::GaussN1DPol
  \ingroup NTools
  \anchor GaussN1DPol
  \verbatim
    Gaussienne_Normalisee+polynome (par[0]=Volume:
     Si polcenter=true: xc=(x-par[1]), sinon xc=x
     f(x) = par[0]/(sqrt(2*Pi)*par[2])*exp[-0.5*((x-par[1])/par[2])**2 ]
           +par[3] + par[4]*xc + .... + par[3+NDegPol]*xc**NDegPol
     NDegPol = degre du polynome, si <0 pas de polynome
  \endverbatim
*/
GaussN1DPol::GaussN1DPol(unsigned int ndegpol,bool polcenter)
: GeneralFunction(1,ndegpol+4), NDegPol(ndegpol), PolCenter(polcenter)
{
}

GaussN1DPol::GaussN1DPol(bool polcenter)
: GeneralFunction(1,3), NDegPol(-1), PolCenter(polcenter)
{
}

GaussN1DPol::~GaussN1DPol()
{
}

double GaussN1DPol::Value(double const xp[], double const* Par)
{
 double xc = (xp[0]-Par[1])/Par[2];
 double xc2 = xc*xc;
 double e = 0.5*xc2;
 if( e<MINEXPM ) e = EXPO(-e)/(S2Pi*Par[2]); else e = 0.;
 double f = Par[0]*e;

 if(NDegPol>=0) {
   double xcp = (PolCenter) ? Par[1] : 0.;
   double xpow = 1.;
   for(int i=0;i<=NDegPol;i++) {
     f += Par[3+i]*xpow;
     xpow *= xp[0]-xcp;
   }
 }

return (f);
}

double GaussN1DPol::Val_Der(double const xp[], double const* Par
                           , double *DgDpar)
{
 double xc = (xp[0]-Par[1])/Par[2];
 double xc2 = xc*xc;
 double e = 0.5*xc2;
 if( e<MINEXPM ) e = EXPO(-e)/(S2Pi*Par[2]); else e = 0.;
 double f = Par[0]*e;

 DgDpar[0] = e;
 DgDpar[1] = xc / Par[2] *f;
 DgDpar[2] = (xc2-1.)/ Par[2] *f;

 if(NDegPol>=0) {
   double xcp = (PolCenter) ? Par[1] : 0.;
   double xpow = 1.;
   for(int i=0;i<=NDegPol;i++) {
     DgDpar[3+i] = xpow;
     f += Par[3+i]*xpow;
     if(PolCenter && i<NDegPol) DgDpar[2] += -(i+1)*xpow*Par[4+i];
     xpow *= xp[0]-xcp;
   }
 }

 return f;
}

/////////////////////////////////////////////////////////////////
/*!
  \class SOPHYA::Exp1DPol
  \ingroup NTools
  \anchor Exp1DPol
  \verbatim
     Exponentielle+polynome:
     xx = x - X_Center
     f(x) = exp[par[0]+par[1]*xx]
           +par[2] + par[3]*xx + .... + par[2+NDegPol]*xx**NDegPol
     NDegPol = degre du polynome, si <0 pas de polynome
  \endverbatim
*/
Exp1DPol::Exp1DPol(unsigned int ndegpol,double x0)
: GeneralFunction(1,ndegpol+3), NDegPol(ndegpol), X_Center(x0)
{
}

Exp1DPol::Exp1DPol(double x0)
: GeneralFunction(1,2), NDegPol(-1), X_Center(x0)
{
}

Exp1DPol::~Exp1DPol()
{
}

double Exp1DPol::Value(double const xp[], double const* Par)
{
double xc = Par[0]+Par[1]*(xp[0]-X_Center);
double f = ( xc>-MINEXPM ) ? EXPO(xc): 0.;

if(NDegPol>=0) {
  double xpow = 1.;
  for(int i=0;i<=NDegPol;i++) {
    f += Par[2+i]*xpow;
    xpow *= xp[0]-X_Center;
  }
}
return (f);
}

double Exp1DPol::Val_Der(double const xp[],double const* Par
                          ,double *DgDpar)
{
double xc = Par[0]+Par[1]*(xp[0]-X_Center);
double f = ( xc>-MINEXPM ) ? EXPO(xc): 0.;

DgDpar[0] = f;
DgDpar[1] = (xp[0]-X_Center) * f;

if(NDegPol>=0) {
  double xpow = 1.;
  for(int i=0;i<=NDegPol;i++) {
    DgDpar[2+i] = xpow;
    f += Par[2+i]*xpow;
    xpow *= xp[0]-X_Center;
  }
}
return f;
}

/////////////////////////////////////////////////////////////////
/*!
  \class SOPHYA::Polyn1D
  \ingroup NTools
  \anchor Polyn1D
  \verbatim
    polynome 1D:
     xx = x - X_Center
     f(x) = par[0] + par[1]*xx + .... + par[NDegPol+1]*xx**NDegPol
     NDegPol = degre du polynome
  \endverbatim
*/
Polyn1D::Polyn1D(unsigned int ndegpol,double x0)
: GeneralFunction(1,ndegpol+1), NDegPol(ndegpol), X_Center(x0)
{
}

Polyn1D::~Polyn1D()
{
}

double Polyn1D::Value(double const xp[], double const* Par)
{
 double xpow = 1.;
 double f = 0.;
 for(int i=0;i<=NDegPol;i++) {
   f += Par[i]*xpow;
   xpow *= xp[0]-X_Center;
 }
 return (f);
}

double Polyn1D::Val_Der(double const xp[], double const* Par
                       , double *DgDpar)
{
 double xpow = 1.;
 double f = 0.;
 for(int i=0;i<=NDegPol;i++) {
   DgDpar[i] = xpow;
   f += Par[i]*xpow;
   xpow *= xp[0]-X_Center;
 }
 return f;
}


/////////////////////////////////////////////////////////////////
/*!
  \class SOPHYA::HarmonieNu
  \ingroup NTools
  \anchor HarmonieNu
  \verbatim
    Analyse harmonique:
     f(t) = par(1) + Sum[par(2k)  *cos(2*pi*k*par(0)*(t-t0)]
                   + Sum[par(2k+1)*sin(2*pi*k*par(0)*(t-t0)]
     la somme Sum porte sur l'indice k qui varie de [1,NHarm()+1]
     avec la convention   k=1  pour le fondamental
                          k>1  pour le (k-1)ieme harmonique
     par(0) = inverse de la periode (frequence)
     par(1) = terme constant
     par(2), par(3) = termes devant le cosinus et le sinus
                      du fondamental
     par(2(m+1)), par(2(m+1)+1) = termes devant le cosinus
                      et le sinus de l'harmonique m (m>=1).
     NHarm() = nombre d'harmoniques a fitter.
     T0() = centrage des temps, ce n'est pas un parametre du fit.
   	`Conseil:' Avant de faire un fit avec les `NHarm()'
   	harmoniques, il est preferable de faire un pre-fit
   	ou seuls les parametres 1,2 et 3 sont libres et d'injecter
   	le resultat du fit sur ces 3 parametres comme valeurs
   	de depart pour le fit global avec les `NHarm()' harmoniques.
   	De toute facon, le fit ne marchera que si la periode
   	est initialisee de facon tres precise.
  \endverbatim
*/
HarmonieNu::HarmonieNu(unsigned int nharm,double t0)
: GeneralFunction(1,4+2*nharm), NHarm(nharm), T0(t0)
{
}

HarmonieNu::~HarmonieNu()
{
}

double HarmonieNu::Value(double const xp[], double const* Par)
{
 double a = 2.*Pi*(xp[0]-T0)*Par[0];
 double f = Par[1];
 for(int k=1;k<=NHarm+1;k++) {
   // k=1 fondamental, k=2 a NHarm+1 harmoniques numero 1 a NHarm
   f += Par[2*k]*cos(k*a) + Par[2*k+1]*sin(k*a);
 }
 return f;
}

double HarmonieNu::Val_Der(double const xp[], double const* Par
                        , double *DgDpar)
{
 double cs,sn;
 double dadp0 = 2.*Pi*(xp[0]-T0);
 double a = dadp0*Par[0];
 double f = Par[1];
 DgDpar[0] = 0.;
 DgDpar[1] = 1.;
 for(int k=1;k<=NHarm+1;k++) {
   cs = cos(k*a); sn = sin(k*a);
   f += Par[2*k]*cs + Par[2*k+1]*sn;
   DgDpar[0] += k*dadp0*(-Par[2*k]*sn+Par[2*k+1]*cs);
   DgDpar[2*k]   = cs;
   DgDpar[2*k+1] = sn;
 }
 return f;
}


/////////////////////////////////////////////////////////////////
/*!
  \class SOPHYA::HarmonieT
  \ingroup NTools
  \anchor HarmonieT
  \verbatim
    Analyse harmonique:
     f(t) = par(1) + Sum[par(2k)  *cos(2*pi*k*(t-t0)/par(0)]
                   + Sum[par(2k+1)*sin(2*pi*k*(t-t0)/par(0)]
     la somme Sum porte sur l'indice k qui varie de [1,NHarm()+1]
     avec la convention   k=1  pour le fondamental
                          k>1  pour le (k-1)ieme harmonique
     par(0) = periode
     par(1) = terme constant
     par(2), par(3) = termes devant le cosinus et le sinus
                      du fondamental
     par(2(m+1)), par(2(m+1)+1) = termes devant le cosinus
                      et le sinus de l'harmonique m (m>=1).
     NHarm() = nombre d'harmoniques a fitter.
     T0() = centrage des temps, ce n'est pas un parametre du fit.
  \endverbatim
*/
HarmonieT::HarmonieT(unsigned int nharm,double t0)
: GeneralFunction(1,4+2*nharm), NHarm(nharm), T0(t0)
{
}

HarmonieT::~HarmonieT()
{
}

double HarmonieT::Value(double const xp[], double const* Par)
{
 double a = 2.*Pi*(xp[0]-T0)/Par[0];
 double f = Par[1];
 for(int k=1;k<=NHarm+1;k++) {
   // k=1 fondamental, k=2 a NHarm+1 harmoniques numero 1 a NHarm
   f += Par[2*k]*cos(k*a) + Par[2*k+1]*sin(k*a);
 }
 return f;
}

double HarmonieT::Val_Der(double const xp[], double const* Par
                        , double *DgDpar)
{
 double cs,sn;
 double dadp0 = 2.*Pi*(xp[0]-T0);
 double a = dadp0/Par[0];
 double f = Par[1];
 DgDpar[0] = 0.;
 DgDpar[1] = 1.;
 for(int k=1;k<=NHarm+1;k++) {
   cs = cos(k*a); sn = sin(k*a);
   f += Par[2*k]*cs + Par[2*k+1]*sn;
   DgDpar[0] += k*dadp0*(-Par[2*k]*sn+Par[2*k+1]*cs)/(-Par[0]*Par[0]);
   DgDpar[2*k]   = cs;
   DgDpar[2*k+1] = sn;
 }
 return f;
}

//================================================================
// CLASSES DE FONCTIONS 2D AVEC PARAMETRES POUR LE FIT
//================================================================

/////////////////////////////////////////////////////////////////
/*!
  \class SOPHYA::Polyn2D
  \ingroup NTools
  \anchor Polyn2D
  \verbatim
    polynome 2D de degre total degre:
     NDegPol = degre du polynome (note N dans la suite)
     x = x - X_Center,  y = y - Y_Center
     f(x,y) = p[0] +sum(k=1,n){ sum(i=0,k){ p[ki]*x^i*y^(k-i) }}
     Il y a k+1 termes de degre k  (ex: x^i*y^(k-i))
     terme de degre k avec x^i: p[ki] avec  ki = k*(k+1)/2 + i
    C'est a dire:
    deg0:   p0
    deg1: + p1*y + p2*x
    deg2: + p3*y^2 + p4*x*y + p5*x^2
    deg3: + p6*y^3 + p7*x*y^2 + p8*x^2*y + p9*x^3
    deg4: + p10*y^4 + p11*x*y^3 + p12*x^2*y^2 + p13*x^3*y + p14*x^4
    deg5: + p15*y^5 + ...                       ... + ... + p20*x^5
    ...
    degk: + p[k*(k+1)/2]*y^k + ...           ... + p[k*(k+3)/2]*x^k
    ...
    degn: + p[n*(n+1)/2]*y^n + ...           ... + p[n*(n+3)/2]*x^n
  \endverbatim
*/
Polyn2D::Polyn2D(unsigned int ndegpol,double x0,double y0)
: GeneralFunction(2,ndegpol*(ndegpol+3)/2+1), NDegPol(ndegpol), X_Center(x0), Y_Center(y0)
{
}

Polyn2D::~Polyn2D()
{
}

double Polyn2D::Value(double const xp[], double const* Par)
{
 double f = 0.;
 double xpow = 1.;
 for(int i=0;i<=NDegPol;i++) { // On gere la puissance x^i i<ndeg
   int ip;
   double ypow = 1.;
   for(int j=0;j<=NDegPol-i;j++) { // On gere la puissance x^j tq degre=i+j<=ndeg
     ip = (i+j)*(i+j+1)/2+i;
     f += Par[ip]*xpow*ypow;
     ypow *= xp[1]-Y_Center;
   }
   xpow *= xp[0]-X_Center;
 }
 return f;
}

double Polyn2D::Val_Der(double const xp[], double const* Par
                       , double *DgDpar)
{
 double f = 0.;
 double xpow = 1.;
 for(int i=0;i<=NDegPol;i++) { // On gere la puissance x^i i<ndeg
   int ip;
   double ypow = 1.;
   for(int j=0;j<=NDegPol-i;j++) { // On gere la puissance x^j tq degre=i+j<=ndeg
     ip = (i+j)*(i+j+1)/2+i;
     DgDpar[ip] = xpow*ypow;
     f += Par[ip]*DgDpar[ip];
     ypow *= xp[1]-Y_Center;
   }
   xpow *= xp[0]-X_Center;
 }
 return f;
}