static const char help[] =
"Solves elasto-plastic torsion problem in 2D using SNESVI.  Option prefix -el_.\n"
"Equation is  - grad^2 u = f(x,y)  where  f(x,y) = 2 C.  Zero Dirichlet\n"
"boundary conditions.  Domain Omega = (0,1)x(0,1).  Constraint is upper bound\n"
"   u(x,y) <= dist((x,y), bdry Omega) = psi(x,y).\n"
"At locations where the constraint is active, the material experiences plastic\n"
"failure.  Where inactive, the equation represents the elasticity.  As with\n"
"related codes ../obstacle.c and dam.c the code reuses the residual and\n"
"Jacobian evaluation code from ch6/.\n"
"Reference: R. Kornhuber (1994) 'Monotone multigrid methods for elliptic\n"
"variational inequalities I', Numerische Mathematik, 69(2), 167-184.\n\n";

#include <petsc.h>
#include "../../ch6/poissonfunctions.h"

// z = psi(x,y) = dist((x,y), bdry Omega)  is the upper obstacle
double psi(double x, double y) {
    if (y <= x)
        if (y <= 1.0 - x)
            return y;
        else
            return 1.0 - x;
    else
        if (y <= 1.0 - x)
            return x;
        else
            return 1.0 - y;
}

double zero(double x, double y, double z, void *ctx) {
    return 0.0;
}

static double C = 2.5;
double f_fcn(double x, double y, double z, void *ctx) {
    return 2.0 * C;
}

extern PetscErrorCode FormBounds(SNES, Vec, Vec);

int main(int argc,char **argv) {
  PetscErrorCode ierr;
  DM             da, da_after;
  SNES           snes;
  Vec            u_initial, u;
  PoissonCtx     user;
  SNESConvergedReason reason;
  int            snesits;
  double         lflops,flops;
  DMDALocalInfo  info;

  PetscInitialize(&argc,&argv,NULL,help);

  ierr = PetscOptionsBegin(PETSC_COMM_WORLD,"el_",
                           "elasto-plastic torsion solver options",""); CHKERRQ(ierr);
  ierr = PetscOptionsReal("-C","f(x,y)=2C is source term",
                          "elasto.c",C,&C,NULL); CHKERRQ(ierr);
  ierr = PetscOptionsEnd(); CHKERRQ(ierr);

  ierr = DMDACreate2d(PETSC_COMM_WORLD,
      DM_BOUNDARY_NONE, DM_BOUNDARY_NONE, DMDA_STENCIL_STAR,
      3,3,                       // override with -da_grid_x,_y
      PETSC_DECIDE,PETSC_DECIDE, // num of procs in each dim
      1,1,NULL,NULL,             // dof = 1 and stencil width = 1
      &da);CHKERRQ(ierr);
  ierr = DMSetFromOptions(da); CHKERRQ(ierr);
  ierr = DMSetUp(da); CHKERRQ(ierr);
  ierr = DMDASetUniformCoordinates(da,0.0,1.0,0.0,1.0,-1.0,-1.0);CHKERRQ(ierr);

  user.cx = 1.0;
  user.cy = 1.0;
  user.cz = 1.0;
  user.g_bdry = &zero;
  user.f_rhs = &f_fcn;
  user.addctx = NULL;
  ierr = DMSetApplicationContext(da,&user);CHKERRQ(ierr);

  ierr = SNESCreate(PETSC_COMM_WORLD,&snes);CHKERRQ(ierr);
  ierr = SNESSetDM(snes,da);CHKERRQ(ierr);
  ierr = SNESSetApplicationContext(snes,&user);CHKERRQ(ierr);

  ierr = SNESSetType(snes,SNESVINEWTONRSLS);CHKERRQ(ierr);
  ierr = SNESVISetComputeVariableBounds(snes,&FormBounds);CHKERRQ(ierr);

  // reuse residual and jacobian from ch6/
  ierr = DMDASNESSetFunctionLocal(da,INSERT_VALUES,
             (DMDASNESFunction)Poisson2DFunctionLocal,&user); CHKERRQ(ierr);
  ierr = DMDASNESSetJacobianLocal(da,
             (DMDASNESJacobian)Poisson2DJacobianLocal,&user); CHKERRQ(ierr);
  ierr = SNESSetFromOptions(snes);CHKERRQ(ierr);

  // initial iterate is zero
  ierr = DMCreateGlobalVector(da,&u_initial);CHKERRQ(ierr);
  ierr = VecSet(u_initial,0.0); CHKERRQ(ierr);

  /* solve; then get solution and DM after solution*/
  ierr = SNESSolve(snes,NULL,u_initial);CHKERRQ(ierr);
  ierr = VecDestroy(&u_initial); CHKERRQ(ierr);
  ierr = DMDestroy(&da); CHKERRQ(ierr);
  ierr = SNESGetDM(snes,&da_after); CHKERRQ(ierr);
  ierr = SNESGetSolution(snes,&u); CHKERRQ(ierr); /* do not destroy u */

  /* performance measures */
  ierr = SNESGetConvergedReason(snes,&reason); CHKERRQ(ierr);
  if (reason <= 0) {
      ierr = PetscPrintf(PETSC_COMM_WORLD,
          "WARNING: SNES not converged ... use -snes_converged_reason to check\n"); CHKERRQ(ierr);
  }
  ierr = SNESGetIterationNumber(snes,&snesits); CHKERRQ(ierr);
  ierr = PetscGetFlops(&lflops); CHKERRQ(ierr);
  ierr = MPI_Allreduce(&lflops,&flops,1,MPI_DOUBLE,MPI_SUM,PETSC_COMM_WORLD); CHKERRQ(ierr);
  ierr = DMDAGetLocalInfo(da_after,&info); CHKERRQ(ierr);
  ierr = PetscPrintf(PETSC_COMM_WORLD,
      "done on %4d x %4d grid; total flops = %.3e; SNES iterations %d\n",
      info.mx,info.my,flops,snesits); CHKERRQ(ierr);

  SNESDestroy(&snes);
  return PetscFinalize();
}

// for call-back: tell SNESVI we want  -infty < u <= psi
PetscErrorCode FormBounds(SNES snes, Vec Xl, Vec Xu) {
  PetscErrorCode ierr;
  DM            da;
  DMDALocalInfo info;
  int           i, j;
  double        **aXu, dx, dy, x, y;
  ierr = VecSet(Xu,PETSC_NINFINITY);CHKERRQ(ierr);
  ierr = SNESGetDM(snes,&da);CHKERRQ(ierr);
  ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
  dx = 1.0 / (PetscReal)(info.mx-1);
  dy = 1.0 / (PetscReal)(info.my-1);
  ierr = DMDAVecGetArray(da, Xu, &aXu);CHKERRQ(ierr);
  for (j=info.ys; j<info.ys+info.ym; j++) {
    y = j * dy;
    for (i=info.xs; i<info.xs+info.xm; i++) {
      x = i * dx;
      aXu[j][i] = psi(x,y);
    }
  }
  ierr = DMDAVecRestoreArray(da, Xu, &aXu);CHKERRQ(ierr);
  return 0;
}


//
//  syr2k.h
//  Linear Algebra Template Library
//
//  Created by Rodney James on 1/4/12.
//  Copyright (c) 2012 University of Colorado Denver. All rights reserved.
//

#ifndef _syr2k_h
#define _syr2k_h

/// @file syr2k.h Performs multiplication of two matrices resulting in a symmetric matrix.

#include <cctype>
#include "latl.h"

namespace LATL
{
   /// @brief Performs multiplcation of real matrices resulting in a symmetric matrix.
   ///
   /// For real matrices A and B, real symmetric matrix C, and real scalars alpha and beta
   ///
   ///        C := alpha*A*B'+alpha*B*A'+beta*C
   /// or
   ///
   ///        C := alpha*A'*B+alpha*B'*A+beta*C
   /// is computed.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param uplo Specifies whether the upper or lower triangular part of the symmetric matrix C
   /// is to be referenced:
   ///
   ///        if uplo = 'U' or 'u' then C is upper triangular,
   ///        if uplo = 'L' or 'l' then C is lower triangular.
   /// @param trans Specifies the operation to be perfomed as follows:
   ///
   ///        if trans = 'N' or 'n' then C := alpha*A*B'+alpha*B*A'+beta*C
   ///        if trans = 'T' or 't' then C := alpha*A'*B+alpha*B'*A+beta*C
   ///        if trans = 'C' or 'c' then C := alpha*A'*B+alpha*B'*A+beta*C
   /// @param n Specifies the order of the complex Hermitian matrix C.  n>=0
   /// @param k Specifies the other dimension of the complex matrices A and B.
   ///
   ///        if trans = 'N' or 'n' then A and B are n-by-k
   ///        if trans = 'T' or 't' then A and B are k-by-n
   ///        if trans = 'C' or 'c' then A and B are k-by-n
   /// @param alpha Real scalar.
   /// @param A Pointer to real matrix.
   /// @param ldA Column length of the matrix A. If trans = 'N' or 'n' ldA>=n, otherwise ldA>=k.
   /// @param B Pointer to real matrix.
   /// @param ldB Column length of the matrix B. If trans = 'N' or 'n' ldB>=n, otherwise ldB>=k.
   /// @param beta Real scalar.
   /// @param C Pointer to real symmetric n-by-n matrix C.
   /// Only the upper or lower triangular part of C is referenced, depending on the value of uplo above.
   /// @param ldC Column length of the matrix C.  ldC>=n
   /// @ingroup BLAS

