// @Copyright 2007-2017 Kristjan Haule
#include <iostream>
#include <fstream>
#include <vector>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multiroots.h>
#include "logging.h"
#include "smesh.h"
#include "sfunction.h"
#include "tanmesh.h"
#include "romberg.h"
#include <time.h>

#ifdef _MPI
#include <mpi.h>
#endif

using namespace std;

inline double fermi_kernel(double t, double w, double beta)
{
  double x = beta*w/2;
  double y = 2.*t/beta-1.;
  if (x>100.) return exp(-x*(y+1.));
  if (x<-100) return exp(x*(1.-y));
  return exp(-x*y)/(2*cosh(x));
}
inline double bose_kernel(double t, double w, double beta)
{
  double x = beta*w/2;
  double y = 2.*t/beta-1.;
  if (x>200.) return w*exp(-x*(y+1.));
  if (x<-200.) return -w*exp(x*(1.-y));
  return w*exp(-x*y)/(2*sinh(x));
}
inline void create_log_mesh(function1D<int>& ind_om, int nom_all, int nom, int ntail_)
{
  //Creates logarithmic mesh on Matsubara axis
  //     Takes first istart points from mesh om and the rest of om mesh is replaced by ntail poinst redistribued logarithmically.
  //     Input:
  //         om      -- original long mesh
  //         nom     -- number of points not in the tail
  //         ntail   -- tail replaced by ntail points only
  //     Output:
  //         ind_om  -- index array which conatins index to kept Matsubara points
  int istart = min(nom, nom_all);
  int ntail = min(ntail_, nom_all-istart);
  ind_om.resize(istart+ntail);
  double alpha = log((nom_all-1.)/istart)/(ntail-1.);
  for (int i=0; i< istart; i++) ind_om[i]=i;
  int ii=istart;
  for (int i=0; i < ntail; i++){
    int t = int(istart*exp(alpha*i)+0.5);
    if (t != ind_om[ii-1])
      ind_om[ii++] = t;
  }
  ind_om.resize(ii);
}

class SVDFunc{
public:
  vector<spline1D<double> > fU, fU_bose; // SVD functions in imaginary time
  mesh1D tau;                             // imaginary time optimized mesh
  int lmax, lmax_bose;                   // cutoff for the singular values
  int k_ovlp;                             // 2^{k_ovlp}+1 is the number of integration points in romberg method
  mesh1D om;                              // real axis mesh for SVD
  function1D<double> S, S_bose;          // eigenvalues of SVD decomposition
  function2D<double> Vt, Vt_bose;        // real axis basis functions
  function1D<double> dxi;                 // parts of fU splines stored in more efficient way for fast interpolation
  function3D<double> ff2;                 // fU splines stored in different way for fast interpolation
  double beta, L, x0;                     // parameters of the mesh should be remembered for reproducibility
  //
  SVDFunc() : lmax(0)  {  };
  //
  bool operator()() //  was it ever initialized?
  { return lmax>0; }
  double operator()(int l, int i){
    if (l<0) l=lmax-l;
    if (i<0) i=tau.size()+i;
    return fU[l][i];
  }
  double operator()(int l, int i) const{
    if (l<0) l=lmax-l;
    if (i<0) i=tau.size()+i;
    return fU[l][i];
  }
  spline1D<double>& operator[](int l){
    return fU[l];
  }

  inline vector<int> get_lll(int ii)
  { // conversion from a single combined index to the three l indices.
    //int ii = l0 + lmax*l1 + lmax*lmax*lb;
    int lb_ = ii / (lmax*lmax);
    int ir = ii % (lmax*lmax);
    int l1_ = ir / lmax;
    int l0_ = ir % lmax;
    int tmp[] = { l0_, l1_, lb_ };
    return vector<int>(tmp, tmp+3 );
  }
  void _cmp_(double beta, int& lmax, vector<spline1D<double> >& fU, function1D<double>& S, function2D<double>& Vt, function2D<double>& K, ostream& clog){
    clock_t t0 = clock();
    int Nl = min(om.size(),tau.size());
    S.resize(Nl);
    Vt.resize(om.size(),Nl);
    function2D<double> U(Nl,tau.size());

    int lwork;
    {
      int n = min(tau.size(),om.size());
      int mx = max(tau.size(),om.size());
      lwork =  4*n*n + 8*max(mx, n*(n+1));
    }
    function1D<double> work(lwork);
    function1D<int> iwork(8*min(om.size(),tau.size()));
    int info=0;
    info = xgesdd(true, tau.size(), om.size(), K.MemPt(), K.fullsize_Nd(), S.MemPt(), U.MemPt(), U.fullsize_Nd(), Vt.MemPt(), Vt.fullsize_Nd(),work.MemPt(), lwork, work.MemPt(), iwork.MemPt());
    if (info!=0){
      clog << "svd is returning "<<info<<" in svdfunc"<<endl;
    }
    double dt = static_cast<double>( clock() - t0 )/CLOCKS_PER_SEC;
    clog<<"svd time="<<dt<<endl;

    for (int l=0; l<lmax; l++)
      if (fabs(S[l])<1e-13){
	lmax=l;
	clog<<"lmax reduced to "<<lmax<<endl;
	break;
      }
    clog<<"last singular value="<<S[lmax-1]<<endl;

    for (int i=0; i<tau.size(); i++)
      for (int l=0; l<Nl; l++)
	U(l,i) *= 1./sqrt(tau.Dh(i));

    // Splines functions, which are singular values of the Kernel.
    vector<spline1D<double> > fu(lmax);
    for (int l=0; l<lmax; l++){
      fu[l].resize(tau.size());
      for (int i=0; i<tau.size(); i++) fu[l][i] = U(l,i);
      int n = tau.size();
      double x1 = 0.5*(tau[1]+tau[0]);
      double df1 = (U(l,1)-U(l,0))/(tau[1]-tau[0]);
      double x2 = 0.5*(tau[2]+tau[1]);
      double df2 = (U(l,2)-U(l,1))/(tau[2]-tau[1]);
      double df0 = df1 + (df2-df1)*(0.0-x1)/(x2-x1);
      x1 = 0.5*(tau[n-1]+tau[n-2]);
      df1 = (U(l,n-1)-U(l,n-2))/(tau[n-1]-tau[n-2]);
      x2 = 0.5*(tau[n-2]+tau[n-3]);
      df2 = (U(l,n-2)-U(l,n-3))/(tau[n-2]-tau[n-3]);
      double dfn = df1 + (df2-df1)*(beta-x1)/(x2-x1);
      fu[l].splineIt(tau, df0, dfn);
    }

    // Calculates overlap between spline interpolations
    function2D<double> overlap(lmax,lmax);
    cmpOverlap(overlap, fu, tau, beta, k_ovlp);
    // Calculates eigensystem of the overlap
    function1D<double> oE(lmax);
    SymEigensystem(overlap, oE);
    function2D<double> sou(overlap);// sou = 1/sqrt(overlap)
    // Computes  1/sqrt(overlap)
    for (int i=0; i<lmax; i++){
      for (int j=0; j<lmax; j++){
	double dsum =0.0;
	for (int k=0; k<lmax; k++) dsum += overlap(k,i)*1/sqrt(oE[k])*overlap(k,j);
	sou(i,j) = dsum;
      }
    }
    // Prepares new spline functions, which are more precisely orthonormal.
    fU.resize(lmax);
    for (int l=0; l<lmax; l++){
      fU[l].resize(tau.size());
      for (int i=0; i<tau.size(); i++){
	double ul=0;
	for (int lp=0; lp<lmax; lp++)  ul += sou(l,lp)*fu[lp][i];
	fU[l][i] = ul;
      }
      int n = tau.size();
      double x1 = 0.5*(tau[1]+tau[0]);
      double df1 = (fU[l][1]-fU[l][0])/(tau[1]-tau[0]);
      double x2 = 0.5*(tau[2]+tau[1]);
      double df2 = (fU[l][2]-fU[l][1])/(tau[2]-tau[1]);
      double df0 = df1 + (df2-df1)*(0.0-x1)/(x2-x1);
      x1 = 0.5*(tau[n-1]+tau[n-2]);
      df1 = (fU[l][n-1]-fU[l][n-2])/(tau[n-1]-tau[n-2]);
      x2 = 0.5*(tau[n-2]+tau[n-3]);
      df2 = (fU[l][n-2]-fU[l][n-3])/(tau[n-2]-tau[n-3]);
      double dfn = df1 + (df2-df1)*(beta-x1)/(x2-x1);
      fU[l].splineIt(tau, df0, dfn);
    }
  }
  //
  void cmp(const string& statistics, double beta_, int lmax_, int Ntau, double x0_, double L_, double Nw, ostream& clog, int k_ovlp_=5){
    k_ovlp=k_ovlp_;
    beta = beta_;
    L = L_;
    x0 = x0_;

    GiveTanMesh(om, x0, L, Nw);
    GiveDoubleExpMesh(tau, beta, Ntau);

    if (statistics=="fermi" || statistics=="both"){
      lmax = lmax_;
      clock_t t0 = clock();
      function2D<double> K(om.size(),tau.size());
      for (int j=0; j<om.size(); j++){
	for (int i=0; i<tau.size(); i++){
	  K(j,i) = fermi_kernel(tau[i],om[j],beta);
	  K(j,i) *= om.Dh(j)*sqrt(tau.Dh(i));
	}
      }
      double dt = static_cast<double>( clock() - t0 )/CLOCKS_PER_SEC;
      clog<<"setup time="<<dt<<endl;

      _cmp_(beta, lmax, fU, S, Vt, K, clog);
    }

    if (statistics=="bose" || statistics=="both"){
      lmax_bose = lmax_;
      clock_t t0 = clock();
      function2D<double> K(om.size(),tau.size());
      for (int j=0; j<om.size(); j++){
	for (int i=0; i<tau.size(); i++){
	  K(j,i) = bose_kernel(tau[i],om[j],beta);
	  K(j,i) *= om.Dh(j)*sqrt(tau.Dh(i));
	}
      }
      double dt = static_cast<double>( clock() - t0 )/CLOCKS_PER_SEC;
      clog<<"setup time="<<dt<<endl;

      _cmp_(beta, lmax_bose, fU_bose, S_bose, Vt_bose, K, clog);
      //cout<<"S_bose="<<S_bose<<endl;
    }
  }

  void Print(const string& filename, const string& statistics="fermi"){
    ofstream pul(filename.c_str());
    pul<<"# "<<lmax<<" "<<tau.size()<<" "<<L<<" "<<x0<<"  "<<(om.size()/2)<<endl;
    pul.precision(16);
    for (int i=0; i<tau.size(); i++){
      pul<<tau[i]<<"  ";
      if (statistics=="fermi")
	for (int l=0; l<lmax; l++) pul<<fU[l][i]<<"  ";
      else
	for (int l=0; l<lmax_bose; l++) pul<<fU_bose[l][i]<<"  ";
      pul<<endl;
    }
  }

  double CheckOverlap(/*double beta*/){
    // Now we check how well is the orthogonality obeyed after reorthogonalization
    function2D<double> overlap(lmax,lmax);
    cmpOverlap(overlap, fU, tau, beta, k_ovlp);
    double dsum=0;
    for (int l1=0; l1<lmax; l1++){
      for (int l2=0; l2<lmax; l2++){
	if (l1!=l2 && overlap(l1,l2)!=0) dsum += fabs(overlap(l1,l2));
	if (l1==l2) dsum += fabs(overlap(l1,l2)-1.0);
      }
    }
    return dsum;
  }

  void cmpOverlap(function2D<double>& overlap, const vector<spline1D<double> >& fu, const mesh1D& tau, double beta, int k=5){
    int lmax = fu.size();

    int M = static_cast<int>(pow(2.0,k))+1;    // Number of points in Romberg integration routine
    vector<double> utu(M); // storage for Romberg

    overlap.resize(lmax,lmax);
    overlap=0.0;
    int nt = tau.size();
    for (int l1=0; l1<lmax; l1++){
      double odd_1 = fu[l1][0]*fu[l1][nt-1];
      for (int l2=0; l2<=l1; l2++){
	double odd_2 = fu[l2][0]*fu[l2][nt-1];
	if (odd_1*odd_2 > 0){// both are even or odd
	  double oij=0; // overlap between u_{l1} and u_{l2} functions
	  for (int i=0; i<tau.size()-1; i++){
	    // here we integrate with a fine mesh of M points using romberg routine
	    double a = tau[i];  // integral only between t_i and t_{i+1} points
	    double b = tau[i+1];
	    double fa = fu[l1][i]   * fu[l2][i];
	    double fb = fu[l1][i+1] * fu[l2][i+1];
	    utu[0]   = fa;
	    utu[M-1] = fb;
	    tint ia=0;
	    for (int j=1; j<M-1; j++){
	      intpar p = tau.InterpLeft( a + (b-a)*j/(M-1.0), ia);
	      utu[j] = fu[l1](p) * fu[l2](p);
	    }
	    oij += romberg(utu, b-a);
	  }
	  overlap(l1,l2) = oij;
	  overlap(l2,l1) = oij;
	}
      }
    }
  }

  template <class container>
  void CreateGfromCoeff(container& gf, const functionb<double>& gl, const string& statistics="fermi"){

    vector<spline1D<double> >* pfU = (statistics=="fermi") ? &fU : &fU_bose;

    gf.resize(tau.size());
    for (int i=0; i<tau.size(); i++) gf[i] = 0.0;

    for (int l=0; l<lmax; l++)
      for (int i=0; i<tau.size(); i++)
	gf[i] += gl[l]*(*pfU)[l][i];
  }

  void CreateSplineFromCoeff(spline1D<double>& gf, const functionb<double>& gl, const string& statistics="fermi"){
    CreateGfromCoeff(gf,gl, statistics);
    {
      double x1 = 0.5*(tau[1]+tau[0]);
      double df1 = (gf[1]-gf[0])/(tau[1]-tau[0]);
      double x2 = 0.5*(tau[2]+tau[1]);
      double df2 = (gf[2]-gf[1])/(tau[2]-tau[1]);
      double df0 = df1 + (df2-df1)*(0.0-x1)/(x2-x1);
      int n = tau.size();
      x1 = 0.5*(tau[n-1]+tau[n-2]);
      df1 = (gf[n-1]-gf[n-2])/(tau[n-1]-tau[n-2]);
      x2 = 0.5*(tau[n-2]+tau[n-3]);
      df2 = (gf[n-2]-gf[n-3])/(tau[n-2]-tau[n-3]);
      double dfn = df1 + (df2-df1)*(tau[n-1]-x1)/(x2-x1);
      gf.splineIt(tau, df0, dfn);
    }
  }

