/*
 * This file is part of the GROMACS molecular simulation package.
 *
 * Copyright (c) 1991-2000, University of Groningen, The Netherlands.
 * Copyright (c) 2001-2004, The GROMACS development team,
 * check out http://www.gromacs.org for more information.
 * Copyright (c) 2012,2013, by the GROMACS development team, led by
 * David van der Spoel, Berk Hess, Erik Lindahl, and including many
 * others, as listed in the AUTHORS file in the top-level source
 * directory and at http://www.gromacs.org.
 *
 * GROMACS is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation; either version 2.1
 * of the License, or (at your option) any later version.
 *
 * GROMACS 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with GROMACS; if not, see
 * http://www.gnu.org/licenses, or write to the Free Software Foundation,
 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA.
 *
 * If you want to redistribute modifications to GROMACS, please
 * consider that scientific software is very special. Version
 * control is crucial - bugs must be traceable. We will be happy to
 * consider code for inclusion in the official distribution, but
 * derived work must not be called official GROMACS. Details are found
 * in the README & COPYING files - if they are missing, get the
 * official version at http://www.gromacs.org.
 *
 * To help us fund GROMACS development, we humbly ask that you cite
 * the research papers on the package. Check out http://www.gromacs.org.
 */
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#ifdef HAVE_LIBGSL
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_multifit_nlin.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_version.h>
#endif

#include <math.h>
#include "typedefs.h"
#include "smalloc.h"
#include "vec.h"
#include "geminate.h"
#include "gmx_omp.h"

/* The first few sections of this file contain functions that were adopted,
 * and to some extent modified, by Erik Marklund (erikm[aT]xray.bmc.uu.se,
 * http://folding.bmc.uu.se) from code written by Omer Markovitch (email, url).
 * This is also the case with the function eq10v2().
 *
 * The parts menetioned in the previous paragraph were contributed under the BSD license.
 */

/* This first part is from complex.c which I recieved from Omer Markowitch.
 * - Erik Marklund
 *
 * ------------- from complex.c ------------- */

/* Complexation of a paired number (r,i)                                     */
static gem_complex gem_cmplx(double r, double i)
{
    gem_complex value;
    value.r = r;
    value.i = i;
    return value;
}

/* Complexation of a real number, x */
static gem_complex gem_c(double x)
{
    gem_complex value;
    value.r = x;
    value.i = 0;
    return value;
}

/* Magnitude of a complex number z                                           */
static double gem_cx_abs(gem_complex z)
{
    return (sqrt(z.r*z.r+z.i*z.i));
}

/* Addition of two complex numbers z1 and z2                                 */
static gem_complex gem_cxadd(gem_complex z1, gem_complex z2)
{
    gem_complex value;
    value.r = z1.r+z2.r;
    value.i = z1.i+z2.i;
    return value;
}

/* Addition of a complex number z1 and a real number r */
static gem_complex gem_cxradd(gem_complex z, double r)
{
    gem_complex value;
    value.r = z.r + r;
    value.i = z.i;
    return value;
}

/* Subtraction of two complex numbers z1 and z2                              */
static gem_complex gem_cxsub(gem_complex z1, gem_complex z2)
{
    gem_complex value;
    value.r = z1.r-z2.r;
    value.i = z1.i-z2.i;
    return value;
}

/* Multiplication of two complex numbers z1 and z2                           */
static gem_complex gem_cxmul(gem_complex z1, gem_complex z2)
{
    gem_complex value;
    value.r = z1.r*z2.r-z1.i*z2.i;
    value.i = z1.r*z2.i+z1.i*z2.r;
    return value;
}

/* Square of a complex number z                                              */
static gem_complex gem_cxsq(gem_complex z)
{
    gem_complex value;
    value.r = z.r*z.r-z.i*z.i;
    value.i = z.r*z.i*2.;
    return value;
}

/* multiplication of a complex number z and a real number r */
static gem_complex gem_cxrmul(gem_complex z, double r)
{
    gem_complex value;
    value.r = z.r*r;
    value.i = z.i*r;
    return value;
}

/* Division of two complex numbers z1 and z2                                 */
static gem_complex gem_cxdiv(gem_complex z1, gem_complex z2)
{
    gem_complex value;
    double      num;
    num = z2.r*z2.r+z2.i*z2.i;
    if (num == 0.)
    {
        fprintf(stderr, "ERROR in gem_cxdiv function\n");
    }
    value.r = (z1.r*z2.r+z1.i*z2.i)/num; value.i = (z1.i*z2.r-z1.r*z2.i)/num;
    return value;
}

/* division of a complex z number by a real number x */
static gem_complex gem_cxrdiv(gem_complex z, double r)
{
    gem_complex value;
    value.r = z.r/r;
    value.i = z.i/r;
    return value;
}

/* division of a real number r by a complex number x */
static gem_complex gem_rxcdiv(double r, gem_complex z)
{
    gem_complex value;
    double      f;
    f       = r/(z.r*z.r+z.i*z.i);
    value.r = f*z.r;
    value.i = -f*z.i;
    return value;
}

/* Exponential of a complex number-- exp (z)=|exp(z.r)|*{cos(z.i)+I*sin(z.i)}*/
static gem_complex gem_cxdexp(gem_complex z)
{
    gem_complex value;
    double      exp_z_r;
    exp_z_r = exp(z.r);
    value.r = exp_z_r*cos(z.i);
    value.i = exp_z_r*sin(z.i);
    return value;
}

/* Logarithm of a complex number -- log(z)=log|z|+I*Arg(z),                  */
/*                                  where -PI < Arg(z) < PI                  */
static gem_complex gem_cxlog(gem_complex z)
{
    gem_complex value;
    double      mag2;
    mag2 = z.r*z.r+z.i*z.i;
    if (mag2 < 0.)
    {
        fprintf(stderr, "ERROR in gem_cxlog func\n");
    }
    value.r = log(sqrt(mag2));
    if (z.r == 0.)
    {
        value.i = PI/2.;
        if (z.i < 0.)
        {
            value.i = -value.i;
        }
    }
    else
    {
        value.i = atan2(z.i, z.r);
    }
    return value;
}

/* Square root of a complex number z = |z| exp(I*the) -- z^(1/2)             */
/*                               z^(1/2)=|z|^(1/2)*[cos(the/2)+I*sin(the/2)] */
/*                               where 0 < the < 2*PI                        */
static gem_complex gem_cxdsqrt(gem_complex z)
{
    gem_complex value;
    double      sq;
    sq      = gem_cx_abs(z);
    value.r = sqrt(fabs((sq+z.r)*0.5)); /* z'.r={|z|*[1+cos(the)]/2}^(1/2) */
    value.i = sqrt(fabs((sq-z.r)*0.5)); /* z'.i={|z|*[1-cos(the)]/2}^(1/2) */
    if (z.i < 0.)
    {
        value.r = -value.r;
    }
    return value;
}

/* Complex power of a complex number z1^z2                                   */
static gem_complex gem_cxdpow(gem_complex z1, gem_complex z2)
{
    gem_complex value;
    value = gem_cxdexp(gem_cxmul(gem_cxlog(z1), z2));
    return value;
}

/* ------------ end of complex.c ------------ */

/* This next part was derived from cubic.c, also received from Omer Markovitch.
 * ------------- from cubic.c ------------- */

/* Solver for a cubic equation: x^3-a*x^2+b*x-c=0                            */
static void gem_solve(gem_complex *al, gem_complex *be, gem_complex *gam,
                      double a, double b, double c)
{
    double      t1, t2, two_3, temp;
    gem_complex ctemp, ct3;

    two_3 = pow(2., 1./3.); t1 = -a*a+3.*b; t2 = 2.*a*a*a-9.*a*b+27.*c;
    temp  = 4.*t1*t1*t1+t2*t2;

    ctemp = gem_cmplx(temp, 0.);   ctemp = gem_cxadd(gem_cmplx(t2, 0.), gem_cxdsqrt(ctemp));
    ct3   = gem_cxdpow(ctemp, gem_cmplx(1./3., 0.));

    ctemp = gem_rxcdiv(-two_3*t1/3., ct3);
    ctemp = gem_cxadd(ctemp, gem_cxrdiv(ct3, 3.*two_3));

    *gam = gem_cxadd(gem_cmplx(a/3., 0.), ctemp);

    ctemp = gem_cxmul(gem_cxsq(*gam), gem_cxsq(gem_cxsub(*gam, gem_cmplx(a, 0.))));
    ctemp = gem_cxadd(ctemp, gem_cxmul(gem_cmplx(-4.*c, 0.), *gam));
    ctemp = gem_cxdiv(gem_cxdsqrt(ctemp), *gam);
    *al   = gem_cxrmul(gem_cxsub(gem_cxsub(gem_cmplx(a, 0.), *gam), ctemp), 0.5);
    *be   = gem_cxrmul(gem_cxadd(gem_cxsub(gem_cmplx(a, 0.), *gam), ctemp), 0.5);
}

/* ------------ end of cubic.c ------------ */

/* This next part was derived from cerror.c and rerror.c, also received from Omer Markovitch.
 * ------------- from [cr]error.c ------------- */

/************************************************************/
/* Real valued error function and related functions         */
/* x, y     : real variables                                */
/* erf(x)   : error function                                */
/* erfc(x)  : complementary error function                  */
/* omega(x) : exp(x*x)*erfc(x)                              */
/* W(x,y)   : exp(-x*x)*omega(x+y)=exp(2*x*y+y^2)*erfc(x+y) */
/************************************************************/

/*---------------------------------------------------------------------------*/
/* Utilzed the series approximation of erf(x)                                */
/* Relative error=|err(x)|/erf(x)<EPS                                        */
/* Handbook of Mathematical functions, Abramowitz, p 297                     */
/* Note: When x>=6 series sum deteriorates -> Asymptotic series used instead */
/*---------------------------------------------------------------------------*/
/*  This gives erfc(z) correctly only upto >10-15 */

static double gem_erf(double x)
{
    double n, sum, temp, exp2, xm, x2, x4, x6, x8, x10, x12;
    temp = x;
    sum  = temp;
    xm   = 26.;
    x2   = x*x;
    x4   = x2*x2;
    x6   = x4*x2;
    x8   = x6*x2;
    x10  = x8*x2;
    x12  = x10*x2;
    exp2 = exp(-x2);
    if (x <= xm)
    {
        for (n = 1.; n <= 2000.; n += 1.)
        {
            temp *= 2.*x2/(2.*n+1.);
            sum  += temp;
            if (fabs(temp/sum) < 1.E-16)
            {
                break;
            }
        }

        if (n >= 2000.)
        {
            fprintf(stderr, "In Erf calc - iteration exceeds %lg\n", n);
        }
        sum *= 2./sPI*exp2;
    }
    else
    {
        /* from the asymptotic expansion of experfc(x) */
        sum = (1. - 0.5/x2 + 0.75/x4
               - 1.875/x6 + 6.5625/x8
               - 29.53125/x10 + 162.421875/x12)
            / sPI/x;
        sum *= exp2; /* now sum is erfc(x) */
        sum  = -sum+1.;
    }
    return x >= 0.0 ? sum : -sum;
}