   template <typename real_t>
   int SYR2K(char uplo, char trans, int_t n, int_t k, real_t alpha, real_t *A, int_t ldA, real_t *B, int_t ldB, real_t beta, real_t *C, int_t ldC)
   {
      using std::toupper;

      const real_t zero(0.0);
      const real_t one(1.0);
      int_t i,j,l;
      real_t *a,*b,*c,*at,*bt;
      real_t s,t;

      uplo=toupper(uplo);
      trans=toupper(trans);

      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -2;
      else if(n<0)
         return -3;
      else if(k<0)
         return -4;
      else if(ldA<((trans=='N')?n:k))
         return -7;
      else if(ldB<((trans=='N')?n:k))
         return -9;
      else if(ldC<n)
         return -12;
      else if((n==0)||(((alpha==zero)||(k==0))&&(beta==one)))
         return 0;

      if(alpha==zero)
      {
         if(uplo=='U')
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=0;i<=j;i++)
                  c[i]*=beta;
               c+=ldC;
            }

         }
         else
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=j;i<n;i++)
                  c[i]*=beta;
               c+=ldC;
            }
         }
      }
      else if(trans=='N')
      {
         if(uplo=='U')
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=0;i<=j;i++)
                  c[i]*=beta;
               a=A;
               b=B;
               for(l=0;l<k;l++)
               {
                  s=alpha*b[j];
                  t=alpha*a[j];
                  for(i=0;i<=j;i++)
                     c[i]+=s*a[i]+t*b[i];
                  a+=ldA;
                  b+=ldB;
               }
               c+=ldC;
            }
         }
         else
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=j;i<n;i++)
                  c[i]*=beta;
               a=A;
               b=B;
               for(l=0;l<k;l++)
               {
                  s=alpha*b[j];
                  t=alpha*a[j];
                  for(i=j;i<n;i++)
                     c[i]+=s*a[i]+t*b[i];
                  a+=ldA;
                  b+=ldB;
               }
               c+=ldC;
            }
         }
      }
      else
      {
         if(uplo=='U')
         {
            c=C;
            at=A;
            bt=B;
            for(j=0;j<n;j++)
            {
               a=A;
               b=B;
               for(i=0;i<=j;i++)
               {
                  s=zero;
                  t=zero;
                  for(l=0;l<k;l++)
                  {
                     s+=b[l]*at[l];
                     t+=a[l]*bt[l];
                  }
                  c[i]=alpha*t+alpha*s+beta*c[i];
                  a+=ldA;
                  b+=ldB;
               }
               at+=ldA;
               bt+=ldB;
               c+=ldC;
            }
         }
         else
         {
            c=C;
            at=A;
            bt=B;
            for(j=0;j<n;j++)
            {
               a=A+j*ldA;
               b=B+j*ldB;
               for(i=j;i<n;i++)
               {
                  s=zero;
                  t=zero;
                  for(l=0;l<k;l++)
                  {
                     s+=b[l]*at[l];
                     t+=a[l]*bt[l];
                  }
                  c[i]=alpha*t+alpha*s+beta*c[i];
                  a+=ldA;
                  b+=ldB;
               }
               at+=ldA;
               bt+=ldB;
               c+=ldC;
            }
         }
      }
      return 0;
   }

   /// @brief Performs multiplcation of complex matrices resulting in a symmetric matrix.
   ///
   /// For complex matrices A and B, complex Hermitian matrix C, and complex scalars alpha and beta
   ///
   ///        C := alpha*A*B.'+alpha*B*A.'+beta*C
   /// or
   ///
   ///        C := alpha*A.'*B+alpha*B.'*A+beta*C
   /// is computed.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param uplo Specifies whether the upper or lower triangular part of the symmetric matrix C
   /// is to be referenced:
   ///
   ///        if uplo = 'U' or 'u' then C is upper triangular,
   ///        if uplo = 'L' or 'l' then C is lower triangular.
   /// @param trans Specifies the operation to be perfomed as follows:
   ///
   ///        if trans = 'N' or 'n' then C := alpha*A*B.'+alpha*B*A.'+beta*C
   ///        if trans = 'T' or 'T' then C := alpha*A.'*B+alpha*B.'*A+beta*C
   /// @param n Specifies the order of the complex symmetric matrix C.  n>=0
   /// @param k Specifies the other dimension of the complex matrices A and B.
   ///
   ///        if trans = 'N' or 'n' then A and B are n-by-k
   ///        if trans = 'T' or 't' then A and B are k-by-n
   /// @param alpha Complex scalar.
   /// @param A Pointer to complex matrix.
   /// @param ldA Column length of the matrix A. If trans = 'N' or 'n' ldA>=n, otherwise ldA>=k.
   /// @param B Pointer to complex matrix.
   /// @param ldB Column length of the matrix B. If trans = 'N' or 'n' ldB>=n, otherwise ldB>=k.
   /// @param beta Complex scalar.
   /// @param C Pointer to complex symmetric n-by-n matrix C.
   /// Only the upper or lower triangular part of C is referenced, depending on the value of uplo above.
   /// @param ldC Column length of the matrix C.  ldC>=n
   /// @ingroup BLAS

   template <typename real_t>
   int SYR2K(char uplo, char trans, int_t n, int_t k, complex<real_t> alpha, complex<real_t> *A, int_t ldA, complex<real_t> *B, int_t ldB, complex<real_t> beta, complex<real_t> *C, int_t ldC)
   {
      using std::toupper;

      const complex<real_t> zero(0.0,0.0);
      const complex<real_t> one(1.0,0.0);
      int_t i,j,l;
      complex<real_t> *a,*b,*c,*at,*bt;
      complex<real_t> s,t;

      uplo=toupper(uplo);
      trans=toupper(trans);

      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if((trans!='N')&&(trans!='T'))
         return -2;
      else if(n<0)
         return -3;
      else if(k<0)
         return -4;
      else if(ldA<((trans=='N')?n:k))
         return -7;
      else if(ldB<((trans=='N')?n:k))
         return -9;
      else if(ldC<n)
         return -12;
      else if((n==0)||(((alpha==zero)||(k==0))&&(beta==one)))
         return 0;

      if(alpha==zero)
      {
         if(uplo=='U')
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=0;i<=j;i++)
                  c[i]*=beta;
               c+=ldC;
            }

         }
         else
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=j;i<n;i++)
                  c[i]*=beta;
               c+=ldC;
            }
         }
      }
      else if(trans=='N')
      {
         if(uplo=='U')
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=0;i<=j;i++)
                  c[i]*=beta;
               a=A;
               b=B;
               for(l=0;l<k;l++)
               {
                  s=alpha*b[j];
                  t=alpha*a[j];
                  for(i=0;i<=j;i++)
                     c[i]+=s*a[i]+t*b[i];
                  a+=ldA;
                  b+=ldB;
               }
               c+=ldC;
            }
         }
         else
         {
            c=C;
            for(j=0;j<n;j++)
            {
               for(i=j;i<n;i++)
                  c[i]*=beta;
               a=A;
               b=B;
               for(l=0;l<k;l++)
               {
                  s=alpha*b[j];
                  t=alpha*a[j];
                  for(i=j;i<n;i++)
                     c[i]+=s*a[i]+t*b[i];
                  a+=ldA;
                  b+=ldB;
               }
               c+=ldC;
            }
         }
      }
      else
      {
         if(uplo=='U')
         {
            c=C;
            at=A;
            bt=B;
            for(j=0;j<n;j++)
            {
               a=A;
               b=B;
               for(i=0;i<=j;i++)
               {
                  s=zero;
                  t=zero;
                  for(l=0;l<k;l++)
                  {
                     s+=b[l]*at[l];
                     t+=a[l]*bt[l];
                  }
                  c[i]=alpha*t+alpha*s+beta*c[i];
                  a+=ldA;
                  b+=ldB;
               }
               at+=ldA;
               bt+=ldB;
               c+=ldC;
            }
         }
         else
         {
            c=C;
            at=A;
            bt=B;
            for(j=0;j<n;j++)
            {
               a=A+j*ldA;
               b=B+j*ldB;
               for(i=j;i<n;i++)
               {
                  s=zero;
                  t=zero;
                  for(l=0;l<k;l++)
                  {
                     s+=b[l]*at[l];
                     t+=a[l]*bt[l];
                  }
                  c[i]=alpha*t+alpha*s+beta*c[i];
                  a+=ldA;
                  b+=ldB;
               }
               at+=ldA;
               bt+=ldB;
               c+=ldC;
            }
         }
      }
      return 0;
   }

#ifdef __latl_cblas
#include <cblas.h>

   template <> int SYR2K<float>(char uplo, char trans, int_t n, int_t k, float alpha, float *A, int_t ldA, float *B, int_t ldB, float beta, float *C, int_t ldC)
   {
      using std::toupper;
      uplo=toupper(uplo);
      trans=toupper(trans);
      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -2;
      else if(n<0)
         return -3;
      else if(k<0)
         return -4;
      else if(ldA<((trans=='N')?n:k))
         return -7;
      else if(ldB<((trans=='N')?n:k))
         return -9;
      else if(ldC<n)
         return -12;

      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;
      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);

      cblas_ssyr2k(CblasColMajor,Uplo,Trans,n,k,alpha,A,ldA,B,ldB,beta,C,ldC);

      return 0;
   }

   template <> int SYR2K<double>(char uplo, char trans, int_t n, int_t k, double alpha, double *A, int_t ldA, double *B, int_t ldB, double beta, double *C, int_t ldC)
   {
      using std::toupper;
      uplo=toupper(uplo);
      trans=toupper(trans);
      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -2;
      else if(n<0)
         return -3;
      else if(k<0)
         return -4;
      else if(ldA<((trans=='N')?n:k))
         return -7;
      else if(ldB<((trans=='N')?n:k))
         return -9;
      else if(ldC<n)
         return -12;

      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;
      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);

      cblas_dsyr2k(CblasColMajor,Uplo,Trans,n,k,alpha,A,ldA,B,ldB,beta,C,ldC);

      return 0;
   }

   template <> int SYR2K<float>(char uplo, char trans, int_t n, int_t k, complex<float> alpha, complex<float> *A, int_t ldA, complex<float> *B, int_t ldB, complex<float> beta, complex<float> *C, int_t ldC)
   {
      using std::toupper;
      uplo=toupper(uplo);
      trans=toupper(trans);
      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if((trans!='N')&&(trans!='T'))
         return -2;
      else if(n<0)
         return -3;
      else if(k<0)
         return -4;
      else if(ldA<((trans=='N')?n:k))
         return -7;
      else if(ldB<((trans=='N')?n:k))
         return -9;
      else if(ldC<n)
         return -12;

      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;
      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);

      cblas_csyr2k(CblasColMajor,Uplo,Trans,n,k,&alpha,A,ldA,B,ldB,&beta,C,ldC);

      return 0;
   }

   template <> int SYR2K<double>(char uplo, char trans, int_t n, int_t k, complex<double> alpha, complex<double> *A, int_t ldA, complex<double> *B, int_t ldB, complex<double> beta, complex<double> *C, int_t ldC)
   {
      using std::toupper;
      uplo=toupper(uplo);
      trans=toupper(trans);
      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if((trans!='N')&&(trans!='T'))
         return -2;
      else if(n<0)
         return -3;
      else if(k<0)
         return -4;
      else if(ldA<((trans=='N')?n:k))
         return -7;
      else if(ldB<((trans=='N')?n:k))
         return -9;
      else if(ldC<n)
         return -12;

      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;
      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);

      cblas_zsyr2k(CblasColMajor,Uplo,Trans,n,k,&alpha,A,ldA,B,ldB,&beta,C,ldC);

      return 0;
   }

#endif
}

#endif


/*
    This code is written by <albanese@fbk.it>.
    (C) 2008 mlpy Developers.

    See DWT in the GSL Library.

    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 3 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, see <http://www.gnu.org/licenses/>.
*/

#include <Python.h>
#include <numpy/arrayobject.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_wavelet.h>
#include <gsl/gsl_math.h>

static PyObject *dwt_dwt(PyObject *self, PyObject *args, PyObject *keywds)
{
  PyObject *x = NULL; PyObject *xcopy = NULL;

  char wf;
  int k, n;
  double *_xcopy;
  PyObject *centered = Py_False;
  gsl_wavelet *w;
  gsl_wavelet_workspace *work;

  /* Parse Tuple*/
  static char *kwlist[] = {"x", "wf", "k", "centered", NULL};
  if (!PyArg_ParseTupleAndKeywords(args, keywds, "Oci|O", kwlist,
				   &x, &wf, &k, &centered))
    return NULL;

  /* Build xcopy */
  xcopy = PyArray_FROM_OTF(x, NPY_DOUBLE, NPY_OUT_ARRAY | NPY_ENSURECOPY);
  if (xcopy == NULL) return NULL;

  n = (int) PyArray_DIM(xcopy, 0);
  _xcopy = (double *) PyArray_DATA(xcopy);

  switch (wf)
    {
    case 'd':
      if (centered == Py_True)
	w = gsl_wavelet_alloc (gsl_wavelet_daubechies_centered, k);
      else
	w = gsl_wavelet_alloc (gsl_wavelet_daubechies, k);
      break;

    case 'h':
      if (centered == Py_True)
	w = gsl_wavelet_alloc (gsl_wavelet_haar_centered, k);
      else
	w = gsl_wavelet_alloc (gsl_wavelet_haar, k);
      break;

    case 'b':
      if (centered == Py_True)
	w = gsl_wavelet_alloc (gsl_wavelet_bspline_centered, k);
      else
	w = gsl_wavelet_alloc (gsl_wavelet_bspline, k);
      break;

    default:
      PyErr_SetString(PyExc_ValueError, "wavelet family is not valid");
      return NULL;
    }

  work = gsl_wavelet_workspace_alloc (n);

  gsl_wavelet_transform_forward (w, _xcopy, 1, n, work);

  gsl_wavelet_free (w);
  gsl_wavelet_workspace_free (work);

  return Py_BuildValue("N", xcopy);
}

static PyObject *dwt_idwt(PyObject *self, PyObject *args, PyObject *keywds)
{
  PyObject *x = NULL; PyObject *xcopy = NULL;

  char wf;
  int k, n;
  double *_xcopy;
  PyObject *centered = Py_False;
  gsl_wavelet *w;
  gsl_wavelet_workspace *work;

  /* Parse Tuple*/
  static char *kwlist[] = {"X", "wf", "k", "centered", NULL};
  if (!PyArg_ParseTupleAndKeywords(args, keywds, "Oci|O", kwlist,
				   &x, &wf, &k, &centered))
    return NULL;

  /* Build xcopy */
  xcopy = PyArray_FROM_OTF(x, NPY_DOUBLE, NPY_OUT_ARRAY | NPY_ENSURECOPY);
  if (xcopy == NULL) return NULL;

  n = (int) PyArray_DIM(xcopy, 0);
  _xcopy = (double *) PyArray_DATA(xcopy);

  switch (wf)
    {
    case 'd':
      if (centered == Py_True)
	w = gsl_wavelet_alloc (gsl_wavelet_daubechies_centered, k);
      else
	w = gsl_wavelet_alloc (gsl_wavelet_daubechies, k);
      break;

    case 'h':
      if (centered == Py_True)
	w = gsl_wavelet_alloc (gsl_wavelet_haar_centered, k);
      else
	w = gsl_wavelet_alloc (gsl_wavelet_haar, k);
      break;

    case 'b':
      if (centered == Py_True)
	w = gsl_wavelet_alloc (gsl_wavelet_bspline_centered, k);
      else
	w = gsl_wavelet_alloc (gsl_wavelet_bspline, k);
      break;

    default:
      PyErr_SetString(PyExc_ValueError, "wavelet family is not valid");
      return NULL;
    }

  work = gsl_wavelet_workspace_alloc (n);

  gsl_wavelet_transform_inverse (w, _xcopy, 1, n, work);

  gsl_wavelet_free (w);
  gsl_wavelet_workspace_free (work);

  return Py_BuildValue("N", xcopy);
}

/* Doc strings: */
static char module_doc[]  = "Discrete Wavelet Transform Module from GSL";

static char dwt_dwt_doc[] =
  "Discrete Wavelet Tranform\n\n"
  ":Parameters:\n"
  "   x : 1d array_like object (the length is restricted to powers of two)\n"
  "      data\n"
  "   wf : string ('d': daubechies, 'h': haar, 'b': bspline)\n"
  "      wavelet family\n"
  "   k : integer\n"
  "      member of the wavelet family\n\n"
  "         * daubechies : k = 4, 6, ..., 20 with k even\n"
  "         * haar : the only valid choice of k is k = 2\n"
  "         * bspline : k = 103, 105, 202, 204, 206, 208, 301, 303, 305 307, 309\n"
  "   centered : bool\n"
  "      align the coefficients of the various sub-bands on edges.\n"
  "      Thus the resulting visualization of the coefficients of the\n"
  "      wavelet transform in the phase plane is easier to understand.\n\n"
  ":Returns:\n"
  "  X : 1d numpy array\n"
  "    discrete wavelet transformed data\n\n"
  "Example\n\n"
  ">>> import numpy as np\n"
  ">>> import mlpy.wavelet as wave\n"
  ">>> x = np.array([1,2,3,4,3,2,1,0])\n"
  ">>> wave.dwt(x=x, wf='d', k=6)\n"
  "array([ 5.65685425,  3.41458985,  0.29185347, -0.29185347, -0.28310081,\n"
  "       -0.07045258,  0.28310081,  0.07045258])\n";