  template <class container>
  void MatsubaraFrequency(container& giom, const spline1D<double>& gf, int nmax, const string& statistics="fermi"){
    giom.resize(nmax);
    //double beta=tau[tau.size()-1];
    double one = (statistics=="fermi") ? 1.0 : 0.0;
    for (int in=0; in<nmax; in++){
      double iom = (2*in+one)*M_PI/beta;
      giom[in] = gf.Fourier_(iom, tau);
    }
  }

  template <class functor>
  void ExpandAnalyticFunction(const functor& fG, function1D<double>& gl, const string& statistics="fermi") const
  {
    const vector<spline1D<double> >* pfU = (statistics=="fermi") ? &fU : &fU_bose;

    gl.resize(lmax);
    int M = static_cast<int>(pow(2.0,k_ovlp))+1;    // Number of points in Romberg integration routine
    vector<double> utu(M); // storage for Romberg
    //double beta = tau[tau.size()-1];
    for (int l=0; l<lmax; l++){
      gl[l]=0;
      for (int i=0; i<tau.size()-1; i++){
	double a = tau[i];  // integral only between t_i and t_{i+1} points
	double b = tau[i+1];
	double fa = (*pfU)[l][i]   * fG(a, beta);
	double fb = (*pfU)[l][i+1] * fG(b, beta);
	utu[0]   = fa;
	utu[M-1] = fb;
	tint ia=0;
	for (int j=1; j<M-1; j++){
	  double t = a + (b-a)*j/(M-1.0);
	  intpar p = tau.InterpLeft( t, ia);
	  utu[j] = (*pfU)[l](p) * fG(t, beta);
	}
	gl[l] += romberg(utu, b-a);
      }
    }
  }

  int l_critical(const functionb<double>& gl, double max_ratio=3., const string& statistics="fermi"){
    int nsa=5;

    int _lmax_ = statistics=="fermi" ? lmax : lmax_bose;

    if (_lmax_<=nsa) return _lmax_;

    function1D<double> gos(_lmax_);

    function1D<double>* pS = (statistics=="fermi") ? &S : &S_bose;

    for (int i=0; i<_lmax_; i++) gos[i] = fabs(gl[i]/(*pS)[i]);

    //cout<<"gos="<<gos<<endl;

    double* p = gos.begin() + nsa+1; // pointer to the fifth entry

    for (int l=nsa+1; l<_lmax_; ++l && ++p){
      double sa = *max_element(p-nsa,p);  // maximum in the past five coefficients
      //cout<<l<<" g[l]="<<gl[l]<<" gos[l]="<<gos[l]<<" gmax="<<sa<<" g/gmax="<<fabs(gos[l]/sa)<<endl;
      if (fabs(gos[l]/sa)>max_ratio){
	//cout<<"lcritical should be "<<l<<endl;
	return l;
      }
    }
    return _lmax_;
  }

  void GiveRealAxis(function2D<double>& Aw, const functionb<double>& gl, double max_ratio=3., const string& statistics="fermi")
  {
    int lcritical = l_critical(gl,max_ratio,statistics);
    Aw.resize(lcritical,om.size());

    function1D<double> gos(gl.size());

    function1D<double>* pS = (statistics=="fermi") ? &S : &S_bose;

    for (int i=0; i<gl.size(); i++) gos[i] = gl[i]/(*pS)[i];

    function2D<double>* pVt = (statistics=="fermi") ? &Vt : &Vt_bose;

    for(int i=0; i<om.size(); i++)
      Aw(0,i) = -gos[0]*(*pVt)(i,0);

    for (int l=1; l<lcritical; l++){
      for (int i=0; i<om.size(); i++)
	Aw(l,i) = Aw(l-1,i) - gos[l]*(*pVt)(i,l);
    }

    if (statistics=="bose"){
      for (int l=0; l<lcritical; l++)
	for (int iw=0; iw<om.size(); iw++)
	  Aw(l,iw) *= M_PI*om[iw];
    }
  }