/* Result --> Alex's code for erfc and experfc behaves better than this      */
/* complementray error function.                Returns 1.-erf(x)            */
static double gem_erfc(double x)
{
    double t, z, ans;
    z = fabs(x);
    t = 1.0/(1.0+0.5*z);

    ans = t * exp(-z*z - 1.26551223 +
                  t*(1.00002368 +
                     t*(0.37409196 +
                        t*(0.09678418 +
                           t*(-0.18628806 +
                              t*(0.27886807 +
                                 t*(-1.13520398 +
                                    t*(1.48851587 +
                                       t*(-0.82215223 +
                                          t*0.17087277)))))))));

    return x >= 0.0 ? ans : 2.0-ans;
}

/* omega(x)=exp(x^2)erfc(x)                                                  */
static double gem_omega(double x)
{
    double xm, ans, xx, x4, x6, x8, x10, x12;
    xm  = 26;
    xx  = x*x;
    x4  = xx*xx;
    x6  = x4*xx;
    x8  = x6*xx;
    x10 = x8*xx;
    x12 = x10*xx;

    if (x <= xm)
    {
        ans = exp(xx)*gem_erfc(x);
    }
    else
    {
        /* Asymptotic expansion */
        ans = (1. - 0.5/xx + 0.75/x4 - 1.875/x6 + 6.5625/x8 - 29.53125/x10 + 162.421875/x12) / sPI/x;
    }
    return ans;
}

/*---------------------------------------------------------------------------*/
/* Utilzed the series approximation of erf(z=x+iy)                           */
/* Relative error=|err(z)|/|erf(z)|<EPS                                      */
/* Handbook of Mathematical functions, Abramowitz, p 299                     */
/* comega(z=x+iy)=cexp(z^2)*cerfc(z)                                         */
/*---------------------------------------------------------------------------*/
static gem_complex gem_comega(gem_complex z)
{
    gem_complex value;
    double      x, y;
    double      sumr, sumi, n, n2, f, temp, temp1;
    double      x2, y2, cos_2xy, sin_2xy, cosh_2xy, sinh_2xy, cosh_ny, sinh_ny, exp_y2;

    x        = z.r;
    y        = z.i;
    x2       = x*x;
    y2       = y*y;
    sumr     = 0.;
    sumi     = 0.;
    cos_2xy  = cos(2.*x*y);
    sin_2xy  = sin(2.*x*y);
    cosh_2xy = cosh(2.*x*y);
    sinh_2xy = sinh(2.*x*y);
    exp_y2   = exp(-y2);

    for (n = 1.0, temp = 0.; n <= 2000.; n += 1.0)
    {
        n2      = n*n;
        cosh_ny = cosh(n*y);
        sinh_ny = sinh(n*y);
        f       = exp(-n2/4.)/(n2+4.*x2);
        /* if(f<1.E-200) break; */
        sumr += (2.*x - 2.*x*cosh_ny*cos_2xy + n*sinh_ny*sin_2xy)*f;
        sumi += (2.*x*cosh_ny*sin_2xy + n*sinh_ny*cos_2xy)*f;
        temp1 = sqrt(sumr*sumr+sumi*sumi);
        if (fabs((temp1-temp)/temp1) < 1.E-16)
        {
            break;
        }
        temp = temp1;
    }
    if (n == 2000.)
    {
        fprintf(stderr, "iteration exceeds %lg\n", n);
    }
    sumr *= 2./PI;
    sumi *= 2./PI;

    if (x != 0.)
    {
        f = 1./2./PI/x;
    }
    else
    {
        f = 0.;
    }
    value.r = gem_omega(x)-(f*(1.-cos_2xy)+sumr);
    value.i = -(f*sin_2xy+sumi);
    value   = gem_cxmul(value, gem_cmplx(exp_y2*cos_2xy, exp_y2*sin_2xy));
    return (value);
}

/* ------------ end of [cr]error.c ------------ */

/*_ REVERSIBLE GEMINATE RECOMBINATION
 *
 * Here are the functions for reversible geminate recombination. */

/* Changes the unit from square cm per s to square Ångström per ps,
 * since Omers code uses the latter units while g_mds outputs the former.
 * g_hbond expects a diffusion coefficent given in square cm per s. */
static double sqcm_per_s_to_sqA_per_ps (real D)
{
    fprintf(stdout, "Diffusion coefficient is %f A^2/ps\n", D*1e4);
    return (double)(D*1e4);
}

static double eq10v2(double theoryCt[], double time[], int manytimes,
                     double ka, double kd, t_gemParams *params)
{
    /* Finding the 3 roots */
    int    i;
    double kD, D, r, a, b, c, tsqrt, sumimaginary;
    gem_complex
           alpha, beta, gamma,
        c1, c2, c3, c4,
        oma, omb, omc,
        part1, part2, part3, part4;

    kD = params->kD;
    D  = params->D;
    r  = params->sigma;
    a  = (1.0 + ka/kD) * sqrt(D)/r;
    b  = kd;
    c  = kd * sqrt(D)/r;

    gem_solve(&alpha, &beta, &gamma, a, b, c);
    /* Finding the 3 roots */

    sumimaginary = 0;
    part1        = gem_cxmul(alpha, gem_cxmul(gem_cxadd(beta,  gamma), gem_cxsub(beta,  gamma)));                 /* 1(2+3)(2-3) */
    part2        = gem_cxmul(beta,  gem_cxmul(gem_cxadd(gamma, alpha), gem_cxsub(gamma, alpha)));                 /* 2(3+1)(3-1) */
    part3        = gem_cxmul(gamma, gem_cxmul(gem_cxadd(alpha, beta), gem_cxsub(alpha, beta)));                   /* 3(1+2)(1-2) */
    part4        = gem_cxmul(gem_cxsub(gamma, alpha), gem_cxmul(gem_cxsub(alpha, beta), gem_cxsub(beta, gamma))); /* (3-1)(1-2)(2-3) */

#pragma omp parallel for \
    private(i, tsqrt, oma, omb, omc, c1, c2, c3, c4) \
    reduction(+:sumimaginary) \
    default(shared) \
    schedule(guided)
    for (i = 0; i < manytimes; i++)
    {
        tsqrt         = sqrt(time[i]);
        oma           = gem_comega(gem_cxrmul(alpha, tsqrt));
        c1            = gem_cxmul(oma, gem_cxdiv(part1, part4));
        omb           = gem_comega(gem_cxrmul(beta, tsqrt));
        c2            = gem_cxmul(omb, gem_cxdiv(part2, part4));
        omc           = gem_comega(gem_cxrmul(gamma, tsqrt));
        c3            = gem_cxmul(omc, gem_cxdiv(part3, part4));
        c4.r          = c1.r+c2.r+c3.r;
        c4.i          = c1.i+c2.i+c3.i;
        theoryCt[i]   = c4.r;
        sumimaginary += c4.i * c4.i;
    }

    return sumimaginary;

} /* eq10v2 */

/* This returns the real-valued index(!) to an ACF, equidistant on a log scale. */
static double getLogIndex(const int i, const t_gemParams *params)
{
    return (exp(((double)(i)) * params->logQuota) -1);
}

extern t_gemParams *init_gemParams(const double sigma, const double D,
                                   const real *t, const int len, const int nFitPoints,
                                   const real begFit, const real endFit,
                                   const real ballistic, const int nBalExp, const gmx_bool bDt)
{
    double       tDelta;
    t_gemParams *p;

    snew(p, 1);

    /* A few hardcoded things here. For now anyway. */
/*   p->ka_min   = 0; */
/*   p->ka_max   = 100; */
/*   p->dka      = 10; */
/*   p->kd_min   = 0; */
/*   p->kd_max   = 2; */
/*   p->dkd      = 0.2; */
    p->ka       = 0;
    p->kd       = 0;
/*   p->lsq      = -1; */
/*   p->lifetime = 0; */
    p->sigma    = sigma*10; /* Omer uses Å, not nm */
/*   p->lsq_old  = 99999; */
    p->D        = sqcm_per_s_to_sqA_per_ps(D);
    p->kD       = 4*acos(-1.0)*sigma*p->D;

    /* Parameters used by calcsquare(). Better to calculate them
     * here than in calcsquare every time it's called. */
    p->len = len;
/*   p->logAfterTime = logAfterTime; */
    tDelta       = (t[len-1]-t[0]) / len;
    if (tDelta <= 0)
    {
        gmx_fatal(FARGS, "Time between frames is non-positive!");
    }
    else
    {
        p->tDelta = tDelta;
    }

    p->nExpFit      = nBalExp;
/*   p->nLin         = logAfterTime / tDelta; */
    p->nFitPoints   = nFitPoints;
    p->begFit       = begFit;
    p->endFit       = endFit;
    p->logQuota     = (double)(log(p->len))/(p->nFitPoints-1);
/*   if (p->nLin <= 0) { */
/*     fprintf(stderr, "Number of data points in the linear regime is non-positive!\n"); */
/*     sfree(p); */
/*     return NULL; */
/*   } */
/* We want the same number of data points in the log regime. Not crucial, but seems like a good idea. */
/* p->logDelta = log(((float)len)/p->nFitPoints) / p->nFitPoints;/\* log(((float)len)/p->nLin) / p->nLin; *\/ */
/*   p->logPF    = p->nFitPoints*p->nFitPoints/(float)len; /\* p->nLin*p->nLin/(float)len; *\/ */
/* logPF and logDelta are stitched together with the macro GETLOGINDEX defined in geminate.h */

/*   p->logMult      = pow((float)len, 1.0/nLin);/\* pow(t[len-1]-t[0], 1.0/p->nLin); *\/ */
    p->ballistic    =  ballistic;
    return p;
}

/* There was a misunderstanding regarding the fitting. From our
 * recent correspondence it appears that Omer's code require
 * the ACF data on a log-scale and does not operate on the raw data.
 * This needs to be redone in gemFunc_residual() as well as in the
 * t_gemParams structure. */