static char dwt_idwt_doc[] =
  "Inverse Discrete Wavelet Tranform\n\n"
  ":Parameters:\n"
  "   X : 1d array_like object\n"
  "      discrete wavelet transformed data\n"
  "   wf : string ('d': daubechies, 'h': haar, 'b': bspline)\n"
  "      wavelet type\n"
  "   k : integer\n"
  "      member of the wavelet family\n\n"
  "         * daubechies : k = 4, 6, ..., 20 with k even\n"
  "         * haar : the only valid choice of k is k = 2\n"
  "         * bspline : k = 103, 105, 202, 204, 206, 208, 301, 303, 305 307, 309\n\n"
  "   centered : bool\n"
  "      if the coefficients are aligned\n\n"
  ":Returns:\n"
  "   x : 1d numpy array\n"
  "      data\n\n"
  "Example:\n\n"
  ">>> import numpy as np\n"
  ">>> import mlpy.wavelet as wave\n"
  ">>> X = np.array([ 5.65685425,  3.41458985,  0.29185347, -0.29185347, -0.28310081,\n"
  "...               -0.07045258,  0.28310081,  0.07045258])\n"
  ">>> wave.idwt(X=X, wf='d', k=6)\n"
  "array([  1.00000000e+00,   2.00000000e+00,   3.00000000e+00,\n"
  "         4.00000000e+00,   3.00000000e+00,   2.00000000e+00,\n"
  "         1.00000000e+00,  -3.53954610e-09])\n";

/* Method table */
static PyMethodDef dwt_methods[] = {
  {"dwt",
   (PyCFunction)dwt_dwt,
   METH_VARARGS | METH_KEYWORDS,
   dwt_dwt_doc},
  {"idwt",
   (PyCFunction)dwt_idwt,
   METH_VARARGS | METH_KEYWORDS,
   dwt_idwt_doc},
  {NULL, NULL, 0, NULL}
};

#if PY_MAJOR_VERSION >= 3

static struct PyModuleDef moduledef = {
  PyModuleDef_HEAD_INIT,
  "_dwt",
  module_doc,
  -1,
  dwt_methods,
  NULL, NULL, NULL, NULL
};

PyObject *PyInit__dwt(void)
{
  PyObject *m;
  m = PyModule_Create(&moduledef);
  if (!m) {
    return NULL;
  }

  import_array();

  return m;
}

#else

PyMODINIT_FUNC init_dwt(void)
{
  PyObject *m;

  m = Py_InitModule3("_dwt", dwt_methods, module_doc);
  if (m == NULL) {
    return;
  }

  import_array();
}

#endif


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_multifit.h>

//DEFINES
#define SUCCESS 1
#define FAILURE -1
#define PROGRESS "===================="
#define PRG_WIDTH 20

//FUNCTIONS DECLARATION
int dcca_comp(double *, double *, double *, int, int, int, int, double *, int);
int rows_number(char *);
double mean(double *, int, int);
void cumsum(double *, double *, int);
void slice_vec(double *, double *, int, int);
void polynomialFit(int, int, double *, double *, double *);

//MAIN
int main(int argc, char **argv)
{
    //inputs
    int input_args = 7;
    if(argc < input_args){
        printf("Not enough input arguments!\n");
        return FAILURE;
    }else if(argc > input_args){
        printf("Too many input arguments!\n");
        return FAILURE;
    }
    char file_name1[255]; //path+name for the first file
    memset(file_name1, 0x00, sizeof(file_name1));
    sprintf(file_name1, "%s", argv[1]);
    char file_name2[255]; //path+name for the second file
    memset(file_name2, 0x00, sizeof(file_name2));
    sprintf(file_name2, "%s", argv[2]);
    int smallest_win_size = atoi(argv[3]); //minimum size for the sliding window
    int biggest_win_num = atoi(argv[4]); //number of windows in the last step
    int pol_ord = atoi(argv[5]); //polynomial order for fit
    char path_out[255]; //path+name for the output file
    memset(path_out, 0x00, sizeof(path_out));
    sprintf(path_out, "%s/rho_mw=%d_Mw=%d_pol=%d.txt", argv[6], smallest_win_size, biggest_win_num, pol_ord);
    //files length
    int N1 = rows_number(file_name1);
    if(N1 == FAILURE){
        printf("Cannot open input file (1).\n");
        return FAILURE;
    }
    int N2 = rows_number(file_name2);
    if(N2 == FAILURE){
        printf("Cannot open input file (2).\n");
        return FAILURE;
    }
    //time series
    double *ts1, *ts2;
    ts1 = calloc(N1, sizeof(double));
    if(!ts1){
        printf("MALLOC ERROR (ts1)\n");
        return FAILURE;
    }
    ts2 = calloc(N2, sizeof(double));
    if(!ts2){
        printf("MALLOC ERROR (ts2)\n");
        return FAILURE;
    }
    FILE *f1, *f2;
    f1 = fopen(file_name1, "r");
    if(!f1){
        printf("Cannot open input file (1).\n");
        return FAILURE;
    }
    for(int i = 0; i < N1; i++)
        fscanf(f1, "%lf", ts1+i);
    fclose(f1);
    f2 = fopen(file_name2, "r");
    if(!f2){
        printf("Cannot open input file (2).\n");
        return FAILURE;
    }
    for(int i = 0; i < N2; i++)
        fscanf(f2, "%lf", ts2+i);
    fclose(f2);
    //time series must have the same length
    int L = N1;
    if(N1 < N2){
        printf("Series 2 is longer than series 1. Data length reduced to: <%d>\n", L);
        slice_vec(ts2, ts2, 0, N1-1);
    }else if(N1 > N2){
        slice_vec(ts1, ts1, 0, N2-1);
        L = N2;
        printf("Series 1 is longer than series 2. Data length reduced to: <%d>\n", L);
    }else{
        printf("Data length: <%d>\n", L);
    }
    //time vector
    double *time;
    time = calloc(L, sizeof(double));
    if(!time){
        printf("MALLOC ERROR (time)\n");
        return FAILURE;
    }
    for(int i = 0; i < L; i++)
        time[i] = (double)(i+1);
    //time series minus its mean
    double ts1_ave = mean(ts1, L, L);
    double ts2_ave = mean(ts2, L, L);
    double *ts1_nomean, *ts2_nomean;
    ts1_nomean = calloc(L, sizeof(double));
    if(!ts1_nomean){
        printf("MALLOC ERROR (ts1_nomean)\n");
        return FAILURE;
    }
    ts2_nomean = calloc(L, sizeof(double));
    if(!ts2_nomean){
        printf("MALLOC ERROR (ts2_nomean)\n");
        return FAILURE;
    }
    for(int i = 0; i < L; i++){
        ts1_nomean[i] = ts1[i] - ts1_ave;
        ts2_nomean[i] = ts2[i] - ts2_ave;
    }
    //cumulative sum
    double *ts1_cum, *ts2_cum;
    ts1_cum = calloc(L, sizeof(double));
    if(!ts1_cum){
        printf("MALLOC ERROR (ts1_cum)\n");
        return FAILURE;
    }
    ts2_cum = calloc(L, sizeof(double));
    if(!ts2_cum){
        printf("MALLOC ERROR (ts2_cum)\n");
        return FAILURE;
    }
    printf("Data integration...");
    cumsum(ts1_nomean, ts1_cum, L);
    cumsum(ts2_nomean, ts2_cum, L);
    printf("done!\n");
    //dcca computation
    int biggest_win_size = L / biggest_win_num;
    int win_range = biggest_win_size - smallest_win_size + 1;
    double *F_DCCA, *F_DFA_x, *F_DFA_y, *rho;
    F_DCCA = calloc(win_range, sizeof(double));
    F_DFA_x = calloc(win_range, sizeof(double));
    F_DFA_y = calloc(win_range, sizeof(double));
    rho = calloc(win_range, sizeof(double));
    printf("Computing DCCA between series 1 and series 2...\n");
    int ret = dcca_comp(time, ts1_cum, ts2_cum, L, smallest_win_size, biggest_win_size, pol_ord, F_DCCA, win_range);
    if(ret == FAILURE)
        return FAILURE;
    printf("Computing DCCA between series 1 and series 1...\n");
    ret = dcca_comp(time, ts1_cum, ts1_cum, L, smallest_win_size, biggest_win_size, pol_ord, F_DFA_x, win_range);
    if(ret == FAILURE)
        return FAILURE;
    printf("Computing DCCA between series 2 and series 2...\n");
    ret = dcca_comp(time, ts2_cum, ts2_cum, L, smallest_win_size, biggest_win_size, pol_ord, F_DFA_y, win_range);
    if(ret == FAILURE)
        return FAILURE;
    FILE *fOut;
    fOut = fopen(path_out, "w");
    if(!fOut){
        printf("Cannot open output file.\n");
        return FAILURE;
    }
    printf("Computing rho...");
    for(int i = 0; i < win_range; i++){
        F_DFA_x[i] = sqrt(F_DFA_x[i]);
        F_DFA_y[i] = sqrt(F_DFA_y[i]);
        rho[i] = F_DCCA[i] / (double)(F_DFA_x[i] * F_DFA_y[i]);
        fprintf(fOut, "%lf %lf %lf %lf\n", F_DCCA[i], F_DFA_x[i], F_DFA_y[i], rho[i]);
    }
    printf("done\n");
    printf("Output file...");
    fclose(fOut);
    printf("done\n");
    //free memory
    free(ts1); free(ts2); free(time);
    free(ts1_nomean); free(ts2_nomean);
    free(ts1_cum); free(ts2_cum);
    free(F_DCCA); free(F_DFA_x); free(F_DFA_y); free(rho);
    return 0;
}

//FUNCTIONS
int dcca_comp(double *t, double *y1, double *y2, int N, int min_win_size, int last_win_len, int ord, double *F, int range_dcca)
{
    //fluctuations vector and other arrays
    int F_len = N - min_win_size;
    double *F_nu, *t_fit, *y_fit1, *y_fit2, *diff_vec, *fit_coeffs1, *fit_coeffs2;
    F_nu = calloc(F_len, sizeof(double));
    if(!F_nu){
        printf("MALLOC ERROR (F_nu)\n");
        return FAILURE;
    }
    t_fit = calloc((last_win_len+1), sizeof(double));
    if(!t_fit){
        printf("MALLOC ERROR (t_fit)\n");
        return FAILURE;
    }
    y_fit1 = calloc((last_win_len+1), sizeof(double));
    if(!y_fit1){
        printf("MALLOC ERROR (y_fit1)\n");
        return FAILURE;
    }
    y_fit2 = calloc((last_win_len+1), sizeof(double));
    if(!y_fit2){
        printf("MALLOC ERROR (y_fit2)\n");
        return FAILURE;
    }
    diff_vec = calloc((last_win_len+1), sizeof(double));
    if(!diff_vec){
        printf("MALLOC ERROR (diff_vec)\n");
        return FAILURE;
    }
    fit_coeffs1 = calloc(ord+1, sizeof(double));
    if(!fit_coeffs1){
        printf("MALLOC ERROR (fit_coeffs1)\n");
        return FAILURE;
    }
    fit_coeffs2 = calloc(ord+1, sizeof(double));
    if(!fit_coeffs2){
        printf("MALLOC ERROR (fit_coeffs2)\n");
        return FAILURE;
    }
    //computation
    int s, N_s, start_lim, end_lim;
    for(int i = 0; i < range_dcca; i++){
        double perc = i * 100 / (double)range_dcca;
        int prg = (i * PRG_WIDTH) / range_dcca;
        printf(" [%.*s%*s] %.2lf%%\r", prg, PROGRESS, PRG_WIDTH-prg, "", perc);
        fflush(stdout);
        s = i + min_win_size;
        N_s = N - s;
        for(int v = 0; v < N_s; v++){
            start_lim = v;
            end_lim = v + s;
            slice_vec(t, t_fit, start_lim, end_lim);
            slice_vec(y1, y_fit1, start_lim, end_lim);
            slice_vec(y2, y_fit2, start_lim, end_lim);
            polynomialFit(s+1, ord+1, t_fit, y_fit1, fit_coeffs1);
            polynomialFit(s+1, ord+1, t_fit, y_fit2, fit_coeffs2);
            for(int j = 0; j <= s; j++){
                for(int k = 0; k < ord+1; k++){
                    y_fit1[j] -= fit_coeffs1[k] * pow(t_fit[j], k);
                    y_fit2[j] -= fit_coeffs2[k] * pow(t_fit[j], k);
                }
                diff_vec[j] = y_fit1[j] * y_fit2[j];
            }
            F_nu[v] = mean(diff_vec, s+1, s-1);
        }
        F[i] = mean(F_nu, N_s, N_s);
    }
    printf(" [%s] 100.00%%\r", PROGRESS);
    fflush(stdout);
    printf("\n");
    free(F_nu); free(t_fit); free(y_fit1); free(y_fit2);
    free(diff_vec); free(fit_coeffs1); free(fit_coeffs2);
    return SUCCESS;
}

int rows_number(char *file_name)
{
    FILE *f;
    int stop;
    int lines = 0;
    f = fopen(file_name,"r");
    if(!f){
        printf("Cannot open file %s\n", file_name);
        return FAILURE;
    }
    while(!feof(f)){
        stop = fgetc(f);
        if(stop == '\n')
            lines++;
    }
    fclose(f);
    return lines;
}

double mean(double *vec, int vecL, int L)
{
    double avg = 0.0;
    for(int i = 0; i < vecL; i++)
        avg += vec[i];
    avg /= (double)L;
    return avg;
}

void cumsum(double *vec, double *sum_vec, int L)
{
    sum_vec[0] = vec[0];
    for(int i = 1; i < L; i++)
        sum_vec[i] = sum_vec[i-1] + vec[i];
}

void slice_vec(double *all_vec, double *sliced_vec, int start, int end)
{
    for(int i = 0; i <= (end-start); i++)
        sliced_vec[i] = all_vec[start+i];
}