  void SetUpFastInterp()
  {
    ff2.resize(tau.size(),lmax,2);
    dxi.resize(tau.size());
    for (int i=0; i<tau.size(); i++) dxi[i] = fU[0].dxi[i];
    for (int l=0; l<lmax; l++){
      for (int i=0; i<tau.size(); i++){
	ff2(i,l,0) = fU[l][i];
	ff2(i,l,1) = fU[l].f2[i];
      }
    }
  }
  void FastInterp(functionb<double>& res, const intpar& ip, double cst) const
  {
    int i= ip.i;
    double p = ip.p, q=1-ip.p;
    double dx26 = dxi[i]*dxi[i]/6.;
    double dq = dx26*q*(q*q-1);
    double dp = dx26*p*(p*p-1);
    q  *= cst;
    dq *= cst;
    p  *= cst;
    dp *= cst;
    double* __restrict__ _res = res.MemPt();
    const double* __restrict__ _ff2 = ff2[i];
    // this is fast equivalent of
    // res[l] += q * fU[l].f[i] + dq * fU[l].f2[i];
    for (int l=0; l<lmax; l++) _res[l] += q * _ff2[2*l] + dq * _ff2[2*l+1];
    _ff2 += 2*lmax;
    // this is fast equivalent of
    // res[l] += p * fu[l].f[i+1] + dp * fU[l].f2[i+1];
    for (int l=0; l<lmax; l++) _res[l] += p * _ff2[2*l] + dp * _ff2[2*l+1];
  }
  void FastInterp_(functionb<double>& res, const intpar& ip, double cst) const
  {// This is the same as FastInterp, except res is zeroth first.
    int i= ip.i;
    double p = ip.p, q=1-ip.p;
    double dx26 = dxi[i]*dxi[i]/6.;
    double dq = dx26*q*(q*q-1);
    double dp = dx26*p*(p*p-1);
    q  *= cst;
    dq *= cst;
    p  *= cst;
    dp *= cst;
    double* __restrict__ _res = res.MemPt();
    const double* __restrict__ _ff2 = ff2[i];
    for (int l=0; l<lmax; l++) _res[l] = q * _ff2[2*l] + dq * _ff2[2*l+1];
    _ff2 += 2*lmax;
    for (int l=0; l<lmax; l++) _res[l] += p * _ff2[2*l] + dp * _ff2[2*l+1];
  }

#ifdef _MPI
  void ComputeClCoefficients(function2D<double>& Cl, int my_rank, int mpi_size, int Master, int nw=0, int ntail=0, int cutoff_iom=0){
#else
  void ComputeClCoefficients(function2D<double>& Cl, int nw=0, int ntail=0, int cutoff_iom=0){
#endif
    bool BRISI = false;

    int Nt = tau.size();
    //double L = om[om.size()-1];
    //double beta = tau[tau.size()-1];
    if (nw==0)    nw = L*beta/3;      // number of Matsubara points treated exactly
    if (ntail==0) ntail=150;          // number of points used for the tail
    if (cutoff_iom==0) cutoff_iom=100;// how far the tail will extend. We will go to Matsubara index nw*cutoff_iom

    function1D<int> ind_om;
    create_log_mesh(ind_om, nw*cutoff_iom, nw, ntail); // log mesh with nw exact points in [0,...nw-1], and ntail points distributed between [nw,nw*cutoff_iom]
    mesh1D iom(ind_om.size());
    for (int in=0; in<ind_om.size(); in++)  iom[in] = ind_om[in];
    iom.SetUp(0);

    if (BRISI){
      cout.precision(11);
      cout<<"# Starting single loop for u(iw) "<<endl;
    }
    // This creates u_l(iom) from u_l(tau)
    function2D<complex<double> > u_om(lmax,iom.size());
    for (int lf=0; lf<lmax; lf++){
      for (int in=0; in<iom.size(); in++){
	double om = (2*iom[in]+1.)*M_PI/beta;
	u_om(lf,in) = fU[lf].Fourier(om, tau);
      }
    }
    if (BRISI) cout<<"# Starting double loop for w(iw) "<<endl;
    // This creates w_l(iom) from u_l(tau).
    // w_{lb,lf}(iom) = 1/beta \sum_{iOm} u^b_{lb}(iOm) * u^f_{lf}(iom+iOm)
    // which is equivalent to
    // w_{lb,lf}(iom) = Integrate[ u^b_{lb}(beta-t) * u^f_{lf}(t) e^{i*om*t}, {t,0,beta}]
    int Nl2 = lmax_bose*lmax;
#ifdef _MPI
    int ipr_proc = (Nl2 % mpi_size==0) ? Nl2/mpi_size : Nl2/mpi_size+1;
    int iistart = ipr_proc*my_rank;
    int iiend   = ipr_proc*(my_rank+1);
    if (iistart>Nl2) iistart=Nl2;
    if (iiend  >Nl2) iiend=Nl2;
    int _Nl2_ = max(Nl2, mpi_size*ipr_proc);
    if (BRISI) cout<<"istart="<<iistart<<" iend="<<iiend<<" pr_proc="<<ipr_proc<<" pr_proc*mpi_size="<<ipr_proc*mpi_size<<" Nl2="<<Nl2<<endl;
#else
    int iistart = 0;
    int iiend = Nl2;
    int ipr_proc = Nl2;
    int _Nl2_ = Nl2;
#endif
    function2D<complex<double> > w_iom(_Nl2_, iom.size());
    w_iom = 0.0;
    for (int ii=iistart; ii<iiend; ii++){
      // ii = lb * lmax + lf
      int lb = ii / lmax;
      int lf = ii % lmax;
      spline1D<double> wt(Nt);
      for (int it=0; it<tau.size(); it++)
	wt[it] = fU_bose[lb][Nt-1-it]*fU[lf][it]; // wt[t] = u^b_{lb}(beta-t) * u^f_{lf}(t)

      int n = tau.size();
      double x1 = 0.5*(tau[1]+tau[0]);
      double df1 = (wt[1]-wt[0])/(tau[1]-tau[0]);
      double x2 = 0.5*(tau[2]+tau[1]);
      double df2 = (wt[2]-wt[1])/(tau[2]-tau[1]);
      double df0 = df1 + (df2-df1)*(0.0-x1)/(x2-x1);
      x1 = 0.5*(tau[n-1]+tau[n-2]);
      df1 = (wt[n-1]-wt[n-2])/(tau[n-1]-tau[n-2]);
      x2 = 0.5*(tau[n-2]+tau[n-3]);
      df2 = (wt[n-2]-wt[n-3])/(tau[n-2]-tau[n-3]);
      double dfn = df1 + (df2-df1)*(beta-x1)/(x2-x1);
      wt.splineIt(tau, df0, dfn);
      // w(iom) = Fourier[ wt[t] ]
      for (int in=0; in<iom.size(); in++){
	double om = (2*iom[in]+1.)*M_PI/beta;
	//w_om(lb,lf,in) = wt.Fourier(om, tau);
	w_iom(ii,in) = wt.Fourier(om, tau);
      }
    }
#ifdef _MPI
    if (mpi_size>1)
      MPI_Allgather(MPI_IN_PLACE, ipr_proc*iom.size()*2, MPI_DOUBLE, w_iom.MemPt(), ipr_proc*iom.size()*2, MPI_DOUBLE, MPI_COMM_WORLD);
#endif
    if (BRISI) cout<<"# Starting last part"<<endl;
    int Nl = (lmax*lmax*lmax_bose);
    int offset=2;
    spline1D<double> ftmp(ntail+offset);
    mesh1D wom(ntail+offset);
    int ifirst = iom.size()-ntail-offset;
    for (int in=ifirst; in<iom.size(); in++) wom[in-ifirst] = iom[in];
    wom.SetUp(0);

    deque<pair<int,int> > icase;
    for (int i=0; i<Nl; i++){
      for (int j=i; j<Nl; j++){
	vector<int> li = get_lll(i);
	vector<int> lj = get_lll(j);
	if ( (li[0] + li[1] + li[2] + lj[0] + lj[1] + lj[2])%2 ) continue; // this vanishes due to symmetry
	icase.push_back( make_pair(i,j) );
      }
    }

#ifdef _MPI
    int pr_proc = (icase.size() % mpi_size==0) ? icase.size()/mpi_size : icase.size()/mpi_size+1;
    int istart = pr_proc*my_rank;
    int iend   = pr_proc*(my_rank+1);
    if (istart>icase.size()) istart=icase.size();
    if (iend  >icase.size()) iend=icase.size();
    if (BRISI) cout<<"istart="<<istart<<" iend="<<iend<<" pr_proc="<<pr_proc<<" pr_proc*mpi_size="<<pr_proc*mpi_size<<" icase.size="<<icase.size()<<endl;
#else
    int istart = 0;
    int iend = icase.size();
    int pr_proc = icase.size();
#endif
    function1D<double> Cl0(pr_proc);
    Cl0=0;
    // This loop is parallelized
    for (int ii=istart; ii<iend; ii++){
      int i = icase[ii].first;
      int j = icase[ii].second;
      vector<int> li = get_lll(i);
      vector<int> lj = get_lll(j);
      if ( (li[0] + li[1] + li[2] + lj[0] + lj[1] + lj[2])%2 ) continue; // this vanishes due to symmetry

      // lj[2],lj[1],lj[0]  :  bose,fermi,fermi
      //                       lb * lmax + lf
      //complex<double> * w_1 = w_iom[ lj[2]*lmax + li[1] ].MemPt();
      //complex<double> * w_2 = w_iom[ li[2]*lmax + lj[1] ].MemPt();
      //complex<double> * u_1 = u_om[ li[0] ].MemPt();
      //complex<double> * u_2 = u_om[ lj[0] ].MemPt();
      complex<double> * w_1 = w_iom[ lj[2]*lmax + li[0] ].MemPt();
      complex<double> * w_2 = w_iom[ li[2]*lmax + lj[0] ].MemPt();
      complex<double> * u_1 = u_om[ li[1] ].MemPt();
      complex<double> * u_2 = u_om[ lj[1] ].MemPt();

      double wsum = 0.0;
      for (int in=0; in<iom.size()-ntail; in++)
	wsum += (w_1[in] * conj(w_2[in]) * u_1[in] * conj(u_2[in])).real();
      // fill int the 1D-interpolation for the tail
      for (int in=ifirst; in<iom.size(); in++)
	ftmp[in-ifirst] = (w_1[in] * conj(w_2[in]) * u_1[in] * conj(u_2[in])).real();
      // intepolate the tail
      int n=ftmp.size();
      ftmp.splineIt(wom, (ftmp[1]-ftmp[0])/(wom[1]-wom[0]), (ftmp[n-1]-ftmp[n-2])/(wom[n-1]-wom[n-2]) );
      // Now anlytically evaluate the sum over all points, but loop only over a few points in the tail.
      // The sum of the spline in the interval [n1,n2] can be analytically evaluated:
      //  1/2*(f[n2]+f[n1])*(n2-n1)  - 1/24*(n2-n1)*((n2-n1)^2-1) * (f2[n2]+f2[n1])
      //  where f2[n1], f2[n2] is the second derivative at n1 and n2 points.
      //  Note that the first and the last point come in with the weight 1/2, hence we need to correct for that.
      wsum += 0.5*ftmp[iom.size()-ntail-ifirst] + 0.5*ftmp[iom.size()-1-ifirst];
      for (int in=iom.size()-ntail; in<iom.size()-1; in++){
	double dh = iom[in+1]-iom[in];
	wsum += 0.5*(ftmp[in-ifirst]+ftmp[in+1-ifirst])*dh - dh/24.0*(dh*dh - 1.0)*(ftmp.f2[in+1-ifirst]+ftmp.f2[in-ifirst]);
      }
      /* //The above few lines are equivalent to
	 int iom_last = iom[iom.size()-1]; tint ia=0;
	 for (int iw=iom.size()-ntail; iw<iom_last; iw++){
	 double ww = ftmp( wom.InterpLeft(iw, ia) );
	 wsum += ww;
	 }
      */
      /*
      Cl(i,j) = 2*(wsum)/beta;
      Cl(j,i) = Cl(i,j);
      */
      Cl0[ii-istart] = 2*(wsum)/beta;
      //cout<<setw(4)<<i<<" "<<setw(4)<<j<<"     "<<setw(3)<<li[2]<<" "<<setw(3)<<li[1]<<" "<<setw(3)<<li[0]<<"; "<<setw(3)<<lj[2]<<" "<<setw(3)<<lj[1]<<" "<<setw(3)<<lj[0]<<"    "<<setw(10)<<left<<Cl(i,j)<<right<<endl;
    }
#ifdef _MPI
    if (mpi_size>1){
      if (BRISI){
	cout<<"MPI_Gather"<<endl;
	cout.flush();
      }
      function1D<double> cCl0;
      cCl0.resize(pr_proc*mpi_size);
      MPI_Gather(Cl0.MemPt(), pr_proc, MPI_DOUBLE, cCl0.MemPt(), pr_proc, MPI_DOUBLE, Master, MPI_COMM_WORLD);
      if (my_rank==Master){
	Cl0.resize(icase.size());
	for (int i=0; i<icase.size(); i++) Cl0[i] = cCl0[i];
      }
    }
    if (my_rank == Master){
#endif
      Cl.resize(Nl,Nl);
      Cl=0.0;
      for (int ii=0; ii<icase.size(); ii++){
	int i = icase[ii].first;
	int j = icase[ii].second;
	Cl(i,j) = Cl0[ii];
	Cl(j,i) = Cl0[ii];
      }
#ifdef _MPI
    }
#endif
  }
};

/*
double fGs(double tau, double beta){
  // On real axis, this corresponds to
  // A(w) = 1/2*(delta(w-x0)+delta(w+x0))
  // with x0=1.
  return -exp(-beta/2.)*cosh(beta/2.-tau);
}

double chis(double tau, double beta){
// On real axis this corresponds to
// chi''(x) = pi*[delta(w-x0)-delta(w+x0)]
// with x0=1/beta
  const double bx2 = 0.5;
  return cosh(bx2*(1-2.*tau/beta))/sinh(bx2);
}

class fGspl{
public:
  spline1D<double>& fg;
  mesh1D t;
  fGspl(spline1D<double>& fg_, mesh1D& t_) : fg(fg_), t(t_) {}
  double operator()(double x, double beta) const{
    return fg(t.Interp(x));
  }
};

int main(){
  double x0=0.005;
  double L=10.;
  int Nw = 500;
  //double beta=9.35887;
  double beta=100;
  int lmax = 40;
  int k_ovlp=5;
  int Ntau = static_cast<int>(5*beta+100);
  bool readGFile=false;//true;

  spline1D<double> ginp;
  mesh1D tc;
  if (readGFile){
    ifstream  inp("../Gtau.dat_4");
    deque<double> ct, gt;
    while (inp){
      double t, g;
      inp >> t >> g;
      //cout<<t<<" "<<g<<endl;
      if (!inp) break;
      ct.push_back(t);
      gt.push_back(g);
      inp.ignore(numeric_limits<streamsize>::max(), '\n');
    }

    beta = ct.back()+ct[0];
    cout<<"beta="<<beta<<endl;
    ginp.resize(gt.size());
    tc.resize(ct.size());
    {
      for (int i=0; i<ct.size(); i++){
	tc[i] = ct[i];
	ginp[i] = gt[i];
      }
      tc.SetUp(0);
      double df0 = (ginp[1]-ginp[0])/(tc[1]-tc[0]);
      int n = tc.size();
      double dfn = (ginp[n-1]-ginp[n-2])/(tc[n-1]-tc[n-2]);
      cout<<"df0="<<df0<<" dfn="<<dfn<<endl;
      ginp.splineIt(tc, df0, dfn);
    }
  }
  fGspl Fg(ginp, tc);

  clog<<"beta="<<beta<<" Ntau="<<Ntau<<endl;

  SVDFunc svdf;

  svdf.cmp("both", beta, lmax, Ntau, x0, L, Nw, clog);

  // Print SVD functions
  svdf.Print("uls.dat",beta);
  svdf.Print("ulb.dat",beta, "bose");

  // Now we check how well is the orthogonality obeyed after reorthogonalization
  clog<<"Final overlap is smaller than "<<svdf.CheckOverlap(beta)<<endl;

  ofstream fo("cc.dat");
  mesh1D& tau = svdf.tau;
  function1D<double> Gts(tau.size());
  for (int i=0; i<tau.size(); i++){
    if (readGFile) Gts[i] = Fg(tau[i],beta);
    else Gts[i] = fGs(tau[i], beta);
    fo<<tau[i]<<" "<<Gts[i]<<" "<<svdf[0][i]<<" "<<svdf[2][i]<<endl;
  }
  fo.close();

  function1D<double> gl(lmax);
  if (readGFile) svdf.ExpandAnalyticFunction(Fg, gl);
  else svdf.ExpandAnalyticFunction(fGs, gl);

  function1D<double> gl2(lmax);
  svdf.SetUpFastInterp();
  {
    k_ovlp=5;
    gl2.resize(lmax);
    gl2 = 0.0;
    int M = pow(2,k_ovlp)+1;    // Number of points in Romberg integration routine
    function2D<double> utu_T(M,lmax); // storage for Romberg
    double beta = tau[tau.size()-1];
    for (int i=0; i<tau.size()-1; i++){
      utu_T=0;
      double a = tau[i];  // integral only between t_i and t_{i+1} points
      double b = tau[i+1];
      tint ia=0;
      for (int j=0; j<M; j++){
	double t = a + (b-a)*j/(M-1.0);
	intpar p = tau.InterpLeft( t, ia);
	double cst;
	if (readGFile) cst = Fg(t,beta);
	else cst = fGs(t, beta);
	///// Slow equivalent ////
	//for (int l=0; l<svdf.lmax; l++) utu_T(j,l) += cst * svdf.fU[l](p);
	svdf.FastInterp(utu_T[j], p, cst );
      }
      function1D<double> utu(M);
      for (int l=0; l<lmax; l++){
	for (int j=0; j<M; j++) utu[j] = utu_T(j,l);
	gl2[l] += romberg(utu, b-a);
      }
    }
  }
  cout<<"Should be equal:"<<endl;
  for (int l=0; l<lmax; l++)
    cout<<l<<" "<<gl[l]<<" "<<gl2[l]<<" "<<gl[l]-gl2[l]<<endl;

  spline1D<double> gf;
  svdf.CreateSplineFromCoeff(gf, gl);
  int nmax=5000;
  function1D<complex<double> > giom;
  svdf.MatsubaraFrequency(giom, gf, nmax);

  ofstream fig("giom.dat");
  fig.precision(16);
  for (int in=0; in<nmax; in++){
    double iom = (2*in+1)*M_PI/beta;
    double gi_exact = -iom/(iom*iom+1.);
    fig<<iom<<" "<<giom[in]<<" "<<gi_exact<<endl;
  }

  ofstream fog("gl.dat");
  fog.precision(16);
  for (int l=0; l<svdf.lmax; l++)
    if (fabs(gl[l])>1e-10)
      fog<<l<<" "<<gl[l]<<endl;
  fog.close();

  ofstream foG("Gapp.dat");
  foG.precision(16);
  for (int i=0; i<tau.size(); i++){
    double dval=0;
    for (int l=0; l<svdf.lmax; l++) dval += svdf[l][i]*gl[l];
    foG<<tau[i]<<" "<<dval<<" "<<Gts[i]<<" "<<fabs(dval-Gts[i])<<endl;
  }
  foG.close();

  //int lc = svdf.l_critical(gl);
  //cout<<lc<<endl;

  function2D<double> Aw;
  ofstream fAw("Aw.out");
  svdf.GiveRealAxis(Aw, gl, 2.0);
  for (int i=0; i<svdf.om.size(); i++){
    fAw << svdf.om[i]<<" ";
    for (int l=0; l<Aw.size_N(); l++){
      fAw << Aw(l,i) <<" ";
    }
    fAw << endl;
  }

  svdf.ExpandAnalyticFunction(chis, gl, "bose");
  svdf.CreateSplineFromCoeff(gf, gl, "bose");
  ofstream foc("Chid.dat");
  foc.precision(16);
  for (int i=0; i<svdf.tau.size(); i++){
    double val = chis(svdf.tau[i],beta);
    foc<<svdf.tau[i]<<" "<<gf[i]<<" "<<val<<" "<<fabs(val-gf[i])<<endl;
  }
  foc.close();

  svdf.MatsubaraFrequency(giom, gf, nmax, "bose");
  ofstream sig("siom.dat");
  sig.precision(16);
  for (int in=0; in<nmax; in++){
    double iom = (2*in+0.0)*M_PI/beta;
    double x0 = 1./beta;
    double gi_exact = 2*x0/(x0*x0+iom*iom);
    sig<<iom<<" "<<giom[in]<<" "<<gi_exact<<" "<<giom[in]-gi_exact<<endl;
  }

  cout<<"bose coefficients are"<<gl<<endl;

  svdf.GiveRealAxis(Aw, gl, 10.0, "bose");
  ofstream sAw("sw.out");
  for (int i=0; i<svdf.om.size(); i++){
    sAw << svdf.om[i]<<" ";
    for (int l=0; l<Aw.size_N(); l++) sAw << Aw(l,i) <<" ";
    sAw << endl;
  }

  svd.Print("usb.dat",common::beta,"bose");
  for (int l=0; l<svdf.lmax_bose; l++) svdf.MatsubaraFrequency(bUw[l], svdf.fU_bose[l], N_W);

  return 0;
}
*/


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "eigenmap.h"
#include <cblas.h>
#include <lapacke.h>
#include <matio.h>
#include <memory.h>
#include <time.h>
/*
 * lanczos computes the smallest n_eigs eigenvalues for L and the
 * corresponding eigenvectors using the Lanczos algorithm.
 *
 * F: an array (n_patch by n_eigs) to store the eigenvectors
 * Es: an array (1 by n_eigs) to store the eigenvalues
 * L: an array (n_patch by n_patch) representing the Laplacian matrix
 * n_patch: the dimension of L
 */
/* ---- corresponding Matlab code ----
 * [F, Es] = lanczos(L, n_eigs)
 */
static double norm2(double *v, int length);
static void divide_copy(double *dest, const double *src, int length,
                        const double divisor);

void lanczos(double *F, double *Es, double *L, int n_eigs, int n_patch,
             int LANCZOS_ITR)
{
    double *b;
    double b_norm;

    double *z;
    double *alpha, *beta;
    double *q;
    int i;

    double *eigvec; // eigenvectors

    // generate random b with norm 1.
    srand((unsigned int)time(NULL));
    b = (double *)malloc(n_patch * sizeof(double));
    for (i = 0; i < n_patch; i++)
        b[i] = rand();
    b_norm = norm2(b, n_patch);
    for (i = 0; i < n_patch; i++)
        b[i] /= b_norm;

    alpha = (double *)malloc( (LANCZOS_ITR + 1) * sizeof(double) );
    beta = (double *)malloc( (LANCZOS_ITR + 1) * sizeof(double) );
    beta[0] = 0.0; // beta_0 <- 0
    z = (double *)malloc( n_patch * sizeof(double));
    q = (double *)malloc( n_patch * (LANCZOS_ITR + 2) * sizeof(double) );
    memset(&q[0], 0, n_patch * sizeof(double)); // q_0 <- 0
    memcpy(&q[n_patch], b, n_patch * sizeof(double)); // q_1 <- b

    for (i = 1; i <= LANCZOS_ITR; i++) {
        // z = L * Q(:, i)
        cblas_dsymv(CblasColMajor, CblasLower, n_patch, 1.0, L,
                    n_patch, &q[i * n_patch], 1, 0.0, z, 1);
        // alpha(i) = Q(:, i)' * z;
        alpha[i] = cblas_ddot(n_patch, &q[i * n_patch], 1, z, 1);
        // z = z - alpha(i) * Q(:, i)
        cblas_daxpy(n_patch, -alpha[i], &q[i * n_patch], 1, z, 1);
        // z = z - beta(i - 1) * Q(:, i - 1);
        cblas_daxpy(n_patch, -beta[i - 1], &q[(i - 1) * n_patch], 1, z, 1);
        /* re-orthogonalize twice */
        // b = Q(:, 1:i-1)' * z
        cblas_dgemv(CblasColMajor, CblasTrans, n_patch, i - 1, 1.0,
                    &q[n_patch], n_patch, z, 1, 0.0, b, 1);
        // z = Q(:, 1:i-1) * b + (-1) * z
        cblas_dgemv(CblasColMajor, CblasNoTrans, n_patch, i - 1, 1.0,
                    &q[n_patch], n_patch, b, 1, -1.0, z, 1);
        // b = Q(:, 1:i-1)' * z
        cblas_dgemv(CblasColMajor, CblasTrans, n_patch, i - 1, 1.0,
                    &q[n_patch], n_patch, z, 1, 0.0, b, 1);
        // z = Q(:, 1:i-1) * b + (-1) * z
        cblas_dgemv(CblasColMajor, CblasNoTrans, n_patch, i - 1, 1.0,
                    &q[n_patch], n_patch, b, 1, -1.0, z, 1);

        // beta(i) = norm(z, 2);
        beta[i] = cblas_dnrm2(n_patch, z, 1);
        // Q(:, i + 1) = z / beta(i);
        divide_copy(&q[(i + 1) * n_patch], z, n_patch, beta[i]);
    }

    // compute approximate eigensystem
    eigvec = (double *)malloc(LANCZOS_ITR * LANCZOS_ITR * sizeof(double));
    LAPACKE_dstedc(LAPACK_COL_MAJOR, 'I', LANCZOS_ITR, &alpha[1], &beta[1],
                   eigvec, LANCZOS_ITR);
    // copy specified number of eigenvalues
    memcpy(Es, &alpha[1], n_eigs * sizeof(double));

    // V = Q(:, 1:k) * U
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, n_patch,
                LANCZOS_ITR, LANCZOS_ITR, 1.0, &q[n_patch], n_patch, eigvec,
                LANCZOS_ITR, 0.0, L, n_patch);
    // copy the corresponding eigenvectors
    memcpy(F, L, n_patch * n_eigs * sizeof(double));