#ifdef HAVE_LIBGSL
static double gemFunc_residual2(const gsl_vector *p, void *data)
{
    gemFitData *GD;
    int         i, iLog, nLin, nFitPoints, nData;
    double      r, residual2, *ctTheory, *y;

    GD         = (gemFitData *)data;
    nLin       = GD->params->nLin;
    nFitPoints = GD->params->nFitPoints;
    nData      = GD->nData;
    residual2  = 0;
    ctTheory   = GD->ctTheory;
    y          = GD->y;

    /* Now, we'd save a lot of time by not calculating eq10v2 for all
     * time points, but only those that we sample to calculate the mse.
     */

    eq10v2(GD->ctTheory, GD->doubleLogTime /* GD->time */, nFitPoints /* GD->nData */,
           gsl_vector_get(p, 0), gsl_vector_get(p, 1),
           GD->params);

    fixGemACF(GD->ctTheory, nFitPoints);

    /* Removing a bunch of points from the log-part. */
#pragma omp parallel for schedule(dynamic) \
    firstprivate(nData, ctTheory, y, nFitPoints) \
    private (i, iLog, r) \
    reduction(+:residual2) \
    default(shared)
    for (i = 0; i < nFitPoints; i++)
    {
        iLog       = GD->logtime[i];
        r          = log(ctTheory[i /* iLog */]);
        residual2 += sqr(r-log(y[iLog]));
    }
    residual2 /= nFitPoints; /* Not really necessary for the fitting, but gives more meaning to the returned data. */
    /* printf("ka=%3.5f\tkd=%3.5f\trmse=%3.5f\n", gsl_vector_get(p,0), gsl_vector_get(p,1), residual2); */
    return residual2;

/*   for (i=0; i<nLin; i++) { */
/*     /\* Linear part ----------*\/ */
/*     r = ctTheory[i]; */
/*     residual2 += sqr(r-y[i]); */
/*     /\* Log part -------------*\/ */
/*     iLog = GETLOGINDEX(i, GD->params); */
/*     if (iLog >= nData) */
/*       gmx_fatal(FARGS, "log index out of bounds: %i", iLog); */
/*     r = ctTheory[iLog]; */
/*     residual2 += sqr(r-y[iLog]); */

/*   } */
/*   residual2 /= GD->n; */
/*   return residual2; */
}
#endif

static real* d2r(const double *d, const int nn);

extern real fitGemRecomb(double *ct, double *time, double **ctFit,
                         const int nData, t_gemParams *params)
{

    int         nThreads, i, iter, status, maxiter;
    real        size, d2, tol, *dumpdata;
    size_t      p, n;
    gemFitData *GD;
    char       *dumpstr, dumpname[128];

    /* nmsimplex2 had convergence problems prior to gsl v1.14,
     * but it's O(N) instead of O(N) operations, so let's use it if v >= 1.14 */
#ifdef HAVE_LIBGSL
    gsl_multimin_fminimizer *s;
    gsl_vector              *x, *dx; /* parameters and initial step size */
    gsl_multimin_function    fitFunc;
#ifdef GSL_MAJOR_VERSION
#ifdef GSL_MINOR_VERSION
#if ((GSL_MAJOR_VERSION == 1 && GSL_MINOR_VERSION >= 14) || \
    (GSL_MAJOR_VERSION > 1))
    const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex2;
#else
    const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex;
#endif /* #if ... */
#endif /* GSL_MINOR_VERSION */
#else
    const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex;
#endif /* GSL_MAJOR_VERSION */
    fprintf(stdout, "Will fit ka and kd to the ACF according to the reversible geminate recombination model.\n");
#else  /* HAVE_LIBGSL */
    fprintf(stderr, "Sorry, can't do reversible geminate recombination without gsl. "
            "Recompile using --with-gsl.\n");
    return -1;
#endif /* HAVE_LIBGSL */

#ifdef HAVE_LIBGSL
#ifdef GMX_OPENMP
    nThreads = gmx_omp_get_max_threads();
    gmx_omp_set_num_threads(nThreads);
    fprintf(stdout, "We will be using %i threads.\n", nThreads);
#endif

    iter    = 0;
    status  = 0;
    maxiter = 100;
    tol     = 1e-10;

    p = 2;                                        /* Number of parameters to fit. ka and kd.  */
    n = params->nFitPoints; /* params->nLin*2 */; /* Number of points in the reduced dataset  */

    if (params->D <= 0)
    {
        fprintf(stderr, "Fitting of D is not implemented yet. It must be provided on the command line.\n");
        return -1;
    }

/*   if (nData<n) { */
/*     fprintf(stderr, "Reduced data set larger than the complete data set!\n"); */
/*     n=nData; */
/*   } */
    snew(dumpdata, nData);
    snew(GD, 1);

    GD->n        = n;
    GD->y        = ct;
    GD->ctTheory = NULL;
    snew(GD->ctTheory, nData);
    GD->LinLog = NULL;
    snew(GD->LinLog, n);
    GD->time   = time;
    GD->ka     = 0;
    GD->kd     = 0;
    GD->tDelta = time[1]-time[0];
    GD->nData  = nData;
    GD->params = params;
    snew(GD->logtime, params->nFitPoints);
    snew(GD->doubleLogTime, params->nFitPoints);

    for (i = 0; i < params->nFitPoints; i++)
    {
        GD->doubleLogTime[i]  = (double)(getLogIndex(i, params));
        GD->logtime[i]        = (int)(GD->doubleLogTime[i]);
        GD->doubleLogTime[i] *= GD->tDelta;

        if (GD->logtime[i] >= nData)
        {
            fprintf(stderr, "Ayay. It seems we're indexing out of bounds.\n");
            params->nFitPoints = i;
        }
    }

    fitFunc.f      = &gemFunc_residual2;
    fitFunc.n      = 2;
    fitFunc.params = (void*)GD;

    x  = gsl_vector_alloc (fitFunc.n);
    dx = gsl_vector_alloc (fitFunc.n);
    gsl_vector_set (x,  0, 25);
    gsl_vector_set (x,  1, 0.5);
    gsl_vector_set (dx, 0, 0.1);
    gsl_vector_set (dx, 1, 0.01);

    s = gsl_multimin_fminimizer_alloc (T, fitFunc.n);
    gsl_multimin_fminimizer_set (s, &fitFunc, x, dx);
    gsl_vector_free (x);
    gsl_vector_free (dx);

    do
    {
        iter++;
        status = gsl_multimin_fminimizer_iterate (s);

        if (status != 0)
        {
            gmx_fatal(FARGS, "Something went wrong in the iteration in minimizer %s:\n \"%s\"\n",
                      gsl_multimin_fminimizer_name(s), gsl_strerror(status));
        }

        d2         = gsl_multimin_fminimizer_minimum(s);
        size       = gsl_multimin_fminimizer_size(s);
        params->ka = gsl_vector_get (s->x, 0);
        params->kd = gsl_vector_get (s->x, 1);

        if (status)
        {
            fprintf(stderr, "%s\n", gsl_strerror(status));
            break;
        }

        status = gsl_multimin_test_size(size, tol);

        if (status == GSL_SUCCESS)
        {
            fprintf(stdout, "Converged to minimum at\n");
        }

        printf ("iter %5d: ka = %2.5f  kd = %2.5f  f() = %7.3f  size = %.3f  chi2 = %2.5f\n",
                iter,
                params->ka,
                params->kd,
                s->fval, size, d2);

        if (iter%1 == 0)
        {
            eq10v2(GD->ctTheory, time, nData, params->ka, params->kd, params);
            /* fixGemACF(GD->ctTheory, nFitPoints); */
            sprintf(dumpname, "Iter_%i.xvg", iter);
            for (i = 0; i < GD->nData; i++)
            {
                dumpdata[i] = (real)(GD->ctTheory[i]);
                if (!gmx_isfinite(dumpdata[i]))
                {
                    gmx_fatal(FARGS, "Non-finite value in acf.");
                }
            }
            dumpN(dumpdata, GD->nData, dumpname);
        }
    }
    while ((status == GSL_CONTINUE) && (iter < maxiter));

    /*   /\* Calculate the theoretical ACF from the parameters one last time. *\/ */
    eq10v2(GD->ctTheory, time, nData, params->ka, params->kd, params);
    *ctFit = GD->ctTheory;

    sfree(GD);
    gsl_multimin_fminimizer_free (s);

    return d2;

#endif /* HAVE_LIBGSL */
}

#ifdef HAVE_LIBGSL
static int balFunc_f(const gsl_vector *x, void *data, gsl_vector *f)
{
    /* C + sum{ A_i * exp(-B_i * t) }*/

    balData *BD;
    int      n, i, j, nexp;
    double  *y, *A, *B, C,     /* There are the parameters to be optimized. */
             t, ct;

    BD   = (balData *)data;
    n    = BD->n;
    nexp = BD->nexp;
    y    = BD->y,
    snew(A, nexp);
    snew(B, nexp);

    for (i = 0; i < nexp; i++)
    {
        A[i] = gsl_vector_get(x, i*2);
        B[i] = gsl_vector_get(x, i*2+1);
    }
    C = gsl_vector_get(x, nexp*2);

    for (i = 0; i < n; i++)
    {
        t  = i*BD->tDelta;
        ct = 0;
        for (j = 0; j < nexp; j++)
        {
            ct += A[j] * exp(B[j] * t);
        }
        ct += C;
        gsl_vector_set (f, i, ct - y[i]);
    }
    return GSL_SUCCESS;
}

/* The derivative stored in jacobian form (J)*/
static int balFunc_df(const gsl_vector *params, void *data, gsl_matrix *J)
{
    balData *BD;
    size_t   n, i, j;
    double  *y, *A, *B, C, /* There are the parameters. */
             t;
    int      nexp;

    BD   = (balData*)data;
    n    = BD->n;
    y    = BD->y;
    nexp = BD->nexp;

    snew(A, nexp);
    snew(B, nexp);

    for (i = 0; i < nexp; i++)
    {
        A[i] = gsl_vector_get(params, i*2);
        B[i] = gsl_vector_get(params, i*2+1);
    }
    C = gsl_vector_get(params, nexp*2);
    for (i = 0; i < n; i++)
    {
        t = i*BD->tDelta;
        for (j = 0; j < nexp; j++)
        {
            gsl_matrix_set (J, i, j*2,   exp(B[j]*t));        /* df(t)/dA_j */
            gsl_matrix_set (J, i, j*2+1, A[j]*t*exp(B[j]*t)); /* df(t)/dB_j */
        }
        gsl_matrix_set (J, i, nexp*2, 1);                     /* df(t)/dC */
    }
    return GSL_SUCCESS;
}

/* Calculation of the function and its derivative */
static int balFunc_fdf(const gsl_vector *params, void *data,
                       gsl_vector *f, gsl_matrix *J)
{
    balFunc_f(params, data, f);
    balFunc_df(params, data, J);
    return GSL_SUCCESS;
}
#endif /* HAVE_LIBGSL */

/* Removes the ballistic term from the beginning of the ACF,
 * just like in Omer's paper.
 */