void polynomialFit(int obs, int degree, double *dx, double *dy, double *store)
{
    gsl_multifit_linear_workspace *ws;
    gsl_matrix *cov, *X;
    gsl_vector *y, *c;
    double chisq;
    int i, j;
    //alloc
    X = gsl_matrix_alloc(obs, degree);
    y = gsl_vector_alloc(obs);
    c = gsl_vector_alloc(degree);
    cov = gsl_matrix_alloc(degree, degree);
    //computation
    for(i = 0; i < obs; i++){
        for(j = 0; j < degree; j++)
            gsl_matrix_set(X, i, j, pow(dx[i], j));
        gsl_vector_set(y, i, dy[i]);
    }
    ws = gsl_multifit_linear_alloc(obs, degree);
    gsl_multifit_linear(X, y, c, cov, &chisq, ws);
    for(i = 0; i < degree; i++)
        store[i] = gsl_vector_get(c, i);
    //free memory
    gsl_multifit_linear_free(ws);
    gsl_matrix_free(X);
    gsl_matrix_free(cov);
    gsl_vector_free(y);
    gsl_vector_free(c);
}


/*
 * Copyright (c) 2009-2010 Simon van Heeringen <s.vanheeringen@ncmls.ru.nl>
 *
 * This module is free software. You can redistribute it and/or modify it under
 * the terms of the MIT License, see the file COPYING included with this
 * distribution.
 *
 * This module contains all the code to compare motifs
 *
 */

#include <Python.h>
#include <math.h>
#include <string.h>
#include <stdio.h>
#include <gsl/gsl_cdf.h>

void fill_matrix(double matrix[][4], PyObject *matrix_o) {
	// Parse a PyObject matrix into a C array
	// Structure of the matrix is as follows: [[freqA, freqC, freqG, freqT], ...]
	int matrix_len = PyList_Size(matrix_o);
	int i,j;
	PyObject *item;
	for (i = 0; i < matrix_len; i++) {

		PyObject *row = PyList_GetItem(matrix_o, i);
		if (!PyList_Check(row)) {
			PyErr_SetString( PyExc_TypeError, "wrong matrix structure");
			return;
		}
		if (PyList_Size(row) != 4) {
			PyErr_SetString( PyExc_TypeError, "4 nucleotides!");
			return;
		}
		for (j = 0; j < 4; j++) {
			item = PyList_GetItem(row, j);
			matrix[i][j] = PyFloat_AsDouble(item);
		}
	}
}

double mean(double array[], double length) {
	double sum = 0;
	int i;
	for (i = 0; i < length; i++) {
		sum += array[i];
	}
	return sum / length;
}

double sum(double array[], double length) {
	double sum = 0;
	int i;
	for (i = 0; i < length; i++) {
		sum += array[i];
	}
	return sum;
}

double wic(double col1[], double col2[]) {
		int n;
		double x,y;
		double log2 = log(2.0);
		double factor = 2.5;
		double score = 0;;
		double score_a = 0.0;
		double score_b = 0.0;
		double pseudo = 0.0000001;

		for (n = 0; n < 4; n++) {
			x = col1[n] * log((col1[n] + pseudo) / 0.25) / log2;
			y = col2[n] * log((col2[n] + pseudo) / 0.25) / log2;
			score += fabs(x - y);
			score_a += x;
			score_b += y;
		}
		return sqrt(score_a * score_b) - factor * score;
}

double matrix_wic_mean(double matrix1[][4], double matrix2[][4], int length) {
	// Return the mean pcc of two matrices
	int i;
	double result[length];

	for (i = 0; i < length; i++ ) {
		result[i] = wic(matrix1[i], matrix2[i]);
	}
	return mean(result, length);
}

double matrix_wic_sum(double matrix1[][4], double matrix2[][4], int length) {
	// Return the mean pcc of two matrices
	int i;
	double result[length];

	for (i = 0; i < length; i++ ) {
		result[i] = wic(matrix1[i], matrix2[i]);
	}
	return sum(result, length);
}

double pcc(double col1[], double col2[]) {
	// Return the Pearson correlation coefficient of two motif columns
	int n;
	double sum1 = 0;
	double sum2 = 0;
	double mean1, mean2;

	for (n = 0; n < 4; n++) {
		sum1 += col1[n];
		sum2 += col2[n];
	}
	mean1 = sum1 / 4;
	mean2 = sum2 / 4;

	float a = 0;
	float x = 0;
	float y = 0;
	for (n = 0; n < 4; n++) {
		if (((col1[n] - mean1) != 0) && ((col2[n] - mean2) != 0)) {
			a += (col1[n] - mean1) * (col2[n] - mean2);
			x += pow((col1[n] - mean1), 2);
			y += pow((col2[n] - mean2), 2);
		}
		else {
			return 0;
		}
	}

	return  a / sqrt(x * y);
}

double matrix_pcc_mean(double matrix1[][4], double matrix2[][4], int length) {
	// Return the mean pcc of two matrices
	int i;
	double result[length];

	for (i = 0; i < length; i++ ) {
		result[i] = pcc(matrix1[i], matrix2[i]);
	}
	return mean(result, length);
}

double ed(double col1[], double col2[]) {
	// Return the euclidian distance of two motif columns
	int n;
	double score = 0;

	for (n = 0; n < 4; n++) {
		score = score + pow((col1[n] - col2[n]), 2);
	}

	return  -sqrt(score);
}

double matrix_ed_mean(double matrix1[][4], double matrix2[][4], int length) {
	// Return the mean euclidian distance of two matrices
	int i;
	double result[length];

	for (i = 0; i < length; i++ ) {
		result[i] = ed(matrix1[i], matrix2[i]);
	}
	return mean(result, length);
}

double distance(double col1[], double col2[]) {
	// Return the distance between two motifs (Harbison et al.)
	int n;
	double d = 0;

	for (n = 0; n < 4; n++) {
		d = d + pow(col1[n] - col2[n], 2);
	}
	d = d * 1 / sqrt(2);

	return  -d;
}

double matrix_distance_mean(double matrix1[][4], double matrix2[][4], int length) {
	// Return the mean distance (Harbison et al.) of two matrices
	int i;
	double result[length];

	for (i = 0; i < length; i++ ) {
		result[i] = distance(matrix1[i], matrix2[i]);
	}
	return mean(result, length);
}

double chisq(double col1[], double col2[]) {
	// Return the distance between two motifs (Harbison et al.)
	int n;
	double sum1 = 0;
	double sum2 = 0;
	double e1, e2;
	double c = 0;

	for (n = 0; n < 4; n++) {
		sum1 += col1[n];
		sum2 += col2[n];
	}

	for (n = 0; n < 4; n++) {
		e1 = (sum1 * (col1[n] + col2[n])) / (sum1 + sum2);
		e2 = (sum2 * (col1[n] + col2[n])) / (sum1 + sum2);
		c += pow((col1[n] - e1), 2) / e1;
		c += pow((col2[n] - e2), 2) / e2;

	}

	return 1 - gsl_cdf_chisq_P(c, 3.0);
}

double matrix_chisq_mean(double matrix1[][4], double matrix2[][4], int length) {
	// Return the mean distance (Harbison et al.) of two matrices
	int i;
	double result[length];

	for (i = 0; i < length; i++ ) {
		result[i] = chisq(matrix1[i], matrix2[i]);
	}
	return mean(result, length);
}

static PyObject * c_metrics_score(PyObject *self, PyObject * args)
{
	PyObject *matrix1_o;
	PyObject *matrix2_o;
	const char *metric;
	const char *combine;
	int matrix1_len;
	int matrix2_len;

	if (!PyArg_ParseTuple(args, "OOss", &matrix1_o, &matrix2_o, &metric, &combine)) {
		PyErr_SetString( PyExc_TypeError, "Error parsing arguments");
		return NULL;
	}

	// Retrieve frequency matrix
	if (!PyList_Check(matrix1_o)) {
		PyErr_SetString( PyExc_TypeError, "Error: missing required argument matrix1");
		return NULL;
	}

	if (!PyList_Check(matrix2_o)) {
		PyErr_SetString( PyExc_TypeError, "Error: missing required argument matrix2");
		return NULL;
	}
	// Fill matrix1
	matrix1_len = PyList_Size(matrix1_o);
	double matrix1[matrix1_len][4];
	fill_matrix(matrix1, matrix1_o);

	// Fill matrix2
	matrix2_len = PyList_Size(matrix2_o);
	double matrix2[matrix2_len][4];
	fill_matrix(matrix2, matrix2_o);

	if (matrix1_len != matrix2_len) {
		PyErr_SetString( PyExc_TypeError, "Error: matrices have different sizes");
		return NULL;
	}
	int i;
	double result[matrix1_len];
	if (!strcmp(metric, "chisq")) {
		for (i = 0; i < matrix1_len; i++ ) {
			result[i] = chisq(matrix1[i], matrix2[i]);
		}
	}
	else if (!strcmp(metric, "wic")) {
		for (i = 0; i < matrix1_len; i++ ) {
			result[i] = wic(matrix1[i], matrix2[i]);
		}
	}
	else if (!strcmp(metric, "ed")) {
		for (i = 0; i < matrix1_len; i++ ) {
			result[i] = ed(matrix1[i], matrix2[i]);
		}
	}
	else if (!strcmp(metric, "pcc")) {
		for (i = 0; i < matrix1_len; i++ ) {
			result[i] = pcc(matrix1[i], matrix2[i]);
		}
	}
	else if (!strcmp(metric, "distance")) {
		for (i = 0; i < matrix1_len; i++ ) {
			result[i] = distance(matrix1[i], matrix2[i]);
		}
	}
	else {
			PyErr_SetString( PyExc_TypeError, "Unknown metric");
			return NULL;
	}

	if (!strcmp(combine, "mean")) {
		return Py_BuildValue("f", mean(result, matrix1_len));
	}
	else if (!strcmp(combine, "sum")) {
		return Py_BuildValue("f", sum(result, matrix1_len));
	}
	else {
			PyErr_SetString( PyExc_TypeError, "Unknown combine");
			return NULL;
	}

}

int get_truncate_len(len1, len2, pos) {
	//

	if (pos < 0) {
		len2 += pos;
	}
	else if (pos > 0) {
		len1 -= pos;
	}

	if (len1 > len2) {
		return len2;
	}
	else {
		return len1;
	}

}

void fill_tmp_matrices(double matrix1[][4], double matrix2[][4], int pos, int l, double tmp_matrix1[][4], double tmp_matrix2[][4]) {
	int start1 = 0;
	int start2 = 0;
	int i, n;

	if (pos < 0) {
		start2 = -pos;
	}
	else {
		start1 = pos;
	}

	for (i = 0; i < l; i++) {
		for (n = 0; n < 4; n++) {
			tmp_matrix1[i][n] = matrix1[start1 + i][n];
			tmp_matrix2[i][n] = matrix2[start2 + i][n];
		}
	}

}

int index_at_max(double scores[], int len) {
	double max = scores[0];
	int i_at_max = 0;
	int i;

	if (len == 1) {
		return 0;
	}

	for (i = 1; i < len; i++) {
		if (scores[i] > max) {
			max = scores[i];
			i_at_max = i;
		}
	}
	return i_at_max;
}

void fill_rc_matrix(double matrix[][4], int len, double rc_matrix[][4]) {
	int i = 0;

	for (i = 0; i < len; i++) {
		rc_matrix[i][0] = matrix[len - i - 1][3];
		rc_matrix[i][1] = matrix[len - i - 1][2];
		rc_matrix[i][2] = matrix[len - i - 1][1];
		rc_matrix[i][3] = matrix[len - i - 1][0];
	}

}