    free(b);
    free(z);
    free(alpha);
    free(beta);
    free(q);
    free(eigvec);
}

static double norm2(double *v, int length)
{
    int i;
    double sum = 0.0;

    for (i = 0; i < length; i++)
        sum += v[i] * v[i];

    return sqrt(sum);
}

static void divide_copy(double *dest, const double *src, int length,
                        const double divisor)
{
    double factor = 1.0 / divisor;
    int i;
    for (i = 0; i < length; i++)
        dest[i] = src[i] * factor;
}


/* rng/mrg.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 James Theiler, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_rng.h>

/* This is a fifth-order multiple recursive generator. The sequence is,

   x_n = (a_1 x_{n-1} + a_5 x_{n-5}) mod m

   with a_1 = 107374182, a_2 = a_3 = a_4 = 0, a_5 = 104480 and m = 2^31-1.

   We initialize the generator with x_n = s_n MOD m for n = 1..5,
   where s_n = (69069 * s_{n-1}) mod 2^32, and s_0 = s is the
   user-supplied seed.

   NOTE: According to the paper the seeds must lie in the range [0,
   2^31 - 2] with at least one non-zero value -- our seeding procedure
   satisfies these constraints.

   We then use 6 iterations of the generator to "warm up" the internal
   state.

   With this initialization procedure the theoretical value of
   z_{10006} is 2064828650 for s = 1. The subscript 10006 means (1)
   seed the generator with s = 1, (2) do the 6 warm-up iterations
   that are part of the seeding process, (3) then do 10000 actual
   iterations.

   The period of this generator is about 2^155.

   From: P. L'Ecuyer, F. Blouin, and R. Coutre, "A search for good
   multiple recursive random number generators", ACM Transactions on
   Modeling and Computer Simulation 3, 87-98 (1993). */

static inline unsigned long int mrg_get (void *vstate);
static double mrg_get_double (void *vstate);
static void mrg_set (void *state, unsigned long int s);

static const long int m = 2147483647;
static const long int a1 = 107374182, q1 = 20, r1 = 7;
static const long int a5 = 104480, q5 = 20554, r5 = 1727;

typedef struct
  {
    long int x1, x2, x3, x4, x5;
  }
mrg_state_t;

static inline unsigned long int
mrg_get (void *vstate)
{
  mrg_state_t *state = (mrg_state_t *) vstate;

  long int p1, h1, p5, h5;

  h5 = state->x5 / q5;
  p5 = a5 * (state->x5 - h5 * q5) - h5 * r5;
  if (p5 > 0)
    p5 -= m;

  h1 = state->x1 / q1;
  p1 = a1 * (state->x1 - h1 * q1) - h1 * r1;
  if (p1 < 0)
    p1 += m;

  state->x5 = state->x4;
  state->x4 = state->x3;
  state->x3 = state->x2;
  state->x2 = state->x1;

  state->x1 = p1 + p5;

  if (state->x1 < 0)
    state->x1 += m;

  return state->x1;
}

static double
mrg_get_double (void *vstate)
{
  return mrg_get (vstate) / 2147483647.0 ;
}

static void
mrg_set (void *vstate, unsigned long int s)
{
  /* An entirely adhoc way of seeding! This does **not** come from
     L'Ecuyer et al */

  mrg_state_t *state = (mrg_state_t *) vstate;

  if (s == 0)
    s = 1;	/* default seed is 1 */

#define LCG(n) ((69069 * n) & 0xffffffffUL)
  s = LCG (s);
  state->x1 = s % m;
  s = LCG (s);
  state->x2 = s % m;
  s = LCG (s);
  state->x3 = s % m;
  s = LCG (s);
  state->x4 = s % m;
  s = LCG (s);
  state->x5 = s % m;

  /* "warm it up" with at least 5 calls to go through
     all the x values */

  mrg_get (state);
  mrg_get (state);
  mrg_get (state);
  mrg_get (state);
  mrg_get (state);
  mrg_get (state);

  return;
}

static const gsl_rng_type mrg_type =
{"mrg",				/* name */
 2147483646,			/* RAND_MAX */
 0,                             /* RAND_MIN */
 sizeof (mrg_state_t),
 &mrg_set,
 &mrg_get,
 &mrg_get_double};

const gsl_rng_type *gsl_rng_mrg = &mrg_type;


#include <string.h>
#include <sys/types.h>
#include <stdint.h>
#include <math.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_wavelet.h>
#include <fftw3.h>
#include "common.h"
#include "utils.h"
#include "filters.h"

filters_t *filters_init(ANALYSIS_WAVEFORM_BASE_TYPE *inWav, size_t n)
{
    filters_t *fHdl;
    fHdl = (filters_t*)malloc(sizeof(filters_t));

    fHdl->wavLen = n;
    fHdl->respLen = 0;
    fHdl->malloced = 0;
    fHdl->fftUsed = 0;
    fHdl->fftwNThreads = FFTW_NTHREADS_DEFAULT;
    fHdl->fftwFlags = FFTW_FLAGS_DEFAULT;

    if(inWav == NULL) {
        fHdl->inWav = (ANALYSIS_WAVEFORM_BASE_TYPE*)
            calloc(fHdl->wavLen, sizeof(ANALYSIS_WAVEFORM_BASE_TYPE));
        fHdl->malloced = 1;
    } else {
        fHdl->inWav = inWav;
    }

    fHdl->outWav = (ANALYSIS_WAVEFORM_BASE_TYPE*)
        calloc(fHdl->wavLen, sizeof(ANALYSIS_WAVEFORM_BASE_TYPE));
    fHdl->waveletWav = (WAVELET_BASE_TYPE*)
        calloc(fHdl->wavLen, sizeof(WAVELET_BASE_TYPE));

    fHdl->gslDWT = gsl_wavelet_alloc(gsl_wavelet_daubechies_centered, 10);
    fHdl->gslDWTWork = gsl_wavelet_workspace_alloc(fHdl->wavLen);

    return fHdl;
}

/* for convolution */
filters_t *filters_init_for_convolution(ANALYSIS_WAVEFORM_BASE_TYPE *inWav, size_t n, size_t np)
{
    filters_t *fHdl;

    if((np > 0) && (np % 2 == 0)) {
        error_printf("%s(): np = %zd is not odd!\n", __FUNCTION__, np);
        return NULL;
    }

    fHdl = filters_init(inWav, n);
    fHdl->respLen = np;
    fHdl->fftUsed = 1;
    fHdl->respWav = (ANALYSIS_WAVEFORM_BASE_TYPE*)
        calloc(fHdl->respLen, sizeof(ANALYSIS_WAVEFORM_BASE_TYPE));
    if(np > 0) {
        fHdl->fftLen = (fHdl->wavLen + fHdl->respLen+1); /* zero padding */
        if(fHdl->fftLen % 2) fHdl->fftLen++; /* ensure fHdl->fftLen is even */
    } else {
        fHdl->fftLen = fHdl->wavLen; /* for spectrum calculation */
    }

    if(fHdl->fftwNThreads > 0) {
        if(FFTW(init_threads)() == 0) {
            error_printf("fftw_init_threads error!\n");
        }
        FFTW(plan_with_nthreads)(fHdl->fftwNThreads);
    }

    fHdl->fftwWork = (FFT_BASE_TYPE*) FFTW(malloc)(sizeof(FFT_BASE_TYPE) * fHdl->fftLen);
    fHdl->fftwWork1 = (FFT_BASE_TYPE*) FFTW(malloc)(sizeof(FFT_BASE_TYPE) * fHdl->fftLen);
    fHdl->fftwWin = (FFT_BASE_TYPE*) FFTW(malloc)(sizeof(FFT_BASE_TYPE) * fHdl->fftLen);
    filters_hanning_window(fHdl); /* Hanning window as the default */
    fHdl->dt = 1.0;

    fHdl->fftwPlan = FFTW(plan_r2r_1d)(fHdl->fftLen, fHdl->fftwWork, fHdl->fftwWork,
                                       FFTW_R2HC, fHdl->fftwFlags);
    fHdl->fftwPlan1 = FFTW(plan_r2r_1d)(fHdl->fftLen, fHdl->fftwWork1, fHdl->fftwWork1,
                                        FFTW_R2HC, fHdl->fftwFlags);
    fHdl->fftwPlan2 = FFTW(plan_r2r_1d)(fHdl->fftLen, fHdl->fftwWork, fHdl->fftwWork,
                                        FFTW_HC2R, fHdl->fftwFlags);

    return fHdl;
}

int filters_close(filters_t *fHdl)
{
    if(fHdl->malloced) {
        if(fHdl->inWav)
            free(fHdl->inWav);
    }
    if(fHdl->outWav)
        free(fHdl->outWav);
    if(fHdl->fftUsed) {
        if(fHdl->respWav)
            free(fHdl->respWav);
        FFTW(destroy_plan)(fHdl->fftwPlan);
        FFTW(destroy_plan)(fHdl->fftwPlan1);
        FFTW(destroy_plan)(fHdl->fftwPlan2);
        FFTW(free)(fHdl->fftwWork);
        FFTW(free)(fHdl->fftwWork1);
        FFTW(free)(fHdl->fftwWin);
        if(fHdl->fftwNThreads > 0) {
            FFTW(cleanup_threads)();
            FFTW(cleanup)();
        }
    }
    gsl_wavelet_free(fHdl->gslDWT);
    gsl_wavelet_workspace_free(fHdl->gslDWTWork);
    if(fHdl->waveletWav)
        free(fHdl->waveletWav);
    return 0;
}

/* Filters should directly write into fHdl->respWav the real space
 * response waveform in wrapped around order */

int filters_SavitzkyGolay(filters_t *fHdl, int m, int ld)
/* m: order of polynomial, np: number of points, ld: degree of derivative*/
{
    int np;
    ANALYSIS_WAVEFORM_BASE_TYPE *c;
    int ipj, imj, mm, j, k, nl, nr;
    double fac, sum;
    gsl_permutation * p;
    gsl_vector *b;
    gsl_matrix *a;

    np = fHdl->respLen;
    if(np<1 || np<m-1 || np%2==0 || ld>m || np!=fHdl->respLen) {
        error_printf("%s(): improper arguments, returning...\n", __FUNCTION__);
        return 1;
    }

    c = calloc(np, sizeof(ANALYSIS_WAVEFORM_BASE_TYPE));

    p = gsl_permutation_alloc (m+1);
    b = gsl_vector_alloc(m+1);
    a = gsl_matrix_alloc(m+1, m+1);

    nl = np/2;
    nr = nl;

    for(ipj=0;ipj<=(m << 1);ipj++) {
        sum=(ipj ? 0.0 : 1.0);
        for(k=1;k<=nr;k++) sum += pow((double)(k),(double)(ipj));
        for(k=1;k<=nl;k++) sum += pow((double)(-k),(double)(ipj));
        mm=MIN(ipj,2*m-ipj);
        for(imj=-mm;imj<=mm;imj+=2) gsl_matrix_set(a,(ipj+imj)/2,(ipj-imj)/2,sum);
    }

    gsl_linalg_LU_decomp(a, p, &k);
    for (j=0;j<m+1;j++) gsl_vector_set(b,j,0.0);
    gsl_vector_set(b,ld,1.0);

    gsl_linalg_LU_solve (a, p, b, b);

    for(k = -nl;k<=nr;k++) {
        sum = gsl_vector_get(b,0);
        fac = 1.0;
        for (mm=1;mm<=m;mm++) sum += gsl_vector_get(b,mm)*(fac *= k);

        j=(np-k) % np;
        c[j]=sum; // c is in wraparound order, convenient for fft convolute

        // c[nl + k] = sum;
    }
    memcpy(fHdl->respWav, c, np * sizeof(ANALYSIS_WAVEFORM_BASE_TYPE));
/*
    for(j=0; j<np; j++) {
        fprintf(stderr, "%g\n", c[j]);
    }
*/
    gsl_vector_free(b);
    gsl_matrix_free(a);
    gsl_permutation_free(p);

/*
    for(k=nl; k<wavlen - nr; k++) {
        sum = 0.0;
        for(j=0; j<np; j++) {
            sum += c[j] * inwav[k+j-nl];
            outwav[k] = sum;
        }
    }
*/
    free(c);
    return 0;
}

int filters_raisedCosine(filters_t *fHdl)
{
    ssize_t i;
    ANALYSIS_WAVEFORM_BASE_TYPE x;

    for(i=0; i<(fHdl->respLen+1)/2; i++) { /* positive side */
        x = 2.0*M_PI/(ANALYSIS_WAVEFORM_BASE_TYPE)(fHdl->respLen-1)*(ANALYSIS_WAVEFORM_BASE_TYPE)i;
        fHdl->respWav[i] = (1.0 + cos(x))/(ANALYSIS_WAVEFORM_BASE_TYPE)fHdl->respLen;
    }
    for(i=-((fHdl->respLen-1)/2); i<0; i++) { /* negative side */
        /* i=-(fHdl->respLen-1)/2 cast to wrong value */
        x = 2.0*M_PI/(ANALYSIS_WAVEFORM_BASE_TYPE)(fHdl->respLen-1)*(ANALYSIS_WAVEFORM_BASE_TYPE)i;
        fHdl->respWav[fHdl->respLen+i] = (1.0 + cos(x))/(ANALYSIS_WAVEFORM_BASE_TYPE)fHdl->respLen;
    }
    return 0;
}