extern void takeAwayBallistic(double *ct, double *t, int len, real tMax, int nexp, gmx_bool bDerivative)
{

    /* Use nonlinear regression with GSL instead.
     * Fit with 4 exponentials and one constant term,
     * subtract the fatest exponential. */

    int      nData, i, status, iter;
    balData *BD;
    double  *guess,           /* Initial guess. */
    *A,                       /* The fitted parameters. (A1, B1, A2, B2,... C) */
             a[2],
             ddt[2];
    gmx_bool sorted;
    size_t   n;
    size_t   p;

    nData = 0;
    do
    {
        nData++;
    }
    while (t[nData] < tMax+t[0] && nData < len);

    p = nexp*2+1;            /* Number of parameters. */

#ifdef HAVE_LIBGSL
    const gsl_multifit_fdfsolver_type *T
        = gsl_multifit_fdfsolver_lmsder;

    gsl_multifit_fdfsolver   *s;           /* The solver itself. */
    gsl_multifit_function_fdf fitFunction; /* The function to be fitted. */
    gsl_matrix               *covar;       /* Covariance matrix for the parameters.
                                            * We'll not use the result, though. */
    gsl_vector_view           theParams;

    nData = 0;
    do
    {
        nData++;
    }
    while (t[nData] < tMax+t[0] && nData < len);

    guess = NULL;
    n     = nData;

    snew(guess, p);
    snew(A, p);
    covar = gsl_matrix_alloc (p, p);

    /* Set up an initial gess for the parameters.
     * The solver is somewhat sensitive to the initial guess,
     * but this worked fine for a TIP3P box with -geminate dd
     * EDIT: In fact, this seems like a good starting pont for other watermodels too. */
    for (i = 0; i < nexp; i++)
    {
        guess[i*2]   = 0.1;
        guess[i*2+1] = -0.5 + (((double)i)/nexp - 0.5)*0.3;
    }
    guess[nexp * 2] = 0.01;

    theParams = gsl_vector_view_array(guess, p);

    snew(BD, 1);
    BD->n      = n;
    BD->y      = ct;
    BD->tDelta = t[1]-t[0];
    BD->nexp   = nexp;

    fitFunction.f      =  &balFunc_f;
    fitFunction.df     =  &balFunc_df;
    fitFunction.fdf    =  &balFunc_fdf;
    fitFunction.n      =  nData;
    fitFunction.p      =  p;
    fitFunction.params =  BD;

    s = gsl_multifit_fdfsolver_alloc (T, nData, p);
    if (s == NULL)
    {
        gmx_fatal(FARGS, "Could not set up the nonlinear solver.");
    }

    gsl_multifit_fdfsolver_set(s, &fitFunction, &theParams.vector);

    /* \=============================================/ */

    iter = 0;
    do
    {
        iter++;
        status = gsl_multifit_fdfsolver_iterate (s);

        if (status)
        {
            break;
        }
        status = gsl_multifit_test_delta (s->dx, s->x, 1e-4, 1e-4);
    }
    while (iter < 5000 && status == GSL_CONTINUE);

    if (iter == 5000)
    {
        fprintf(stderr, "The non-linear fitting did not converge in 5000 steps.\n"
                "Check the quality of the fit!\n");
    }
    else
    {
        fprintf(stderr, "Non-linear fitting of ballistic term converged in %d steps.\n\n", (int)iter);
    }
    for (i = 0; i < nexp; i++)
    {
        fprintf(stdout, "%c * exp(%c * t) + ", 'A'+(char)i*2, 'B'+(char)i*2);
    }

    fprintf(stdout, "%c\n", 'A'+(char)nexp*2);
    fprintf(stdout, "Here are the actual numbers for A-%c:\n", 'A'+nexp*2);

    for (i = 0; i < nexp; i++)
    {
        A[i*2]   = gsl_vector_get(s->x, i*2);
        A[i*2+1] = gsl_vector_get(s->x, i*2+1);
        fprintf(stdout, " %g*exp(%g * x) +", A[i*2], A[i*2+1]);
    }
    A[i*2] = gsl_vector_get(s->x, i*2);        /* The last and constant term */
    fprintf(stdout, " %g\n", A[i*2]);

    fflush(stdout);

    /* Implement some check for parameter quality */
    for (i = 0; i < nexp; i++)
    {
        if (A[i*2] < 0 || A[i*2] > 1)
        {
            fprintf(stderr, "WARNING: ----------------------------------\n"
                    " | A coefficient does not lie within [0,1].\n"
                    " | This may or may not be a problem.\n"
                    " | Double check the quality of the fit!\n");
        }
        if (A[i*2+1] > 0)
        {
            fprintf(stderr, "WARNING: ----------------------------------\n"
                    " | One factor in the exponent is positive.\n"
                    " | This could be a problem if the coefficient\n"
                    " | is large. Double check the quality of the fit!\n");
        }
    }
    if (A[i*2] < 0 || A[i*2] > 1)
    {
        fprintf(stderr, "WARNING: ----------------------------------\n"
                " | The constant term does not lie within [0,1].\n"
                " | This may or may not be a problem.\n"
                " | Double check the quality of the fit!\n");
    }

    /* Sort the terms */
    sorted = (nexp > 1) ?  FALSE : TRUE;
    while (!sorted)
    {
        sorted = TRUE;
        for (i = 0; i < nexp-1; i++)
        {
            ddt[0] = A[i*2] * A[i*2+1];
            ddt[1] = A[i*2+2] * A[i*2+3];

            if ((bDerivative && (ddt[0] < 0 && ddt[1] < 0 && ddt[0] > ddt[1])) || /* Compare derivative at t=0... */
                (!bDerivative && (A[i*2+1] > A[i*2+3])))                          /* Or just the coefficient in the exponent */
            {
                sorted = FALSE;
                a[0]   = A[i*2];   /* coefficient */
                a[1]   = A[i*2+1]; /* parameter in the exponent */

                A[i*2]   = A[i*2+2];
                A[i*2+1] = A[i*2+3];

                A[i*2+2] = a[0];
                A[i*2+3] = a[1];
            }
        }
    }

    /* Subtract the fastest component */
    fprintf(stdout, "Fastest component is %g * exp(%g * t)\n"
            "Subtracting fastest component from ACF.\n", A[0], A[1]);

    for (i = 0; i < len; i++)
    {
        ct[i] = (ct[i] - A[0] * exp(A[1] * i*BD->tDelta)) / (1-A[0]);
    }

    sfree(guess);
    sfree(A);

    gsl_multifit_fdfsolver_free(s);
    gsl_matrix_free(covar);
    fflush(stdout);

#else
    /* We have no gsl. */
    fprintf(stderr, "Sorry, can't take away ballistic component without gsl. "
            "Recompile using --with-gsl.\n");
    return;
#endif /* HAVE_LIBGSL */

}

extern void dumpN(const real *e, const int nn, char *fn)
{
    /* For debugging only */
    int   i;
    FILE *f;
    char  standardName[] = "Nt.xvg";
    if (fn == NULL)
    {
        fn = standardName;
    }

    f = fopen(fn, "w");
    fprintf(f,
            "@ type XY\n"
            "@ xaxis label \"Frame\"\n"
            "@ yaxis label \"N\"\n"
            "@ s0 line type 3\n");

    for (i = 0; i < nn; i++)
    {
        fprintf(f, "%-10i %-g\n", i, e[i]);
    }

    fclose(f);
}

static real* d2r(const double *d, const int nn)
{
    real *r;
    int   i;

    snew(r, nn);
    for (i = 0; i < nn; i++)
    {
        r[i] = (real)d[i];
    }

    return r;
}

static void _patchBad(double *ct, int n, double dy)
{
    /* Just do lin. interpolation for now. */
    int i;

    for (i = 1; i < n; i++)
    {
        ct[i] = ct[0]+i*dy;
    }
}

static void patchBadPart(double *ct, int n)
{
    _patchBad(ct, n, (ct[n] - ct[0])/n);
}

static void patchBadTail(double *ct, int n)
{
    _patchBad(ct+1, n-1, ct[1]-ct[0]);

}

extern void fixGemACF(double *ct, int len)
{
    int      i, j, b, e;
    gmx_bool bBad;

    /* Let's separate two things:
     * - identification of bad parts
     * - patching of bad parts.
     */

    b    = 0; /* Start of a bad stretch */
    e    = 0; /* End of a bad stretch */
    bBad = FALSE;

    /* An acf of binary data must be one at t=0. */
    if (abs(ct[0]-1.0) > 1e-6)
    {
        ct[0] = 1.0;
        fprintf(stderr, "|ct[0]-1.0| = %1.6d. Setting ct[0] to 1.0.\n", abs(ct[0]-1.0));
    }

    for (i = 0; i < len; i++)
    {

#ifdef HAS_ISFINITE
        if (isfinite(ct[i]))
#elif defined(HAS__ISFINITE)
        if (_isfinite(ct[i]))
#else
        if (1)
#endif
        {
            if (!bBad)
            {
                /* Still on a good stretch. Proceed.*/
                continue;
            }

            /* Patch up preceding bad stretch. */
            if (i == (len-1))
            {
                /* It's the tail */
                if (b <= 1)
                {
                    gmx_fatal(FARGS, "The ACF is mostly NaN or Inf. Aborting.");
                }
                patchBadTail(&(ct[b-2]), (len-b)+1);
            }

            e = i;
            patchBadPart(&(ct[b-1]), (e-b)+1);
            bBad = FALSE;
        }
        else
        {
            if (!bBad)
            {
                b = i;

                bBad = TRUE;
            }
        }
    }
}


/* C functions for running SampleTau from Cython*/

/*System includes*/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <sys/stat.h>

/*GSL includes*/
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_blas.h>

/*User includes*/
//#include "c_sample_tau.h"

static gsl_rng *ptGSLRNG;

void c_initRNG()
{
    //const gsl_rng_type * T;

    gsl_rng_env_setup();

    //T = gsl_rng_default;
    ptGSLRNG = gsl_rng_alloc (gsl_rng_mt19937);
}

void c_setRNG(unsigned long int seed)
{
    //printf("GSL RNG initialise %lu\n",seed);
    gsl_rng_set (ptGSLRNG, seed);
}

void c_freeRNG()
{
    gsl_rng_free (ptGSLRNG);
}

void normaliseLog4(double *adLogProb)
{
    double dMax = adLogProb[0], dSum = 0.0;
    int b = 0;

    for(b = 1; b < 4; b++){
        if(adLogProb[b] > dMax){
            dMax = adLogProb[b];
        }
    }

    for(b = 0; b < 4; b++){
        adLogProb[b] = adLogProb[b] - dMax;
        dSum += exp(adLogProb[b]);
    }

    for(b = 0; b < 4; b++){
        adLogProb[b] = exp(adLogProb[b])/dSum;
        //printf("b=%d,p=%f\n",b,adLogProb[b]);
    }

    return;
}

