#include "sopnamsp.h"
#include "machdefs.h"
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <vector>

#include "pexceptions.h"

#include "histos.h"
#include "hisprof.h"
#include "srandgen.h"

#include "geneutils.h"

namespace SOPHYA {

//-------------------------------------------------------------------
// Classe d'interpolation lineaire:
// Le vecteur y a n elements y_i tels que y_i = f(x_i)
//   ou les x_i sont regulierement espaces
//   et x_0=xmin et x_{n-1}=xmax   (xmax inclus!)
InterpFunc::InterpFunc(double xmin,double xmax,vector<double>& y)
  : _xmin(xmin), _xmax(xmax), _y(y)
{
  if(_xmin>=_xmax || _y.size()<=2) {  // au moins 3 points!
   cout<<"InterpFunc::InterpFunc : bad arguments values"<<endl;
   throw ParmError("InterpFunc::InterpFunc : bad arguments values");
  }
  _nm1   = _y.size()-1;
  _dx    = (_xmax-_xmin)/(double)_nm1;
}

double InterpFunc::Linear(double x,unsigned short& ok)
{
  ok=0; if(x<_xmin) ok=1; else if(x>_xmax) ok=2;
  x -= _xmin;
  long i = long(x/_dx);  // On prend le "i" juste en dessous
  if(i<0) i=0; else if(i>=_nm1) i=_nm1-1;
  return _y[i] + (_y[i+1]-_y[i])/_dx*(x-i*_dx);
}

double InterpFunc::Parab(double x,unsigned short& ok)
{
  ok=0; if(x<_xmin) ok=1; else if(x>_xmax) ok=2;
  x -= _xmin;
  long i = long(x/_dx+0.5);  // On prend le "i" le + proche
  if(i<1) i=1; else if(i>=_nm1-1) i=_nm1-2;
  double a = (_y[i+1]-2.*_y[i]+_y[i-1])/(2.*_dx*_dx);
  double b = (_y[i+1]-_y[i-1])/(2.*_dx);
  return _y[i] + (x-i*_dx)*(a*(x-i*_dx)+b);
}

//-------------------------------------------------------------------
// Classe d'inversion d'une fonction STRICTEMENT MONOTONE CROISSANTE
//
// - On part de deux vecteurs x,y de "Nin" elements tels que "y_i = f[x_i]"
//   ou la fonction "f" est strictement monotone croissante:
//          x(i) < x(i+1)   et   y(i) < y(i+1)
// - Le but de la classe est de remplir un vecteur X de "Nout" elements
//   tels que:    X_j = f^-1[Y_j]    avec   j=[0,Nout[
//   avec les Y_j regulierement espaces entre ymin=y(0) , ymax=y(Nin -1)
//   cad:     X_j = f^-1[ ymin+j*(ymax-ymin)/(Nout-1) ]
// - La construction du vecteur X est realisee
//   par interpolation lineaire (ComputeLinear) ou parabolique (ComputeParab)
InverseFunc::InverseFunc(vector<double>& x,vector<double>& y)
  : _ymin(0.) , _ymax(0.) , _x(x) , _y(y)
{
  uint_4 ns = _x.size();
  if(ns<3 || _y.size()<=0 || ns!=_y.size())
    throw ParmError("InverseFunc::InverseFunc_Error: bad array size");

  // Check "x" strictement monotone croissant
  for(uint_4 i=0;i<ns-1;i++)
    if(_x[i+1]<=_x[i]) {
      cout<<"InverseFunc::InverseFunc_Error: _x array not stricly growing"<<endl;
      throw ParmError("InverseFunc::InverseFunc_Error: _x array not stricly growing");
    }

  // Check "y" monotone croissant
  for(uint_4 i=0;i<ns-1;i++)
    if(_y[i+1]<_y[i]) {
      cout<<"InverseFunc::InverseFunc_Error: _y array not growing"<<endl;
      throw ParmError("InverseFunc::InverseFunc_Error: _y array not growing");
    }

  // define limits
  _ymin = _y[0];
  _ymax = _y[ns-1];

}

InverseFunc::~InverseFunc(void)
{
}

int InverseFunc::ComputeLinear(long nout,vector<double>& xfcty)
// Compute table "xfcty" by linear interpolation of "x" versus "y"
//   on "nout" points from "ymin" to "ymax":
// xfcty[i] = interpolation of function "x" for "ymin+i*(ymax-ymin)/(nout-1)"
{
  if(nout<3) return -1;

  xfcty.resize(nout);

  long i1,i2;
  double x;
  for(int_4 i=0;i<nout;i++) {
    double y = _ymin + i*(_ymax-_ymin)/(nout-1.);
    find_in_y(y,i1,i2);
    double dy = _y[i2]-_y[i1];
    if(dy==0.) {
      x = (_x[i2]+_x[i1])/2.; // la fct a inverser est plate!
    } else {
      x = _x[i1] + (_x[i2]-_x[i1])/dy * (y-_y[i1]);
    }
    xfcty[i] = x;
  }

  return 0;
}

int InverseFunc::ComputeParab(long nout,vector<double>& xfcty)
{
  if(nout<3) return -1;

  xfcty.resize(nout);

  long i1,i2,i3;
  double x;
  for(int_4 i=0;i<nout;i++) {
    double y = _ymin + i*(_ymax-_ymin)/(nout-1.);
    find_in_y(y,i1,i2);
    // On cherche le 3ieme point selon la position de y / au 2 premiers
    double my = (_y[i1]+_y[i2])/2.;
    if(y<my) {i3=i2; i2=i1; i1--;} else {i3=i2+1;}
    // Protection
    if(i1<0) {i1++; i2++; i3++;}
    if(i3==(long)_y.size()) {i1--; i2--; i3--;}
    // Interpolation parabolique
    double dy = _y[i3]-_y[i1];
    if(dy==0.) {
      x = (_x[i3]+_x[i1])/2.; // la fct a inverser est plate!
    } else {
      double X1=_x[i1]-_x[i2], X3=_x[i3]-_x[i2];
      double Y1=_y[i1]-_y[i2], Y3=_y[i3]-_y[i2];
      double den = Y1*Y3*dy;
      double a = (X3*Y1-X1*Y3)/den;
      double b = (X1*Y3*Y3-X3*Y1*Y1)/den;
      y -= _y[i2];
      x = (a*y+b)*y + _x[i2];
    }
    xfcty[i] = x;
  }

  return 0;
}

//----------------------------------------------------
double InterpTab(double x0,vector<double>& X,vector<double>& Y,unsigned short typint)
// Interpole in x0 the table Y = f(X)
//           X doit etre ordonne par ordre croissant (strictement)
// typint = 0 : nearest value
//          1 : linear interpolation
//          2 : parabolique interpolation
{
 long n = X.size();
 if(n>(long)Y.size() || n<2)
   throw ParmError("InterpTab_Error :  incompatible size between X and Y tables!");

 if(x0<X[0] || x0>X[n-1]) return 0.;
 if(typint>2) typint = 0;

 // Recherche des indices encadrants par dichotomie
 long k, klo=0, khi=n-1;
 while (khi-klo > 1) {
   k = (khi+klo) >> 1;
   if (X[k] > x0) khi=k; else klo=k;
 }

 // Quel est le plus proche?
 k = (x0-X[klo]<X[khi]-x0) ? klo: khi;

 // On retourne le plus proche
 if(typint==0) return Y[k];

 // On retourne l'extrapolation lineaire
 if(typint==1 || n<3)
   return Y[klo] + (Y[khi]-Y[klo])/(X[khi]-X[klo])*(x0-X[klo]);

 // On retourne l'extrapolation parabolique
 if(k==0) k++; else if(k==n-1) k--;
 klo = k-1; khi = k+1;
 double x1 = X[klo]-X[k], x2 = X[khi]-X[k];
 double y1 = Y[klo]-Y[k], y2 = Y[khi]-Y[k];
 double den = x1*x2*(x1-x2);
 double a = (y1*x2-y2*x1)/den;
 double b = (y2*x1*x1-y1*x2*x2)/den;
 x0 -= X[k];
 return Y[k] + (a*x0+b)*x0;;

}

//-------------------------------------------------------------------
int FuncToHisto(GenericFunc& func,Histo& h,bool logaxex)
// Remplit l'histo 1D "h" avec la fonction "func"
// INPUT:
// logaxex = false : remplissage lineaire
//        les abscisses "x" des bins sont remplis avec f(x)
// logaxex = true : remplissage logarithmique (base 10)
//        les abscisses "x" des bins sont remplis avec f(10^x)
// RETURN:
//       0 = OK
//       1 = error
{
  if(h.NBins()<=0) return 1;

  h.Zero();

  for(int_4 i=0;i<h.NBins();i++) {
    double x = h.BinCenter(i);
    if(logaxex) x = pow(10.,x);
    h.SetBin(i,func(x));
  }

  return 0;
}

int FuncToVec(GenericFunc& func,TVector<r_8>& v,double xmin,double xmax,bool logaxex)
// Remplit le TVector avec la fonction "func"
// INPUT:
// logaxex = false : remplissage lineaire
//        les abscisses "x" des bins sont remplis avec f(x)
// logaxex = true : remplissage logarithmique (base 10)
//        les abscisses "x" des bins sont remplis avec f(10^x)
// RETURN:
//       0 = OK
//       1 = error
// Remarque:
//  v(i) = f(xmin+i*dx) avec dx = (xmax-xmin)/v.NElts()
{
  if(v.NElts()<=0 || xmax<=xmin) return 1;

  v = 0.;
  double dx = (xmax-xmin)/v.NElts();
   
  for(int_4 i=0;i<v.NElts();i++) {
    double x = xmin + i * dx;;
    if(logaxex) x = pow(10.,x);
    v(i) = func(x);
  }

  return 0;
}

//-------------------------------------------------------------------
double AngSol(double dtheta,double dphi,double theta0)
// Retourne l'angle solide d'un "rectangle" et coordonnees spheriques
// de DEMI-COTE "dtheta" x "dphi" et centre en "theta0"
// Attention: Le "theta0" de l'equateur est Pi/2  (et non pas zero)
//            Les unites des angles sont en radians
//               theta0 in [0,Pi]
//               dtheta in [0,Pi]
//               dphi   in [0,2Pi]
// Return: l'angle solide en steradian
{
  double theta1 = theta0-dtheta, theta2 = theta0+dtheta;
  if(theta1<0.) theta1=0.;
  if(theta2>M_PI) theta2=M_PI;

  return   2.*dphi * (cos(theta1)-cos(theta2));
}

double AngSol(double dtheta)
// Retourne l'angle solide d'une calotte spherique de demi-ouverture "dtheta"
// Attention: Les unites des angles sont en radians
//               dtheta in [0,Pi]
// Return: l'angle solide en steradian
// Approx pour theta petit: PI * theta^2
{
  return   2.*M_PI * (1.-cos(dtheta));
}

double FrAngSol(double angsol)
// Retourne la demi-ouverture "dtheta" d'une calotte spherique d'angle solide "angsol"
// Input: angle solide de la calotte spherique en steradians
// Return: demi-ouverture de la calotte spherique en radians
{
  angsol = 1. - angsol/(2.*M_PI);
  if(angsol<-1. || angsol>1.) return -1.;
  return acos(angsol);
}

//-------------------------------------------------------------------
double SinXsX(double x,bool app)
// Calcul de sin(x)/x
// Le Dl est en O[x]^10  (cad on va jusqu'au terme en x^8 compris)
// Approx: terme en x^6/(6*20*42) ~ 1e-13 -> x^2 ~ 1.7e-4
{
  double x2 = x*x;
  if(app || x2<1.7e-4) return 1.-x2/6.*(1.-x2/20.*(1.-x2/42.*(1.-x2/72.)));
  return sin(x)/x;
}

double SinXsX_Sqr(double x,bool app)
// Calcul de (sin(x)/x)^2
// Le Dl est en O[x]^10  (cad on va jusqu'au terme en x^8 compris)
// Approx: terme 2*x^6/(3*15*14) ~ 1e-13 -> x^2 ~ 6.8e-5
{
  double x2 = x*x;
  if(app || x2<6.8e-5) return 1.-x2/3.*(1.-2.*x2/15.*(1.-x2/14.*(1.-2.*x2/45.)));
  x2 = sin(x)/x;
  return x2*x2;
}

double SinNXsX(double x,unsigned long N,bool app)
// Calcul de sin(N*x)/sin(x)  avec N entier positif
// ATTENTION: N est entier 
//  1. pour que sin(N*x) et sin(x) soient nuls en meme temps
//     (on peut faire le DL popur sin(N*x) et sin(x))
//  2. pour traiter correctement le DL en x = p*Pi+e  avec e<<1 et p entier
//     sin(N*x)/sin(x) = sin(N*p*Pi+N*e)/sin(p*Pi+e)
//                     = [sin(N*p*Pi)*cos(N*e)+cos(N*p*Pi)*sin(N*e)]
//                         / [sin(p*Pi)*cos(e)+cos(p*Pi)*sin(e)]
//                     comme sin(N*p*Pi)=0
//                     = [cos(N*p*Pi)*sin(N*e)] / [cos(p*Pi)*sin(e)]
//                     = [sin(N*e)/sin(e)] * [cos(N*p*Pi)/cos(p*Pi)]
//                     = [DL autour de x=0] * (+1 ou -1)
//     Le seul cas ou on a "-1" est quand "p=impair" (cos(p*Pi)=-1) et "N=pair"
// Approx: dernier terme x^4*N^4/120 ~ 1e-13 -> x^2 ~ 3.5e-6/N^2
{
  if(N==0) return 0;
  double sx = sin(x), N2 = N*N;
  if(app || sx*sx<3.5e-6/N2) {
    double x2 = asin(sx); x2 *= x2;
    double s = 1.;
    if(N%2==0 && cos(x)<0.) s = -1.; // cas x ~ (2p+1)*Pi et N pair
    return s*N*(1.-(N-1.)*(N+1.)/6.*x2*(1.-(3.*N2-7.)/60.*x2));
  }
  return sin((double)N*x)/sx;
}

double SinNXsX_Sqr(double x,unsigned long N,bool app)
// Calcul de [sin(N*x)/sin(x)]^2  avec N entier positif
// ATTENTION: cf remarque pour N entier dans SinNXsX
//            maximum de la fonction: N^2
// Approx: dernier terme x^4*2*N^4/45 ~ 1e-13 -> x^2 ~ 1.5e-6/N^2
{
  if(N==0) return 0;
  double sx = sin(x), N2 = N*N;
  if(app || sx*sx<1.5e-6/N2) {
    double x2 = asin(sx); x2 *= x2;
    //return N2*(1.-(N*N-1.)/3.*x2);
    return N2*(1.-(N-1.)*(N+1.)/3.*x2*(1.-(2.*N2-3.)/15.*x2));
  }
  sx = sin((double)N*x)/sx;
  return sx*sx;
}

//-------------------------------------------------------------------

static unsigned short IntegrateFunc_GlOrder = 0;
static vector<double> IntegrateFunc_x;
static vector<double> IntegrateFunc_w;

///////////////////////////////////////////////////////////
//************** Integration of Functions ***************//
///////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////////////////
double IntegrateFunc(GenericFunc& func,double xmin,double xmax
       ,double perc,double dxinc,double dxmax,unsigned short glorder)
// --- Integration adaptative ---
//     Integrate[func[x], {x,xmin,xmax}]
// ..xmin,xmax are the integration limits
// ..dxinc is the searching increment x = xmin+i*dxinc
//         if <0  npt = int(|dxinc|)   (si<2 alors npt=100)
//                et dxinc = (xmax-xmin)/npt
// ..dxmax is the maximum possible increment (if dxmax<=0 no test)
// ---
// Partition de [xmin,xmax] en intervalles [x(i),x(i+1)]:
// on parcourt [xmin,xmax] par pas de "dxinc" : x(i) = xmin + i*dxinc
// on cree un intervalle [x(i),x(i+1)]
//     - si |f[x(i+1)] - f[x(i)]| > perc*|f[[x(i)]|
//     - si |x(i+1)-x(i)| >= dxmax  (si dxmax>0.)
// Dans un intervalle [x(i),x(i+1)] la fonction est integree
//   avec une methode de gauss-legendre d'ordre "glorder"
{
  double signe = 1.;
  if(xmin>xmax) {double tmp=xmax; xmax=xmin; xmin=tmp; signe=-1.;}

  if(glorder==0) glorder = 4;
  if(perc<=0.) perc=0.1;
  if(dxinc<=0.) {int n=int(-dxinc); if(n<2) n=100; dxinc=(xmax-xmin)/n;}
  if(glorder != IntegrateFunc_GlOrder) {
    IntegrateFunc_GlOrder = glorder;
    Compute_GaussLeg(glorder,IntegrateFunc_x,IntegrateFunc_w,0.,1.);
  }

  // Recherche des intervalles [x(i),x(i+1)]
  int_4 ninter = 0;
  double sum = 0., xbas=xmin, fbas=func(xbas), fabsfbas=fabs(fbas);
  for(double x=xmin+dxinc; x<xmax+dxinc/2.; x += dxinc) {
    double f = func(x);
    double dx = x-xbas;
    bool doit = false;
    if( x>xmax ) {doit = true; x=xmax;}
    else if( dxmax>0. && dx>dxmax ) doit = true;
    else if( fabs(f-fbas)>perc*fabsfbas ) doit = true;
    if( ! doit ) continue;
    double s = 0.;
    for(unsigned short i=0;i<IntegrateFunc_GlOrder;i++)
      s += IntegrateFunc_w[i]*func(xbas+IntegrateFunc_x[i]*dx);
    sum += s*dx;
    xbas = x; fbas = f; fabsfbas=fabs(fbas); ninter++;
  }
  //cout<<"Ninter="<<ninter<<endl;

  return sum*signe;
}

////////////////////////////////////////////////////////////////////////////////////
double IntegrateFuncLog(GenericFunc& func,double lxmin,double lxmax
         ,double perc,double dlxinc,double dlxmax,unsigned short glorder)
// --- Integration adaptative ---
// Idem IntegrateFunc but integrate on logarithmic (base 10) intervals:
//    Int[ f(x) dx ] = Int[ x*f(x) dlog10(x) ] * log(10)
// ..lxmin,lxmax are the log10() of the integration limits
// ..dlxinc is the searching logarithmic (base 10) increment lx = lxmin+i*dlxinc
// ..dlxmax is the maximum possible logarithmic (base 10) increment (if dlxmax<=0 no test)
// Remarque: to be use if "x*f(x) versus log10(x)" looks like a polynomial
//            better than "f(x) versus x"
// ATTENTION: la fonction func que l'on passe en argument
//            est "func(x)" et non pas "func(log10(x))"
{
  double signe = 1.;
  if(lxmin>lxmax) {double tmp=lxmax; lxmax=lxmin; lxmin=tmp; signe=-1.;}

  if(glorder==0) glorder = 4;
  if(perc<=0.) perc=0.1;
  if(dlxinc<=0.) {int n=int(-dlxinc); if(n<2) n=100; dlxinc=(lxmax-lxmin)/n;}
  if(glorder != IntegrateFunc_GlOrder) {
    IntegrateFunc_GlOrder = glorder;
    Compute_GaussLeg(glorder,IntegrateFunc_x,IntegrateFunc_w,0.,1.);
  }

  // Recherche des intervalles [lx(i),lx(i+1)]
  int_4 ninter = 0;
  double sum = 0., lxbas=lxmin, fbas=func(pow(10.,lxbas)), fabsfbas=fabs(fbas);
  for(double lx=lxmin+dlxinc; lx<lxmax+dlxinc/2.; lx += dlxinc) {
    double f = func(pow(10.,lx));
    double dlx = lx-lxbas;
    bool doit = false;
    if( lx>lxmax ) {doit = true; lx=lxmax;}
    else if( dlxmax>0. && dlx>dlxmax ) doit = true;
    else if( fabs(f-fbas)>perc*fabsfbas ) doit = true;
    if( ! doit ) continue;
    double s = 0.;
    for(unsigned short i=0;i<IntegrateFunc_GlOrder;i++) {
      double y = pow(10.,lxbas+IntegrateFunc_x[i]*dlx);
      s += IntegrateFunc_w[i]*y*func(y);
    }
    sum += s*dlx;
    lxbas = lx; fbas = f; fabsfbas=fabs(fbas); ninter++;
  }
  //cout<<"Ninter="<<ninter<<endl;

  return M_LN10*sum*signe;
}

////////////////////////////////////////////////////////////////////////////////////
/*
Integration des fonctions a une dimension y=f(x) par la Methode de Gauss-Legendre.
  --> Calcul des coefficients du developpement pour [x1,x2]
| INPUT:
|  x1,x2 : bornes de l'intervalle (dans nbinteg.h -> x1=-0.5 x2=0.5)
|  glorder = degre n du developpement de Gauss-Legendre
| OUTPUT:
|  x[] = valeur des abscisses ou l'on calcule (dim=n)
|  w[] = valeur des coefficients associes
| REMARQUES:
|  - x et w seront dimensionnes a n.
|  - l'integration est rigoureuse si sur l'intervalle d'integration
|    la fonction f(x) peut etre approximee par un polynome
|    de degre 2*m (monome le + haut x**(2*n-1) )
*/
void Compute_GaussLeg(unsigned short glorder,vector<double>& x,vector<double>& w,double x1,double x2)
{
  if(glorder==0) return;
  int n = (int)glorder;
  x.resize(n,0.); w.resize(n,0.);

   int m,j,i;
   double z1,z,xm,xl,pp,p3,p2,p1;

   m=(n+1)/2;
   xm=0.5*(x2+x1);
   xl=0.5*(x2-x1);
   for (i=1;i<=m;i++)  {
      z=cos(M_PI*(i-0.25)/(n+0.5));
      do {
         p1=1.0;
         p2=0.0;
         for (j=1;j<=n;j++) {
            p3=p2;
            p2=p1;
            p1=((2.0*j-1.0)*z*p2-(j-1.0)*p3)/j;
         }
         pp=n*(z*p1-p2)/(z*z-1.0);
         z1=z;
         z=z1-p1/pp;
      } while (fabs(z-z1) > 3.0e-11);  // epsilon
      x[i-1]=xm-xl*z;
      x[n-i]=xm+xl*z;
      w[i-1]=2.0*xl/((1.0-z*z)*pp*pp);
      w[n-i]=w[i-1];
   }
}

}  // Fin namespace SOPHYA