static PyObject * c_metrics_max_subtotal(PyObject *self, PyObject * args)
{
	PyObject *matrix1_o;
	PyObject *matrix2_o;
	const char *metric;
	const char *combine;
	int matrix1_len;
	int matrix2_len;
	if (!PyArg_ParseTuple(args, "OOss", &matrix1_o, &matrix2_o, &metric, &combine)) {
		PyErr_SetString( PyExc_TypeError, "Error parsing arguments");
		return NULL;
	}

	// Retrieve frequency matrix
	if (!PyList_Check(matrix1_o)) {
		PyErr_SetString( PyExc_TypeError, "Error: missing required argument matrix1");
		return NULL;
	}

	if (!PyList_Check(matrix2_o)) {
		PyErr_SetString( PyExc_TypeError, "Error: missing required argument matrix2");
		return NULL;
	}
	// Fill matrix1
	matrix1_len = PyList_Size(matrix1_o);
	double matrix1[matrix1_len][4];
	fill_matrix(matrix1, matrix1_o);

	// Fill matrix2
	matrix2_len = PyList_Size(matrix2_o);
	double matrix2[matrix2_len][4];
	fill_matrix(matrix2, matrix2_o);

	// Fill rc matrix2
	double rc_matrix2[matrix2_len][4];
	fill_rc_matrix(matrix2, matrix2_len, rc_matrix2);

	// Assign correct function

	int pos = -2;
	int l;
	float max_score;
	int min_overlap = 6;
	int nr_matches;
	int start_pos = -(matrix2_len - min_overlap);
	nr_matches = matrix1_len + matrix2_len - 2 * min_overlap + 1;
	int i;
	int positions[nr_matches * 2];
	double scores[nr_matches * 2];
	int orients[nr_matches * 2];

	double (*ptr_metric_function)(double[][4], double[][4], int) = NULL;

	if ((!strcmp(metric, "chisq")) && (!strcmp(combine, "mean"))) {
		ptr_metric_function = &matrix_chisq_mean;
	}
	else if ((!strcmp(metric, "wic")) && (!strcmp(combine, "mean"))) {
		ptr_metric_function = &matrix_wic_mean;
	}
	else if ((!strcmp(metric, "wic")) && (!strcmp(combine, "sum"))) {
		ptr_metric_function = &matrix_wic_sum;
	}
	else if ((!strcmp(metric, "pcc")) && (!strcmp(combine, "mean"))) {
		ptr_metric_function = &matrix_pcc_mean;
	}
	else if ((!strcmp(metric, "ed")) && (!strcmp(combine, "mean"))) {
		ptr_metric_function = &matrix_ed_mean;
	}
	else if ((!strcmp(metric, "distance")) && (!strcmp(combine, "mean"))) {
		ptr_metric_function = &matrix_distance_mean;
	}
	else {
		PyErr_SetString( PyExc_TypeError, "Unknown metric or combination");
		return NULL;
	}

	for (i = 0; i < nr_matches; i++) {
		pos = i + start_pos;
		l = get_truncate_len(matrix1_len, matrix2_len, pos);

		// Normal matrix
		double tmp_matrix1[l][4];
		double tmp_matrix2[l][4];

		fill_tmp_matrices(matrix1, matrix2, pos, l, tmp_matrix1, tmp_matrix2);
		max_score = (*ptr_metric_function) (tmp_matrix1, tmp_matrix2, l);
		positions[i] = pos;
		scores[i] = max_score;
		orients[i] = 1;
		//printf("CC 1 Len %i score %f\n", l, max_score);

		// Reverse complement matrix
		fill_tmp_matrices(matrix1, rc_matrix2, pos, l, tmp_matrix1, tmp_matrix2);
		max_score = (*ptr_metric_function) (tmp_matrix1, tmp_matrix2, l);
		positions[i + nr_matches] = pos;
		scores[i + nr_matches] = max_score;
		orients[i+ nr_matches] = -1;

		//printf("CC -1 Len %i score %f\n", l, max_score);
	}

	i = index_at_max(scores, nr_matches * 2);
	return Py_BuildValue("fii", scores[i], positions[i], orients[i]);

}
static PyObject * c_metrics_pwmscan(PyObject *self, PyObject * args)
{
	PyObject *seq_o;
	PyObject *pwm_o;
	PyObject *cutoff_o;
	char *seq;
	int seq_len;
	int n_report;
	int pwm_len;
	int i, j;
	double zeta = 0.01;
	int scan_rc;

	if (!PyArg_ParseTuple(args, "OOOii", &seq_o, &pwm_o, &cutoff_o, &n_report, &scan_rc))
		return NULL;

	// Sequence and length
	if (!PyString_Check(seq_o))
		return NULL;
	seq = PyString_AsString(seq_o);
	seq_len = PyString_Size(seq_o);
	//Py_DECREF(seq_o);

	// Retrieve frequency matrix
	if (!PyList_Check(pwm_o))
		return NULL;

	// Weight matrices
	pwm_len = PyList_Size(pwm_o);
	double pwm[pwm_len][4];
	fill_matrix(pwm, pwm_o);
	//Py_DECREF(pwm_o);

	// Cutoff for every spacer length
	double cutoff;
	cutoff = PyFloat_AsDouble(cutoff_o);

	// Scan sequence
	int j_max = seq_len - pwm_len + 1;
	double score_matrix[j_max];
	double rc_score_matrix[j_max];
	double score, rc_score;

	if (j_max < 0) { j_max = 0;}
	int m;
	double g = 0.25;
	double z = 0.01;
	for (j = 0; j < j_max; j++) {
		score = 0;
		rc_score = 0;
		for (m = 0; m < pwm_len; m++) {
			switch(seq[j + m]) {
				case 'A':
					score += log(pwm[m][0] / g + z);
					rc_score += log(pwm[pwm_len - m - 1][3] / g + z);
					break;
				case 'C':
					score += log(pwm[m][1] / g + z);
					rc_score += log(pwm[pwm_len - m - 1][2] / g + z);
					break;
				case 'G':
					score += log(pwm[m][2] / g + z);
					rc_score += log(pwm[pwm_len - m - 1][1] / g + z);
					break;
				case 'T':
					score += log(pwm[m][3] / g + z);
					rc_score += log(pwm[pwm_len - m - 1][0] / g + z);
					break;
			}

		}
		score_matrix[j] = score;
		rc_score_matrix[j] = rc_score;
	}

	// Initialize matrices of n_report highest scores and corresponding positions + strands
	double maxScores[n_report];
	double maxPos[n_report];
	int maxStrand[n_report];
	for (j = 0; j < n_report; j++) {
		maxScores[j] = -100;
		maxPos[j] = -1;
		maxStrand[j] = 1;
	}
	PyObject*  return_list = PyList_New(0);

	int p,q;
	PyObject *x;
	for (j = 0; j < j_max; j++) {
		score = score_matrix[j];
		if (n_report > 0) {
			if (score >= cutoff) {
				p = n_report - 1;
				while ((p >= 0) && (score > maxScores[p])) {
					p--;
				}
				if (p < (n_report-1)) {
					for (q = n_report - 1; q > (p + 1); q--) {
						maxScores[q] = maxScores[q - 1];
						maxPos[q] = maxPos[q - 1];
						maxStrand[q] = maxStrand[q - 1];
					}
					maxScores[p + 1] = score;
					maxPos[p + 1] = j;
					maxStrand[p + 1] = 1;
				}
			}
		}
		else {
			if (score >= cutoff) {
				PyObject* row = PyList_New(0);
				x = PyFloat_FromDouble(score);
				PyList_Append(row, x); Py_DECREF(x);
				x = PyInt_FromLong((long) j);
				PyList_Append(row, x); Py_DECREF(x);
				x = PyInt_FromLong((long) 1);
				PyList_Append(row, x); Py_DECREF(x);
				PyList_Append(return_list, row);
				Py_DECREF(row);
			}
		}
	}

	if (scan_rc) {
		for (j = 0; j < j_max; j++) {
			score = rc_score_matrix[j];
			if (n_report > 0) {
				if (score >= cutoff) {
					p = n_report - 1;
					while ((p >= 0) && (score > maxScores[p])) {
						p--;
					}
					if (p < (n_report-1)) {
						for (q = n_report - 1; q > (p + 1); q--) {
							maxScores[q] = maxScores[q - 1];
							maxPos[q] = maxPos[q - 1];
							maxStrand[q] = maxStrand[q - 1];
						}
						maxScores[p + 1] = score;
						maxPos[p + 1] = j;
						maxStrand[p + 1] = -1;
					}
				}
			}
			else {
				if (score >= cutoff) {
					PyObject* row = PyList_New(0);
					x = PyFloat_FromDouble(score);
					PyList_Append(row, x); Py_DECREF(x);
					x = PyInt_FromLong((long) j);
					PyList_Append(row, x); Py_DECREF(x);
					x =  PyInt_FromLong((long) 1);
					PyList_Append(row, x); Py_DECREF(x);
					PyList_Append(return_list, row);
					Py_DECREF(row);
				}
			}
		}
	}

	for (i = 0; i < n_report; i++) {
		if (maxPos[i] > - 1) {
			PyObject* row = PyList_New(0);
			x = PyFloat_FromDouble(maxScores[i]);
			PyList_Append(row, x);  Py_DECREF(x);
			x = PyInt_FromLong((long)maxPos[i]);
			PyList_Append(row, x); Py_DECREF(x);
			x = PyInt_FromLong((long)maxStrand[i]);
			PyList_Append(row, x); Py_DECREF(x);
			PyList_Append(return_list, row);
			Py_DECREF(row);
		}
	}

	return return_list;
}

static PyMethodDef CoreMethods[] = {
	{"score", c_metrics_score, METH_VARARGS,"Test"},
	{"c_max_subtotal", c_metrics_max_subtotal, METH_VARARGS,"Test"},
	{"pwmscan", c_metrics_pwmscan, METH_VARARGS,"Test"},
	{NULL, NULL, NULL, 0, NULL}
};

PyMODINIT_FUNC
initc_metrics(void)
{
	(void) Py_InitModule("c_metrics", CoreMethods);
};


#include <cblas.h>

const char* dgemm_desc = "Reference dgemm.";

/*
 * This routine performs a dgemm operation
 *  C := C + A * B
 * where A, B, and C are lda-by-lda matrices stored in column-major format.
 * On exit, A and B maintain their input values.
 * This function wraps a call to the BLAS-3 routine DGEMM,
 * via the standard FORTRAN interface - hence the reference semantics.
 */
void square_dgemm(int n, double* A, double* B, double* C) {
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, n, n, n, 1., A, n, B, n, 1., C, n);
}


#include <stdio.h>
#include <stdlib.h>
#include <cblas.h>
#include <math.h>
#include "../inc/knnring.h"

// Application Entry Point
knnresult kNN(double * X, double * Y, int n, int m, int d, int k)
{

	// Calculate distances matrix D - D is row-major and nxm
	double* tempD = calculateD(X, Y, n, m, d, k);

	double* D = calloc(m*n, sizeof(double));

	// Transpose D to mxn
	//cblas_dimatcopy(CblasRowMajor, CblasTrans, n, m, 1.0, D, m, n);

	double* identityMat = calloc(n*n, sizeof(double));
	for(int f=0; f<n; f++)
		identityMat[f*n + f] = 1;

	cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans, m, n, n, 1, tempD, m, identityMat, n, 0, D, n);

	// Free memory
	free(identityMat);
	free(tempD);

	// Create results struct
	knnresult results;
	results.k = k;
	results.m = m;
	results.ndist = calloc(m*k, sizeof(double));
	results.nidx = calloc(m*k, sizeof(int));

	// Create ids array for the X array
	int* ids = calloc(n, sizeof(int));

	// K-Select using quickselect to find k smallest distances for each point of Y
	for(int j = 0; j < m; j++){

		// Re-set ids vector before executing quickselect each time
		for(int i = 0; i < n; i++)
				ids[i] = i;

		// Call quickselect on the row of D to sort it up to its k-th element
		kselect(D + j * n, ids, n, k);

		// Quicksort the results
		quickSort(D + j * n, ids, 0, n-1);

		// Write results (point id and distance)
		for(int l = 0; l < k; l++){

			results.ndist[j * k + l] = D[j * n + l];
			results.nidx[j * k + l] = ids[l];
		}
	}

	// Free memory and return results
	free(ids);
	return results;

}

// Function used to calculate D = sqrt(sum(X.^2,2) -2* X*Y.' + sum(Y.^2,2).');
double* calculateD(double * X, double * Y, int n, int m, int d, int k){

		// Temporary variable
		double temp = 0;

		// Distance matrix for results
		double* D = calloc(n*m, sizeof(double));

		// Euclidean norm vectors
		double* normX = calloc(n, sizeof(double));
		double* normY = calloc(m, sizeof(double));

		// Matrice to store -2*X*Y'
		double* XY = calloc(n*m, sizeof(double));

		// Calculate -2*XY (https://software.intel.com/en-us/mkl-tutorial-c-multiplying-matrices-using-dgemm)
		cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, n, m, d, -2, X, d, Y, d, 0, XY, m);

		// Calculate sum(X.^2,2), sum(Y.^2,2)
		for(int i = 0; i < n; i++){
			temp = cblas_dnrm2(d, X+i*d, 1);
			normX[i] = temp * temp;
		}
		for(int i = 0; i < m; i++){
			temp = cblas_dnrm2(d, Y+i*d, 1);
			normY[i] = temp * temp;
		}

		// XY = sum(X.^2,2) -2* X*Y.'
	        for (int i=0; i<n; i++)
	    		for(int j=0; j<m; j++)
				XY[i*m+j] += normX[i] + normY[j];

		// D = sqrt(sum(X.^2,2) -2* X*Y.' + sum(Y.^2,2).');
		for(int i = 0; i < n*m; i++)
			D[i] = sqrt(fabs(XY[i]));

		// Free memory
		free(normX);
		free(normY);
		free(XY);

		return D;
}

// A utility function to swap two elements
void swap_double(double *a, double *b){
    double t = *a;
    *a = *b;
    *b = t;
}

// A utility function to swap two elements
void swap_int(int *a, int *b){
    int t = *a;
    *a = *b;
    *b = t;
}

// QuickSort Partition function. low and high are the range of indexes in arr where partition should work
int partition (double arr[], int *ids, int low, int high){

    // Select a pivot and initialize flag to position of smallest element before pivot
    double pivot = arr[high];
    int i = (low - 1);

    // Go through the array examining each element
    for (int j = low; j <= high - 1; j++)
    {
        // If current element is smaller than the pivot, increment i and swap it out with the one currently pointed by i
        if (arr[j] < pivot)
        {
            i++;
            // Swap distances and corresponding point ids
            swap_double(&arr[i], &arr[j]);
            swap_int(&ids[i], &ids[j]);
        }
    }

    // Finally place pivot in its correct position in the array and return the position as the middle point
    swap_double(&arr[i + 1], &arr[high]);
    swap_int(&ids[i + 1], &ids[high]);
    return (i + 1);
}

// Returns the median using the QuickSelect algorithm
double kselect(double arr[], int *ids, int length, int k){
	return quickSelect(arr, ids, length, k);
}