int sample4(double *adProb, double dU){
    double adCProb[4];

    adCProb[0] = adProb[0];
    adCProb[1] = adProb[1] + adCProb[0];
    adCProb[2] = adProb[2] + adCProb[1];
    adCProb[3] = 1.0;

    if(dU < adCProb[0]){
        return 0;
    }
    else if(dU < adCProb[1])
        return 1;
    else if(dU < adCProb[2]){
        return 2;
    }
    else{
        return 3;
    }
}

int c_sample_tau (long *anTau, double* adPi, double *adEta, long* anVariants, int nV, int nG, int nS)
{
    int a = 0, b = 0;
    int g = 0, h = 0, v = 0, s = 0, t = 0;
    int nchange = 0;
    double adPSB[nS][4];
    double adPSBStore[nS][4];
    double dLogProb[4];
    int** anTauIndex = NULL;
    double u = 0.0;

    anTauIndex = (int **) malloc(nV*sizeof(int*));
    if(!anTauIndex)
        goto memoryError;

    for(v = 0; v < nV; v++){
        anTauIndex[v] = (int *) malloc(nG*sizeof(int));
        if(!anTauIndex[v])
            goto memoryError;
        for(g = 0; g < nG; g++){
            for(b = 0; b < 4; b++){
                int vIndex = v*4*nG + 4*g + b;

                if(anTau[vIndex] == 1){
                    anTauIndex[v][g] = b;
                    break;
                }
            }
            //printf("%d,%d,%d\n",v,g,anTauIndex[v][g]);
        }

    }

    //loop V positions
    for(v = 0; v < nV; v++){

        //loop G strains
        for(g = 0; g < nG; g++){

            //calc contribution from all other strains
            for(s = 0; s < nS; s++){
                for(b = 0; b < 4; b++){
                    adPSBStore[s][b] = 0.0;

                    for(h = 0; h < nG; h++){
                        if(h != g){
                            int pIndex = s*nG + h;
                            int eIndex = anTauIndex[v][h]*4 + b;

                            adPSBStore[s][b] += adEta[eIndex]*adPi[pIndex];
                        }
                    }

                }
            }

            for(a = 0; a < 4; a++){
                for(s = 0; s < nS; s++){
                    for(b = 0; b < 4; b++){
                        adPSB[s][b] = adPSBStore[s][b];

                        adPSB[s][b] += adEta[a*4 + b]*adPi[s*nG + g];
                    }
                }
                dLogProb[a] = 0.0;
                for(s = 0; s < nS; s++){
                    for(b = 0; b < 4; b++){
                        int vIndex = v*4*nS + 4*s + b;
                        double temp = ((float) anVariants[vIndex])*log(adPSB[s][b]);
                        //printf("s=%d,b=%d,cont=%f\n",s,b,temp);
                        dLogProb[a] += temp;
                    }
                }
            }
  //          printf("u=%f,p1=%f,p2=%f,p3=%f,p4 =%f\n",u,dLogProb[0],dLogProb[1],dLogProb[2],dLogProb[3]);

            normaliseLog4(dLogProb);

            u = gsl_rng_uniform (ptGSLRNG);
       //     printf("u=%f,p1=%f,p2=%f,p3=%f,p4 =%f\n",u,dLogProb[0],dLogProb[1],dLogProb[2],dLogProb[3]);
            t = sample4(dLogProb, u);
         //   printf("v=%d,g=%d,tnew=%d,told=%d\n",v,g,t,anTauIndex[v][g]);
            if(t != anTauIndex[v][g]){
                int vIndex = v*4*nG + 4*g;
                anTau[vIndex + anTauIndex[v][g]] = 0;
                anTau[vIndex + t] = 1;

                nchange++;
                anTauIndex[v][g] = t;
            }
        } //finish sampling strain g

    }//finish sampling position v

    //Free up tau indices
    for(v = 0; v < nV; v++){
        free(anTauIndex[v]);
    }
    free(anTauIndex);

    return nchange;

    memoryError:
    fprintf(stderr, "Failed allocating memory in c_sample_tau\n");
    fflush(stderr);
    exit(EXIT_FAILURE);
}


#include <config.h>
#include <math.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_sort_vector.h>

static int compute_rank(gsl_vector *v);

#define BASE_LONG_DOUBLE
#include "templates_on.h"
#include "covariance_source.c"
#include "templates_off.h"
#undef  BASE_LONG_DOUBLE

#define BASE_DOUBLE
#include "templates_on.h"
#include "covariance_source.c"
#include "templates_off.h"
#undef  BASE_DOUBLE

#define BASE_FLOAT
#include "templates_on.h"
#include "covariance_source.c"
#include "templates_off.h"
#undef  BASE_FLOAT

#define BASE_ULONG
#include "templates_on.h"
#include "covariance_source.c"
#include "templates_off.h"
#undef  BASE_ULONG

#define BASE_LONG
#include "templates_on.h"
#include "covariance_source.c"
#include "templates_off.h"
#undef  BASE_LONG

#define BASE_UINT
#include "templates_on.h"
#include "covariance_source.c"
#include "templates_off.h"
#undef  BASE_UINT

#define BASE_INT
#include "templates_on.h"
#include "covariance_source.c"
#include "templates_off.h"
#undef  BASE_INT

#define BASE_USHORT
#include "templates_on.h"
#include "covariance_source.c"
#include "templates_off.h"
#undef  BASE_USHORT

#define BASE_SHORT
#include "templates_on.h"
#include "covariance_source.c"
#include "templates_off.h"
#undef  BASE_SHORT

#define BASE_UCHAR
#include "templates_on.h"
#include "covariance_source.c"
#include "templates_off.h"
#undef  BASE_UCHAR

#define BASE_CHAR
#include "templates_on.h"
#include "covariance_source.c"
#include "templates_off.h"
#undef  BASE_CHAR

/*
compute_rank()
  Compute rank of a sorted vector

Inputs: v - sorted data vector on input; rank vector on output

Notes: ranks are always computed in double precision
*/

static int
compute_rank(gsl_vector *v)
{
  const size_t n = v->size;
  size_t i = 0;

  while (i < n - 1)
    {
      double vi = gsl_vector_get(v, i);

      if (vi == gsl_vector_get(v, i + 1))
        {
          size_t j = i + 2;
          size_t k;
          double rank = 0.0;

          /* we have detected a tie, find number of equal elements */
          while (j < n && vi == gsl_vector_get(v, j))
            ++j;

          /* compute rank */
          for (k = i; k < j; ++k)
            rank += k + 1.0;

          /* divide by number of ties */
          rank /= (double) (j - i);

          for (k = i; k < j; ++k)
            gsl_vector_set(v, k, rank);

          i = j;
        }
      else
        {
          /* no tie - set rank to natural ordered position */
          gsl_vector_set(v, i, i + 1.0);
          ++i;
        }
    }

  if (i == n - 1)
    gsl_vector_set(v, n - 1, (double) n);

  return GSL_SUCCESS;
} /* compute_rank() */


/**
 * @file piecewise_regimes.c
 * @author Carl Boettiger <cboettig@gmail.com>
 * @date 17 May 2010
 *
 * @section LICENSE
 *
 * GPLv3
 *
 * @section DESCRIPTION
 *
 * Code for computing the likelihood of a multitype OU process on a phylogenetic tree
 *
 */

#include "likelihood.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_errno.h>
#include "optimizers.h"
#include "mvn.h"

/**
* @brief Function which wraps general format used by siman and multimin
*        into the likelihood format of calc_lik
*
* @param v vector of values being optimized by the algorithm
* @param params specifies the tree and observed data
*
* @return minus log likelihood
*/
double optim_func (const gsl_vector *v, void *params)
{
  tree * mytree = (tree *) params;
  int i, n_regimes = mytree->n_regimes;
// Force root to match Theta of regime assigned to it, should also reduce vector size!
  mytree->Xo[0] = mytree->theta[mytree->regimes[0]];
 // mytree->Xo[0] = gsl_vector_get(v, 0);
  for(i = 0; i < n_regimes; i++){
    mytree->alpha[i] = v->data[i];
    mytree->theta[i] = v->data[n_regimes+i];
    mytree->sigma[i] = v->data[2*n_regimes+i];

    if( (mytree->alpha[i] < 0) | (mytree->sigma[i] < 0) ){ return GSL_POSINF; }
    if(  mytree->theta[i] < 0 ){ return GSL_POSINF; }
  }

  double llik = 0;
    calc_lik(mytree->Xo, mytree->alpha, mytree->theta, mytree->sigma,
         mytree->regimes, mytree->ancestor, mytree->branch_length,
             mytree->traits, &(mytree->n_nodes), mytree->lca_matrix, &llik);
    return -llik;
}

/** Fit the model by maximum likelihood, called by R */
void fit_model(double * Xo,
  double * alpha,
  double * theta,
  double * sigma,
  int * regimes,
  int * ancestor,
  double * branch_length,
  double * traits,
  int * n_nodes,
  int * n_regimes,
  double * llik,
  int * use_siman)
{

//  Perhaps don't want to see gsl_errors once we're handling in R?
//  Still want better/smarter behavior of the optimizer
  gsl_set_error_handler_off ();

    /** First, initialize the tree structure to facilitate passing all these elements */
  int i,j;
  tree * mytree = (tree  *) malloc(sizeof(tree));
  mytree->Xo = Xo;
  mytree->alpha = alpha;
  mytree->theta = theta;
  mytree->sigma = sigma;
  mytree->regimes = regimes;
  mytree->ancestor = ancestor;
  mytree->branch_length = branch_length;
  mytree->traits = traits;
  mytree->n_nodes = *n_nodes;
  mytree->n_regimes = *n_regimes;

    /** Second, save time by storing the least common ancestor matrix */
  /* Save time by calculating least common ancestor ahead of time.
   * Though this does the calc for all nodes, only tip ones are used.
   * Current get_lca algorithm is only designed for tips anyway.  */
  double sep; // separation time, not actually used
  mytree->lca_matrix = (int *) malloc(gsl_pow_2(*n_nodes) * sizeof(int) );
  for(i=0; i < *n_nodes; i++){
    for(j=0; j < *n_nodes; j++){
      mytree->lca_matrix[*n_nodes*i+j] = get_lca(i,j, *n_nodes, ancestor, branch_length, &sep);
    }
  }

    /** Then initialize the vector of parameters to be searched */
  gsl_vector *x = gsl_vector_alloc(3 * *n_regimes);
//  gsl_vector_set(x, 0, *Xo);  /* Forces Xo to match theta (?) */
  for(i = 0; i < *n_regimes; i++){
    gsl_vector_set(x, i, alpha[i]);
    gsl_vector_set(x, *n_regimes+i, theta[i]);
    gsl_vector_set(x, 2 * *n_regimes+i, mytree->sigma[i]);
  }

    /** Select and call a likelihood optimization routine
     *  given the tree (with data) and parameters */
  gsl_rng * rng = gsl_rng_alloc(gsl_rng_default);
  if(*use_siman)
    *llik = siman(x, mytree, rng);
  else
    *llik = multimin(x, mytree);

  gsl_rng_free(rng);
  gsl_vector_free(x);
  free(mytree->lca_matrix);
  free(mytree);
}