int filters_convolute(filters_t *fHdl)
{
    size_t i;
    ANALYSIS_WAVEFORM_BASE_TYPE re, im;

    for(i=0; i<fHdl->wavLen; i++) {
        fHdl->fftwWork[i] = fHdl->inWav[i];
    }
    for(i=fHdl->wavLen; i<fHdl->fftLen; i++) {
        fHdl->fftwWork[i] = 0.0;
    }

    // fill in with respwav in wrap-around order
    fHdl->fftwWork1[0] = fHdl->respWav[0];
    for(i=1; i<(fHdl->respLen+1)/2; i++) {
        fHdl->fftwWork1[i] = fHdl->respWav[i];
        fHdl->fftwWork1[fHdl->fftLen-i] = fHdl->respWav[fHdl->respLen-i];
    }
    for(i=(fHdl->respLen+1)/2; i<(fHdl->fftLen-(fHdl->respLen+1)/2); i++) {
        fHdl->fftwWork1[i] = 0.0;
    }

    // do fft
    FFTW(execute)(fHdl->fftwPlan);
    FFTW(execute)(fHdl->fftwPlan1);

    // multiply in complex fourier space, half-complex format
    fHdl->fftwWork[0] = fHdl->fftwWork[0] * fHdl->fftwWork1[0]
        / (ANALYSIS_WAVEFORM_BASE_TYPE)fHdl->fftLen;
    for(i=1; i<fHdl->fftLen/2; i++) {
        re = fHdl->fftwWork[i] * fHdl->fftwWork1[i]
            - fHdl->fftwWork[fHdl->fftLen-i] * fHdl->fftwWork1[fHdl->fftLen-i];
        im = fHdl->fftwWork[i] * fHdl->fftwWork1[fHdl->fftLen-i]
            + fHdl->fftwWork[fHdl->fftLen-i] * fHdl->fftwWork1[i];
        fHdl->fftwWork[i] = re / (ANALYSIS_WAVEFORM_BASE_TYPE)fHdl->fftLen;
        fHdl->fftwWork[fHdl->fftLen-i] = im / (ANALYSIS_WAVEFORM_BASE_TYPE)fHdl->fftLen;
    }
    fHdl->fftwWork[fHdl->fftLen/2] = fHdl->fftwWork[fHdl->fftLen/2] * fHdl->fftwWork1[fHdl->fftLen/2]
        / (ANALYSIS_WAVEFORM_BASE_TYPE)fHdl->fftLen;

    // ifft
    FFTW(execute)(fHdl->fftwPlan2);

    // copy the output to outwav
    memcpy(fHdl->outWav, fHdl->fftwWork, fHdl->wavLen * sizeof(ANALYSIS_WAVEFORM_BASE_TYPE));
    return 0;
}

int filters_hanning_window(filters_t *fHdl)
{
    size_t i;

    fHdl->fftwS1 = 0.0; fHdl->fftwS2 = 0.0;
    for(i=0; i<fHdl->fftLen; i++) {
        fHdl->fftwWin[i] = 0.5 * (1.0 - cos(2*M_PI*i/(double)fHdl->fftLen));
        fHdl->fftwS1 += fHdl->fftwWin[i];
        fHdl->fftwS2 += fHdl->fftwWin[i] * fHdl->fftwWin[i];
    }

    return 0;
}

int filters_fft_spectrum(filters_t *fHdl)
{
    size_t i;

    for(i=0; i<fHdl->fftLen; i++) {
        fHdl->fftwWork[i] = fHdl->inWav[i] * fHdl->fftwWin[i];
    }
    FFTW(execute)(fHdl->fftwPlan);

    /* Compute linearized power spectrum into fftwWork1, in [V] for example, normalized.
     * Total length should be (int)(n/2)+1 */
    fHdl->fftwWork1[0] = fHdl->fftwWork[0] / fHdl->fftwS1;
    for(i=1; i<(fHdl->fftLen+1)/2; i++) {
        fHdl->fftwWork1[i] = hypot(fHdl->fftwWork[i],
                                   fHdl->fftwWork[fHdl->fftLen - i]) * sqrt(2.0) / fHdl->fftwS1;
    }
    if(fHdl->fftLen % 2 == 0) { /* even number */
        fHdl->fftwWork1[fHdl->fftLen/2] = fHdl->fftwWork[fHdl->fftLen/2] / fHdl->fftwS1;
    }
    /* For spectra density, normalization should be * sqrt(2.0 * dt / fftwS2) */
    for(i=0; i<=fHdl->fftLen/2; i++) {
        fHdl->fftwWork[i] = fHdl->fftwWork1[i] * fHdl->fftwS1 * sqrt(fHdl->dt / fHdl->fftwS2);
    }
    return 0;
}

int filters_DWT(filters_t *fHdl) /* discrete wavelet transform */
{
    gsl_wavelet_transform_forward(fHdl->gslDWT, fHdl->waveletWav, 1, fHdl->wavLen,
                                  fHdl->gslDWTWork);
    return 0;
}

int filters_median(filters_t *fHdl, size_t n) /* median filter with moving window size n */
{
    size_t i, mid=n/2;

    for(i=0; i<mid;i++) /* overhang at the beginning */
        fHdl->outWav[i] = quickselect(fHdl->inWav, mid+i+1, (mid+i+1)/2);
    for(i=0; i<fHdl->wavLen-1 - mid; i++)
        fHdl->outWav[mid+i] = quickselect(fHdl->inWav + i, n, n/2);
    for(i=0; i<mid; i++) /* overhang at the end */
        fHdl->outWav[fHdl->wavLen-1 - i] = quickselect(fHdl->inWav + fHdl->wavLen - (mid-i+1),
                                                       mid-i+1, (mid-i+1)/2);
    return 0;
}

int filters_trapezoidal(filters_t *fHdl, size_t k, size_t l, double M)
{
    double s, pp;
    ssize_t i, j, jk, jl, jkl;
    double vj, vjk, vjl, vjkl, dkl;

    s = 0.0; pp = 0.0;

    for(i=0; i<fHdl->wavLen; i++) {
        j=i; jk = j-k; jl = j-l; jkl = j-k-l;
        vj   = j>=0   ? fHdl->inWav[j]   : fHdl->inWav[0];
        vjk  = jk>=0  ? fHdl->inWav[jk]  : fHdl->inWav[0];
        vjl  = jl>=0  ? fHdl->inWav[jl]  : fHdl->inWav[0];
        vjkl = jkl>=0 ? fHdl->inWav[jkl] : fHdl->inWav[0];

        dkl = vj - vjk - vjl + vjkl;
        pp = pp + dkl;
        if(M>=0.0) {
            s = s + pp + dkl * M;
        } else { /* infinit decay time, so the input is a step function */
            s = s + dkl;
        }
        fHdl->outWav[i] = s / (fabs(M) * (double)k);
    }
    return 0;
}

#ifdef FILTERS_DEBUG_ENABLEMAIN
int main(int argc, char **argv)
{
    #define PLEN 1503
    ANALYSIS_WAVEFORM_BASE_TYPE pulse[PLEN] = {0.0};
    size_t i;
    filters_t *fHdl;

    i = 2;
    pulse[i++]=0.0; pulse[i++]=1.0; pulse[i++]=10.0; pulse[i++]=8.0; pulse[i++]=6.0;
    pulse[i++]=4.0; pulse[i++]=2.0; pulse[i++]=1.0; pulse[i++]=0.5; pulse[i++]=0.2;
    i = 100;
    pulse[i--]=0.0; pulse[i--]=1.0; pulse[i--]=10.0; pulse[i--]=8.0; pulse[i--]=6.0;
    pulse[i--]=4.0; pulse[i--]=2.0; pulse[i--]=1.0; pulse[i--]=0.5; pulse[i--]=0.2;

    fHdl = filters_init(pulse, PLEN);
    filters_median(fHdl, 11);
    for(i=0; i<fHdl->wavLen; i++) {
        printf("%g %g\n", fHdl->inWav[i], fHdl->outWav[i]);
    }
    printf("\n\n");

#if 0
    for(i=0; i<PLEN; i++) {
        pulse[i] += 10.0 * cos(2.0 * M_PI/3.0 * i);
    }

    fHdl = filters_init_for_convolution(pulse, PLEN, 0);
    filters_fft_spectrum(fHdl);
    for(i=0; i<fHdl->fftLen; i++) {
        printf("%g %g\n", fHdl->fftwWork[i], fHdl->fftwWork1[i]);
    }
    printf("\n\n");
#endif
#if 0
    fHdl = filters_init_for_convolution(NULL, PLEN, 31);
    memcpy(fHdl->inWav, pulse, PLEN * sizeof(ANALYSIS_WAVEFORM_BASE_TYPE));

    filters_raisedCosine(fHdl);
    filters_convolute(fHdl);

    for(i=0; i<fHdl->fftLen/*PLEN*/; i++) {
        // printf("%g %g\n", pulse[i], fHdl->outWav[i]);
        printf("%g %g\n", fHdl->fftwWork[i], fHdl->fftwWork1[i]);
    }
    printf("\n\n");

    filters_SavitzkyGolay(fHdl, 5, 0);
    filters_convolute(fHdl);

    for(i=0; i<fHdl->fftLen/*PLEN*/; i++) {
        // printf("%g %g\n", pulse[i], fHdl->outWav[i]);
        printf("%g %g\n", fHdl->fftwWork[i], fHdl->fftwWork1[i]);
    }
#endif

    filters_close(fHdl);
    return EXIT_SUCCESS;
}
#endif


#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multiroots.h>

struct rparams
  {
    double a;
    double b;
  };

int
rosenbrock_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  double a = ((struct rparams *) params)->a;
  double b = ((struct rparams *) params)->b;

  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);

  double y0 = a * (1 - x0);
  double y1 = b * (x1 - x0 * x0);

  gsl_vector_set (f, 0, y0);
  gsl_vector_set (f, 1, y1);

  return GSL_SUCCESS;
}

#ifdef DERIV
int
rosenbrock_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
  double a = ((struct rparams *) params)->a;
  double b = ((struct rparams *) params)->b;

  double x0 = gsl_vector_get (x, 0);

  double df00 = -a;
  double df01 = 0;
  double df10 = -2 * b  * x0;
  double df11 = b;

  gsl_matrix_set (df, 0, 0, df00);
  gsl_matrix_set (df, 0, 1, df01);
  gsl_matrix_set (df, 1, 0, df10);
  gsl_matrix_set (df, 1, 1, df11);

  return GSL_SUCCESS;
}

int
rosenbrock_fdf (const gsl_vector * x, void *params,
                gsl_vector * f, gsl_matrix * df)
{
  rosenbrock_f (x, params, f);
  rosenbrock_df (x, params, df);

  return GSL_SUCCESS;
}
#endif

#ifdef DERIV
#define SOLVER gsl_multiroot_fdfsolver
#define SOLVER_TYPE gsl_multiroot_fdfsolver_type
#else
#define SOLVER gsl_multiroot_fsolver
#define SOLVER_TYPE gsl_multiroot_fsolver_type
#endif

int
main (void)
{
  const SOLVER_TYPE *T;
  SOLVER *s;

  int status;
  size_t i, iter = 0;

  const size_t n = 2;
  struct rparams p = {1.0, 10.0};
#ifdef DERIV
  gsl_multiroot_function_fdf f = {&rosenbrock_f, &rosenbrock_df, &rosenbrock_fdf, n, &p};
#else
  gsl_multiroot_function f = {&rosenbrock_f, n, &p};
#endif

  double x_init[2] = {-10.0, -5.0};
  gsl_vector *x = gsl_vector_alloc (n);

  gsl_vector_set (x, 0, x_init[0]);
  gsl_vector_set (x, 1, x_init[1]);

#ifdef DERIV
  T = gsl_multiroot_fdfsolver_gnewton;
  s = gsl_multiroot_fdfsolver_alloc (T, &f, x);
#else
  T = gsl_multiroot_fsolver_hybrids;
  s = gsl_multiroot_fsolver_alloc (T, &f, x);
#endif

  print_state (iter, s);

  do
    {
      iter++;
#ifdef DERIV
      status = gsl_multiroot_fdfsolver_iterate (s);
#else
      status = gsl_multiroot_fsolver_iterate (s);
#endif

      print_state (iter, s);

      if (status)
        break;

      status = gsl_multiroot_test_residual (s->f, 0.0000001);
    }
  while (status == GSL_CONTINUE && iter < 1000);

  printf ("status = %s\n", gsl_strerror (status));

#ifdef DERIV
  gsl_multiroot_fdfsolver_free (s);
#else
  gsl_multiroot_fsolver_free (s);
#endif

  gsl_vector_free (x);
}

int
print_state (size_t iter, SOLVER * s)
{
  printf ("iter = %3u x = % 15.8f % 15.8f  f(x) = % .3e % .3e\n",
          iter,
          gsl_vector_get (s->x, 0), gsl_vector_get (s->x, 1),
          gsl_vector_get (s->f, 0), gsl_vector_get (s->f, 1));
}


/*
** detrending, fit polynomial or order 1 (linear), 3 or 5
** G.Lohmann, Aug 2016, MPI-KYB
*/
#include <viaio/VImage.h>
#include <viaio/Vlib.h>
#include <viaio/mu.h>
#include <viaio/option.h>

#include <stdio.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>

#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_fit.h>
#include <gsl/gsl_multifit.h>

#define ABS(x) ((x) < 0 ? -(x) : (x))

void VGetStats(double *data,int nt,int i0,double *ave,double *sigma)
{
  int i;
  double u,sum1,sum2,nx;
  sum1 = sum2 = 0;
  for (i=i0; i<nt; i++) {
    u = data[i];
    sum1 += u;
    sum2 += u*u;
  }
  nx = (double)nt;
  double mean = sum1/nx;
  (*ave) = mean;
  (*sigma) = sqrt((sum2 - nx * mean * mean) / (nx - 1.0));
}