// Returns the idx-th element of arr when arr is sorted. idx is the index (starting from 1) of the point we want to find when the array is sorted.
double quickSelect(double arr[], int *ids, int length, int idx){

    // Check to end recursion
    if (length == 1){
        return arr[0];
    }

    // Select last array element as pivot
    double pivot = arr[length - 1];
    // Get index of pivot after we partition the array
    int pivotIndex = partition(arr, ids, 0, length - 1);

    // Create the higher and lower arrays that occur after partitioning in QuickSort fashion
    int lowerLength = pivotIndex;
    pivotIndex++;
    int higherLength = (length - (lowerLength + 1));
    // At this point pivotIndex, lowerLength and higherLength all start from 1 not 0

    // Variable to store result of following recursive calls
    double result = 0;

    // This means that the point we're looking (median in our case) is in the lower partition
    if (idx <= lowerLength){
        result = quickSelect(arr, ids, lowerLength, idx);
    }
    // This means that idx-th element is our pivot point
    else if(idx == pivotIndex){
        result = pivot;
    }
    // This means that the median is in the higher partition
    else{
        result = quickSelect(arr + pivotIndex, ids + pivotIndex, higherLength, idx - pivotIndex);
    }

    // Return result
    return result;
}

// Simple quicksort implementation that also sorts the ids array according to the way the main array was sorted
void quickSort(double arr[], int *ids, int low, int high)
{
    if (low < high)
    {
        // idx is the partition point
        int idx = partition(arr, ids, low, high);

        // Divide and conquer
        quickSort(arr, ids, low, idx - 1);
        quickSort(arr, ids, idx + 1, high);
    }
}


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

#include <gsl/gsl_math.h>
#include <gsl/gsl_movstat.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_statistics.h>

double
func(const size_t n, double x[], void * params)
{
  const double alpha = *(double *) params;

  gsl_sort(x, 1, n);

  return gsl_stats_trmean_from_sorted_data(alpha, x, 1, n);
}

int
main(void)
{
  const size_t N = 1000;                /* length of time series */
  const size_t K = 11;                  /* window size */
  double alpha = 0.1;                   /* trimmed mean parameter */
  gsl_vector *x = gsl_vector_alloc(N);  /* input vector */
  gsl_vector *y = gsl_vector_alloc(N);  /* filtered output vector for alpha1 */
  gsl_rng *r = gsl_rng_alloc(gsl_rng_default);
  gsl_movstat_workspace *w = gsl_movstat_alloc(K);
  gsl_movstat_function F;
  size_t i;
  double sum = 0.0;

  /* generate input signal */
  for (i = 0; i < N; ++i)
    {
      double ui = gsl_ran_gaussian(r, 1.0);
      double outlier = (gsl_rng_uniform(r) < 0.01) ? 10.0*GSL_SIGN(ui) : 0.0;
      sum += ui;
      gsl_vector_set(x, i, sum + outlier);
    }

  /* apply moving window function */
  F.function = func;
  F.params = α
  gsl_movstat_apply(GSL_MOVSTAT_END_PADVALUE, &F, x, y, w);

  /* print results */
  for (i = 0; i < N; ++i)
    {
      double xi = gsl_vector_get(x, i);
      double yi = gsl_vector_get(y, i);

      printf("%f %f\n", xi, yi);
    }

  gsl_vector_free(x);
  gsl_vector_free(y);
  gsl_rng_free(r);
  gsl_movstat_free(w);

  return 0;
}


/* deriv/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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_deriv.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_ieee_utils.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 (deriv_fn) (const gsl_function * f, double x, double h, double * res, double *abserr);

void
test (deriv_fn * deriv, gsl_function * f, gsl_function * df, double x,
      const char * desc)
{
  double result, abserr;
  double expected = GSL_FN_EVAL (df, x);
  (*deriv) (f, x, 1e-4, &result, &abserr);

  gsl_test_abs (result, expected, GSL_MIN(1e-4,fabs(expected)) + GSL_DBL_EPSILON, desc);

  if (abserr < fabs(result-expected))
    {
      gsl_test_factor (abserr, fabs(result-expected), 2, "%s error estimate", desc);
    }
  else if (result == expected || expected == 0.0)
    {
      gsl_test_abs (abserr, 0.0, 1e-6, "%s abserr", desc);
    }
  else
    {
      double d = fabs(result - expected);
      gsl_test_abs (abserr, fabs(result-expected), 1e6*d, "%s abserr", desc);
    }
}

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

  gsl_ieee_env_setup ();

  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_deriv_central, &F1, &DF1, 1.0, "exp(x), x=1, central deriv");
  test (&gsl_deriv_forward, &F1, &DF1, 1.0, "exp(x), x=1, forward deriv");
  test (&gsl_deriv_backward, &F1, &DF1, 1.0, "exp(x), x=1, backward deriv");

  test (&gsl_deriv_central, &F2, &DF2, 0.1, "x^(3/2), x=0.1, central deriv");
  test (&gsl_deriv_forward, &F2, &DF2, 0.1, "x^(3/2), x=0.1, forward deriv");
  test (&gsl_deriv_backward, &F2, &DF2, 0.1, "x^(3/2), x=0.1, backward deriv");

  test (&gsl_deriv_central, &F3, &DF3, 0.45, "sin(1/x), x=0.45, central deriv");
  test (&gsl_deriv_forward, &F3, &DF3, 0.45, "sin(1/x), x=0.45, forward deriv");
  test (&gsl_deriv_backward, &F3, &DF3, 0.45, "sin(1/x), x=0.45, backward deriv");

  test (&gsl_deriv_central, &F4, &DF4, 0.5, "exp(-x^2), x=0.5, central deriv");
  test (&gsl_deriv_forward, &F4, &DF4, 0.5, "exp(-x^2), x=0.5, forward deriv");
  test (&gsl_deriv_backward, &F4, &DF4, 0.5, "exp(-x^2), x=0.5, backward deriv");

  test (&gsl_deriv_central, &F5, &DF5, 0.0, "x^2, x=0, central deriv");
  test (&gsl_deriv_forward, &F5, &DF5, 0.0, "x^2, x=0, forward deriv");
  test (&gsl_deriv_backward, &F5, &DF5, 0.0, "x^2, x=0, backward deriv");

  test (&gsl_deriv_central, &F6, &DF6, 10.0, "1/x, x=10, central deriv");
  test (&gsl_deriv_forward, &F6, &DF6, 10.0, "1/x, x=10, forward deriv");
  test (&gsl_deriv_backward, &F6, &DF6, 10.0, "1/x, x=10, backward deriv");

  exit (gsl_test_summary ());
}


/* multiroots/test_funcs.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 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 <math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_multiroots.h>

#include "test_funcs.h"

/* For information on testing see the following paper,

   J.J More, B.S. Garbow, K.E. Hillstrom, "Testing Unconstrained
   Optimization Software", ACM Transactions on Mathematical Software,
   Vol 7, No 1, (1981) p 17-41

   */

/* Rosenbrock Function */

gsl_multiroot_function_fdf rosenbrock =
{&rosenbrock_f,
 &rosenbrock_df,
 &rosenbrock_fdf,
 2, 0};

void
rosenbrock_initpt (gsl_vector * x)
{
  gsl_vector_set (x, 0, -1.2);
  gsl_vector_set (x, 1, 1.0);
}

int
rosenbrock_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);

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

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

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
rosenbrock_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
  double x0 = gsl_vector_get (x, 0);

  double df00 = -1;
  double df01 = 0;
  double df10 = -20 * x0;
  double df11 = 10;

  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);

  params = 0;			/* avoid warning about unused parameters */

  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;
}

/* Freudenstein and Roth function */

gsl_multiroot_function_fdf roth =
{&roth_f,
 &roth_df,
 &roth_fdf,
 2, 0};

void
roth_initpt (gsl_vector * x)
{
  gsl_vector_set (x, 0, 4.5);  /* changed from the value in the paper */
  gsl_vector_set (x, 1, 3.5);  /* otherwise the problem is too hard */
}

int
roth_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);

  double y0 = -13.0 + x0 + ((5.0 - x1)*x1 - 2.0)*x1;
  double y1 = -29.0 + x0 + ((x1 + 1.0)*x1 - 14.0)*x1;

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

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
roth_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
  double x1 = gsl_vector_get (x, 1);

  double df00 = 1;
  double df01 = -3 * x1 * x1 + 10 * x1 - 2;
  double df10 = 1;
  double df11 = 3 * x1 * x1 + 2 * x1 - 14;

  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);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
roth_fdf (const gsl_vector * x, void *params,
		gsl_vector * f, gsl_matrix * df)
{
  roth_f (x, params, f);
  roth_df (x, params, df);

  return GSL_SUCCESS;
}

/* Powell badly scaled function */

gsl_multiroot_function_fdf powellscal =
{&powellscal_f,
 &powellscal_df,
 &powellscal_fdf,
 2, 0};

void
powellscal_initpt (gsl_vector * x)
{
  gsl_vector_set (x, 0, 0.0);
  gsl_vector_set (x, 1, 1.0);
}

int
powellscal_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);

  double y0 = 10000.0 * x0 * x1 - 1.0;
  double y1 = exp (-x0) + exp (-x1) - 1.0001;

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

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
powellscal_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);

  double df00 = 10000.0 * x1, df01 = 10000.0 * x0;
  double df10 = -exp (-x0), df11 = -exp (-x1);

  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);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
powellscal_fdf (const gsl_vector * x, void *params,
		  gsl_vector * f, gsl_matrix * df)
{
  powellscal_f (x, params, f);
  powellscal_df (x, params, df);

  return GSL_SUCCESS;
}

/* Brown badly scaled function */

gsl_multiroot_function_fdf brownscal =
{&brownscal_f,
 &brownscal_df,
 &brownscal_fdf,
 2, 0};

void
brownscal_initpt (gsl_vector * x)
{
  gsl_vector_set (x, 0, 1.0);
  gsl_vector_set (x, 1, 1.0);
}

int
brownscal_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);

  double y0 = x0 - 1e6;
  double y1 = x0 * x1 - 2;

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

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
brownscal_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);

  double df00 = 1.0, df01 = 0.0;
  double df10 = x1, df11 = x0;

  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);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
brownscal_fdf (const gsl_vector * x, void *params,
		  gsl_vector * f, gsl_matrix * df)
{
  brownscal_f (x, params, f);
  brownscal_df (x, params, df);

  return GSL_SUCCESS;
}

/* Powell Singular Function */

gsl_multiroot_function_fdf powellsing =
{&powellsing_f,
 &powellsing_df,
 &powellsing_fdf,
 4, 0};

void
powellsing_initpt (gsl_vector * x)
{
  gsl_vector_set (x, 0, 3.0);
  gsl_vector_set (x, 1, -1.0);
  gsl_vector_set (x, 2, 0.0);
  gsl_vector_set (x, 3, 1.0);
}

int
powellsing_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);
  double x2 = gsl_vector_get (x, 2);
  double x3 = gsl_vector_get (x, 3);

  double y0 = x0 + 10 * x1;
  double y1 = sqrt (5.0) * (x2 - x3);
  double y2 = pow (x1 - 2 * x2, 2.0);
  double y3 = sqrt (10.0) * pow (x0 - x3, 2.0);

  gsl_vector_set (f, 0, y0);
  gsl_vector_set (f, 1, y1);
  gsl_vector_set (f, 2, y2);
  gsl_vector_set (f, 3, y3);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
powellsing_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);
  double x2 = gsl_vector_get (x, 2);
  double x3 = gsl_vector_get (x, 3);

  double df00 = 1, df01 = 10, df02 = 0, df03 = 0;
  double df10 = 0, df11 = 0, df12 = sqrt (5.0), df13 = -df12;
  double df20 = 0, df21 = 2 * (x1 - 2 * x2), df22 = -2 * df21, df23 = 0;
  double df30 = 2 * sqrt (10.0) * (x0 - x3), df31 = 0, df32 = 0, df33 = -df30;

  gsl_matrix_set (df, 0, 0, df00);
  gsl_matrix_set (df, 0, 1, df01);
  gsl_matrix_set (df, 0, 2, df02);
  gsl_matrix_set (df, 0, 3, df03);

  gsl_matrix_set (df, 1, 0, df10);
  gsl_matrix_set (df, 1, 1, df11);
  gsl_matrix_set (df, 1, 2, df12);
  gsl_matrix_set (df, 1, 3, df13);

  gsl_matrix_set (df, 2, 0, df20);
  gsl_matrix_set (df, 2, 1, df21);
  gsl_matrix_set (df, 2, 2, df22);
  gsl_matrix_set (df, 2, 3, df23);

  gsl_matrix_set (df, 3, 0, df30);
  gsl_matrix_set (df, 3, 1, df31);
  gsl_matrix_set (df, 3, 2, df32);
  gsl_matrix_set (df, 3, 3, df33);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
powellsing_fdf (const gsl_vector * x, void *params,
		    gsl_vector * f, gsl_matrix * df)
{
  powellsing_f (x, params, f);
  powellsing_df (x, params, df);

  return GSL_SUCCESS;
}

/* Wood function */

gsl_multiroot_function_fdf wood =
{&wood_f,
 &wood_df,
 &wood_fdf,
 4, 0};

void
wood_initpt (gsl_vector * x)
{
  gsl_vector_set (x, 0, -3.0);
  gsl_vector_set (x, 1, -1.0);
  gsl_vector_set (x, 2, -3.0);
  gsl_vector_set (x, 3, -1.0);
}

int
wood_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);
  double x2 = gsl_vector_get (x, 2);
  double x3 = gsl_vector_get (x, 3);

  double t1 = x1 - x0 * x0;
  double t2 = x3 - x2 * x2;

  double y0 = -200.0 * x0 * t1 - (1 - x0);
  double y1 = 200.0 * t1 + 20.2 * (x1 - 1) + 19.8 * (x3 - 1);
  double y2 = -180.0 * x2 * t2 - (1 - x2);
  double y3 = 180.0 * t2 + 20.2 * (x3 - 1) + 19.8 * (x1 - 1);

  gsl_vector_set (f, 0, y0);
  gsl_vector_set (f, 1, y1);
  gsl_vector_set (f, 2, y2);
  gsl_vector_set (f, 3, y3);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