/* Externally exported function to determine LCA matrix */
/**
* @brief Determine the last common ancestor matrix of the tips,
* used in the likelihood calculation
*
* @param ancestor ancestor of the specified nide
* @param branch_length double branch lengths below each of the nodes
* @param n_nodes: integer number of nodes (including tips) in the tree
* @param lca_matrix must be an integer array of size n_nodes^2
*/
void calc_lca(int * ancestor, double * branch_length,
              int * n_nodes, int * lca_matrix)
{
  int i,j;
  /* Save time by calculating least common ancestor ahead of time.
   * Though this does the calc for all nodes, only tip ones are used.
   * Current get_lca algorithm is only designed for tips anyway.  */
  double sep; // separation time, not actually used
  for(i=0; i < *n_nodes; i++){
    for(j=0; j < *n_nodes; j++){
      lca_matrix[*n_nodes*i+j] = get_lca(i,j, *n_nodes, ancestor,
                                               branch_length, &sep);
    }
  }

}

/** Simulate under the model.  Called by R */
void simulate_model (double * Xo, double * alpha, double * theta,
                     double * sigma, int * regimes, int * ancestor,
                     double * branch_length, double * traits, int * n_nodes,
                     int * n_regimes, double * llik, double * seed)
{

  /* Turn off likelihood calcuation on the simulated data for speed */
  int return_likelihood = 0; // consider making an option at the .C level

  int i,j;
  tree * mytree = (tree  *) malloc(sizeof(tree));
  mytree->Xo = Xo;
  mytree->alpha = alpha;
  mytree->theta = theta;
  mytree->sigma = sigma;
  mytree->regimes = regimes;
  mytree->ancestor = ancestor;
  mytree->branch_length = branch_length;
  mytree->traits = traits;
  mytree->n_nodes = *n_nodes;
  mytree->n_regimes = *n_regimes;

  /* Save time by calculating least common ancestor ahead of time. */
  double sep; // separation time, not actually used
  mytree->lca_matrix = (int *) malloc(gsl_pow_2(*n_nodes) * sizeof(int) );
  for(i=0; i < *n_nodes; i++){
    for(j=0; j < *n_nodes; j++){
      mytree->lca_matrix[*n_nodes*i+j] =
                get_lca(i,j, *n_nodes, ancestor, branch_length, &sep);
    }
  }

    /* Allocate the parameter vector */
  gsl_vector *x = gsl_vector_alloc(1 + 3 * *n_regimes);
  gsl_vector_set(x, 0, *Xo);
  for(i = 0; i < *n_regimes; i++){
    gsl_vector_set(x, 1+i, alpha[i]);
    gsl_vector_set(x, 1+*n_regimes+i, theta[i]);
    gsl_vector_set(x, 1+2 * *n_regimes+i, mytree->sigma[i]);
  }

  gsl_rng * rng = gsl_rng_alloc(gsl_rng_default);
  gsl_rng_set(rng, *seed);

   /* These are the only lines that differ from the fit_models wrapper */
  simulate (rng, mytree);

  if(return_likelihood){
    calc_lik(mytree->Xo, mytree->alpha, mytree->theta, mytree->sigma,
             mytree->regimes, mytree->ancestor, mytree->branch_length,
                   mytree->traits, &(mytree->n_nodes), mytree->lca_matrix, llik);
  }

//  for(i=0; i< mytree->n_regimes; i++) printf("%g %g %g\n", mytree->alpha[i], mytree->theta[i], mytree->sigma[i] );
//  printf("sim llik: %g\n", *llik);

  gsl_rng_free(rng);
  gsl_vector_free(x);
  free(mytree->lca_matrix);
  free(mytree);
}

/** Test code by running in pure C */
int main(void)
{
  /** Define the tree and the trait data (Anoles tree from "ouch" R package) */
  int n_nodes = 45;

  double branch_length[] = { 0.0, 12./38, 20./38,  2./38,  2./38,  4./38,
               8./38,  5./38,  5./38,  5./38,  5./38, 10./38,
               9./38,  4./38,  8./38,  2./38, 20./38,  2./38,
               4./38,  2./38, 1./38,  2./38, 26./38, 4./38,
               2./38, 2./38,  2./38,  2./38, 15./38, 10./38,
               10./38, 10./38, 10./38, 16./38, 12./38,  4./38,
               2./38,  2./38, 10./38,  8./38,  2./38,  1./38,
               1./38,  2./38,  2./38};

/* Almost a star tree */
/*  double branch_length[] = { 0.000, .001, .001,  .001,  .001,
			     0.001, .001, .001,  .001,  .001,
			     0.001, .001, .001,  .001,  .001,
			     0.001, .001, .001,  .001,  .001,
			     0.001, .001, .999,  .999,  .999,
			     0.999, .999, .999,  .999,  .999,
			     0.999, .999, .999,  .999,  .999,
			     0.999, .999, .999,  .999,  .999,
			     0.999, .999, .999,  .999,  .999 };
*/

  int ancestor[] = {-1,  0,  1,  2,  3,  2,  0,  6,  7,  8,  9,
                     8,  7, 12, 13,14,  6, 16, 17, 18, 19, 18, 1,
                     3,  4,  4,  5,  5,  9, 10, 10, 11, 11, 12,
                    13, 14, 15, 15, 16, 17, 19, 20, 20, 21, 21};

  double traits[] = {0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
                     0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
                     0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
                     0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
                     0.000000, 0.000000, 2.602690, 2.660260, 2.660260,
                     2.653242, 2.674149, 2.701361, 7.161247, 13.299534,
                     3.328627, 3.353407, 3.360375, 3.049273, 2.906901,
                     2.980619, 2.933857, 3.975530, -3.104587, 3.346389,
                     12.928524, -12.939162, 7.990720, 8.058707, -5.068053};

/*
  int regimes[] =  {1, 1, 2, 2, 2,
                    2, 1, 1, 0, 0,
                    0, 0, 1, 1, 1,
                    1, 1, 1, 1, 1,
                    1, 1, 2, 2, 2,
                    2, 2, 2, 0, 0,
                    0, 0, 0, 1, 1,
                    1, 1, 0, 1, 1,
                    1, 1, 1, 1, 1};
*/

  int regimes[] =   {0, 0, 0, 0, 0,
                    0, 0, 0, 0, 0,
                    0, 0, 0, 0, 0,
                    0, 0, 0, 0, 0,
                    0, 0, 0, 0, 0,
                    0, 0, 0, 0, 0,
                    0, 0, 0, 0, 0,
                    0, 0, 0, 0, 0,
                    0, 0, 0, 0, 0};

  double Xo = 3;

  double alpha[3] = {2.6, 2.6, 2.6};
  double theta[3] = {3., 3.,3};
  double sigma[3] = {0.2, 0.2, 0.2 };
  int n_regimes = 3;

  double llik = 0;

  /* Initialize the lca matrix */
  int * lca = (int *) calloc(n_nodes*n_nodes, sizeof(int));

  /* test the pure likelihood function first */
  calc_lca(ancestor, branch_length, &n_nodes, lca);

  calc_lik(&Xo, alpha, theta, sigma, regimes, ancestor,
           branch_length, traits, &n_nodes, lca, &llik);
  printf("log likelihood: %g\n", llik);

  /* We'll use the faster Nelder-Mead algorithm for the check */
  int use_siman = 0;
  /* Test the fitting routine */

  fit_model(&Xo, alpha, theta, sigma, regimes, ancestor, branch_length,
            traits, &n_nodes, &n_regimes, &llik, &use_siman);
  printf("Xo = %g\n", Xo);
  printf("alphas: %g %g %g\n", alpha[0], alpha[1], alpha[2]);
  printf("thetas: %g %g %g\n", theta[0], theta[1], theta[2]);
  printf("sigmas: %g %g %g\n", sigma[0], sigma[1], sigma[2]);
  printf("log likelihood: %g\n", llik);

  /* Test the simulation code */
/*
  double seed = 1.0;
  printf("seed: %lo\n", (unsigned long int) seed);
  simulate_model(&Xo, alpha, theta, sigma, regimes, ancestor, branch_length,
                 traits, &n_nodes, &n_regimes, &llik, &seed);
  int i;
  printf("simulated traits:\n");
  for(i=0; i < 45; i++)
    printf("%g ", traits[i]);
  printf("\n");
*/

  free(lca);
  return 0;
}


/* Copyright (c) 2016 PaddlePaddle Authors. All Rights Reserve.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License. */

#ifndef MATHFUNCTIONS_H_
#define MATHFUNCTIONS_H_

#ifdef PADDLE_USE_MKL
#include <mkl.h>
#include <mkl_lapacke.h>
#else
extern "C" {
#include <cblas.h>
}
#ifdef PADDLE_USE_ATLAS
extern "C" {
#include <clapack.h>
}
#else
#include <lapacke.h>
#endif
#endif

#include <cmath>

namespace paddle {

template <class T>
void gemm(const CBLAS_TRANSPOSE transA,
          const CBLAS_TRANSPOSE transB,
          const int M,
          const int N,
          const int K,
          const T alpha,
          const T* A,
          const int lda,
          const T* B,
          const int ldb,
          const T beta,
          T* C,
          const int ldc);

template <class T>
int getrf(const CBLAS_ORDER Order,
          const int M,
          const int N,
          T* A,
          const int lda,
          int* ipiv);

template <class T>
int getri(
    const CBLAS_ORDER Order, const int N, T* A, const int lda, const int* ipiv);

template <class T>
void axpy(const int n, const T alpha, const T* x, T* y);

template <class T>
T dotProduct(const int n, const T* x, const T* y);

template <class T>
void vExp(const int n, const T* a, T* r);

template <class T>
void vPow(const int n, const T* a, const T b, T* r);

template <class T>
void vLog(const int n, const T* a, T* r);

template <class T>
void vAdd(const int n, const T* a, const T* b, T* r);

template <class T>
void vInvSqrt(const int n, const T* a, T* r);

template <class T>
void vLog1p(const int n, const T* a, T* r);

template <class T>
void vTanh(const int n, const T* a, T* r);

}  // namespace paddle

#endif  // MATHFUNCTIONS_H_


#pragma once