void VDetrend(VAttrList list,VFloat minval,VShort type,VShort i0)
{
  int slice,row,col,i,j;

  /* get image dimensions */
  int nslices = VAttrListNumImages(list);
  VImage *src = VAttrListGetImages(list,nslices);
  int nrows  = VImageNRows (src[0]);
  int ncols  = VImageNColumns (src[0]);
  int ntimesteps = VImageNBands (src[0]);
  if (nslices < 1) VError(" no slices");

  double smax = VPixelMaxValue(src[0]);
  double smin = VPixelMinValue(src[0]);

  /* ini regression matrix */
  int p = 4;
  if (type == 2) p = 6;
  gsl_vector *y = gsl_vector_calloc(ntimesteps);
  gsl_vector *c = gsl_vector_calloc(p);
  gsl_matrix *X = gsl_matrix_calloc(ntimesteps,p);
  for (j=0; j<ntimesteps; j++) {
    double u = (double)j;
    gsl_matrix_set(X,j,0,1);
    gsl_matrix_set(X,j,1,u);
    gsl_matrix_set(X,j,2,u*u);
    gsl_matrix_set(X,j,3,u*u*u);
    if (type == 2) {
      gsl_matrix_set(X,j,4,u*u*u*u);
      gsl_matrix_set(X,j,5,u*u*u*u*u);
    }
  }
  gsl_multifit_linear_workspace *workspace = gsl_multifit_linear_alloc ((size_t)ntimesteps,(size_t)p);
  gsl_matrix *cov = gsl_matrix_calloc(p,p);

  double *x = (double *) VCalloc(ntimesteps,sizeof(double));
  for (j=0; j<ntimesteps; j++) x[j] = (double)j;

  gsl_vector *w = gsl_vector_calloc(ntimesteps);
  for (j=0; j<ntimesteps; j++) w->data[j] = (double)1.0;
  for (i=0; i<=i0; i++) w->data[i] = 0.01; /* ignore initial time steps */

  /* remove baseline drift */
  double c0=0,c1=0,c2=0,c3=0,c4=0,c5=0,cov00,cov01,cov11,chisq;
  for (slice=0; slice<nslices; slice++) {
    if (slice%5==0)fprintf(stderr," slice  %5d  of  %d\r",slice,nslices);
    for (row=0; row<nrows; row++) {
      for (col=0; col<ncols; col++) {

	for (j=0; j<ntimesteps; j++) {
	  y->data[j] = VGetPixel(src[slice],j,row,col);
	}
	if (y->data[0] < minval+1.0e-8) {
	  for (j=0; j<ntimesteps; j++) VSetPixel(src[slice],j,row,col,0.0);
	  goto skip;
	}
	double mean=0,sigma=0;
	VGetStats(y->data,ntimesteps,i0,&mean,&sigma);

	/* remove linear trend */
	if (type == 0) {
	  gsl_fit_wlinear (x,1,w->data,1,y->data,1,j,&c0,&c1,&cov00,&cov01,&cov11,&chisq);
	  for (j=0; j<i0; j++) y->data[j] = 0;
	  for (j=i0; j<ntimesteps; j++) {
	    y->data[j] = y->data[j] - (c0 + c1*x[j]);
	  }
	}

	/* remove cubic polynomial */
	if (type == 1) {
	  gsl_multifit_wlinear (X,w,y,c,cov,&chisq,workspace);
	  c0 = c->data[0];
	  c1 = c->data[1];
	  c2 = c->data[2];
	  c3 = c->data[3];

	  for (j=0; j<i0; j++) y->data[j] = 0;
	  for (j=i0; j<ntimesteps; j++) {
	    double u = x[j];
	    y->data[j] = y->data[j] - (c0 + c1*u + c2*u*u + c3*u*u*u);
	  }
	}

	/* remove polynomial of order 5 */
	if (type == 2) {
	  gsl_multifit_wlinear (X,w,y,c,cov,&chisq,workspace);
	  c0 = c->data[0];
	  c1 = c->data[1];
	  c2 = c->data[2];
	  c3 = c->data[3];
	  c4 = c->data[4];
	  c5 = c->data[5];
	  for (j=0; j<i0; j++) y->data[j] = 0;
	  for (j=i0; j<ntimesteps; j++) {
	    double u = x[j];
	    double u2 = u*u;
	    double u3 = u2*u;
	    y->data[j] = y->data[j] - (c0 + c1*u + c2*u2 + c3*u3 + c4*u2*u2 + c5*u2*u3);
	  }
	}

	double a = 1.25;
	double sum=0,nx=0;
	for (j=i0; j<ntimesteps; j++) {
	  sum += a*y->data[j] + mean;
	  nx++;
	}
	if (nx > 0) sum /= nx;

	for (j=0; j<i0; j++) VSetPixel(src[slice],j,row,col,sum);
	for (j=i0; j<ntimesteps; j++) {
	  double u = a*y->data[j] + mean;
	  if (u > smax) u = smax;
	  if (u < smin) u = smin;
	  VSetPixel(src[slice],j,row,col,u);
	}
      skip: ;
      }
    }
  }
  fprintf(stderr,"\n");
}


/*=========================================================================

  Copyright (c) Yue Sheng, University of Manchester and David Worth, STFC
  All rights reserved.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

/** \file vvQuan3D.c
\brief This is a VolView plugin to calculate several characteristics from a
labelled image.

The following characteristics are calculated:
\li Volume by voxel counts
\li Equivalent sphere diameter by voxel counts
\li Bounding box diagonal
\li Principal Component Analysis
\li Ellipsoid fitting by PCA
\li Equivalent circle diameter by PCA
\li Isosurface by marching cube
\li Surface area
\li Surface volume
\li Equivalent sphere diameter from surface volume
\li Sphercity
\li Normalised surface area to volume ratio (Radius*Sa/Vol)
*/

#include "vvHelper.h"
#include "vtkVVPluginAPI.h"
#include <dlfcn.h>
#include <limits.h>
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>

/**
Struct used when calculating the number of voxels with each voxel value
*/
typedef struct {
  /** The voxel value */
  int value;
  /** The number of voxels with this value */
  int count;
} vvQuan3DVoxelCount;

/**
Comparison function for sorting the image data.
\param a Integer value
\param b Integer value
\return 1 if \c a>b; -1 if \c a<b; 0 if \c a==b.
*/
int cmpfun(void *a, void *b)
{
  return ( *(int*)a - *(int*)b );
}

/**
\brief Calculate sphere diameter from the sphere volume.

Use the formula \f$ 2*(3v/(4\pi))^{\frac{1}{3}} \f$ where \f$ v \f$ is the
sphere volume.

\param volume The sphere volume
\return Diameter of the sphere
*/
double vol2dia(double volume)
{
  return 2.0 * pow((3.0*volume)/(4.0*3.14159265),(1.0/3.0));
}

/**
\brief Find the (i,j,k) coordinates for each voxel id and also calculate
maximum, minimum and mean coordinates.

The positions pointer must be allocated by the calling function as follows
\code
positions = malloc(3*sizeof(int)*voxelCount);
\endcode

\param voxelSubArray Array of voxel ids
\param voxelCount Number of voxels in voxelSubArray <b>NB This is not the
length of the array</b>
\param dim The dimsnesions of the image
\param positions Pointer to matrix which will hold the (i,j,k) coordinates
\param min  The minimum coordinate in each dimension
\param max  The maximum coordinate in each dimension
\param mean The mean of the coordinates in each dimension
*/
void getCoords(int *voxelSubArray, int voxelCount, int dim[],
               gsl_matrix *positions, int min[], int max[], int mean[])
{
  int i;
  int voxelId;
  int icoord, jcoord, kcoord, rem;
  min[0] = min[1] = min[2] = INT_MAX;
  max[0] = max[1] = max[2] = INT_MIN;
  mean[0] = mean[1] = mean[2] = 0;

  /* Loop over the voxels and calulate i,j,k coords for each one */
  for (i = 0; i < voxelCount; i++) {
    voxelId = voxelSubArray[i];
    kcoord = floor(voxelId / (dim[0]*dim[1]));
    rem = voxelId % (dim[0]*dim[1]);
    jcoord = floor(rem / dim[0]);
    icoord = rem % dim[0];
    //printf("voxel %d (%d,%d,%d)\n",voxelId,icoord,jcoord,kcoord);
    gsl_matrix_set(positions,i,0,icoord);
    gsl_matrix_set(positions,i,1,jcoord);
    gsl_matrix_set(positions,i,2,kcoord);
    if (icoord > max[0]) max[0] = icoord;
    if (jcoord > max[1]) max[1] = jcoord;
    if (kcoord > max[2]) max[2] = kcoord;
    if (icoord < min[0]) min[0] = icoord;
    if (jcoord < min[1]) min[1] = jcoord;
    if (kcoord < min[2]) min[2] = kcoord;
    mean[0] += icoord;
    mean[1] += jcoord;
    mean[2] += kcoord;
  }
  mean[0] /= voxelCount;
  mean[1] /= voxelCount;
  mean[2] /= voxelCount;
}

/**
\brief Calculate the data from the image

\author David Worth, STFC
\date May 2012
\todo This only works for unsigned short image type. Make it do other integer
datatypes.

\param inf Pointer to object that holds data for this plugin. The voxel size
can be obtained from it.
\param pds Pointer to object that carries information on data set to be
processed. Includes actual buffer of voxel data, number of voxels along each
dimension, voxel spacing and voxel type.
*/
static int ProcessData(void *inf, vtkVVProcessDataStruct *pds)
{
  /* Information about the plugin and the image */
  vtkVVPluginInfo *info = (vtkVVPluginInfo *)inf;
  /* Number of voxels in each direction of image */
  int *dim = info->InputVolumeDimensions;
  /* Size of datatype in the image */
  unsigned int rsize = info->InputVolumeScalarSize;
  /* Voxel size - can be set by user in VolView */
  double voxelSize = atof(info->GetGUIProperty(info, 0, VVP_GUI_VALUE));
  /* Minimum and maximum raw values from image. */
  double imgMin = info->InputVolumeScalarRange[0],
         imgMax = info->InputVolumeScalarRange[1];
  /* Array of voxel counts for each value. Array index is the value. */
  int maxVoxelVals = (int)ceil(imgMax)-(int)floor(imgMin)+1;
  int *voxelCounts = malloc(sizeof(int)*maxVoxelVals);
  /* Array of arrays - each sub-array holding the ids of voxels with that value */
  int* *voxelArray = malloc(sizeof(int*)*maxVoxelVals);
  /* Pointer to sub array used when creating voxelArray */
  int *voxelSubArray = NULL;
  /* Index array used to write into voxelSubArray */
  int *indexVoxelSubArray = malloc(sizeof(int)*maxVoxelVals);
  /* Number of the non-background voxels */
  int sumVoxels = 0;
  /* Number of different voxel values */
  int totalParticles = 0;
  /* Array of (i,j,k) coordinates for voxels with a particular value */
  gsl_matrix *positions = NULL;
  /* Stats for voxel data for each value */
  int max[3], min[3], mean[3], boundingBoxSize[3];
  double boundingBoxDiam;
  /* Multiplication factors based on physical voxel size */
  double areaFactor = pow(voxelSize,2), volumeFactor = pow(voxelSize,3);
  /* Output data */
  int voxelLabel, voxelCount;
  double voxelCountVolume, voxelCountDia;
  /* Data used when covariance matrix is calculated */
  double* *matrix = NULL;
  double *row = NULL;
  double* *covMatrix = NULL;
  /* Data for eigenanalysis of covariance matrix */
  gsl_matrix *gslCovMatrix = NULL;
  gsl_vector *eval = NULL;
  gsl_matrix *evecs = NULL;
  gsl_eigen_symmv_workspace *workspace = NULL;
  gsl_vector vector;
  int result;

  void *gslcblas_handle, *gsl_handle;

  /* Buffer for output to user */
  char buffer[1000];
  int i, j = 0;

  /* The raw image data */
  unsigned short *ptr = (unsigned short *)pds->inData;

  /* Check the data type */
  switch (info->InputVolumeScalarType) {
    case VTK_CHAR:
    case VTK_UNSIGNED_CHAR:
    case VTK_FLOAT:
    case VTK_DOUBLE: {
      sprintf(buffer, "Image datatype must be integer or boolean. This \
      algorithm cannot continue");
      info->SetProperty(info, VVP_ERROR, buffer);
      return -1;
      break;
    }
  }

  printf("Image has type %d size %d\n", info->InputVolumeScalarType,rsize);
  printf("The dimensions of the image are: %d, %d, %d \n",
          dim[0], dim[1], dim[2]);
  sprintf(buffer, "The dimensions of the image are: %d, %d, %d \n",
          dim[0], dim[1], dim[2]);
  printf("There are %d components\n",info->InputVolumeNumberOfComponents);
  printf("The minimum value in the image is %d\n", (int)imgMin);
  printf("and the maximum value is %d\n", (int)imgMax);

  /* Sort the image values */
  /*qsort(ptr, (size_t)(dim[0]*dim[1]*dim[2]), (size_t)info->InputVolumeScalarSize,
        (__compar_fn_t)cmpfun);*/

  /*
   * Create array of voxel counts for each voxel value except the min value
   * which we take to be the background
   */
  for (i = 0; i < maxVoxelVals; i++) voxelCounts[i]=0;
  for (i = 0; i < dim[0]*dim[1]*dim[2]; i++) {
    if (ptr[i] != imgMin) { /* Not the minimum value */
      if (ptr[i] > imgMax) printf("%d %d\n",i,ptr[i]); /* Something wrong */
      /* Not seen this value before so it's a new particle */
      if (voxelCounts[ptr[i]] == 0) totalParticles++;
      voxelCounts[ptr[i]]++;
      sumVoxels++;
    }
  }

  /* Create the array of arrays with voxel ids for each voxel value */
  for (i = 0; i < maxVoxelVals; i++) {
    if (voxelCounts[i] > 0) {
      /*printf("Non zero voxel count for i = %d - count is %d\n",i,voxelCounts[i]);*/
      voxelArray[i] = malloc(voxelCounts[i]*sizeof(int));
    }
    else voxelArray[i] = NULL;
    indexVoxelSubArray[i] = 0;
  }
  /* Now populate that array with values */
  for (i = 0; i < dim[0]*dim[1]*dim[2]; i++) {
    if (ptr[i] != imgMin) {
      //printf("add entry to voxel array %d - entry id %d\n",ptr[i],i);
      voxelSubArray = voxelArray[ptr[i]];
      voxelSubArray[ indexVoxelSubArray[ptr[i]] ] = i;
      indexVoxelSubArray[ptr[i]]++;
    }
  }

  printf("Porosity is %f\n",(double)sumVoxels/(double)(dim[0]*dim[1]*dim[2]));
  sprintf(buffer+strlen(buffer), "Porosity is %f\n",
          (double)sumVoxels/(double)(dim[0]*dim[1]*dim[2]));
  printf("Number of particles is %d\n",totalParticles);

  /* Load the GSL BLAS and GSL itself. RTLD_GLOBAL means we can use the
   * function names from GSL rather than having to get function pointers with
   * dlsym. */
  gslcblas_handle = dlopen("/usr/lib/libgslcblas.so", RTLD_LAZY|RTLD_GLOBAL);
  if (!gslcblas_handle) {
    printf("Error opening libgslcblas.so - %s\n",dlerror());
  }
  gsl_handle = dlopen("/usr/lib/libgsl.so", RTLD_LAZY|RTLD_GLOBAL);
  if (!gsl_handle) {
    printf("Error opening libgsl.so - %s\n",dlerror());
  }

  /* Allocate the GSL matrices and vectors for eigenanalysis */
  gslCovMatrix = gsl_matrix_alloc(3,3);
  eval = gsl_vector_alloc(3);
  evecs = gsl_matrix_alloc(3,3);
  workspace = gsl_eigen_symmv_alloc(3);

  /* Loop over the particles */
  for (i = 0; i < 1/*totalParticles*/; i++) {
    /* Find the next particle with a non-zero voxel count */
    while ((j < maxVoxelVals) && (voxelCounts[j] == 0)) j++;
    /* Do some simple stuff */
    voxelLabel = j;
    voxelCount = voxelCounts[j];
    voxelCountVolume = voxelCount * volumeFactor;
    voxelCountDia = voxelSize*vol2dia(voxelCount);
    printf("%d, %d, %f, %f",voxelLabel,voxelCount,voxelCountVolume,
      voxelCountDia);

    /* Find the (i,j,k) coordinates in the image of the voxels */
    positions = gsl_matrix_alloc(voxelCount,3);
    voxelSubArray = voxelArray[voxelLabel];
    getCoords(voxelSubArray, voxelCount, dim, positions, min, max, mean);
    /*printf(" (%d, %d, %d)",min[0],min[1],min[2]);
    printf(" (%d, %d, %d)",max[0],max[1],max[2]);
    printf(" (%d, %d, %d)",mean[0],mean[1],mean[2]);*/
    boundingBoxSize[0] = max[0]-min[0]+1;
    boundingBoxSize[1] = max[1]-min[1]+1;
    boundingBoxSize[2] = max[2]-min[2]+1;
    boundingBoxDiam = sqrt(boundingBoxSize[0]*boundingBoxSize[0] +
                           boundingBoxSize[1]*boundingBoxSize[1] +
                           boundingBoxSize[2]*boundingBoxSize[2])*voxelSize;
    printf(" (%d, %d, %d)",boundingBoxSize[0],boundingBoxSize[1],
                           boundingBoxSize[2]);
    printf(" %f", boundingBoxDiam);

    /* Calculate positions-mean */
    matrix = malloc(voxelCount*sizeof(double*));
    for (j = 0; j < voxelCount; j++) {
      row = malloc(3*sizeof(double));
      row[0] = gsl_matrix_get(positions,j,0)-mean[0];
      row[1] = gsl_matrix_get(positions,j,1)-mean[1];
      row[2] = gsl_matrix_get(positions,j,2)-mean[2];
      matrix[j] = row;
    }
    covMatrix = CovMat(matrix, voxelCount, 3);

    for (j = 0; j < voxelCount; j++) {
      free(matrix[j]);
    }
    free(matrix);

    /*printf("\nCovariance matrix\n");
    for (j = 0; j < 3; j++) {
      printf("%f %f %f\n", covMatrix[j][0], covMatrix[j][1], covMatrix[j][2]);
    }*/

    /* To do the eigenanalysis use GNU Scientific library so create matrix
     * in that format */
    for (j = 0; j < 3; j++) {
      gsl_matrix_set(gslCovMatrix,0,j,covMatrix[0][j]);
      gsl_matrix_set(gslCovMatrix,1,j,covMatrix[1][j]);
      gsl_matrix_set(gslCovMatrix,2,j,covMatrix[2][j]);
    }

    /* Calculate eigenvalues and vectors */
    result = gsl_eigen_symmv(gslCovMatrix, eval, evecs, workspace);

    printf("\nEigenvalues\n");
    gsl_vector_fprintf(stdout, eval, "%g");
    printf("\nEigenvectors\n");
    vector = gsl_matrix_const_column(evecs,0).vector;
    gsl_vector_fprintf(stdout, &vector, "%g");
    printf("\n");
    vector = gsl_matrix_const_column(evecs,1).vector;
    gsl_vector_fprintf(stdout, &vector, "%g");
    printf("\n");
    vector = gsl_matrix_const_column(evecs,2).vector;
    gsl_vector_fprintf(stdout, &vector, "%g");

    for (j = 0; j < 3; j++) {
      free(covMatrix[j]);
    }
    free(covMatrix);

    gsl_matrix_free(positions);

    printf("\n");
    /* Move on to next particle */
    j++;
  }

  dlclose(gsl_handle);
  dlclose(gslcblas_handle);

  info->SetProperty(info, VVP_REPORT_TEXT, buffer);

  free(voxelCounts);
  for (i = 0; i < maxVoxelVals; i++) {
    if (voxelArray[i] != NULL) free(voxelArray[i]);
  }

  gsl_matrix_free(gslCovMatrix);
  gsl_vector_free(eval);
  gsl_matrix_free(evecs);
  gsl_eigen_symmv_free(workspace);

  free(voxelArray);
  free(indexVoxelSubArray);
  return 0;
}