wood_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);
  double x2 = gsl_vector_get (x, 2);
  double x3 = gsl_vector_get (x, 3);

  double t1 = x1 - 3 * x0 * x0;
  double t2 = x3 - 3 * x2 * x2;

  double df00 = -200.0 * t1 + 1, df01 = -200.0 * x0, df02 = 0, df03 = 0;
  double df10 = -400.0*x0, df11 = 200.0 + 20.2, df12 = 0, df13 = 19.8;
  double df20 = 0, df21 = 0, df22 = -180.0 * t2 + 1, df23 = -180.0 * x2;
  double df30 = 0, df31 = 19.8, df32 = -2 * 180 * x2, df33 = 180.0 + 20.2;

  gsl_matrix_set (df, 0, 0, df00);
  gsl_matrix_set (df, 0, 1, df01);
  gsl_matrix_set (df, 0, 2, df02);
  gsl_matrix_set (df, 0, 3, df03);

  gsl_matrix_set (df, 1, 0, df10);
  gsl_matrix_set (df, 1, 1, df11);
  gsl_matrix_set (df, 1, 2, df12);
  gsl_matrix_set (df, 1, 3, df13);

  gsl_matrix_set (df, 2, 0, df20);
  gsl_matrix_set (df, 2, 1, df21);
  gsl_matrix_set (df, 2, 2, df22);
  gsl_matrix_set (df, 2, 3, df23);

  gsl_matrix_set (df, 3, 0, df30);
  gsl_matrix_set (df, 3, 1, df31);
  gsl_matrix_set (df, 3, 2, df32);
  gsl_matrix_set (df, 3, 3, df33);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
wood_fdf (const gsl_vector * x, void *params,
		    gsl_vector * f, gsl_matrix * df)
{
  wood_f (x, params, f);
  wood_df (x, params, df);

  return GSL_SUCCESS;
}

/* Helical Valley Function */

gsl_multiroot_function_fdf helical =
{&helical_f,
 &helical_df,
 &helical_fdf,
 3, 0};

void
helical_initpt (gsl_vector * x)
{
  gsl_vector_set (x, 0, -1.0);
  gsl_vector_set (x, 1, 0.0);
  gsl_vector_set (x, 2, 0.0);
}

int
helical_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);
  double x2 = gsl_vector_get (x, 2);

  double t1, t2;
  double y0, y1, y2;

  if (x0 > 0)
    {
      t1 = atan(x1/x0) / (2.0 * M_PI);
    }
  else if (x0 < 0)
    {
      t1 = 0.5 + atan(x1/x0) / (2.0 * M_PI);
    }
  else
    {
      t1 = 0.25 * (x1 > 0 ? +1 : -1);
    }

  t2 = sqrt(x0*x0 + x1*x1) ;

  y0 = 10 * (x2 - 10 * t1);
  y1 = 10 * (t2 - 1);
  y2 = x2 ;

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

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
helical_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);

  double t = x0 * x0 + x1 * x1 ;
  double t1 = 2 * M_PI * t ;
  double t2 = sqrt(t) ;

  double df00 = 100*x1/t1, df01 = -100.0 * x0/t1, df02 = 10.0;
  double df10 = 10*x0/t2, df11 = 10*x1/t2, df12 = 0;
  double df20 = 0, df21 = 0, df22 = 1.0;

  gsl_matrix_set (df, 0, 0, df00);
  gsl_matrix_set (df, 0, 1, df01);
  gsl_matrix_set (df, 0, 2, df02);

  gsl_matrix_set (df, 1, 0, df10);
  gsl_matrix_set (df, 1, 1, df11);
  gsl_matrix_set (df, 1, 2, df12);

  gsl_matrix_set (df, 2, 0, df20);
  gsl_matrix_set (df, 2, 1, df21);
  gsl_matrix_set (df, 2, 2, df22);

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
helical_fdf (const gsl_vector * x, void *params,
		    gsl_vector * f, gsl_matrix * df)
{
  helical_f (x, params, f);
  helical_df (x, params, df);

  return GSL_SUCCESS;
}

/* Discrete Boundary Value Function */

#define N 10

gsl_multiroot_function_fdf dbv =
{&dbv_f,
 &dbv_df,
 &dbv_fdf,
 N, 0};

void
dbv_initpt (gsl_vector * x)
{
  size_t i;
  double h = 1.0 / (N + 1.0);

  for (i = 0; i < N; i++)
    {
      double t = (i + 1) * h;
      double z = t * (t - 1);
      gsl_vector_set (x, i, z);
    }
}

int
dbv_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  size_t i;

  double h = 1.0 / (N + 1.0);

  for (i = 0; i < N; i++)
    {
      double z, ti = (i + 1) * h;
      double xi = 0, xim1 = 0, xip1 = 0;

      xi = gsl_vector_get (x, i);

      if (i > 0)
        xim1 = gsl_vector_get (x, i - 1);

      if (i < N - 1)
        xip1 = gsl_vector_get (x, i + 1);

      z = 2 * xi - xim1 - xip1 + h * h * pow(xi + ti + 1, 3.0) / 2.0;

      gsl_vector_set (f, i, z);

    }

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
dbv_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
  size_t i, j;

  double h = 1.0 / (N + 1.0);

  for (i = 0; i < N; i++)
    for (j = 0; j < N; j++)
      gsl_matrix_set (df, i, j, 0.0);

  for (i = 0; i < N; i++)
    {
      double dz_dxi, ti = (i + 1) * h;

      double xi = gsl_vector_get (x, i);

      dz_dxi = 2.0 + (3.0 / 2.0) * h * h * pow(xi + ti + 1, 2.0) ;

      gsl_matrix_set (df, i, i, dz_dxi);

      if (i > 0)
        gsl_matrix_set (df, i, i-1, -1.0);

      if (i < N - 1)
        gsl_matrix_set (df, i, i+1, -1.0);

    }

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
dbv_fdf (const gsl_vector * x, void *params,
		    gsl_vector * f, gsl_matrix * df)
{
  dbv_f (x, params, f);
  dbv_df (x, params, df);

  return GSL_SUCCESS;
}

/* Trigonometric Function */

gsl_multiroot_function_fdf trig =
{&trig_f,
 &trig_df,
 &trig_fdf,
 N, 0};

void
trig_initpt (gsl_vector * x)
{
  size_t i;

  for (i = 0; i < N; i++)       /* choose an initial point which converges */
    {
      gsl_vector_set (x, i, 0.05);
    }
}

int
trig_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  size_t i;
  double sum = 0;

  for (i = 0; i < N; i++)
    {
      sum += cos(gsl_vector_get(x,i));
    }

  for (i = 0; i < N; i++)
    {
      double xi = gsl_vector_get (x,i);
      double z = N - sum + (i + 1) * (1 - cos(xi)) - sin(xi);

      gsl_vector_set (f, i, z);
    }

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
trig_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
  size_t i, j;

  for (i = 0; i < N; i++)
    {
      for (j = 0; j < N; j++)
        {
          double dz;
          double xi = gsl_vector_get(x, i);
          double xj = gsl_vector_get(x, j);

          if (j == i)
            dz = sin(xi) + (i + 1) * sin(xi) - cos(xi);
          else
            dz = sin(xj);

          gsl_matrix_set(df, i, j, dz);
        }
    }

  params = 0;			/* avoid warning about unused parameters */

  return GSL_SUCCESS;
}

int
trig_fdf (const gsl_vector * x, void *params,
		    gsl_vector * f, gsl_matrix * df)
{
  trig_f (x, params, f);
  trig_df (x, params, df);

  return GSL_SUCCESS;
}


#ifndef __FUNCTIONLIB_H__
#define __FUNCTIONLIB_H__

#include <iostream>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_poly.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_sort_vector.h>

#endif

///////////////////////// Definition of the structures, mainly used as parameters for functions ///////////////////////

///////////Meaning of Words:////////////////////
//
//  coef ---> coefficient
//  transl ---> tranlation
//  stddev ---> standard deviation
//  magnif ---> magnification of the variable,
//  e.g.: sin(2x) where 2 is the magnification
//
//  in all functions the combination of magnification
//  and translation is interpreted as
//  f( magnif( x - transl) ).
//
////////////////////////////////////////////////

//struct of chained list describing a compound function of several terms multiplying together, where ID refers to the type of funcion (in other words the 'true' struct the pointer 'target' is referring to) . 'flag' signifies this term is 'multiplying' (value 1) or 'dividing' (other value) the whole term.

typedef struct MulMap
{
    void * NEXT;
    void * target;
    int ID;
    int flag;
}MulMap;

//struct of chained list describing a series of multiplied compound function summed up together, where flag means the term is summing (value 1) or subtracting (other value) to the whole term (though coef in each struct describing the functions has the ability to describe this)

typedef struct SumMap
{
    void *NEXT;
    void *MulMaphead;
    int flag;
}SumMap;

typedef struct intresult
{
    double result;
    double error;
}intresult;

//ID: 1
typedef struct PolyFunc
{
    int length;
    double* coef;
    double transl;
    double magnif;
}PolyFunc;

//ID: 2
typedef struct GaussianFunc
{
    double mean;
    double stddev;
    double coef;
}GaussianFunc;

//ID: 3
typedef struct SinCosFunc
{
    double* sincoef;
    double* coscoef;
    double transl;
    double magnif;
    int length;
}SinCosFunc;

//ID: 4
typedef struct Exp
{
    double coef;
    double transl;
    double magnif;
}Exp;

//ID: 5
typedef struct Log
{
    double coef;
    double magnif;
    double base;
    double transl;
}Log;

//ID: 6
typedef struct LegendreP
{
    int l;
    double coef;
    double magnif;
    double transl;
}LegendreP;

//ID: 7
typedef struct Rect
{
    double a;
    double b;

    double height;
}Rect;

double IDMapping(double x, int ID, void * param);
double FunctionMultiply(double x, void * HeadMulMap);
double FunctionSum(double x,void * summap);

double Combination(int n,int r);

double PolyFunction(double x, void * param);
double GaussianFunction(double x,void * param);
double SinCosFunction(double x,void * param);
double Exponential(double x,void * param);
double Logarithm(double x,void * param);

//Series of functions to help create the structs
void FuncStructConstruct(FILE * inputfile,void * target, int ID);
void MulMapConstruct(FILE * inputfile, MulMap * headpointer);
void SumMapConstruct(FILE * inputfile, SumMap * headpointer);

//Series of functions to help free the memory taken by the structs
void FuncStructDelete(void* target, int ID);
void MulMapDelete(void * target);
void SumMapDelete(void * target);

void OrthoPolyCoefGen(gsl_matrix * target,int rows,int columns,double a,double b);
void PolynomialTranslationCoef(gsl_vector * target,PolyFunc * param,int vectorlength);
void LegendrePCoefficient(PolyFunc * target, LegendreP * param);

//GSL trifles
void gsl_vector_complex_convert(gsl_vector * source, gsl_vector_complex * target, int length);
void gsl_matrix_complex_convert(gsl_matrix * source, gsl_matrix_complex * target, int rows, int columns);
void gsl_vector_poly_convert(PolyFunc * target,gsl_vector * source, int vectorlength);
void gsl_vector_complex_extract(gsl_vector_complex * source, gsl_vector * real, gsl_vector * imag, int length);
void gsl_matrix_complex_extract(gsl_vector_complex * source, gsl_matrix * real,gsl_matrix * imag, int rows, int columns);
void gsl_vector_complex_combine(gsl_vector * real, gsl_vector * imag, gsl_vector_complex * target);
void gsl_matrix_complex_combine(gsl_matrix * real,gsl_matrix * imag, gsl_matrix_complex * target);
void gsl_matrix_diag(gsl_matrix * target, gsl_vector * diag, int length);
void gsl_matrix_complex_diag(gsl_matrix_complex * target, gsl_vector_complex * diag, int length);
void gsl_matrix_mul(gsl_matrix * A, gsl_matrix *B, gsl_matrix * Result,int Acolumn,int Arow,int Bcolumn);
void gsl_matrix_complex_mul(gsl_matrix_complex * A, gsl_matrix_complex *B, gsl_matrix_complex * Result,int Acolumn,int Arow,int Bcolumn);
double gsl_vector_inner_product(gsl_vector * A, gsl_vector * B,int length);
gsl_complex gsl_vector_complex_product(gsl_vector_complex * A, gsl_vector_complex * B, int length);
gsl_complex gsl_vector_complex_inner_product(gsl_vector_complex * A, gsl_vector_complex * B, int length);
void gsl_vector_transform(gsl_vector * vec,gsl_matrix * trf,int length);
void gsl_vector_complex_transform(gsl_vector_complex * vec, gsl_matrix_complex * trf,int length);
void gsl_matrix_unitmatrix(gsl_matrix * m,int length);
void gsl_matrix_complex_unitmatrix(gsl_matrix_complex * m,int length);
void gsl_vector_complex_conjugate(gsl_vector_complex * v, int length);
void gsl_matrix_complex_conjugate(gsl_matrix_complex * m, int rows, int columns);

void gsl_vector_poly_convert(PolyFunc * target,gsl_vector * source, int vectorlength);


/*
 Copyright (C) 2002 M. Marques, A. Castro, A. Rubio, G. Bertsch, M. Oliveira

 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, 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., 51 Franklin Street, Fifth Floor, Boston, MA
 02110-1301, USA.

 $Id$
*/

#include <config.h>

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

#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_sf_expint.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_combination.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_chebyshev.h>
#include <gsl/gsl_deriv.h>

#include "string_f.h"

#include <fortran_types.h>

/* Numerical threshold for oct_bessel_k0 and oct_bessel_k1 */
#define  BESSEL_K_THRES  1.0e2