#include <utility>
#include <array>
#include <vector>
#include <unordered_map>
#include <DirectXMathExtend.h>
#include <gsl.h>
#include <VertexTraits.h>
#include <minmax>

namespace Geometrics
{
	template <typename IndexType>
	struct Triangle
	{
		static constexpr size_t VertexCount = 3;
		typedef std::array<IndexType, VertexCount> container_type;
		container_type v;

		typedef IndexType value_type;
		typedef size_t size_type;
		typedef ptrdiff_t difference_type;
		typedef IndexType *pointer;
		typedef const IndexType *const_pointer;
		typedef IndexType& reference;
		typedef const IndexType& const_reference;

		typedef typename container_type::iterator iterator;
		typedef typename container_type::const_iterator const_iterator;

		typedef typename container_type::reverse_iterator reverse_iterator;
		typedef typename container_type::const_reverse_iterator const_reverse_iterator;

		Triangle() {}
		Triangle(const IndexType &v0, const IndexType &v1, const IndexType &v2)
		{
			v[0] = v0; v[1] = v1; v[2] = v2;
		}
		Triangle(const IndexType *v_)
		{
			v[0] = v_[0]; v[1] = v_[1]; v[2] = v_[2];
		}
		template <class S> explicit Triangle(const S &x)
		{
			v[0] = x[0];  v[1] = x[1];  v[2] = x[2];
		}
		IndexType &operator[] (int i) { return v[i]; }
		const IndexType &operator[] (int i) const { return v[i]; }
		operator const IndexType * () const { return &(v[0]); }
		operator IndexType * () { return &(v[0]); }

		void rewind() {
			std::swap(v[0], v[2]);
		}

		int indexof(IndexType v_) const
		{
			return (v[0] == v_) ? 0 :
				(v[1] == v_) ? 1 :
				(v[2] == v_) ? 2 : -1;
		}

		// contrainer access
		auto begin() { return v.begin(); }
		auto end() { return v.end(); }
		auto begin() const { return v.begin(); }
		auto end() const { return v.end(); }

		auto rbegin() { return v.rbegin(); }
		auto rend() { return v.rend(); }
		auto rbegin() const { return v.rbegin(); }
		auto rend() const { return v.rend(); }

		size_t size() const { return VertexCount; }
	};

	// generate per vertex normal for given triangle mesh
	template <typename VertexType, typename IndexType>
	bool generate_normal(gsl::span<VertexType> vertices, gsl::span<const Triangle<IndexType>> facets)
	{
		using namespace DirectX::VertexTraits;
		using namespace DirectX;

		if (!has_normal<VertexType>::value)
			return false;

		//static_assert(has_normal<VertexType>::value, "The vertex type dose not contains normal field");

		std::vector<XMVECTOR, XMAllocator> normals(vertices.size());

		// set zero
		std::memset(normals.data(), 0, normals.size() * sizeof(XMVECTOR));

		XMVECTOR v0, v1, v2, n;
		for (const auto& face : facets)
		{
			v0 = get_position(vertices[face[0]]);
			v1 = get_position(vertices[face[1]]);
			v2 = get_position(vertices[face[2]]);
			v1 -= v0;
			v2 -= v0;

			v1 = XMVector3Normalize(v1);
			v2 = XMVector3Normalize(v2);
			n = XMVector3Cross(v1, v2); // weighted normal

			normals[face[0]] += n;
			normals[face[1]] += n;
			normals[face[2]] += n;
		}

		for (size_t i = 0; i < vertices.size(); i++)
		{
			n = XMVector3Normalize(normals[i]);
			set_normal(vertices[i], n);
		}

		return true;
	}

	// generate per vertex tangent for given triangle mesh
	template <typename VertexType, typename IndexType>
	bool generate_tangent(gsl::span<VertexType> vertices, gsl::span<const Triangle<IndexType>> facets)
	{
		using namespace DirectX::VertexTraits;
		using namespace DirectX;

		if (!has_tangent<VertexType>::value)
			return false;
		//static_assert(has_tangent<VertexType>::value, "The vertex type dose not contains tangent field");

		std::vector<XMVECTOR, XMAllocator> tan1(vertices.size() * 2);
		XMVECTOR* tan2 = &tan1[vertices.size()];
		std::memset(tan1.data(), 0, tan1.size() * sizeof(XMVECTOR));

		for (const auto& face : facets)
		{
			{
				XMVECTOR v0, v1, v2, w0, w1, w2;
				v0 = get_position(vertices[face[0]]);
				w0 = get_uv(vertices[face[0]]);

				v1 = get_position(vertices[face[1]]);
				w1 = get_uv(vertices[face[1]]);

				v2 = get_position(vertices[face[2]]);
				w2 = get_uv(vertices[face[2]]);
			}
			XMFLOAT4A v_1, v_2, w_1, w_2;

			XMStoreA(v_1, v1 - v0);
			XMStoreA(v_2, v2 - v0);
			XMStoreA(w_1, w1 - w0);
			XMStoreA(w_2, w2 - w0);

			float x1 = v_1.x; float x2 = v_2.x;
			float y1 = v_1.y; float y2 = v_2.y;
			float z1 = v_1.z; float z2 = v_2.z;

			float s1 = w_1.x; float s2 = w_2.x;
			float t1 = w_1.y; float t2 = w_2.y;

			float r = 1.0F / (s1 * t2 - s2 * t1);
			XMVECTOR sdir = XMVectorSet(
				(t2 * x1 - t1 * x2) * r,
				(t2 * y1 - t1 * y2) * r,
				(t2 * z1 - t1 * z2) * r, .0f);
			XMVECTOR tdir = XMVectorSet(
				(s1 * x2 - s2 * x1) * r,
				(s1 * y2 - s2 * y1) * r,
				(s1 * z2 - s2 * z1) * r, .0f);

			tan1[face[0]] += sdir;
			tan1[face[1]] += sdir;
			tan1[face[2]] += sdir;

			tan2[face[0]] += tdir;
			tan2[face[1]] += tdir;
			tan2[face[2]] += tdir;
		}

		for (size_t i = 0; i < vertices.size(); i++)
		{
			XMVECTOR n = get_normal(vertices[i], n);
			XMVECTOR t = tan1[i];

			// Gram-Schmidt orthogonalize
			XMVECTOR nt = XMVector3Normalize(t - n * XMVector3Dot(n, t));

			XMVECTOR w = XMVectorLess(XMVector3Dot(XMVector3Cross(n, t), tan2[i]), XMVectorZero());
			w = XMVectorSelect(g_XMNegativeOne.v, g_XMOne.v, w);

			nt = _DXMEXT XMVectorSelect<0, 0, 0, 1>(nt, w);
			set_tangent(vertices[i], nt);
		}

		return true;
	}

	template <typename _VertexType, typename _IndexType = uint16_t, typename _FaceType = Triangle<_IndexType>>
	class PolygonSoup
	{
	public:
		typedef _VertexType VertexType;
		typedef _IndexType	IndexType;
		typedef _FaceType	FaceType;

		static constexpr size_t PolygonVertex = FaceType::VertexCount;

		PolygonSoup() {}

		bool empty() const {
			return vertices.empty() || indices.empty();
		}

		void clear()
		{
			vertices.clear();
			indices.clear();
		}

		inline const FaceType& facet(int idx) const
		{
			return reinterpret_cast<const FaceType&>(indices[idx * FaceType::VertexCount]);
		}

		inline FaceType& facet(int idx)
		{
			return reinterpret_cast<FaceType&>(indices[idx * FaceType::VertexCount]);
		}

		void add_facet(const FaceType& new_facet)
		{
			indices.insert(indices.end(), new_facet.begin(), new_facet.end());
		}

		template <class... _TIndecies>
		std::enable_if_t<sizeof...(_TIndecies) == FaceType::VertexCount> add_facet(_TIndecies... _indices)
		{
			indices.insert(indices.end(), { static_cast<IndexType>(_indices)... });
		}

		gsl::span<FaceType> facets() {
			return gsl::span<FaceType>(
				reinterpret_cast<FaceType*>(indices.data()),
				indices.size() / FaceType::VertexCount);
		}

		gsl::span<const FaceType> facets() const {
			return gsl::span<const FaceType>(
				reinterpret_cast<const FaceType*>(indices.data()),
				indices.size() / FaceType::VertexCount);
		}

		const VertexType& vertex(int facet, int vidx) const
		{
			return this->vertices[facet * FaceType::VertexCount + vidx];
		}

		VertexType& vertex(int facet, int vidx)
		{
			return this->vertices[facet * FaceType::VertexCount + vidx];
		}

		// flip all the polygons' winding and vertices' normal
		void flip()
		{
			using namespace DirectX::VertexTraits;

			if (has_normal<VertexType>::value)
			{
				for (auto& v : vertices)
					set_normal(v, -get_normal(v));
			}

			static constexpr int vc = FaceType::VertexCount;
			for (int i = 0; i < indices.size() / vc; i++)
				std::reverse(indices.begin() + i * vc, indices.begin() + (i + 1)* vc);
		}

		void generate_normal()
		{
			static_assert(PolygonVertex == 3, "This mesh is not trigle mesh, please trianglize it first");
			generate_normal<VertexType,IndexType>(this->vertices, this->facets());
		}

		void generate_tangent()
		{
			static_assert(PolygonVertex == 3, "This mesh is not trigle mesh, please trianglize it first");
			generate_tangent<VertexType, IndexType>(this->vertices, this->facets());
		}

		// applies an uniform transform to all vertices in the mesh
		void XM_CALLCONV transform(DirectX::FXMMATRIX M)
		{
			using namespace DirectX::VertexTraits;
			for (auto& v : vertices)
			{
				XMVECTOR p = get_position(v);
				p = _DXMEXT XMVector3TransformCoord(p, M);
				set_position(v, p);

				if (has_normal<VertexType>::value)
				{
					p = get_normal(v);
					p = _DXMEXT XMVector3TransformNormal(p, M);
					set_normal(v, p);

					if (has_tangent<VertexType>::value)
					{
						p = get_tangent(v);
						p = _DXMEXT XMVector3TransformNormal(p, M);
						set_tangent(v, p);
					}
				}
			}
		}

		std::vector<VertexType> vertices;
		std::vector<IndexType>	indices;
	};

	template <class _VertexType, class _IndexType>
	int XM_CALLCONV intersect(const PolygonSoup<_VertexType, _IndexType, Triangle<_IndexType>> &Mesh, DirectX::FXMVECTOR Origin, DirectX::FXMVECTOR Direction, std::vector<float>* distances = nullptr)
	{
		using namespace DirectX;
		using namespace DirectX::VertexTraits;
		int count = 0;
		XMVECTOR vDir = XMVector3Normalize(Direction);
		for (const auto& tri : Mesh.facets())
		{
			float distance;

			XMVECTOR v0 = get_position(&Mesh.vertices[tri[0]]);
			XMVECTOR v1 = get_position(&Mesh.vertices[tri[1]]);
			XMVECTOR v2 = get_position(&Mesh.vertices[tri[2]]);

			bool hr = DirectX::TriangleTests::Intersects(Origin, vDir, v0, v1, v2, distance);
			if (hr) {
				++count;
				if (distances) {
					distances->push_back(distances);
				}
			}
		}
		return count;
	}