/**
\brief Update the VolView GUI to display user parameters.

Sets one GUI parameter - the physical size of the voxels in the image.

\param inf Pointer to object that should be modified to set up GUI elements for
this plugin. It also contains details of the input and output images.
*/
static int UpdateGUI(void *inf)
{
  int i;
  vtkVVPluginInfo *info = (vtkVVPluginInfo *)inf;

  info->SetGUIProperty(info, 0, VVP_GUI_LABEL, "Voxel Size");
  info->SetGUIProperty(info, 0, VVP_GUI_TYPE, VVP_GUI_SCALE);
  info->SetGUIProperty(info, 0, VVP_GUI_DEFAULT , "1");
  info->SetGUIProperty(info, 0, VVP_GUI_HELP,
               "The physical size of a voxel in the image");

  /* what range should we show for possible output values */
  vvPluginSetGUIScaleRange(0);

  info->OutputVolumeScalarType = info->InputVolumeScalarType;
  info->OutputVolumeNumberOfComponents = info->InputVolumeNumberOfComponents;
  for (i = 0; i < 3; i++)
    {
    info->OutputVolumeDimensions[i] = info->InputVolumeDimensions[i];
    info->OutputVolumeSpacing[i] = info->InputVolumeSpacing[i];
    info->OutputVolumeOrigin[i] = info->InputVolumeOrigin[i];
    }

  return 1;
}

/**
\brief Initialise this plugin.

Sets the name and group for this plugin in the plugin list shown to the VolView
user and gives some documentation. Also defines properties so VolView can judge
the memory requirements and potential for undoing this plugin.

\param info Pointer to object that should be modified to give details about this
plugin.
*/
void VV_PLUGIN_EXPORT vvQuan3DCInit(vtkVVPluginInfo *info)
{
  /* always check the version */
  vvPluginVersionCheck();

  /* setup information that never changes */
  info->ProcessData = ProcessData;
  info->UpdateGUI = UpdateGUI;
  info->SetProperty(info, VVP_NAME, "Quantify 3D in C");
  info->SetProperty(info, VVP_GROUP, "Quantification");
  info->SetProperty(info, VVP_TERSE_DOCUMENTATION,
     "Quantify several characteristics from a labelled image");
  info->SetProperty(info, VVP_FULL_DOCUMENTATION,
    "Quantify the following characteristics from a labelled image: Volume by \
voxel counts, Equivalent sphere diameter by voxel counts, Bounding box \
diagonal, Principal Component Analysis, Ellipsoid fitting by PCA, Equivalent \
circle diameter by PCA, Isosurface by marching cube, Surface area, Surface \
volume, Equivalent sphere diameter from surface volume, Sphercity, Normalised \
surface area to volume ratio");

  info->SetProperty(info, VVP_SUPPORTS_IN_PLACE_PROCESSING, "1");
  info->SetProperty(info, VVP_SUPPORTS_PROCESSING_PIECES,   "1");
  info->SetProperty(info, VVP_REQUIRED_Z_OVERLAP,           "0");
  info->SetProperty(info, VVP_NUMBER_OF_GUI_ITEMS,          "1");
  info->SetProperty(info, VVP_REQUIRES_SERIES_INPUT,        "0");
  info->SetProperty(info, VVP_SUPPORTS_PROCESSING_SERIES_BY_VOLUMES, "0");
  info->SetProperty(info, VVP_PRODUCES_OUTPUT_SERIES, "0");
  info->SetProperty(info, VVP_PRODUCES_PLOTTING_OUTPUT, "0");
}


#include <fftw.h>
#include <stdio.h>
#include <stdlib.h>
#define N 16
main()
{
     int i;
     fftw_complex in[N], out[N];
     float fact;
     fftw_plan p,p2;
printf(" stuff in data\n");
     for (i=0;i<N;i++) {
     	in[i].re=(i*i);
     	in[i].im=1;
     }
printf(" create plans\n");
     p = fftw_create_plan(N, FFTW_FORWARD,  FFTW_ESTIMATE);
     p2= fftw_create_plan(N, FFTW_BACKWARD, FFTW_ESTIMATE);
printf(" do it\n");
     fftw_one(p, in, out);
     for (i=0;i<N;i++) {
	printf("%12.4f %12.4f\n",out[i].re,out[i].im);
     }
printf(" \n");
printf(" undo it\n");
     fftw_one(p2, out,in);
     fact=1.0/(float)N;
     for (i=0;i<N;i++) {
	printf("%10.2f %10.2f\n",in[i].re*fact,in[i].im*fact);
     }
printf(" clean up\n");
     fftw_destroy_plan(p);
     fftw_destroy_plan(p2);
}


#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#define COMPEARTH_PRIVATE_UPDOWN_ARGSORT3 1
#define COMPEARTH_PRIVATE_UPDOWN_ABS_ARGSORT3 1
#include "compearth.h"
#ifdef COMPEARTH_USE_MKL
#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wreserved-id-macro"
#pragma clang diagnostic ignored "-Wstrict-prototypes"
#endif
#include <mkl_lapacke.h>
#ifdef __clang__
#pragma clang diagnostic pop
#endif
#else
#include <lapacke.h>
#endif

/*
static int argsort3_upDown(const double *__restrict__ x,
                           const bool lascend,
                           int *__restrict__ iperm);
static int argsort3(const double *__restrict__ x, int *__restrict__ iperm);
*/

#define LWORK 102 //(NB+2)*N hence NB=32; must be atleast 3*N-1 which is 8

/*!
 * @brief Decomposes a set of moment tensors into eigenvalues + basis.
 *
 * @param[in] nmt    Number of moment tensors.
 * @param[in] M      6 x n moment tensors with some unspecified basis
 *                   (e.g., up-south-east) packed
 *                    M = [M11 M22 M33 M12 M13 M23].
 * @param[in] isort  Sorting of eigenvalues. \n
 *                   If 1 then the eigenvalues are sorted highest to lowest. \n
 *                   If 2 then the eigenvalues are sorted lowest to highest. \n
 *                   If 3 then the eigenvalues are sorted by absolute value
 *                   from highest to lowest. \n
 *                   If 4 then the eigenvalues are sorted by absolute value
 *                   from lowest to highest.
 *
 * @param[out] lam   3 x n set of eigenvalues.
 * @param[out] U     3 x 3 x n set of bases.  The first 3 x 3 matrix is in
 *                   column major order.  This has the same basis as M.
 *
 * @author Carl Tape.  Converted to C by Ben Baker.
 *
 * @copyright MIT
 *
 * @bug The
 */
int compearth_CMTdecom(const int nmt, const double *__restrict__ M,
                       const int isort,
                       double *__restrict__ lam,
                       double *__restrict__ U)
{
    double Lams[3], Mx[9], Ut[9], work[LWORK];
    int perm[3], c, i, ierr, info, r;
    // Error checks
    if (nmt < 1 || M == NULL || lam == NULL || U == NULL)
    {
        if (nmt < 1){fprintf(stderr, "%s: Error no mts\n", __func__);}
        if (M == NULL){fprintf(stderr, "%s: Error M is NULL\n", __func__);}
        if (lam == NULL){fprintf(stderr, "%s: Error lam is NULL\n", __func__);}
        if (U == NULL){fprintf(stderr, "%s: Error U is NULL\n", __func__);}
        return -1;
    }
    if (isort < 1 || isort > 4)
    {
        fprintf(stderr, "%s: Error isort=%d is invalid\n", __func__, isort);
        return -1;
    }
    // Decompose the moment tensors
    for (i=0; i<nmt; i++)
    {
        // Create the symmetric moment tensor matrix from the 6x1 vector
        compearth_Mvec2Mmat(1, M, 1, Mx);
        // Compute eigenvalues in ascending order
        info = LAPACKE_dsyev_work(LAPACK_COL_MAJOR, 'V', 'U', 3, Mx, 3,
                                  Lams, work, LWORK);
        if (info != 0)
        {
            fprintf(stderr, "%s: Error computing eigenvalues\n", __func__);
            return -1;
        }
        // Descending
        if (isort == 1)
        {
            argsort3_upDown(Lams, false, perm);
        }
        else if (isort == 2)
        {
            argsort3_upDown(Lams, true, perm);
        }
        // Descending on absolute value
        else if (isort == 3)
        {
            argsort3_absUpDown(Lams, false, perm);
        }
        else //if (isort == 4)
        {
            argsort3_absUpDown(Lams, true,  perm);
        }
        // And permute and save
        for (r=0; r<3; r++)
        {
            lam[3*i+r] = Lams[perm[r]];
        }
        // Permute columns
        for (c=0; c<3; c++)
        {
            for (r=0; r<3; r++)
            {
                Ut[3*c+r] = Mx[3*perm[c]+r];
            }
        }
        // Ensure the determinant is > 0
        ierr = compearth_Udetcheck(1, Ut, &U[9*i]);
        if (ierr != 0)
        {
            fprintf(stderr, "%s: Error checking determinant\n", __func__);
            return -1;
        }
    }
    return 0;
}

/*
static int argsort3_upDown(const double *__restrict__ x,
                           const bool lascend,
                           int *__restrict__ iperm)
{
    int ipermt[3], ierr;
    if (lascend)
    {
        ierr = argsort3(x, iperm);
    }
    else
    {
        ierr = argsort3(x, ipermt);
        iperm[0] = ipermt[2];
        iperm[1] = ipermt[1];
        iperm[2] = ipermt[0];
    }
    return ierr;
}

static int argsort3(const double *__restrict__ x, int *__restrict__ iperm)
{
    int i, temp;
    const int a = 0;
    const int b = 1;
    const int c = 2;
    // Copy
    iperm[a] = a;
    iperm[b] = b;
    iperm[c] = c;
    if (x[iperm[a]] > x[iperm[c]])
    {
        temp = iperm[c];
        iperm[c] = iperm[a];
        iperm[a] = temp;
    }
    if (x[iperm[a]] > x[iperm[b]])
    {
        temp = iperm[b];
        iperm[b] = iperm[a];
        iperm[a] = temp;
    }
    //Now the smallest element is the first one. Just check the 2-nd and 3-rd
    if (x[iperm[b]] > x[iperm[c]])
    {
        temp = iperm[c];
        iperm[c] = iperm[b];
        iperm[b] = temp;
    }
    // Verify
    for (i=1; i<3; i++)
    {
        if (x[iperm[i-1]] > x[iperm[i]])
        {
            fprintf(stderr, "%s: Failed to sort numbers in ascending order\n",
                     __func__);
            return -1;
        }
    }
    return 0;
}
*/


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_interp.h>