/* ---------------------- Interface to GSL functions ------------------------ */

/* Mathematical Functions */
double FC_FUNC_(oct_asinh, OCT_ASINH)
     (const double *x)
{
  return gsl_asinh(*x);
}

/* Special Functions */
double FC_FUNC_(oct_gamma, OCT_GAMMA)
     (const double *x)
{
  return gsl_sf_gamma(*x);
}

double FC_FUNC_(oct_hypergeometric, OCT_HYPERGEOMETRIC)
     (const double *a, const double*b, const double *x)
{
  return gsl_sf_hyperg_U(*a, *b, *x);
}

double FC_FUNC_(oct_incomplete_gamma, OCT_INCOMPLETE_GAMMA)
     (const double *a, const double *x)
{
  return gsl_sf_gamma_inc_Q(*a, *x);
}

double FC_FUNC_(oct_sph_bessel, OCT_SPH_BESSEL)
     (const fint *l, const double*x)
{
  return gsl_sf_bessel_jl(*l, *x);
}

double FC_FUNC_(oct_bessel, OCT_BESSEL)
     (const fint *n, const double *x)
{
  return gsl_sf_bessel_Jn(*n, *x);
}

double FC_FUNC_(oct_bessel_in, OCT_BESSEL_IN)
     (const fint *n, const double *x)
{
  return gsl_sf_bessel_In(*n, *x);
}

double FC_FUNC_(oct_bessel_j0, OCT_BESSEL_J0)
     (const double *x)
{
  return gsl_sf_bessel_J0(*x);
}

double FC_FUNC_(oct_bessel_j1, OCT_BESSEL_J1)
     (const double *x)
{
  return gsl_sf_bessel_J1(*x);
}

double FC_FUNC_(oct_bessel_k0, OCT_BESSEL_K0)
     (const double *x)
{
  if( *x > BESSEL_K_THRES )
  {
    return 0.0e0;
  } else {
    return gsl_sf_bessel_K0(*x);
  }
}

double FC_FUNC_(oct_bessel_k1, OCT_BESSEL_K1)
     (const double *x)
{
  if( *x > BESSEL_K_THRES )
  {
    return 0.0e0;
  } else {
    return gsl_sf_bessel_K1(*x);
  }
}

/* the GSL headers specify double x, not const double x */
double FC_FUNC_(oct_erfc, OCT_ERFC)
     (const double *x)
{
  /* avoid floating invalids in the asymptotic limit */
  if(*x >  20.0) return  0.0;
  if(*x < -20.0) return  2.0;
  /* otherwise call gsl */
  return gsl_sf_erfc(*x);
}

/* the GSL headers specify double x, not const double x */
double FC_FUNC_(oct_erf, OCT_ERF)
     (const double *x)
{
  /* avoid floating invalids in the asymptotic limit */
  if(*x >  20.0) return  1.0;
  if(*x < -20.0) return -1.0;
  /* otherwise call gsl */
  return gsl_sf_erf(*x);
}

double FC_FUNC_(oct_legendre_sphplm, OCT_LEGENDRE_SPHPLM)
     (const fint *l, const int *m, const double *x)
{
  return gsl_sf_legendre_sphPlm(*l, *m, *x);
}

double FC_FUNC_(oct_sine_integral, OCT_SINE_INTEGRAL)
     (const double *x)
{
  return gsl_sf_Si(*x);
}

/* generalized Laguerre polynomials */
double FC_FUNC_(oct_sf_laguerre_n, OCT_SF_LAGUERRE_N)
     (const int *n, const double *a, const double *x)
{
  return gsl_sf_laguerre_n(*n, *a, *x);
}

/* Vectors and Matrices */

/* Permutations */

/* Combinations */

void FC_FUNC_(oct_combination_init, OCT_COMBINATION_INIT)
     (gsl_combination **c, const fint *n, const fint *k)
{
  *c = gsl_combination_calloc (*n, *k);
}

void FC_FUNC_(oct_combination_next, OCT_COMBINATION_NEXT)
     (gsl_combination **c, fint *next)
{
  *next = gsl_combination_next (((gsl_combination *)(*c)));
}

void FC_FUNC_(oct_get_combination, OCT_GET_COMBINATION)
     (gsl_combination **c, fint *comb)
{
  int i;
  for (i=0;i< ((gsl_combination *)(*c))->k; i++) {
    comb[i]=(fint)((gsl_combination *)(*c))->data[i];
  }
}

void FC_FUNC_(oct_combination_end, OCT_COMBINATION_END)
     (gsl_combination **c)
{
  gsl_combination_free (((gsl_combination *)(*c)));
}

/* Linear Algebra */

/* Random Number Generation */
void FC_FUNC_(oct_ran_init, OCT_RAN_INIT)
     (gsl_rng **r)
{
  gsl_rng_env_setup();
  *r = gsl_rng_alloc(gsl_rng_default);
}

void FC_FUNC_(oct_ran_end, OCT_RAN_END)
     (gsl_rng **r)
{
  gsl_rng_free(*r);
}

/* Random Number Distributions */
double FC_FUNC_(oct_ran_gaussian, OCT_RAN_GAUSSIAN)
     (const gsl_rng **r, const double *sigma)
{
  return gsl_ran_gaussian(*r, *sigma);
}

double FC_FUNC_(oct_ran_flat, OCT_RAN_FLAT)
     (const gsl_rng **r, const double *a, const double *b)
{
  return gsl_ran_flat(*r, *a, *b);
}

/* Interpolation */
void FC_FUNC_(oct_spline_end, OCT_SPLINE_END)
     (void **spl, void **acc)
{
  gsl_spline_free((gsl_spline *)(*spl));
  gsl_interp_accel_free((gsl_interp_accel *)(*acc));
}

void FC_FUNC_(oct_spline_fit, OCT_SPLINE_FIT)
		 (const fint *nrc, const double *x, const double *y, void **spl, void **acc)
{
  /* the GSL headers actually specify size_t instead of const int for nrc */
  *acc = (void *)gsl_interp_accel_alloc();
  *spl = (void *)gsl_spline_alloc(gsl_interp_cspline, *nrc);
  gsl_spline_init((gsl_spline *)(*spl), x, y, *nrc);
  fflush(stdout);
}

double FC_FUNC_(oct_spline_eval, OCT_SPLINE_EVAL)
     (const double *x, const void **spl, void **acc)
{
  /* the GSL headers specify double x instead of const double x */
  return gsl_spline_eval((gsl_spline *)(*spl), *x, (gsl_interp_accel *)(*acc));
}

void FC_FUNC_(oct_spline_eval_array, OCT_SPLINE_EVAL_ARRAY)
     (const fint * nn, double *xf, const void **spl, void **acc)
{
  int ii;
  for(ii = 0; ii < *nn; ii++){
    xf[ii] = gsl_spline_eval((gsl_spline *)(*spl), xf[ii], (gsl_interp_accel *)(*acc));
  }
}

void FC_FUNC_(oct_spline_eval_array4, OCT_SPLINE_EVAL_ARRAY4)
     (const fint * nn, float *xf, const void **spl, void **acc)
{
  int ii;
  for(ii = 0; ii < *nn; ii++){
    xf[ii] = (float) gsl_spline_eval((gsl_spline *)(*spl), (double) xf[ii], (gsl_interp_accel *)(*acc));
  }
}

/* use a stride of 2 to store into just the real part of a Fortran complex array */
void FC_FUNC_(oct_spline_eval_arrayz, OCT_SPLINE_EVAL_ARRAYZ)
     (const fint * nn, double *xf, const void **spl, void **acc)
{
  int ii;
  for(ii = 0; ii < *nn; ii++){
    xf[ii*2] = gsl_spline_eval((gsl_spline *)(*spl), xf[ii*2], (gsl_interp_accel *)(*acc));
  }
}

/* use a stride of 2 to store into just the real part of a Fortran complex array */
void FC_FUNC_(oct_spline_eval_arrayc, OCT_SPLINE_EVAL_ARRAYC)
     (const fint * nn, float *xf, const void **spl, void **acc)
{
  int ii;
  for(ii = 0; ii < *nn; ii++){
    xf[ii*2] = (float) gsl_spline_eval((gsl_spline *)(*spl), (double) xf[ii*2], (gsl_interp_accel *)(*acc));
  }
}

/* This function returns the number of points with which a spline
	 was constructed (the size component of the gsl_spline struct). */
fint FC_FUNC_(oct_spline_npoints, OCT_SPLINE_NPOINTS)
     (const void **spl)
{
  return (fint)((gsl_spline *)(*spl))->size;
}

/* This function places in the x array the x values of a given spline spl*/
void FC_FUNC_(oct_spline_x, OCT_SPLINE_X)
     (const void **spl, double *x)
{
  int size, i;

  size = (int)((gsl_spline *)(*spl))->size;
  for(i=0; i<size; i++)
    x[i] = ((gsl_spline *)(*spl))->x[i];
}

/* This function places in the y array the y values of a given spline spl*/
void FC_FUNC_(oct_spline_y, OCT_SPLINE_Y)
     (const void **spl, double *y)
{
  int size, i;

  size = (int)((gsl_spline *)(*spl))->size;
  for(i=0; i<size; i++)
    y[i] = ((gsl_spline *)(*spl))->y[i];
}

/* Returns the integral of the spline stored in spl, between a and b */
double FC_FUNC_(oct_spline_eval_integ, OCT_SPLINE_EVAL_INTEG)
     (const void **spl, const double *a, const double *b, void **acc)
{
  /* the GSL headers specify double a, double b */
  return gsl_spline_eval_integ((gsl_spline *)(*spl), *a, *b, (gsl_interp_accel *)(* acc));
}

/* Performs the derivative of a spline */
double FC_FUNC_(oct_spline_eval_der, OCT_SPLINE_EVAL_DER)
     (const double *x, const void **spl, void **acc)
{
  /* the GSL headers specify double x */
  return gsl_spline_eval_deriv((gsl_spline *)(*spl), *x, (gsl_interp_accel *)(*acc));
}

/* Performs the second derivative of a spline */
double FC_FUNC_(oct_spline_eval_der2, OCT_SPLINE_EVAL_DER2)
     (const double *x, const void **spl, void **acc)
{
  /* the GSL headers specify double x */
  return gsl_spline_eval_deriv2((gsl_spline *)(*spl), *x, (gsl_interp_accel *)(*acc));
}

void FC_FUNC_(oct_strerror, OCT_STRERROR)
     (const fint *err, STR_F_TYPE res STR_ARG1)
{
  const char *c;

  c = gsl_strerror(*err);
  TO_F_STR1(c, res);
}

/* Chebyshev Approximations */
/*
void FC_FUNC_(oct_chebyshev_coeffs, OCT_CHEBYSHEV_COEFFS)
     (gsl_complex *coeffs, int *order)
{
  int i;
  double f (double x, void *p){return cos(x);}
  double g (double x, void *p){return -sin(x);}
  gsl_cheb_series *cs = gsl_cheb_alloc (*order);
  gsl_function F;
  F.function = f;
  F.params = 0;
  gsl_cheb_init (cs, &F, -1.0, 1.0);
  for(i=0; i<=12; i++){GSL_SET_REAL(&coeffs[i], (*cs).c[i]);}
  F.function = g;
  F.params = 0;
  gsl_cheb_init (cs, &F, -1.0, 1.0);
  for(i=0; i<=12; i++){GSL_SET_IMAG(&coeffs[i], (*cs).c[i]);}
}
*/

 /* Numerical Derivatives.
    The following is an interface to the GSL support for numerical derivatives,
    in particular the gsl_deriv_central function. It computes a four points
    approximation to the derivative of a function, supplying an error estimation. */

 /* This is a type used to communicate with Fortran; func_nd is the type of the
    interface to a Fortran subroutine that calculates the value of the function.
    The first argument is the function argument, whereas the second is the function
    value. For convenience reasons, it is wrapped around the "param_nd_t" struct. */
typedef void (*func_nd)(double*, double*);
typedef struct{
  func_nd func;
} param_nd_t;

 /* This is the function that is called by the GSL function gsl_deriv_central. It
    receives as first argument the function argument, and as second argument
    a pointer to a params data type (a GSL data type), which in this case should
    be a pointer to a param_nd_t data type, where the address of the Fortran
    subroutine is. */
double function_oct_numerical_derivative (double x, void * params)
{
  double res;
  param_nd_t * p;

  p = (param_nd_t *) params;
  p->func(&x, &res);
  return res;
}
 /* This is the function that should be called by Fortran. The interface is defined
    in loct_math.F90 file. */
void FC_FUNC_(oct_numerical_derivative, OCT_NUMERICAL_DERIVATIVE)
     (const double *x, const double *h, double *result, double *abserr, const func_nd f)
{
  gsl_function F;
  param_nd_t p;

  p.func = f;
  F.function = &function_oct_numerical_derivative;
  F.params = (void *) &p;
  /* the GSL headers specify double x, double h */
  gsl_deriv_central (&F, *x, *h, result, abserr);
  return;
}


/*
  NAME:
     bovy_det
  PURPOSE:
     returns the determinant of a matrix
  CALLING SEQUENCE:
     bovy_det(gsl_matrix * A)
  INPUT:
     A - matrix
  OUTPUT:
     det(A)
  REVISION HISTORY:
     2008-09-21 - Written Bovy
 */
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>

double bovy_det(gsl_matrix * A){
  gsl_permutation * p = gsl_permutation_alloc (A->size1);
  int signum;
  double det;
  gsl_linalg_LU_decomp(A,p,&signum);
  det= gsl_linalg_LU_det(A,signum);
  gsl_permutation_free(p);

  return det;
}