	struct MeshRayIntersectionInfo
	{
		using Vector3 = DirectX::Vector3;

		Vector3 position;
		float	distance;
		Vector3 barycentric;
		int		facet; // facet id

		inline bool operator < (const MeshRayIntersectionInfo& rhs)
		{
			return this->distance < rhs.distance;
		}
	};

	/// <summary>
	/// Basic triangle mesh, each index represent an edge, which is the edge oppsite to the vertex in it's owner triangle
	/// </summary>
	template <typename _VertexType, typename _IndexType = uint16_t>
	class TriangleMesh : public PolygonSoup<_VertexType, _IndexType, Triangle<_IndexType>>
	{
	public:
		static const size_t VertexCount = FaceType::VertexCount;

	public:
		// edge's reverse edge
		// stores the adjacent edges of a edge in a triangle
		std::vector<IndexType> revedges;

		// vertex's first adjacant edge
		// std::vector<IndexType> vedges;

		union EdgeType
		{
			_IndexType v[2];
			struct {
				_IndexType v0, v1;
			};
		};

		inline EdgeType edge(int facet, int edge) const
		{
			EdgeType e;
			auto& tri = this->facet(facet);
			switch (edge)
			{
			case 0:
				e.v0 = tri[1];
				e.v1 = tri[2];
				break;
			case 1:
				e.v0 = tri[2];
				e.v1 = tri[0];
				break;
			case 2:
				e.v0 = tri[0];
				e.v1 = tri[1];
				break;
			default:
				e.v0 = -1;
				e.v1 = -1;
				break;
			}
			return e;
		}

		inline EdgeType edge(int eid) const
		{
			return edge(eid / VertexCount, eid % VertexCount);
		}

		inline int adjacentFacet(int facet, int edge) const
		{
			return revedges[facet * VertexCount + edge] / VertexCount;
		}

		inline int adjacentFacet(int eid) const
		{
			return revedges[eid] / VertexCount;
		}

		// build the adjacent map so we can access all the 1 rings
		void build()
		{
			// intialize all adjacant edges to -1
			revedges.resize(this->indices.size(), -1);
			_IndexType vsize = this->vertices.size();
			int esize = this->indices.size();

			using hash_type = uint64_t;
			// make sure int for hash is enough
			assert(vsize*vsize < std::numeric_limits<hash_type>::max());
			std::unordered_map<hash_type, _IndexType> edges(esize * 2);
			// max items inside this table should be less than (esize / 2)

			_IndexType fid = 0;
			for (const Triangle<_IndexType>& tri : this->facets())
			{
				for (_IndexType i = 0; i < VertexCount; i++)
				{
					_IndexType eid = i + fid * 3;
					auto e = edge(fid, i);

					hash_type ehash = e.v0 * vsize + e.v1;
					hash_type revehash = e.v0 + e.v1 * vsize;

					auto revItr = edges.find(revehash);
					if (revItr == edges.end())
					{
						edges[ehash] = fid + 0;
					}
					else // find reversed edge, remove from edges map
					{
						auto revEid = revItr->second;
						revedges[eid] = revEid;
						revedges[revEid] = eid;
						edges.erase(revItr);
					}
				}

				++fid;
			}
		}

		int XM_CALLCONV intersect(DirectX::FXMVECTOR Origin, DirectX::FXMVECTOR Direction, std::vector<MeshRayIntersectionInfo>* output) const
		{
			//assert(revedges.size() == indices.size());
			using namespace DirectX;
			using namespace DirectX::VertexTraits;
			size_t count = 0;
			XMVECTOR vDir = XMVector3Normalize(Direction);
			auto fid = 0;
			for (const auto& tri : this->facets())
			{
				float distance;

				XMVECTOR v0 = get_position(&this->vertices[tri[0]]);
				XMVECTOR v1 = get_position(&this->vertices[tri[1]]);
				XMVECTOR v2 = get_position(&this->vertices[tri[2]]);

				bool hr = DirectX::TriangleTests::Intersects(Origin, vDir, v0, v1, v2, distance);
				if (hr) {
					++count;
					if (output) {
						output->emplace_back();
						auto& info = output->back();
						XMVECTOR pos = distance * vDir + Origin;
						XMVECTOR bc = DirectX::TriangleTests::BarycentricCoordinate(pos, v0, v1, v2);
						info.facet = fid;
						info.position = pos;
						info.distance = distance;
						info.barycentric = bc;
					}
				}
				++fid;
			}

			if (output)
				std::sort(output->begin(), output->end());
			return count;
		}
	};

	namespace Internal
	{
#ifndef min
		template <typename T>
		inline T clamp(T value, T minV, T maxV)
		{
			return std::max(std::min(value, maxV), minV);
		}
#else
		template <typename T>
		inline T clamp(T value, T minV, T maxV)
		{
			return max(min(value, maxV), minV);
		}
#endif
	}

	/// <summary>
	/// Compute the closest projection point from P0 to triangle(V0,V1,V2).
	/// </summary>
	/// <param name="P0">The p0.</param>
	/// <param name="V0">The v0.</param>
	/// <param name="V1">The v1.</param>
	/// <param name="V2">The v2.</param>
	/// <returns></returns>
	inline DirectX::XMVECTOR XM_CALLCONV Projection(DirectX::FXMVECTOR P0, DirectX::FXMVECTOR V0, DirectX::FXMVECTOR V1, DirectX::GXMVECTOR V2)
	{
		using namespace DirectX;
		using namespace Geometrics::Internal;
		XMVECTOR edge0 = V1 - V0;
		XMVECTOR edge1 = V2 - V0;
		XMVECTOR p0 = V0 - P0;

		//XMVectorBaryCentric();

		float a = XMVectorGetX(_DXMEXT XMVector3LengthSq(edge0));
		float b = XMVectorGetX(_DXMEXT XMVector3Dot(edge0, edge1));
		float c = XMVectorGetX(_DXMEXT XMVector3LengthSq(edge1));
		float d = XMVectorGetX(_DXMEXT XMVector3Dot(edge0, p0));
		float e = XMVectorGetX(_DXMEXT XMVector3Dot(edge1, p0));

		float det = a*c - b*b;
		float s = b*e - c*d;
		float t = b*d - a*e;

		if (s + t < det)
		{
			if (s < 0.f)
			{
				if (t < 0.f)
				{
					if (d < 0.f)
					{
						s = clamp(-d / a, 0.f, 1.f);
						t = 0.f;
					}
					else
					{
						s = 0.f;
						t = clamp(-e / c, 0.f, 1.f);
					}
				}
				else
				{
					s = 0.f;
					t = clamp(-e / c, 0.f, 1.f);
				}
			}
			else if (t < 0.f)
			{
				s = clamp(-d / a, 0.f, 1.f);
				t = 0.f;
			}
			else
			{
				float invDet = 1.f / det;
				s *= invDet;
				t *= invDet;
			}
		}
		else
		{
			if (s < 0.f)
			{
				float tmp0 = b + d;
				float tmp1 = c + e;
				if (tmp1 > tmp0)
				{
					float numer = tmp1 - tmp0;
					float denom = a - 2 * b + c;
					s = clamp(numer / denom, 0.f, 1.f);
					t = 1 - s;
				}
				else
				{
					t = clamp(-e / c, 0.f, 1.f);
					s = 0.f;
				}
			}
			else if (t < 0.f)
			{
				if (a + d > b + e)
				{
					float numer = c + e - b - d;
					float denom = a - 2 * b + c;
					s = clamp(numer / denom, 0.f, 1.f);
					t = 1 - s;
				}
				else
				{
					s = clamp(-e / c, 0.f, 1.f);
					t = 0.f;
				}
			}
			else
			{
				float numer = c + e - b - d;
				float denom = a - 2 * b + c;
				s = clamp(numer / denom, 0.f, 1.f);
				t = 1.f - s;
			}
		}

		return V0 + s * edge0 + t * edge1;
	}

	inline float XM_CALLCONV Distance(DirectX::FXMVECTOR P0, DirectX::FXMVECTOR V0, DirectX::FXMVECTOR V1, DirectX::GXMVECTOR V2)
	{
		using namespace DirectX;
		XMVECTOR vProj = Projection(P0, V0, V1, V2);
		vProj -= P0;
		return XMVectorGetX(_DXMEXT XMVector3Length(vProj));
	}

	template <typename _VertexType, typename _IndexType>
	inline float XM_CALLCONV Distance(const TriangleMesh<_VertexType, _IndexType> &Mesh, DirectX::FXMVECTOR Point)
	{
		using namespace DirectX;
		float minDis = numeric_limits<float>::max();
		for (const auto& tri : Mesh.facets())
		{
			float dis;

			XMVECTOR v0 = XMLoadFloat3(&Mesh.vertices[tri[0]].position);
			XMVECTOR v1 = XMLoadFloat3(&Mesh.vertices[tri[1]].position);
			XMVECTOR v2 = XMLoadFloat3(&Mesh.vertices[tri[2]].position);

			dis = Distance(Point, v0, v1, v2);
			if (dis < minDis) minDis = dis;
			//minDis = std::min(dis,minDis);
		}

		//XMVECTOR vDis = XMVectorReplicate(numeric_limits<float>::max());
		//for (const auto& vertex : Mesh.vertices)
		//{
		//	XMVECTOR vPos = XMLoadFloat3(&vertex.position);
		//	vPos -= Point;
		//	XMVECTOR dis = XMVector3Length(vPos);
		//	vDis = XMVectorMin(vDis,dis);
		//}

		//float minDisV = XMVectorGetX(vDis);
		//assert(minDisV >= minDis);
		return minDis;
		//return XMVectorGetX(minDis);
	}

	template <typename _VertexType, typename _IndexType>
	bool XM_CALLCONV Inside(const TriangleMesh<_VertexType, _IndexType> &Mesh, DirectX::FXMVECTOR Point)
	{
		XMFLOAT3A Direction;
		Direction.x = (float)std::rand() / (RAND_MAX + 1);
		Direction.y = (float)std::rand() / (RAND_MAX + 1);
		Direction.z = (float)std::rand() / (RAND_MAX + 1);
		XMVECTOR vDir = XMLoadFloat3A(&Direction);
		auto count = intersect(Mesh, Point, vDir, nullptr);
		return count & 1; //count % 2
	}

}