static double fun(double x) {
    return x + cos(x * x);
}

int main(void) {
    const double a = 1.0;
    const double b = 10.0;
    const int steps = 10;
    double xi, yi, x[100], y[100], dx;
    FILE *input, *output;
    int i;
    const gsl_interp_type *ts[] = {
            gsl_interp_linear,
            gsl_interp_polynomial,
            gsl_interp_cspline,
            gsl_interp_cspline_periodic,
            gsl_interp_akima,
            gsl_interp_akima_periodic,
            gsl_interp_steffen,
    };
    char *tsn[] = {
            "gsl_interp_linear.txt",
            "gsl_interp_polynomial.txt",
            "gsl_interp_cspline.txt",
            "gsl_interp_cspline_periodic.txt",
            "gsl_interp_akima.txt",
            "gsl_interp_akima_periodic.txt",
            "gsl_interp_steffen.txt",
    };
    int tsc = sizeof(ts) / sizeof(const gsl_interp_type *);
    input = fopen("wartosci.txt", "w");
    dx = (b - a) / (double) steps;
    for (i = 0; i <= steps; ++i) {
        x[i] = a + (double) i * dx + 0.5 * sin((double) i * dx);
        y[i] = fun(x[i]);
        fprintf(input, "%g %g\n", x[i], y[i]);
    }
    for (i = 0; i < tsc; i++) {
        output = fopen(tsn[i], "w");
        gsl_interp_accel *acc = gsl_interp_accel_alloc();
        gsl_spline *spline = gsl_spline_alloc(ts[i], steps + 1);
        gsl_spline_init(spline, x, y, steps + 1);
        for (xi = a; xi <= b; xi += 0.01) {
            yi = gsl_spline_eval(spline, xi, acc);
            fprintf(output, "%g %g\n", xi, yi);
        }
        gsl_spline_free(spline);
        gsl_interp_accel_free(acc);
    }
    return EXIT_SUCCESS;
}


#include <stdio.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>
#include <gbpLib.h>
#include <gbpRNG.h>
#include <gbpMCMC.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_fit.h>
#include <gsl/gsl_interp.h>

void set_MCMC_covariance(MCMC_info *MCMC, double *V) {
    int i_P, j_P;
    if(V == NULL)
        SID_log("Initializing the covariance matrix...", SID_LOG_OPEN);
    else
        SID_log("Updating the covariance matrix...", SID_LOG_OPEN);
    if(MCMC->V == NULL)
        MCMC->V = (double *)SID_malloc(sizeof(double) * MCMC->n_P * MCMC->n_P);
    if(MCMC->m == NULL)
        MCMC->m = gsl_matrix_calloc(MCMC->n_P, MCMC->n_P);
    if(MCMC->b == NULL)
        MCMC->b = gsl_vector_calloc(MCMC->n_P);
    if(V == NULL) {
        for(i_P = 0; i_P < MCMC->n_P; i_P++) {
            for(j_P = 0; j_P < MCMC->n_P; j_P++) {
                if(i_P == j_P) {
                    // Matrix decomposition fails if we initialize an element to zero here.
                    //   Check for this and start with an different value if true.
                    if(MCMC->P_init[i_P] == 0.)
                        MCMC->V[i_P * MCMC->n_P + j_P] = 1e-3 * MCMC->P_limit_max[i_P];
                    else
                        MCMC->V[i_P * MCMC->n_P + j_P] = pow(MCMC->P_init[i_P], 2.);
                } else
                    MCMC->V[i_P * MCMC->n_P + j_P] = 0.;
            }
        }
    } else
        memcpy(MCMC->V, V, (size_t)(MCMC->n_P * MCMC->n_P) * sizeof(double));
    for(i_P = 0; i_P < MCMC->n_P; i_P++)
        for(j_P = 0; j_P < MCMC->n_P; j_P++)
            gsl_matrix_set(MCMC->m, i_P, j_P, MCMC->V[i_P * MCMC->n_P + j_P]);
    gsl_linalg_cholesky_decomp(MCMC->m);

    // Set the upper-diagonal to zero to get L where covariance=L*L^T
    for(i_P = 0; i_P < MCMC->n_P; i_P++)
        for(j_P = i_P + 1; j_P < MCMC->n_P; j_P++)
            gsl_matrix_set(MCMC->m, i_P, j_P, 0.);
    SID_log("Done.", SID_LOG_CLOSE);
}


#include <stdio.h>
#include <errno.h>
#include <string.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_permutation.h>
#include "invwishpdf.h"

int main(int argc, char *argv[]){

	int args;
	int m, n;
	double pdf, dof;

	args = 1;

	m = atoi(argv[args++]);
	n = atoi(argv[args++]);

	// allocate space for matrix X, test_copy, and Scale.
	gsl_matrix *X = gsl_matrix_alloc(m, n);
	gsl_matrix *inv = gsl_matrix_alloc(m,n);
	gsl_matrix *Scale = gsl_matrix_alloc(m, n);

	gsl_matrix_set_identity(Scale);
	fill_matrix(X);

	dof = m + 1;
	pdf = iwishpdf(X, Scale, inv, dof);

	// Print the data
	printf("\n%s", "Inverse Wishart matrix");
	print_matrix(X);
	printf("\n%s", "Scale matrix");
	print_matrix(Scale);
	printf("\n%s", "Wishart matrix");
	print_matrix(inv);
	printf("\n%s", "Density");
	printf("\n%f\n", pdf);

	return(0);
}

gsl_matrix fill_matrix(gsl_matrix *X)
{
	int i,j;
	int step = 0;
	int n = X->size1;
	int m = X->size2;

	// Create a test matrix
	double a_vect[] = {0.17955079, -0.09332222, -0.01273225, -0.09332222,  0.28282278,  0.01724885, -0.01273225,  0.01724885,  0.14424177};
	for (i = 0; i < m; ++i){
		for (j = 0; j < n; ++j){
			gsl_matrix_set(X, i, j, a_vect[step++]);
		}
	}
	return(*X);
}

double iwishpdf(gsl_matrix *X, gsl_matrix *Scale, gsl_matrix *inv, double dof)
{
	double X_det, scale_det, denom, pdf, trace, numerator;
	int m = X->size1;
	int n = X->size2;

	// Allocate matrix for inv(X)
	gsl_matrix *for_mult = gsl_matrix_alloc(m, n);

	// Get determinant of X and Scale matrix
	X_det = matrix_determ(X);
	scale_det = matrix_determ(Scale);

	// Invert X
	inv_matrix(X,inv);

	// Multiple Scale * inv(X)
	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Scale, inv, 1.0, for_mult);

	// Get trace of above.
	trace = matrix_trace(for_mult);

	numerator =  pow(scale_det, dof / 2.0) * pow(X_det, (-dof-m-1)/ 2.0) * exp(-0.5 * trace);
	denom = pow(2,dof * m / 2) * mv_gamma(dof/2, m);

	pdf = (numerator/denom);

	return(pdf);
}

gsl_matrix inv_matrix(gsl_matrix *X, gsl_matrix *inv)
{
	int s;
	int n = X->size1;
	int m = X->size2;
	gsl_matrix *a_copy = gsl_matrix_alloc(m, n);
	gsl_matrix_memcpy( a_copy, X );
	gsl_permutation *P = gsl_permutation_alloc(n);
	gsl_linalg_LU_decomp(a_copy, P, &s);
	gsl_linalg_LU_invert(a_copy, P, inv);
	return(*inv);
}

double matrix_determ(gsl_matrix *X)
{
	int s;
	int n = X->size1;
	int m = X->size2;
	gsl_matrix *a_copy = gsl_matrix_alloc(m, n);
	gsl_matrix_memcpy(a_copy, X );
	gsl_permutation *P = gsl_permutation_alloc(n);
	gsl_linalg_LU_decomp(a_copy, P, &s);
	double my_det = gsl_linalg_LU_det (a_copy, s);
	return(my_det);
}

double matrix_trace(gsl_matrix *X){
	int i, m;
	m = X->size1;
	double trace = 0.0;
	for(i=0;i<m;i++){
		trace += gsl_matrix_get(X,i,i);
	}
	return(trace);
}

double mv_gamma(double a, double d){
	double val = 1.0;
	int i;
	for(i = 1; i <= d; i++){
		val *= gsl_sf_gamma(a - (0.5 * (i - 1)));
	}
	val *=  pow(M_PI, (d * (d - 1) / 4.0));
	return(val);
}

void print_matrix(gsl_matrix *X)
{
	int i, j;
	int n = X->size1;
	int m = X->size2;
	printf("\n");
	for(i=0; i<m; i++){
		for(j=0; j<n; j++){
			printf("%05.2f ", gsl_matrix_get(X, i, j));
		}
		printf("\n");
	}
}


/* cdf/negbinom.c
 *
 * Copyright (C) 2004 Jason H. Stover.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307, USA.
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_cdf.h>

#include "error.h"

/*
 * Pr(X <= k) for a negative binomial random variable X, i.e.,
 * the probability of k or fewer failuers before success n.
 */

double
gsl_cdf_negative_binomial_P (const unsigned int k, const double p, const double n)
{
  double P;
  double a;
  double b;

  if (p > 1.0 || p < 0.0)
    {
      CDF_ERROR ("p < 0 or p > 1", GSL_EDOM);
    }

  if (n < 0)
    {
      CDF_ERROR ("n < 0", GSL_EDOM);
    }

  a = (double) n;
  b = (double) k + 1.0;
  P = gsl_cdf_beta_P (p, a, b);

  return P;
}

/*
 * Pr ( X > k ).
 */

double
gsl_cdf_negative_binomial_Q (const unsigned int k, const double p, const double n)
{
  double Q;
  double a;
  double b;

  if (p > 1.0 || p < 0.0)
    {
      CDF_ERROR ("p < 0 or p > 1", GSL_EDOM);
    }

  if (n < 0)
    {
      CDF_ERROR ("n < 0", GSL_EDOM);
    }

  a = (double) n;
  b = (double) k + 1.0;
  Q = gsl_cdf_beta_Q (p, a, b);

  return Q;
}


/* diff/test.c
 *
 * Copyright (C) 2000 David Morrison
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_diff.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_test.h>

double
f1 (double x, void *params)
{
  return exp (x);
}

double
df1 (double x, void *params)
{
  return exp (x);
}

double
f2 (double x, void *params)
{
  if (x >= 0.0)
    {
      return x * sqrt (x);
    }
  else
    {
      return 0.0;
    }
}

double
df2 (double x, void *params)
{
  if (x >= 0.0)
    {
      return 1.5 * sqrt (x);
    }
  else
    {
      return 0.0;
    }
}

double
f3 (double x, void *params)
{
  if (x != 0.0)
    {
      return sin (1 / x);
    }
  else
    {
      return 0.0;
    }
}

double
df3 (double x, void *params)
{
  if (x != 0.0)
    {
      return -cos (1 / x) / (x * x);
    }
  else
    {
      return 0.0;
    }
}

double
f4 (double x, void *params)
{
  return exp (-x * x);
}

double
df4 (double x, void *params)
{
  return -2.0 * x * exp (-x * x);
}

double
f5 (double x, void *params)
{
  return x * x;
}

double
df5 (double x, void *params)
{
  return 2.0 * x;
}

double
f6 (double x, void *params)
{
  return 1.0 / x;
}

double
df6 (double x, void *params)
{
  return -1.0 / (x * x);
}

typedef int (diff_fn) (const gsl_function * f, double x, double * res, double *abserr);

void
test (diff_fn * diff, gsl_function * f, gsl_function * df, double x,
      const char * desc)
{
  double result, abserr;
  double expected = GSL_FN_EVAL (df, x);
  (*diff) (f, x, &result, &abserr);
  gsl_test_abs (result, expected, abserr, desc);
  gsl_test (fabs(result-expected) >  abserr, "%s, valid error estimate", desc);
}

int
main ()
{
  gsl_function F1, DF1, F2, DF2, F3, DF3, F4, DF4, F5, DF5, F6, DF6;

  F1.function = &f1;
  DF1.function = &df1;

  F2.function = &f2;
  DF2.function = &df2;

  F3.function = &f3;
  DF3.function = &df3;

  F4.function = &f4;
  DF4.function = &df4;

  F5.function = &f5;
  DF5.function = &df5;

  F6.function = &f6;
  DF6.function = &df6;

  test (&gsl_diff_central, &F1, &DF1, 1.0, "exp(x), x=1, central diff");
  test (&gsl_diff_forward, &F1, &DF1, 1.0, "exp(x), x=1, forward diff");
  test (&gsl_diff_backward, &F1, &DF1, 1.0, "exp(x), x=1, backward diff");

  test (&gsl_diff_central, &F2, &DF2, 0.1, "x^(3/2), x=0.1, central diff");
  test (&gsl_diff_forward, &F2, &DF2, 0.1, "x^(3/2), x=0.1, forward diff");
  test (&gsl_diff_backward, &F2, &DF2, 0.1, "x^(3/2), x=0.1, backward diff");

  test (&gsl_diff_central, &F3, &DF3, 0.45, "sin(1/x), x=0.45, central diff");
  test (&gsl_diff_forward, &F3, &DF3, 0.45, "sin(1/x), x=0.45, forward diff");
  test (&gsl_diff_backward, &F3, &DF3, 0.45, "sin(1/x), x=0.45, backward diff");

  test (&gsl_diff_central, &F4, &DF4, 0.5, "exp(-x^2), x=0.5, central diff");
  test (&gsl_diff_forward, &F4, &DF4, 0.5, "exp(-x^2), x=0.5, forward diff");
  test (&gsl_diff_backward, &F4, &DF4, 0.5, "exp(-x^2), x=0.5, backward diff");

  test (&gsl_diff_central, &F5, &DF5, 0.0, "x^2, x=0, central diff");
  test (&gsl_diff_forward, &F5, &DF5, 0.0, "x^2, x=0, forward diff");
  test (&gsl_diff_backward, &F5, &DF5, 0.0, "x^2, x=0, backward diff");

  test (&gsl_diff_central, &F6, &DF6, 10.0, "1/x, x=10, central diff");
  test (&gsl_diff_forward, &F6, &DF6, 10.0, "1/x, x=10, forward diff");
  test (&gsl_diff_backward, &F6, &DF6, 10.0, "1/x, x=10, backward diff");

  exit (gsl_test_summary ());
}