#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <stdlib.h>
#include <math.h>
#include <inttypes.h>
#include <string.h>
#include <adept_source.h>
#include <adept.h>
#include <adept_arrays.h>
using adept::adouble;
using adept::aVector;

template<typename Return, typename... T>
Return __enzyme_autodiff(T...);
extern "C" {
  extern int enzyme_dup;
  extern int enzyme_const;
  extern int enzyme_dupnoneed;
}

float tdiff(struct timeval *start, struct timeval *end) {
  return (end->tv_sec-start->tv_sec) + 1e-6*(end->tv_usec-start->tv_usec);
}

#define BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
#define BOOST_NO_EXCEPTIONS
#include <iostream>
#include <boost/array.hpp>

#include <boost/numeric/odeint.hpp>

#include <boost/throw_exception.hpp>
void boost::throw_exception(std::exception const & e){
    //do nothing
}

using namespace std;
using namespace boost::numeric::odeint;

#define N 32
#define xmin 0.
#define xmax 1.
#define ymin 0.
#define ymax 1.

#include <assert.h>
#define RANGE(min, max, i, N) ((max-min)/(N-1)*i + min)
#define GETnb(x, i, j) (x)[N*i+j]
#define GET(x, i, j) GETnb(x, i, j)
//#define GET(x, i, j) ({ assert(i >=0); assert( j>=0); assert(j<N); assert(j<N); GETnb(x, i, j); })

template <typename T>
T brusselator_f(T x, T y, T t) {
  bool eq1 = ((x-0.3)*(x-0.3) + (y-0.6)*(y-0.6)) <= 0.1*0.1;
  bool eq2 = t >= 1.1;
  if (eq1 && eq2) {
    return T(5);
  } else {
    return T(0);
  }
}

void init_brusselator(double* __restrict u, double* __restrict v) {
  for(int i=0; i<N; i++) {
    for(int j=0; j<N; j++) {

      double x = RANGE(xmin, xmax, i, N);
      double y = RANGE(ymin, ymax, j, N);

      GETnb(u, i, j) = 22*(y*(1-y))*sqrt(y*(1-y));
      GETnb(v, i, j) = 27*(x*(1-x))*sqrt(x*(1-x));
    }
  }
}

__attribute__((noinline))
void brusselator_2d_loop(double* __restrict du, double* __restrict dv, const double* __restrict u, const double* __restrict v, const double* __restrict p, double t) {
  double A = p[0];
  double B = p[1];
  double alpha = p[2];
  double dx = (double)1/(N-1);

  alpha = alpha/(dx*dx);

  for(int i=0; i<N; i++) {
    for(int j=0; j<N; j++) {

      double x = RANGE(xmin, xmax, i, N);
      double y = RANGE(ymin, ymax, j, N);

      unsigned ip1 = (i == N-1) ? i : (i+1);
      unsigned im1 = (i == 0) ? i : (i-1);

      unsigned jp1 = (j == N-1) ? j : (j+1);
      unsigned jm1 = (j == 0) ? j : (j-1);

      double u2v = GET(u, i, j) * GET(u, i, j) * GET(v, i, j);

      GETnb(du, i, j) = alpha*( GET(u, im1, j) + GET(u, ip1, j) + GET(u, i, jp1) + GET(u, i, jm1) - 4 * GET(u, i, j))
                      + B + u2v - (A + 1)*GET(u, i, j) + brusselator_f(x, y, t);

      GETnb(dv, i, j) = alpha*( GET(v, im1, j) + GET(v, ip1, j) + GET(v, i, jp1) + GET(v, i, jm1) - 4 * GET(v, i, j))
                      + A * GET(u, i, j) - u2v;
    }
  }
}

typedef boost::array< double , 2 * N * N > state_type;

void lorenz( const state_type &x , state_type &dxdt , double t )
{
    // Extract the parameters
  double p[3] = { /*A*/ 3.4, /*B*/ 1, /*alpha*/10. };
  brusselator_2d_loop(dxdt.c_array(), dxdt.c_array() + N * N, x.data(), x.data() + N * N, p, t);
}

// init_brusselator(x.c_array(), x.c_array() + N*N)

double foobar(const double* p, const state_type x, const state_type adjoint, double t) {
    double dp[3] = { 0. };

    state_type dx = { 0. };

    state_type dadjoint_inp = adjoint;

    state_type dxdu;

    __enzyme_autodiff<void>(brusselator_2d_loop,
//                            enzyme_dup, dxdu.c_array(), dadjoint_inp.c_array(),
//                            enzyme_dup, dxdu.c_array() + N * N, dadjoint_inp.c_array() + N * N,
                            enzyme_dupnoneed, nullptr, dadjoint_inp.data(),
                            enzyme_dupnoneed, nullptr, dadjoint_inp.data() + N * N,
                            enzyme_dup, x.data(), dx.data(),
                            enzyme_dup, x.data() + N * N, dx.data() + N * N,
                            enzyme_dup, p, dp,
                            enzyme_const, t);

    return dx[0];
}

#undef GETnb
#define GETnb(x, i, j) (x)(N*i+j)

void abrusselator_2d_loop(aVector& du, aVector& dv, aVector& u, aVector& v, aVector& p, double t) {
  adouble A = p(0);
  adouble B = p(1);
  adouble alpha = p(2);
  adouble dx = (double)1/(N-1);

  alpha = alpha/(dx*dx);

  for(int i=0; i<N; i++) {
    for(int j=0; j<N; j++) {

      adouble x = RANGE(xmin, xmax, i, N);
      adouble y = RANGE(ymin, ymax, j, N);

      unsigned ip1 = (i == N-1) ? i : (i+1);
      unsigned im1 = (i == 0) ? i : (i-1);

      unsigned jp1 = (j == N-1) ? j : (j+1);
      unsigned jm1 = (j == 0) ? j : (j-1);

      adouble u2v = GET(u, i, j) * GET(u, i, j) * GET(v, i, j);

      GETnb(du, i, j) = alpha*( GET(u, im1, j) + GET(u, ip1, j) + GET(u, i, jp1) + GET(u, i, jm1) - 4 * GET(u, i, j))
                      + B + u2v - (A + 1)*GET(u, i, j) + brusselator_f<adouble>(x, y, t);

      GETnb(dv, i, j) = alpha*( GET(v, im1, j) + GET(v, ip1, j) + GET(v, i, jp1) + GET(v, i, jm1) - 4 * GET(v, i, j))
                      + A * GET(u, i, j) - u2v;
    }
  }
}

double afoobar(const double* p_in, const state_type x, const state_type adjoint, double t) {
    adept::Stack stack;

    aVector p(3);
    for(unsigned i=0; i<3; i++) p(i) = p_in[i];
    aVector ax(N*N);
    aVector ay(N*N);
    for(unsigned i=0; i<N*N; i++) {
      ax(i) = x[i];
      ay(i) = x[i+N*N];
    }

    aVector dxdu(N*N);
    aVector dydu(N*N);

    stack.new_recording();

    abrusselator_2d_loop(dxdu, dydu, ax, ay, p, t);

    for(unsigned i=0; i<N*N; i++) {
      dxdu(i).set_gradient(adjoint[i]);
      dydu(i).set_gradient(adjoint[i+N*N]);
    }
    stack.compute_adjoint();

    return ax(0).get_gradient();
}

//! Tapenade
extern "C" {
  /*        Generated by TAPENADE     (INRIA, Ecuador team)
    Tapenade 3.15 (master) -  8 Jan 2020 10:48
*/
#include <adBuffer.h>

/*
  Differentiation of get in reverse (adjoint) mode (with options i4 dr8 r4):
   gradient     of useful results: *x get
   with respect to varying inputs: *x
   Plus diff mem management of: x:in
*/
void get_b(const double *x, double *xb, unsigned int i, unsigned int j, double getb)
{
    double get;
    xb[N*i + j] = xb[N*i + j] + getb;
}

double get_nodiff(const double *x, unsigned int i, unsigned int j) {
    return x[N*i + j];
}

double brusselator_f_nodiff(double x, double y, double t) {
    if ((x-0.3)*(x-0.3) + (y-0.6)*(y-0.6) <= 0.1*0.1 && t >= 1.1)
        return 5.0;
    else
        return 0.0;
}

#if 1
void brusselator_2d_loop_b(double *du, double *dub, double *dv, double *dvb,
        const double *u, double *ub, const double *v, double *vb, const double *p, double *pb,
        double t) {
    double A = p[0];
    double Ab = 0.0;
    double B = p[1];
    double Bb = 0.0;
    double alpha = p[2];
    double alphab = 0.0;
    double dx = (double)1/(N-1);
    alpha = alpha/(dx*dx);
    for (int i = 0; i < N; ++i)
        for (int j = 0; j < N; ++j) {
            double x = (xmax-xmin)/(N-1)*i + xmin;
            double y = (ymax-ymin)/(N-1)*j + ymin;
            unsigned int ip1 = (i == N - 1 ? i : i + 1);
            unsigned int im1 = (i == 0 ? i : i - 1);
            unsigned int jp1 = (j == N - 1 ? j : j + 1);
            unsigned int jm1 = (j == 0 ? j : j - 1);
            double u2v = u[N*i+j]*u[N*i+j]*v[N*i+j];
            double result1;
            pushInteger4(jm1);
            pushInteger4(jp1);
            pushInteger4(im1);
            pushInteger4(ip1);
        }
    *ub = 0.0;
    *vb = 0.0;
    alphab = 0.0;
    Ab = 0.0;
    Bb = 0.0;
    for (int i = N-1; i > -1; --i)
        for (int j = N-1; j > -1; --j) {
            double x;
            double y;
            unsigned int ip1;
            unsigned int im1;
            unsigned int jp1;
            unsigned int jm1;
            double u2v;
            double u2vb = 0.0;
            double result1;
            double temp;
            double tempb;
            popInteger4((int*)&ip1);
            popInteger4((int*)&im1);
            popInteger4((int*)&jp1);
            popInteger4((int*)&jm1);
            temp = u[N*i + j];
            alphab = alphab + (v[N*im1+j]+v[N*ip1+j]+v[N*i+jp1]+v[N*i+jm1]
                -4*v[N*i+j])*dvb[N*i+j] + (u[N*im1+j]+u[N*ip1+j]+u[N*i+
                jp1]+u[N*i+jm1]-4*u[N*i+j])*dub[N*i+j];
            tempb = alpha*dvb[N*i+j];
            Ab = Ab + u[N*i+j]*dvb[N*i+j] - u[N*i+j]*dub[N*i+j];
            ub[N*i + j] = ub[N*i + j] + A*dvb[N*i+j] - (A+1)*dub[N*i+j];
            u2vb = dub[N*i + j] - dvb[N*i + j];
            dvb[N*i + j] = 0.0;
            vb[N*im1 + j] = vb[N*im1 + j] + tempb;
            vb[N*ip1 + j] = vb[N*ip1 + j] + tempb;
            vb[N*i + jp1] = vb[N*i + jp1] + tempb;
            vb[N*i + jm1] = vb[N*i + jm1] + tempb;
            vb[N*i + j] = vb[N*i + j] + temp*temp*u2vb - 4*tempb;
            tempb = alpha*dub[N*i+j];
            Bb = Bb + dub[N*i + j];
            dub[N*i + j] = 0.0;
            ub[N*im1 + j] = ub[N*im1 + j] + tempb;
            ub[N*ip1 + j] = ub[N*ip1 + j] + tempb;
            ub[N*i + jp1] = ub[N*i + jp1] + tempb;
            ub[N*i + jm1] = ub[N*i + jm1] + tempb;
            ub[N*i + j] = ub[N*i + j] + 2*temp*v[N*i+j]*u2vb - 4*tempb;
        }
    alphab = alphab/(dx*dx);
    pb[2] = pb[2] + alphab;
    pb[1] = pb[1] + Bb;
    pb[0] = pb[0] + Ab;

}
#else
/*
  Differentiation of brusselator_2d_loop in reverse (adjoint) mode (with options i4 dr8 r4):
   gradient     of useful results: *du *dv
   with respect to varying inputs: *p *u *du *v *dv
   RW status of diff variables: *p:out *u:out *du:in-out *v:out
                *dv:in-out
   Plus diff mem management of: p:in u:in du:in v:in dv:in
*/
void brusselator_2d_loop_b(double *du, double *dub, double *dv, double *dvb,
        const double *u, double *ub, const double *v, double *vb, const double *p, double *pb,
        double t) {
    double A = p[0];
    double Ab = 0.0;
    double B = p[1];
    double Bb = 0.0;
    double alpha = p[2];
    double alphab = 0.0;
    double dx = (double)1/(N-1);
    alpha = alpha/(dx*dx);
    for (int i = 0; i < N; ++i)
        for (int j = 0; j < N; ++j) {
            double x = (xmax-xmin)/(N-1)*i + xmin;
            double y = (ymax-ymin)/(N-1)*j + ymin;
            unsigned int ip1 = (i == N - 1 ? i : i + 1);
            unsigned int im1 = (i == 0 ? i : i - 1);
            unsigned int jp1 = (j == N - 1 ? j : j + 1);
            unsigned int jm1 = (j == 0 ? j : j - 1);
            double u2v;
            double result1;
            double result2;
            double result3;
            double result4;
            double result5;
            double result6;
            double result7;
            result1 = get_nodiff(u, i, j);
            result2 = get_nodiff(u, i, j);
            result3 = get_nodiff(v, i, j);
            pushReal8(result1);
            result1 = get_nodiff(u, im1, j);
            pushReal8(result2);
            result2 = get_nodiff(u, ip1, j);
            pushReal8(result3);
            result3 = get_nodiff(u, i, jp1);
            result4 = get_nodiff(u, i, jm1);
            result5 = get_nodiff(u, i, j);
            result6 = get_nodiff(u, i, j);
            pushReal8(result1);
            result1 = get_nodiff(v, im1, j);
            pushReal8(result2);
            result2 = get_nodiff(v, ip1, j);
            pushReal8(result3);
            result3 = get_nodiff(v, i, jp1);
            pushReal8(result4);
            result4 = get_nodiff(v, i, jm1);
            pushReal8(result5);
            result5 = get_nodiff(v, i, j);
            pushReal8(result6);
            result6 = get_nodiff(u, i, j);
            pushInteger4(jm1);
            pushInteger4(jp1);
            pushReal8(result6);
            pushReal8(result5);
            pushReal8(result4);
            pushReal8(result3);
            pushReal8(result2);
            pushReal8(result1);
            pushInteger4(im1);
            pushInteger4(ip1);
        }
    *ub = 0.0;
    *vb = 0.0;
    alphab = 0.0;
    Ab = 0.0;
    Bb = 0.0;
    for (int i = N-1; i > -1; --i)
        for (int j = N-1; j > -1; --j) {
            double x;
            double y;
            unsigned int ip1;
            unsigned int im1;
            unsigned int jp1;
            unsigned int jm1;
            double u2v;
            double u2vb;
            double result1;
            double result1b;
            double result2;
            double result2b;
            double result3;
            double result3b;
            double result4;
            double result4b;
            double result5;
            double result5b;
            double result6;
            double result6b;
            double result7;
            double tempb;
            popInteger4((int*)&ip1);
            popInteger4((int*)&im1);
            popReal8(&result1);
            popReal8(&result2);
            popReal8(&result3);
            popReal8(&result4);
            popReal8(&result5);
            popReal8(&result6);
            popInteger4((int*)&jp1);
            popInteger4((int*)&jm1);
            alphab = alphab + (result1+result2+result3+result4-4*result5)*dvb[
                N*i+j];
            tempb = alpha*dvb[N*i+j];
            Ab = Ab + result6*dvb[N*i+j];
            result6b = A*dvb[N*i+j];
            u2vb = dub[N*i + j] - dvb[N*i + j];
            dvb[N*i + j] = 0.0;
            result1b = tempb;
            result2b = tempb;
            result3b = tempb;
            result4b = tempb;
            result5b = -(4*tempb);
            popReal8(&result6);
            get_b(u, ub, i, j, result6b);
            popReal8(&result5);
            get_b(v, vb, i, j, result5b);
            popReal8(&result4);
            get_b(v, vb, i, jm1, result4b);
            popReal8(&result3);
            get_b(v, vb, i, jp1, result3b);
            popReal8(&result2);
            get_b(v, vb, ip1, j, result2b);
            popReal8(&result1);
            get_b(v, vb, im1, j, result1b);
            alphab = alphab + (result1+result2+result3+result4-4*result5)*dub[
                N*i+j];
            tempb = alpha*dub[N*i+j];
            Bb = Bb + dub[N*i + j];
            Ab = Ab - result6*dub[N*i+j];
            result6b = -((A+1)*dub[N*i+j]);
            dub[N*i + j] = 0.0;
            result1b = tempb;
            result2b = tempb;
            result3b = tempb;
            result4b = tempb;
            result5b = -(4*tempb);
            get_b(u, ub, i, j, result6b);
            get_b(u, ub, i, j, result5b);
            get_b(u, ub, i, jm1, result4b);
            popReal8(&result3);
            get_b(u, ub, i, jp1, result3b);
            popReal8(&result2);
            get_b(u, ub, ip1, j, result2b);
            popReal8(&result1);
            get_b(u, ub, im1, j, result1b);
            result1b = result2*result3*u2vb;
            result2b = result1*result3*u2vb;
            result3b = result1*result2*u2vb;
            get_b(v, vb, i, j, result3b);
            get_b(u, ub, i, j, result2b);
            get_b(u, ub, i, j, result1b);
        }
    alphab = alphab/(dx*dx);
    pb[2] = pb[2] + alphab;
    pb[1] = pb[1] + Bb;
    pb[0] = pb[0] + Ab;
}
#endif
}

double tfoobar(const double* p, const state_type x, const state_type adjoint, double t) {
    double dp[3] = { 0. };

    state_type dx = { 0. };

    state_type dadjoint_inp = adjoint;

    state_type dxdu;

    brusselator_2d_loop_b(nullptr, dadjoint_inp.data(),
                          nullptr, dadjoint_inp.data() + N * N,
                          x.data(), dx.data(),
                          x.data() + N * N, dx.data() + N * N,
                          p, dp,
                          t);

    return dx[0];
}

//! Main
int main(int argc, char** argv) {
  const double p[3] = { /*A*/ 3.4, /*B*/ 1, /*alpha*/10. };

  state_type x;
  init_brusselator(x.data(), x.data() + N * N);

  state_type adjoint;
  init_brusselator(adjoint.data(), adjoint.data() + N * N);

  double t = 2.1;

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res;
  for(int i=0; i<10000; i++)
  res = afoobar(p, x, adjoint, t);

  gettimeofday(&end, NULL);
  printf("Adept combined %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res;
  for(int i=0; i<10000; i++)
  res = tfoobar(p, x, adjoint, t);

  gettimeofday(&end, NULL);
  printf("Tapenade combined %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double res;
  for(int i=0; i<10000; i++)
  res = foobar(p, x, adjoint, t);

  gettimeofday(&end, NULL);
  printf("Enzyme combined %0.6f res=%f\n", tdiff(&start, &end), res);
  }
  //printf("res=%f\n", foobar(1000));
}

#if 0

typedef boost::array< double , 6 > state_type;

void lorenz( const state_type &x , state_type &dxdt , double t )
{
    // Extract the parameters
    double k1 = x[3];
    double k2 = x[4];
    double k3 = x[5];

    dxdt[0] = -k1 * x[0] + k3 * x[1] * x[2];
    dxdt[1] = k1 * x[0] - k2 * x[1] * x[1] - k3 * x[1] * x[2];
    dxdt[2] = k2 * x[1] * x[1];

    // Don't change the parameters p
    dxdt[3] = 0;
    dxdt[4] = 0;
    dxdt[5] = 0;
}

double foobar(double* p, uint64_t iters) {
    state_type x = { 1.0, 0, 0, p[0], p[1], p[2] }; // initial conditions
    double t = 1e5;
    typedef controlled_runge_kutta< runge_kutta_dopri5< state_type , typename state_type::value_type , state_type , double > > stepper_type;
    //typedef euler< state_type , typename state_type::value_type , state_type , double > stepper_type;
    integrate_const( stepper_type(), lorenz , x , 0.0 , t, t/iters );

    return x[0];
}

typedef boost::array< adouble , 6 > astate_type;

void alorenz( const astate_type &x , astate_type &dxdt , adouble t )
{
    // Extract the parameters
    adouble k1 = x[3];
    adouble k2 = x[4];
    adouble k3 = x[5];

    dxdt[0] = -k1 * x[0] + k3 * x[1] * x[2];
    dxdt[1] = k1 * x[0] - k2 * x[1] * x[1] - k3 * x[1] * x[2];
    dxdt[2] = k2 * x[1] * x[1];

    // Don't change the parameters p
    dxdt[3] = 0;
    dxdt[4] = 0;
    dxdt[5] = 0;
}

adouble afoobar(adouble* p, uint64_t iters) {
    astate_type x = { 1.0, 0, 0, p[0], p[1], p[2] }; // initial conditions
    double t = 1e5;
    typedef controlled_runge_kutta< runge_kutta_dopri5< astate_type , typename astate_type::value_type , astate_type , adouble > > stepper_type;
    //typedef euler< astate_type , typename astate_type::value_type , astate_type , adouble > stepper_type;
    integrate_const( stepper_type(), alorenz , x , 0.0 , t, t/iters );

    return x[0];
}

static
double afoobar_and_gradient(double* p_in, double* dp_out, uint64_t iters) {
    adept::Stack stack;
    adouble x[3] = { p_in[0], p_in[1], p_in[2] };
    stack.new_recording();
    adouble y = afoobar(x, iters);
    y.set_gradient(1.0);
    stack.compute_adjoint();
    for(int i=0; i<3; i++)
      dp_out[i] = x[i].get_gradient();
    return y.value();
}

static void adept_sincos(uint64_t iters) {
  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double p[3] = { 0.04,3e7,1e4 };
  double res = foobar(p, iters);

  gettimeofday(&end, NULL);
  printf("Adept real %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  adept::Stack stack;
  adouble p[3] = { 0.04,3e7,1e4 };
 // stack.new_recording();
  adouble resa = afoobar(p, iters);
  double res = resa.value();

  gettimeofday(&end, NULL);
  printf("Adept forward %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double p[3] = { 0.04,3e7,1e4 };
  double dp[3] = { 0 };
  afoobar_and_gradient(p, dp, iters);

  gettimeofday(&end, NULL);
  printf("Adept combined %0.6f res'=%f\n", tdiff(&start, &end), dp[0]);
  }
}

static void enzyme_sincos(double inp, uint64_t iters) {

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double p[3] = { 0.04,3e7,1e4 };
  double res = foobar(p, iters);

  gettimeofday(&end, NULL);
  printf("Enzyme real %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double p[3] = { 0.04,3e7,1e4 };
  double res = foobar(p, iters);

  gettimeofday(&end, NULL);
  printf("Enzyme forward %0.6f res=%f\n", tdiff(&start, &end), res);
  }

  {
  struct timeval start, end;
  gettimeofday(&start, NULL);

  double p[3] = { 0.04,3e7,1e4 };
  double dp[3] = { 0 };
  __enzyme_autodiff<void>(foobar, p, dp, iters);

  gettimeofday(&end, NULL);
  printf("Enzyme combined %0.6f res'=%f\n", tdiff(&start, &end), dp[0]);
  }
}

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

  int max_iters = atoi(argv[1]) ;
  double inp = 2.1;

  //for(int iters=max_iters/20; iters<=max_iters; iters+=max_iters/20) {
  auto iters = max_iters;
    printf("iters=%d\n", iters);
    adept_sincos(inp, iters);
    enzyme_sincos(inp, iters);
  //}
}
#endif


#pragma once
#include "world.hpp"
#include <Eigen/Core>
#include <cstdint>

using RenderBuffer = Eigen::Matrix<uint8_t, world_size, world_size>;

RenderBuffer render_world(const World &world, int channel) {
  RenderBuffer result;
  for (int y=0; y<world_size; y++) {
    for (int x=0; x<world_size; x++) {
      result(y, x) = 0;
      auto blend = [&](Eigen::Vector3i rgb, float alpha=1.0) {
        float value = alpha * rgb[channel] + (1.0 - alpha) * result(y, x);
        result(y, x) = std::round(value);
      };
      switch (world.pixels_[y*world_size+x].block) {
        case Food: blend({200, 80, 80}); break;
        case Wall: blend({255, 255, 255}); break;
      }
      if (world.pixels_[y*world_size+x].pheromone_1) {
        blend({0, 128, 0}, 0.4);
      }
    }
  }
  for (auto &a: world.agents_) {
    result(a.y, a.x) = 0;
    if (channel == 1) result(a.y, a.x) = 255;
  }
  return result;
}


/**
 * @file ann_dist_test.cpp
 * @author Atharva Khandait
 *
 * Tests the ann distributions.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/dists/bernoulli_distribution.hpp>
#include <mlpack/methods/ann/init_rules/random_init.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

#include <mlpack/methods/ann/activation_functions/logistic_function.hpp>

using namespace mlpack;
using namespace mlpack::ann;

BOOST_AUTO_TEST_SUITE(ANNDistTest);

/**
 * Simple bernoulli distribution module test.
 */
BOOST_AUTO_TEST_CASE(SimpleBernoulliDistributionTest)
{
  arma::mat param = arma::mat("1 1 0");
  BernoulliDistribution<> module(std::move(param), false);

  arma::mat sample = module.Sample();
  // As the probabilities are [1, 1, 0], the bernoulli samples should be
  // [1, 1, 0] as well.
  CheckMatrices(param, sample);
}

/**
 * Jacobian bernoulli distribution module test when we don't apply logistic.
 */
BOOST_AUTO_TEST_CASE(JacobianBernoulliDistributionTest)
{
  for (size_t i = 0; i < 5; i++)
  {
    const size_t targetElements = math::RandInt(2, 1000);

    arma::mat param;
    param.randn(targetElements, 1);

    arma::mat target;
    target.randn(targetElements, 1);

    BernoulliDistribution<> module(std::move(param), false);

    const double perturbation = 1e-6;
    double outputA, outputB, original;
    arma::mat jacobianA, jacobianB;

    // Initialize the jacobian matrix.
    jacobianA = arma::zeros(targetElements, 1);

    for (size_t j = 0; j < targetElements; ++j)
    {
      original = module.Probability()(j);
      module.Probability()(j) = original - perturbation;
      outputA = module.LogProbability(std::move(target));
      module.Probability()(j) = original + perturbation;
      outputB = module.LogProbability(std::move(target));
      module.Probability()(j) = original;
      outputB -= outputA;
      outputB /= 2 * perturbation;
      jacobianA(j) = outputB;
    }

    module.LogProbBackward(std::move(target), std::move(jacobianB));
    BOOST_REQUIRE_LE(arma::max(arma::max(arma::abs(jacobianA - jacobianB))),
        1e-5);
  }
}

/**
 * Jacobian bernoulli distribution module test when we apply logistic.
 */
BOOST_AUTO_TEST_CASE(JacobianBernoulliDistributionLogisticTest)
{
  for (size_t i = 0; i < 5; i++)
  {
    const size_t targetElements = math::RandInt(2, 1000);

    arma::mat param;
    param.randn(targetElements, 1);

    arma::mat target;
    target.randn(targetElements, 1);

    BernoulliDistribution<> module(std::move(param));

    const double perturbation = 1e-6;
    double outputA, outputB, original;
    arma::mat jacobianA, jacobianB;

    // Initialize the jacobian matrix.
    jacobianA = arma::zeros(targetElements, 1);

    for (size_t j = 0; j < targetElements; ++j)
    {
      original = module.Logits()(j);
      module.Logits()(j) = original - perturbation;
      LogisticFunction::Fn(module.Logits(), module.Probability());
      outputA = module.LogProbability(std::move(target));
      module.Logits()(j) = original + perturbation;
      LogisticFunction::Fn(module.Logits(), module.Probability());
      outputB = module.LogProbability(std::move(target));
      module.Logits()(j) = original;
      LogisticFunction::Fn(module.Logits(), module.Probability());
      outputB -= outputA;
      outputB /= 2 * perturbation;
      jacobianA(j) = outputB;
    }

    module.LogProbBackward(std::move(target), std::move(jacobianB));
    BOOST_REQUIRE_LE(arma::max(arma::max(arma::abs(jacobianA - jacobianB))),
        3e-5);
  }
}

BOOST_AUTO_TEST_SUITE_END();


#include <Eigen/Core>
#include <Eigen/Sparse>
#include <vector>

namespace ials11 {
using Real = float;
using IndexType = std::size_t;
using SparseMatrix = Eigen::SparseMatrix<Real, Eigen::RowMajor>;
using DenseMatrix =
    Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
using DenseVector = Eigen::Matrix<Real, Eigen::Dynamic, 1>;
} // namespace ials11


#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Cholesky>
#include <adept_arrays.h>
#include <chrono>

using namespace adept;
using namespace std;
using namespace std::chrono;
int main(int argc, char **argv)
{
   string output_filename = argv[1];
   cout << output_filename << endl;

   int num_params = 4010;
   int num_vars = 1;

   Stack stack;
   aVector  k(num_params);

   Eigen::VectorXd args(num_params * num_vars);
   Eigen::VectorXd ders(num_params * num_vars);

   std::ifstream file("./tests/utils/params.txt");
   int i = 0;
   for (std::string line; std::getline(file, line);)
   {
       args(i) = stod(line.c_str());
       i++;
   }
   file.close();

   for (int index = 0; index < num_params; index++) {
			k(index) = args[index * num_vars + 0];
    }

   ofstream outfile;
   outfile.open(output_filename);

   auto start = high_resolution_clock::now();
   stack.new_recording();                 // Clear any existing differential statements
   aReal J = sum(((k*k+3*k)-k/4)/k+k*k*k*k+k*k*(22/7*k)+k*k*k*k*k*k*k*k*k);
   J.set_gradient(1.0);                   // Seed the dependent variable
   stack.reverse();                       // Reverse-mode differentiation

   auto stop = high_resolution_clock::now();
   auto duration = duration_cast<microseconds>(stop - start);
   outfile << (double)duration.count() / 1000000.0 << " ";
   for (int index = 0; index < num_params; index++) {
			ders[index * num_vars + 0] = k.get_gradient()[index];
    }
   for (int i = 0; i < num_params * num_vars; i++)
   {
        ostringstream ss;
        ss.precision(3);
       ss << fixed << ders[i];
       outfile << ss.str() << " ";
   }

   outfile.close();

   return 0;
}


#ifndef CVUTILS_HPP
#define CVUTILS_HPP

#include <vector>
#include <opencv2/opencv.hpp>
#include <boost/circular_buffer.hpp>

namespace cvk {

inline boost::circular_buffer<cv::Scalar> create_color_palette(int n, double scale = 255.0) {
  std::vector<cv::Vec3b> palette;
  palette.reserve(n);

  for(int i=0; i<n; i++) {
    palette.push_back(cv::Vec3b((180.0 / (n+1)) * i, 220, 220));
  }
  cv::cvtColor(palette, palette, CV_HSV2BGR);

  boost::circular_buffer<cv::Scalar> hsv(n);
  double s = scale / 255.0;
  for(const auto& col: palette) {
    hsv.push_back(cv::Scalar(col[0] * s, col[1] * s, col[2] * s));
  }
  return hsv;
}

inline cv::Rect clip_roi(const cv::Rect& rect, const cv::Size& size) {
  int top = std::max(0, rect.y);
  int left = std::max(0, rect.x);
  int bottom = std::min(size.height, rect.y + rect.height);
  int right = std::min(size.width, rect.x + rect.width);
  return cv::Rect(left, top, right - left, bottom - top);
}

inline cv::Rect enlarge_rect(const cv::Rect& rect, double scale) {
  double dsize = (scale - 1.0) / 2.0;
  return cv::Rect(rect.x - rect.width * dsize, rect.y - rect.height * dsize, rect.width * scale, rect.height * scale);
}

inline cv::Rect shift_rect(const cv::Rect& rect, const cv::Point& pt) {
  return cv::Rect(rect.tl() + pt, rect.size());
}

}

#endif // CVUTILS_HPP


//  (C) Copyright Nick Thompson 2020.
//  (C) Copyright John Maddock 2020.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
#include <iostream>
#include <boost/math/tools/ulps_plot.hpp>
#include <boost/core/demangle.hpp>
#include <nil/crypto3/multiprecision/mpfr.hpp>
#include <nil/crypto3/multiprecision/cpp_bin_float.hpp>

using boost::math::tools::ulps_plot;

int main() {
   using PreciseReal = nil::crypto3::multiprecision::mpfr_float_100;
   using CoarseReal = nil::crypto3::multiprecision::cpp_bin_float_50;

   typedef boost::math::policies::policy<
      boost::math::policies::promote_float<false>,
      boost::math::policies::promote_double<false> >
      no_promote_policy;

   auto ai_coarse = [](CoarseReal const& x)->CoarseReal {
      return log(x);
   };
   auto ai_precise = [](PreciseReal const& x)->PreciseReal {
      return log(x);
   };

   std::string filename = "cpp_bin_float_log.svg";
   int samples = 100000;
   // How many pixels wide do you want your .svg?
   int width = 700;
   // Near a root, we have unbounded relative error. So for functions with roots, we define an ULP clip:
   PreciseReal clip = 20;
   // Should we perturb the abscissas?
   bool perturb_abscissas = false;
   auto plot = ulps_plot<decltype(ai_precise), PreciseReal, CoarseReal>(ai_precise, CoarseReal(0), CoarseReal(200), samples, perturb_abscissas);
   // Note the argument chaining:
   plot.clip(clip).width(width);
   plot.background_color("white").font_color("black");
   // Sometimes it's useful to set a title, but in many cases it's more useful to just use a caption.
   //std::string title = "Airy Ai ULP plot at " + boost::core::demangle(typeid(CoarseReal).name()) + " precision";
   //plot.title(title);
   plot.vertical_lines(6);
   plot.add_fn(ai_coarse);
   // You can write the plot to a stream:
   //std::cout << plot;
   // Or to a file:
   plot.write(filename);
}


#include <fstream>
#include <iostream>
#include <unordered_set>

#include <boost/functional/hash.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/program_options.hpp>

#include <yaml-cpp/yaml.h>

#include "timer.hpp"

struct Location {
  Location(int x, int y) : x(x), y(y) {}
  int x;
  int y;

  bool operator<(const Location& other) const {
    return std::tie(x, y) < std::tie(other.x, other.y);
  }

  bool operator==(const Location& other) const {
    return std::tie(x, y) == std::tie(other.x, other.y);
  }

  friend std::ostream& operator<<(std::ostream& os, const Location& c) {
    return os << "(" << c.x << "," << c.y << ")";
  }
};

namespace std {
template <>
struct hash<Location> {
  size_t operator()(const Location& s) const {
    size_t seed = 0;
    boost::hash_combine(seed, s.x);
    boost::hash_combine(seed, s.y);
    return seed;
  }
};
}

#include "shortest_path_heuristic.hpp"

int main(int argc, char* argv[]) {
  namespace po = boost::program_options;
  // Declare the supported options.
  po::options_description desc("Allowed options");
  std::string inputFile;
  std::string outputFile;
  desc.add_options()("help", "produce help message")(
      "input,i", po::value<std::string>(&inputFile)->required(),
      "input file (YAML)")("output,o",
                           po::value<std::string>(&outputFile)->required(),
                           "output file (CSV)");

  try {
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
      std::cout << desc << "\n";
      return 0;
    }
  } catch (po::error& e) {
    std::cerr << e.what() << std::endl << std::endl;
    std::cerr << desc << std::endl;
    return 1;
  }

  YAML::Node config = YAML::LoadFile(inputFile);

  std::unordered_set<Location> obstacles;
#if 0
  typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::undirectedS > searchGraphTraits_t;
  typedef searchGraphTraits_t::vertex_descriptor vertex_t;
  typedef searchGraphTraits_t::edge_descriptor edge_t;

  struct Vertex
  {
  };

  struct Edge
  {
    float weight;
  };

  typedef boost::adjacency_list<
          boost::vecS, boost::vecS, boost::undirectedS,
          Vertex, Edge>
          searchGraph_t;
  typedef boost::exterior_vertex_property<searchGraph_t, float> distanceProperty_t;
  typedef distanceProperty_t::matrix_type distanceMatrix_t;
  typedef distanceProperty_t::matrix_map_type distanceMatrixMap_t;

  searchGraph_t searchGraph;
  std::map<Location, vertex_t> mapLocToVertex;
#endif
  const auto& dim = config["map"]["dimensions"];
  int dimx = dim[0].as<int>();
  int dimy = dim[1].as<int>();

  for (const auto& node : config["map"]["obstacles"]) {
    obstacles.insert(Location(node[0].as<int>(), node[1].as<int>()));
  }
#if 0
  // add vertices
  for (int x = 0; x < dimx; ++x) {
    for (int y = 0; y < dimy; ++y) {
      Location l(x, y);
      auto v = boost::add_vertex(searchGraph);
      mapLocToVertex[l] = v;
    }
  }

  // add edges
  for (int x = 0; x < dimx; ++x) {
    for (int y = 0; y < dimy; ++y) {
      Location l(x, y);
      if (obstacles.find(l) == obstacles.end()) {
        Location right(x+1, y);
        if (x < dimx - 1 && obstacles.find(right) == obstacles.end()) {
          auto e = boost::add_edge(mapLocToVertex[l], mapLocToVertex[right], searchGraph);
          searchGraph[e.first].weight = 1;
        }
        Location below(x, y+1);
        if (y < dimy - 1 && obstacles.find(below) == obstacles.end()) {
          auto e = boost::add_edge(mapLocToVertex[l], mapLocToVertex[below], searchGraph);
          searchGraph[e.first].weight = 1;
        }
      }
    }
  }

  distanceMatrix_t m_shortestDistance(boost::num_vertices(searchGraph));
  distanceMatrixMap_t distanceMap(m_shortestDistance, searchGraph);
  boost::floyd_warshall_all_pairs_shortest_paths(searchGraph, distanceMap, boost::weight_map(boost::get(&Edge::weight, searchGraph)));

  std::ofstream fileStream(outputFile.c_str());
  fileStream << dimx << "," << dimy << std::endl;
  for (size_t row = 0; row < dimx * dimy; ++row) {
    for (size_t column = 0; column < m_shortestDistance[row].size(); ++column) {
      fileStream << m_shortestDistance[row][column] << ",";
    }
    fileStream << std::endl;
  }
#endif

  ShortestPathHeuristic h(dimx, dimy, obstacles);
  std::cout << h.getValue(Location(0, 0), Location(3, 0)) << std::endl;

  return 0;
}


#include <Eigen/Core>
#include <iostream>
#include <LBFGS.h>

using Eigen::VectorXd;
using namespace LBFGSpp;

typedef struct VectorRep {
  double *vdata;
  Eigen::Index vsize;
} VectorRep;

// A function that can be optimized
typedef double (*optfun) (const double *x, double *grad);

// C++ wrapper around a C function pointer, for use with LBFGS++
class WrappedFun {
private:
  optfun f;
public:
  WrappedFun (optfun f_in) { f = f_in; }
  double operator()(const VectorXd& x, VectorXd& grad) {
    return f (((VectorRep*)&x)->vdata, ((VectorRep*)&grad)->vdata);
  }
};

// External entrypoint to call LBFGS++
extern "C" double optimize_lbfgs (uint32_t size, optfun f, double *init_xs) {
  // Set up parameters
  LBFGSParam<double> param;
  param.epsilon = 1e-6;
  param.max_iterations = 100;

  // Create solver and function objects, and return value slot
  VectorRep init_rep = { init_xs, size };
  VectorXd *vect = (VectorXd*)&init_rep;
  LBFGSSolver<double> solver(param);
  WrappedFun fun (f);
  double val;

  // Call solver
  solver.minimize (fun, *vect, val);
  return val;
}


#include <iostream>
#include <map>
using std::map;
#include <string>
using std::string;
#include <boost/shared_ptr.hpp>
using boost::shared_ptr;

#include "Resources.h"

#include "flint/app/App.h"
#include "flint/cairo/Cairo.h"
#include "flint/Fill.h"

#define TEST_SVG
#define TEST_PDF
#define TEST_PNG

using namespace fli;

struct TestBase {
 virtual void run( fli::cairo::Context &ctx ) = 0;
};

// Arc
struct TestArc : public TestBase {
	virtual void run( fli::cairo::Context &ctx ) {
		double xc = 128.0;
		double yc = 128.0;
		double radius = 100.0;
		double angle1 = 45.0  * ( M_PI / 180.0 );  /* angles are specified */
		double angle2 = 180.0 * ( M_PI / 180.0 );  /* in radians           */

		ctx.setLineWidth( 10.0 );
		ctx.arc( xc, yc, radius, angle1, angle2 );
		ctx.stroke();

		/* draw helping lines */
		ctx.setSourceRgba( 1, 0.2, 0.2, 0.6 );
		ctx.setLineWidth( 6.0 );

		ctx.arc( xc, yc, 10.0, 0, 2*M_PI );
		ctx.fill();

		ctx.arc( xc, yc, radius, angle1, angle1 );
		ctx.lineTo( xc, yc);
		ctx.arc( xc, yc, radius, angle2, angle2 );
		ctx.lineTo( xc, yc );
		ctx.stroke();
	}
};

// Arc Negative
struct TestArcNegative : public TestBase {
	virtual void run( fli::cairo::Context &ctx ) {
		double xc = 128.0;
		double yc = 128.0;
		double radius = 100.0;
		double angle1 = 45.0  * ( M_PI / 180.0 );  /* angles are specified */
		double angle2 = 180.0 * ( M_PI / 180.0 );  /* in radians           */

		ctx.setSourceRgb( 0.0, 0.0, 0.0 );
		ctx.setLineWidth( 10.0 );
		ctx.arcNegative( xc, yc, radius, angle1, angle2 );
		ctx.stroke();

		/* draw helping lines */
		ctx.setSourceRgba( 1, 0.2, 0.2, 0.6 );
		ctx.setLineWidth( 6.0 );

		ctx.arc( xc, yc, 10.0, 0, 2*M_PI );
		ctx.fill();

		ctx.arc( xc, yc, radius, angle1, angle1 );
		ctx.lineTo( xc, yc);
		ctx.arc( xc, yc, radius, angle2, angle2 );
		ctx.lineTo( xc, yc );
		ctx.stroke();
	}
};

// clip
struct TestClip : public TestBase {
	virtual void run( fli::cairo::Context &ctx ) {
		ctx.setSourceRgb( 0, 0, 0 );
		ctx.arc( 128.0, 128.0, 76.8, 0, 2 * M_PI );
		ctx.clip();

		ctx.newPath();  // current path is not consumed by ctx.clip()
		ctx.rectangle( 0, 0, 256, 256 );
		ctx.fill();
		ctx.setSourceRgb( 0, 1, 0 );
		ctx.moveTo( 0, 0 );
		ctx.lineTo( 256, 256 );
		ctx.moveTo( 256, 0 );
		ctx.lineTo( 0, 256 );
		ctx.setLineWidth( 10.0 );
		ctx.stroke();
	}
};

// clip image
struct TestClipImage : public TestBase {
	virtual void run( fli::cairo::Context &ctx ) {
		int w, h;

		ctx.arc( 128.0, 128.0, 76.8, 0, 2 * M_PI );
		ctx.clip();
		ctx.newPath(); // path not consumed by clip()

//		fli::shared_ptr<fli::cairo::SurfaceImage> image( fli::cairo::SurfaceImage::createFromPng( "/Users/andrewfb/Code/dt/libdt_2.0/test/fullCairoTest/data/romedalen.png" ) );
		Surface beyonceSurface( loadImage( app::App::loadResource( "romedalen.png", RES_ROMEDALEN_PNG, "PNG" ) ), SurfaceConstraintsCairo() );
		fli::shared_ptr<fli::cairo::SurfaceImage> image( new cairo::SurfaceImage( beyonceSurface ) );
		w = image->getWidth();
		h = image->getHeight();

		ctx.scale( 256.0 / w, 256.0 / h );

		ctx.setSourceSurface( *image, 0, 0 );
		ctx.paint();
	}
};

// curve rectangle
struct TestCurveRectangle : public TestBase {
	virtual void run( fli::cairo::Context &ctx ) {
		double x0 = 25.6, y0 = 25.6, rect_width = 204.8, rect_height = 204.8, radius = 102.4;

		double x1,y1;
		x1 = x0 + rect_width;
		y1 = y0 + rect_height;
		if( ( ! rect_width ) || ( ! rect_height ) )
			return;

		ctx.moveTo( x0, y0 + radius );
        ctx.curveTo( x0 , y0, x0 , y0, x0 + radius, y0 );
        ctx.lineTo( x1 - radius, y0 );
        ctx.curveTo( x1, y0, x1, y0, x1, y0 + radius );
        ctx.lineTo( x1 , y1 - radius );
        ctx.curveTo( x1, y1, x1, y1, x1 - radius, y1 );
        ctx.lineTo( x0 + radius, y1 );
        ctx.curveTo( x0, y1, x0, y1, x0, y1- radius );

        ctx.closePath();

		ctx.setSourceRgb( 0.5, 0.5, 1 );
		ctx.fillPreserve();
		ctx.setSourceRgba( 0.5, 0, 0, 0.5 );
		ctx.setLineWidth( 10.0 );
		ctx.stroke();
	}
};

// curve to
struct TestCurveTo : public TestBase {
	virtual void run( fli::cairo::Context &ctx ) {
		double x=25.6, y=128.0;
		double x1=102.4, y1=230.4, x2=153.6, y2=25.6, x3=230.4, y3=128.0;

		ctx.moveTo( x, y );
		ctx.curveTo( x1, y1, x2, y2, x3, y3 );

		ctx.setLineWidth( 10.0 );
		ctx.stroke();

		ctx.setSourceRgba( 1, 0.2, 0.2, 0.6 );
		ctx.setLineWidth( 6.0 );
		ctx.moveTo( x, y );
		ctx.lineTo( x1, y1 );
		ctx.moveTo( x2, y2 );
		ctx.lineTo( x3, y3 );
		ctx.stroke();
	}
};

// dash
struct TestDash : public TestBase {
	virtual void run( fli::cairo::Context &ctx ) {
		double dashes[] = {50.0,  /* ink */ 10.0,  /* skip */ 10.0,  /* ink */ 10.0   /* skip*/ };
		int ndash  = sizeof( dashes ) / sizeof( dashes[0] );
		double offset = -50.0;

		ctx.setDash( dashes, ndash, offset );
		ctx.setLineWidth( 10.0 );

		ctx.moveTo( 128.0, 25.6 );
		ctx.lineTo( 230.4, 230.4 );
		ctx.relLineTo( -102.4, 0.0 );
		ctx.curveTo( 51.2, 230.4, 51.2, 128.0, 128.0, 128.0 );

		ctx.stroke();
	}
};

// fill and stroke
struct TestFillAndStroke : public TestBase {
	virtual void run( fli::cairo::Context &ctx ) {
		ctx.moveTo( 128.0, 25.6 );
		ctx.lineTo( 230.4, 230.4 );
		ctx.relLineTo( -102.4, 0.0 );
		ctx.curveTo( 51.2, 230.4, 51.2, 128.0, 128.0, 128.0 );
		ctx.closePath();

		ctx.moveTo( 64.0, 25.6 );
		ctx.relLineTo( 51.2, 51.2 );
		ctx.relLineTo( -51.2, 51.2 );
		ctx.relLineTo( -51.2, -51.2 );
		ctx.closePath();

		ctx.setLineWidth( 10.0 );
		ctx.setSourceRgb( 0, 0, 1 );
		ctx.fillPreserve();
		ctx.setSourceRgb( 0, 0, 0 );
		ctx.stroke();
	}
};

// fill style
struct TestFillStyle : public TestBase {
	virtual void run( fli::cairo::Context &ctx ) {
		ctx.setLineWidth( 6 );

		ctx.rectangle( 12, 12, 232, 70 );
		ctx.newSubPath();
		ctx.arc( 64, 64, 40, 0, 2 * M_PI );
		ctx.newSubPath();
		ctx.arcNegative( 192, 64, 40, 0, -2 * M_PI );

		ctx.setFillRule( fli::cairo::FILL_RULE_EVEN_ODD );
		ctx.setSourceRgb( 0, 0.7, 0 );
		ctx.fillPreserve();
		ctx.setSourceRgb( 0, 0, 0 );
		ctx.stroke();

		ctx.translate( 0, 128 );
		ctx.rectangle( 12, 12, 232, 70 );
		ctx.newSubPath();
		ctx.arc( 64, 64, 40, 0, 2 * M_PI );
		ctx.newSubPath();
		ctx.arcNegative( 192, 64, 40, 0, -2 * M_PI );

		ctx.setFillRule( fli::cairo::FILL_RULE_WINDING );
		ctx.setSourceRgb( 0, 0, 0.9);
		ctx.fillPreserve();
		ctx.setSourceRgb( 0, 0, 0);
		ctx.stroke();
	}
};

// gradient
struct TestGradient : public TestBase {
	virtual void run( fli::cairo::Context &ctx ) {
		shared_ptr<fli::cairo::Pattern> pat;

		pat = shared_ptr<fli::cairo::Pattern>( fli::cairo::Pattern::createLinear( 0.0, 0.0, 0.0, 256.0 ) );
		pat->addColorStopRgba( 1, 0, 0, 0, 1 );
		pat->addColorStopRgba( 0, 1, 1, 1, 1);
		ctx.rectangle( 0, 0, 256, 256 );
		ctx.setSource( pat.get() );
		ctx.fill();

		pat = shared_ptr<fli::cairo::Pattern>( fli::cairo::Pattern::createRadial( 115.2, 102.4, 25.6, 102.4, 102.4, 128.0 ) );
		pat->addColorStopRgba( 0, 1, 1, 1, 1 );
		pat->addColorStopRgba( 1, 0, 0, 0, 1 );
		ctx.setSource( pat.get() );
		ctx.arc( 128.0, 128.0, 76.8, 0, 2 * M_PI );
		ctx.fill();
	}
};

// image
struct TestImage : public TestBase {
	virtual void run( fli::cairo::Context &ctx ) {
		Surface beyonceSurface( loadImage( app::App::loadResource( "Beyonce.jpg", RES_BEYONCE_JPG, "JPG" ) ), SurfaceConstraintsCairo() );
		fli::shared_ptr<fli::cairo::SurfaceImage> image( new cairo::SurfaceImage( beyonceSurface ) );

		ctx.translate( 128.0, 128.0 );
		ctx.rotate( 45 * M_PI / 180 );
		ctx.scale( 256.0 / image->getWidth(), 256.0 / image->getHeight() );
		ctx.translate( -0.5 * image->getWidth(), -0.5 * image->getHeight() );

		ctx.setSourceSurface( *image, 0, 0 );
		ctx.paint();
	}
};

// image pattern
struct TestImagePattern : public TestBase {
	virtual void run( fli::cairo::Context &ctx ) {
		Surface beyonceSurface( loadImage( app::App::loadResource( "Beyonce.jpg", RES_BEYONCE_JPG, "JPG" ) ), SurfaceConstraintsCairo() );
		fli::shared_ptr<fli::cairo::SurfaceImage> image( new cairo::SurfaceImage( beyonceSurface ) );
		fli::shared_ptr<fli::cairo::Pattern> pattern( fli::cairo::Pattern::createForSurface( image.get() ) );
		pattern->setExtend( fli::cairo::EXTEND_REPEAT );

		ctx.translate( 128.0, 128.0 );
		ctx.rotate( M_PI / 4 );
		ctx.scale( 1 / sqrt( 2.0 ), 1 / sqrt( 2.0 ) );
		ctx.translate( -128.0, -128.0 );

		fli::cairo::Matrix mtx;
		mtx.initScale( image->getWidth() / 256.0 * 5.0, image->getHeight() / 256.0 * 5.0 );
		pattern->setMatrix( &mtx );

		ctx.setSource( pattern.get() );
		ctx.rectangle( 0, 0, 256.0, 256.0 );
		ctx.fill();
	}
};

// multi segment caps
struct TestMultiSegmentCaps : public TestBase {
	virtual void run( fli::cairo::Context &ctx ) {
		ctx.moveTo( 50.0, 75.0 );
		ctx.lineTo( 200.0, 75.0 );

		ctx.moveTo( 50.0, 125.0 );
		ctx.lineTo( 200.0, 125.0 );

		ctx.moveTo( 50.0, 175.0 );
		ctx.lineTo( 200.0, 175.0 );

		ctx.setLineWidth( 30.0 );
		ctx.setLineCap( fli::cairo::LINE_CAP_ROUND );
		ctx.stroke();
	}
};

// set line cap
struct TestSetLineCap : public TestBase {
	virtual void run( fli::cairo::Context &ctx ) {
		ctx.setLineWidth( 30.0 );
		ctx.setLineCap( fli::cairo::LINE_CAP_BUTT ); /* default */
		ctx.moveTo( 64.0, 50.0 );
		ctx.lineTo( 64.0, 200.0 );
		ctx.stroke();
		ctx.setLineCap( fli::cairo::LINE_CAP_ROUND );
		ctx.moveTo( 128.0, 50.0 );
		ctx.lineTo( 128.0, 200.0 );
		ctx.stroke();
		ctx.setLineCap( fli::cairo::LINE_CAP_SQUARE );
		ctx.moveTo( 192.0, 50.0 );
		ctx.lineTo( 192.0, 200.0 );
		ctx.stroke();

		/* draw helping lines */
		ctx.setSourceRgb( 1, 0.2, 0.2 );
		ctx.setLineWidth( 2.56 );
		ctx.moveTo( 64.0, 50.0 );
		ctx.lineTo( 64.0, 200.0 );
		ctx.moveTo( 128.0, 50.0 );
		ctx.lineTo( 128.0, 200.0 );
		ctx.moveTo( 192.0, 50.0 );
		ctx.lineTo( 192.0, 200.0 );
		ctx.stroke();
	}
};

// set line join
struct TestSetLineJoin : public TestBase {
	virtual void run( fli::cairo::Context &ctx ) {
		ctx.setLineWidth( 40.96 );
		ctx.moveTo( 76.8, 84.48 );
		ctx.relLineTo( 51.2, -51.2 );
		ctx.relLineTo( 51.2, 51.2 );
		ctx.setLineJoin( fli::cairo::LINE_JOIN_MITER );
		ctx.stroke();

		ctx.moveTo( 76.8, 161.28 );
		ctx.relLineTo( 51.2, -51.2 );
		ctx.relLineTo( 51.2, 51.2 );
		ctx.setLineJoin( fli::cairo::LINE_JOIN_BEVEL );
		ctx.stroke();

		ctx.moveTo( 76.8, 238.08 );
		ctx.relLineTo( 51.2, -51.2 );
		ctx.relLineTo( 51.2, 51.2 );
		ctx.setLineJoin( fli::cairo::LINE_JOIN_ROUND );
		ctx.stroke();
	}
};

// text
struct TestText : public TestBase {
	virtual void run( fli::cairo::Context &ctx ) {
		ctx.selectFontFace( "Batang", fli::cairo::FONT_SLANT_NORMAL, fli::cairo::FONT_WEIGHT_BOLD );
		ctx.setFontSize( 90.0 );

		ctx.moveTo( 10.0, 135.0 );
		ctx.showText( "Hello" );

		ctx.moveTo( 70.0, 165.0 );

// This code crashes PDF on the Mac
//	fli::cairo::FontFace *otherFont = new fli::cairo::FontFace( "Baskerville" );
//		cairo_set_font_face( ctx.getCairo(), cf );

		ctx.textPath( "World" );
		ctx.setSourceRgb( 0.5, 0.5, 1 );
		ctx.fillPreserve();
		ctx.setSourceRgb( 0, 0, 0 );
		ctx.setLineWidth( 2.56 );
		ctx.stroke();

		// draw helping lines
		ctx.setSourceRgba( 1, 0.2, 0.2, 0.6 );
		ctx.arc( 10.0, 135.0, 5.12, 0, 2 * M_PI );
		ctx.closePath();
		ctx.arc( 70.0, 165.0, 5.12, 0, 2 * M_PI );
		ctx.fill();
	}
};

// text align center
struct TestTextAlignCenter : public TestBase {
	virtual void run( fli::cairo::Context &ctx ) {
		const std::string text = "cairo";
		double x,y;

		ctx.selectFontFace( "Sans", fli::cairo::FONT_SLANT_NORMAL, fli::cairo::FONT_WEIGHT_NORMAL );

		ctx.setFontSize( 52.0 );
		fli::cairo::TextExtents extents = ctx.textExtents( text );
		x = 128.0 - ( extents.width() / 2 + extents.xBearing() );
		y = 128.0 - ( extents.height() / 2 + extents.yBearing() );

		ctx.moveTo( x, y );
		ctx.showText( text );

		// draw helping lines
		ctx.setSourceRgba( 1, 0.2, 0.2, 0.6 );
		ctx.setLineWidth( 6.0 );
		ctx.arc( x, y, 10.0, 0, 2 * M_PI );
		ctx.fill();
		ctx.moveTo( 128.0, 0 );
		ctx.relLineTo( 0, 256 );
		ctx.moveTo( 0, 128.0 );
		ctx.relLineTo( 256, 0 );
		ctx.stroke();
	}
};

// text extents
struct TestTextExtents : public TestBase {
	virtual void run( fli::cairo::Context &ctx ) {
		const std::string text = "cairo";
		double x,y;

		ctx.selectFontFace( "Sans", fli::cairo::FONT_SLANT_NORMAL, fli::cairo::FONT_WEIGHT_NORMAL );

		ctx.setFontSize( 100.0 );
		fli::cairo::TextExtents extents = ctx.textExtents( text );
		x = 25.0;
		y = 150.0;

		ctx.moveTo( x, y );
		ctx.showText( text );

		// draw helping lines
		ctx.setSourceRgba( 1, 0.2, 0.2, 0.6 );
		ctx.setLineWidth( 6.0 );
		ctx.arc( x, y, 10.0, 0, 2 * M_PI );
		ctx.fill();
		ctx.moveTo( x, y );
		ctx.relLineTo( 0, -extents.height() );
		ctx.relLineTo( extents.width(), 0 );
		ctx.relLineTo( extents.xBearing(), -extents.yBearing() );
		ctx.stroke();
	}
};

int main( int argc, char **argv )
{
	map<string,TestBase*> tests;

	tests["arc"] = new TestArc();
	tests["arc_negative"] = new TestArcNegative();
	tests["clip"] = new TestClip();
	tests["clip_image"] = new TestClipImage();
	tests["curve_rectangle"] = new TestCurveRectangle();
	tests["curve_to"] = new TestCurveTo();
	tests["dash"] = new TestDash();
	tests["fill_and_stroke"] = new TestFillAndStroke();
	tests["fill_style"] = new TestFillStyle();
	tests["gradient"] = new TestGradient();
	tests["image"] = new TestImage();
	tests["image_pattern"] = new TestImagePattern();
	tests["multi_segment_caps"] = new TestMultiSegmentCaps();
	tests["set_line_cap"] = new TestSetLineCap();
	tests["set_line_join"] = new TestSetLineJoin();
	tests["text"] = new TestText();
	tests["text_align_center"] = new TestTextAlignCenter();
	tests["text_extents"] = new TestTextExtents();
#if defined( TEST_PNG )
	{
		std::cout << "-= Testing PNG =-" << std::endl;
		fli::cairo::SurfaceImage surface( 512, 512 );
		for( map<string,TestBase*>::const_iterator it = tests.begin(); it != tests.end(); ++it ) {
			fli::cairo::Context ctx( &surface );
			std::cout << "Running test: " << it->first << std::endl;
			fli::fill( &surface.getSurface(), fli::Color8u( 255, 255, 255 ) );
			it->second->run( ctx );
			surface.writeToPng( it->first + ".png" );
		}
	}
#endif

#if defined( TEST_PDF )
	{
		std::cout << "-= Testing PDF =-" << std::endl;
		fli::cairo::SurfacePDF surface( "tests.pdf", 512, 512 );
		fli::cairo::Context ctx( &surface );
		ctx.save();
		for( map<string,TestBase*>::const_iterator it = tests.begin(); it != tests.end(); ++it ) {
			std::cout << "Running test: " << it->first << std::endl;
			it->second->run( ctx );
			ctx.showPage();
			ctx.restore();
			ctx.save();
		}
	}
#endif

#if defined( TEST_SVG )
	{
		std::cout << "-= Testing SVG =-" << std::endl;
		for( map<string,TestBase*>::const_iterator it = tests.begin(); it != tests.end(); ++it ) {
			fli::cairo::SurfaceSVG surface( ( it->first + ".svg" ).c_str(), 512, 512 );
			fli::cairo::Context ctx( &surface );
			std::cout << "Running test: " << it->first << std::endl;
			it->second->run( ctx );
			ctx.showPage();
		}
	}
#endif

	return 0;
}


//
//  MonteCarloTools.cpp
//  AmericanGeometric
//
//  Created by Jose Alcala Burgos on 6/28/13.
//  Copyright (c) 2013 Jose V. Alcala. All rights reserved.
//

//include C++11 random
#include <random>
#include <math.h>
#include "MonteCarloTools.h"
#include <armadillo>

using namespace arma;

/*
void coefficientsGBM( mat *drift , mat *volatility , mat x , double t)
{   //Check that dimensions are correct
    int BM_dim ;
    BM_dim = mu.n_rows ;

    if ( sigma.n_rows != BM_dim || sigma.n_cols != BM_dim ){
        throw invalid_argument("The dimensions of sigma and mu do not agree.");
    }

	//Check if the sigma matrix is invertible
    if ( fabs(det(sigma)) < 0.000000000000001){
        cout << "sigma is not invertible, det_sigma = " << fabs(det(sigma))<< endl;
    }

    //Calculate the volatility
    for (int i = 0 ; i < BM_dim; i++) {
		for (int j=0; j < BM_dim; j++) {
			(*volatility)(i,j) = x(i,0)*sigma(i,j) ;
		}
	}

    //Calculate the drift
    for (int i = 0 ; i < BM_dim; i++) {
		(*drift)(i,0) = mu(i,0) ;
		for (int l=0; l < BM_dim; l++) {
			(*drift)(i,0) = (*drift)(i,0) + (0.5)*sigma(i,l)*sigma(i,l) ;
		}
		(*drift)(i,0) = x(i,0)*((*drift)(i,0));
	}

}*/


/**
 *          Copyright Matthias Walter 2010.
 * Distributed under the Boost Software License, Version 1.0.
 *    (See accompanying file LICENSE_1_0.txt or copy at
 *          http://www.boost.org/LICENSE_1_0.txt)
 **/

#ifndef GEN_NETWORK_HPP_
#define GEN_NETWORK_HPP_

#include "gen_generic.hpp"
#include "matrix.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/two_bit_color_map.hpp>

template <typename OutputIterator, typename Graph, typename IndexMap, typename Vertex>
OutputIterator find_path(Graph graph, IndexMap index_map, Vertex s, Vertex t, OutputIterator result)
{
  if (s == t)
  {
    *result++ = s;
    return result;
  }

  typedef boost::graph_traits <Graph> graph_traits_t;
  typedef typename graph_traits_t::vertex_descriptor vertex_t;

  /// Define a color map for DFS
  typedef boost::two_bit_color_map <> color_map_t;
  color_map_t color_map(boost::num_vertices(graph));

  /// Declare predecessor map
  typedef std::vector <vertex_t> predecessors_t;
  typedef boost::iterator_property_map <typename predecessors_t::iterator, IndexMap> predecessor_map_t;

  predecessors_t predecessors(boost::num_vertices(graph), graph_traits_t::null_vertex());
  predecessor_map_t predecessor_map(predecessors.begin(), index_map);

  boost::depth_first_visit(graph, s, boost::make_dfs_visitor(record_predecessors(predecessor_map, boost::on_tree_edge())), color_map);

  vertex_t current_vertex = t;
  while (current_vertex != s)
  {
    vertex_t next_vertex = boost::get(predecessor_map, current_vertex);
    if (next_vertex == graph_traits_t::null_vertex())
      return result;

    *result++ = current_vertex;

    current_vertex = next_vertex;
  }

  *result++ = s;
  return result;
}

class network_matrix_generator: public matrix_generator
{
public:
  network_matrix_generator(size_t height, size_t width, unimod::log_level level) :
    matrix_generator("network", height, width, level)
  {

  }

  virtual ~network_matrix_generator()
  {

  }

  template <typename MatrixType>
  void generate(MatrixType& matrix)
  {
    size_t nodes = matrix.size1() + 1;

    if (_level != unimod::LOG_QUIET)
      std::cerr << "Creating a spanning tree with " << nodes << " nodes..." << std::flush;

    /// Create a spanning tree
    typedef boost::adjacency_list <boost::vecS, boost::vecS, boost::undirectedS> tree_graph_t;
    typedef boost::graph_traits <tree_graph_t> tree_traits_t;

    tree_graph_t tree_graph(nodes);
    std::vector <tree_traits_t::vertex_descriptor> used_vertices;

    tree_traits_t::vertex_iterator vertex_iter, vertex_beyond;
    for (boost::tie(vertex_iter, vertex_beyond) = boost::vertices(tree_graph); vertex_iter != vertex_beyond; ++vertex_iter)
    {
      if (!used_vertices.empty())
      {
        boost::uniform_int <int> dist(0, used_vertices.size() - 1);
        tree_traits_t::vertex_descriptor other = used_vertices[dist(_rng)];

        boost::add_edge(*vertex_iter, other, tree_graph);
      }
      used_vertices.push_back(*vertex_iter);
    }

    if (_level != unimod::LOG_QUIET)
      std::cerr << " done.\nAdding edges and filling matrix..." << std::flush;

    for (size_t column = 0; column < _matrix.size2(); ++column)
    {
      /// Choose an edge not in the tree
      boost::uniform_int <int> dist(0, nodes - 1);
      tree_traits_t::vertex_descriptor u, v;
      do
      {
        u = boost::vertex(dist(_rng), tree_graph);
        v = boost::vertex(dist(_rng), tree_graph);
      }
      while (u == v || boost::edge(u, v, tree_graph).second);

      std::set <tree_traits_t::vertex_descriptor> path_vertices;
      find_path(tree_graph, boost::get(boost::vertex_index, tree_graph), u, v, std::inserter(path_vertices, path_vertices.end()));

      size_t row = 0;
      tree_traits_t::edge_iterator edge_iter, edge_beyond;
      for (boost::tie(edge_iter, edge_beyond) = boost::edges(tree_graph); edge_iter != edge_beyond; ++edge_iter)
      {
        u = boost::source(*edge_iter, tree_graph);
        v = boost::target(*edge_iter, tree_graph);

        _matrix(row, column) = (path_vertices.find(u) != path_vertices.end() && path_vertices.find(v) != path_vertices.end()) ? 1 : 0;

        ++row;
      }
    }

    if (_level != unimod::LOG_QUIET)
      std::cerr << " done.\nCorrecting the signs..." << std::flush;
    sign();
    if (_level != unimod::LOG_QUIET)
      std::cerr << " done." << std::endl;
  }

  virtual void generate()
  {
    if (_height <= _width)
    {
      generate(_matrix);
    }
    else
    {
      unimod::matrix_transposed <unimod::integer_matrix> transposed(_matrix);
      generate(transposed);
    }
  }

};
#endif


#include <sparse.h>
#include <sparse_fill.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(SPARSE);

BOOST_AUTO_TEST_CASE(product_2cm_vector_test)
{

  typedef sparse::Block<4,1,float> block4_type;
  typedef sparse::Block<6,1,float> block6_type;
  typedef sparse::Block<4,6,float> block4x6_type;

  typedef sparse::TwoColumnMatrix<block4x6_type> matrix_type;
	typedef sparse::Vector<block6_type> vector6_type;
	typedef sparse::Vector<block4_type> vector4_type;

	matrix_type J(1,2,2);
  sparse::fill(J(0,0), 1.0f);
	sparse::fill(J(0,1), 2.0f);

	vector6_type u(2);
	sparse::fill(u(0), 0.5f);
	sparse::fill(u(1), 6.5f);

  vector4_type r(1);
	sparse::prod(J, u, r);
	BOOST_CHECK( r(0)(0) == 341  );
  BOOST_CHECK( r(0)(1) == 773  );
  BOOST_CHECK( r(0)(2) == 1205 );
  BOOST_CHECK( r(0)(3) == 1637 );

	J.resize(2,4,4);
  sparse::fill(J(1,1), 3.0f);
	sparse::fill(J(1,2), 4.0f);
	u.resize(4);
	sparse::fill(u(2), 12.5f);
	sparse::fill(u(3), 18.5f);
	r.clear();
	r.resize(2);
	sparse::prod(J, u, r);
	BOOST_CHECK( r(0)(0) == 341  );
  BOOST_CHECK( r(0)(1) == 773  );
  BOOST_CHECK( r(0)(2) == 1205 );
  BOOST_CHECK( r(0)(3) == 1637 );
	BOOST_CHECK( r(1)(0) == 917  );
  BOOST_CHECK( r(1)(1) == 1781 );
  BOOST_CHECK( r(1)(2) == 2645 );
  BOOST_CHECK( r(1)(3) == 3509 );

}

BOOST_AUTO_TEST_SUITE_END();


#include "postprocess.h"

#include <fstream>
#include <map>

#include <Eigen/Dense>

#include "simple_svg.h"

using namespace svg;

int EvaluateEdgeType(Arrangement_2::Halfedge_const_handle he, const Mesh& mesh, const Arrangement_2& overlay, double angle_thres) {
	int fid = he->face()->data() - 1;
	auto& params = mesh.GetPlanes();
	if (he->twin()->face() == overlay.unbounded_face()) {
		return 2;
	}
	else {
		int fid1 = he->twin()->face()->data() - 1;
		if (fid1 == -1)
			return 2;
		if (fid1 != fid) {
			K z = params[fid].ComputeDepth(he->source()->point());
			K n_z = params[fid1].ComputeDepth(he->source()->point());
			if (std::abs((z-n_z).convert_to<double>()) > 1e-6) {
				return 2;
			} else {
				K z = params[fid].ComputeDepth(he->target()->point());
				K n_z = params[fid1].ComputeDepth(he->target()->point());
				if (std::abs((z-n_z).convert_to<double>()) > 1e-6)
					return 2;

				auto& p1 = mesh.GetPlanes()[fid];
				auto& p2 = mesh.GetPlanes()[fid1];
				K dot = p1.n1_ * p2.n1_ + p1.n2_ * p2.n2_ + p1.n3_ * p2.n3_;
				double t1 = dot.convert_to<double>();
				double t2 = cos(angle_thres * 3.141592654 / 180.0);

				if (std::abs(t1) < t2) {
					return 1;
				}
			}
		}
	}

	return 0;
}

void PostProcess::CollectFaceAndVertices(const Mesh& mesh, const Arrangement_2& overlay, double angle_thres) {
	auto& points = points_;
	auto& facets_id = facets_id_;
	auto& facets = facets_;
	auto& params = mesh.GetPlanes();
	auto& depths = depths_;
	std::map<std::pair<Arrangement_2::Vertex_const_handle,int>, int> vertexID;

	for (auto fit = overlay.faces_begin(); fit != overlay.faces_end(); ++fit) {
		int fid = fit->data() - 1;
		if (fit == overlay.unbounded_face() || fid < 0)
			continue;

		EdgeList e;
		auto e_handle = fit->outer_ccb();
		Arrangement_2::Ccb_halfedge_const_circulator curr = e_handle;
		do {
			Arrangement_2::Halfedge_const_handle he = curr;
			auto key = std::make_pair(he->source(), fid);
			auto it = vertexID.find(key);
			if (it == vertexID.end()) {
				vertexID[key] = points.size();
				e.outer_indices.push_back(points.size());
				K z = params[fid].ComputeDepth(he->source()->point());
				points.push_back(key);
				depths.push_back(z);
			} else {
				e.outer_indices.push_back(it->second);
			}
			e.outer_type.push_back(EvaluateEdgeType(he, mesh, overlay, angle_thres));
		} while (++curr != e_handle);

		for (auto hi = fit->holes_begin(); hi != fit->holes_end(); ++hi) {
			auto circ = *hi;
			Arrangement_2::Ccb_halfedge_const_circulator curr = circ;
			e.inner_indices.push_back(std::vector<int>());
			e.inner_type.push_back(std::vector<int>());
			do {
				Arrangement_2::Halfedge_const_handle he = curr;
				auto key = std::make_pair(he->source(), fid);
				auto it = vertexID.find(key);
				if (it == vertexID.end()) {
					vertexID[key] = points.size();
					e.inner_indices.back().push_back(points.size());
					K z = params[fid].ComputeDepth(he->source()->point());
					points.push_back(key);
					depths.push_back(z);

				} else {
					e.inner_indices.back().push_back(it->second);
				}
				e.inner_type.back().push_back(EvaluateEdgeType(he, mesh, overlay, angle_thres));
			} while (++curr != circ);
		}
		facets.push_back(e);
		facets_id.push_back(fid);
	}
}

void PostProcess::RemoveRedundantVertices()
{
	auto& facets = facets_;
	auto& points = points_;
	auto& depths = depths_;
	auto& degrees = degrees_;

	ComputeDegree();

	for (auto& f : facets) {
		RemoveRedundantVerticesFromLoop(f.outer_indices, f.outer_type);
		for (int i = 0; i < f.inner_indices.size(); ++i) {
			RemoveRedundantVerticesFromLoop(f.inner_indices[i], f.inner_type[i]);
		}
	}

	ComputeDegree();
	std::vector<int> compressed_vertexID(degrees.size());
	compressed_vertexID[0] = 0;
	for (int i = 1; i < compressed_vertexID.size(); ++i) {
		compressed_vertexID[i] = compressed_vertexID[i - 1] + (degrees[i - 1] > 0 ? 1 : 0);
	}

	int top = 0;
	for (int i = 0; i < points.size(); ++i) {
		if (degrees[i]) {
			points[top] = points[i];
			depths[top] = depths[i];
			top += 1;
		}
	}

	points.resize(top);
	depths.resize(top);
	for (auto& i : facets) {
		for (auto& e : i.outer_indices) {
			e = compressed_vertexID[e];
		}
		for (auto& es : i.inner_indices) {
			for (auto& e : es)
				e = compressed_vertexID[e];
		}
	}
}

void PostProcess::MergeDuplex(const Mesh& mesh)
{
	auto & vertices = mesh.GetVertices();
	auto & faces = mesh.GetFaces();

	auto& facets = facets_;
	auto& points = points_;
	auto& depths = depths_;
	// merge duplex
	std::map<std::pair<int,std::pair<int,int> >, int> vID;
	std::vector<int> compressed_vertexID;

	int top = 0;
	compressed_vertexID.resize(points.size());
	std::vector<VertexSignature> points_buf;
	std::vector<K> depths_buf;
	for (int i = 0; i < points.size(); ++i) {
		double x = points[i].first->point().x().convert_to<double>();
		double y = points[i].first->point().y().convert_to<double>();
		double z = depths[i].convert_to<double>();

		auto key = std::make_pair(int(x * 1e5), std::make_pair(int(y * 1e5), int(z * 1e5)));
		auto it = vID.find(key);
		if (it == vID.end()) {
			compressed_vertexID[i] = top;
			points_buf.push_back(points[i]);
			depths_buf.push_back(depths[i]);
			vID[key] = top++;
		} else {
			compressed_vertexID[i] = it->second;
		}
	}
	points = points_buf;
	depths = depths_buf;

	auto shrink = [&](std::vector<int>& v) {
		std::vector<int> m(v.size(), 1);
		for (int i = 0; i < v.size(); ++i) {
			int curr = v[i];
			int next = v[(i + 1) % v.size()];
			if (curr == next) {
				m[i] = 0;
			}
		}
		int top = 0;
		for (int i = 0; i < v.size(); ++i) {
			if (m[i] == 1)
				v[top++] = v[i];
		}
		v.resize(top);
	};

	for (auto& i : facets) {
		for (auto& e : i.outer_indices) {
			e = compressed_vertexID[e];
		}
		shrink(i.outer_indices);
		for (auto& es : i.inner_indices) {
			for (auto& e : es) {
				e = compressed_vertexID[e];
			}
			shrink(es);
		}
	}
}

void PostProcess::CollectEdges(const Mesh& mesh) {
	auto & vertices = mesh.GetVertices();
	auto & faces = mesh.GetFaces();
	auto & face_normals = mesh.GetFaceNormals();
	auto & facets_id = facets_id_;
	auto & edge2face = edge2face_;

	auto& facets = facets_;
	auto& points = points_;

	for (int i = 0; i < facets.size(); ++i) {
		for (int j = 0; j < facets[i].outer_indices.size(); ++j) {
			int v0 = facets[i].outer_indices[j];
			int v1 = facets[i].outer_indices[(j + 1) % facets[i].outer_indices.size()];
			auto key = (v0 < v1) ? std::make_pair(v0, v1) : std::make_pair(v1, v0);
			edge2face[key].insert(facets_id[i]);
		}
		for (auto& es : facets[i].inner_indices) {
			for (int j = 0; j < es.size(); ++j) {
				int v0 = es[j];
				int v1 = es[(j + 1) % es.size()];
				auto key = (v0 < v1) ? std::make_pair(v0, v1) : std::make_pair(v1, v0);
				edge2face[key].insert(facets_id[i]);
			}
		}
	}
}

void PostProcess::SaveToFile(const Mesh& mesh, const char* filename) {
	auto & vertices = mesh.GetVertices();
	auto & faces = mesh.GetFaces();
	auto & facets = facets_;
	auto & face_normals = mesh.GetFaceNormals();
	auto & facets_id = facets_id_;
	auto & edge2face = edge2face_;
	auto & points = points_;
	auto & depths = depths_;

	std::ofstream os;

	os.open(filename);

	for (int i = 0; i < points.size(); ++i) {
		double x = points[i].first->point().x().convert_to<double>();
		double y = points[i].first->point().y().convert_to<double>();
		double z = depths[i].convert_to<double>();

		Eigen::Vector3d v(x * z, y * z, z);
		//Eigen::Vector3d v(x, y, z);
		os << "v " << v[0] << " " << v[1] << " " << v[2] << "\n";
	}

	for (auto& i : facets) {
		if (i.outer_indices.size() < 3)
			continue;
		os << "f";
		for (auto& e : i.outer_indices) {
			os << " " << e + 1;
		}
		os << "\n";
		for (auto& es : i.inner_indices) {
			if (es.size() < 3)
				continue;
			os << "###holes### f";
			for (auto& e : es) {
				os << " " << e + 1;
			}
			os << "\n";
		}
	}

	for (int i = 0; i < points.size(); ++i) {
		double x = points[i].first->point().x().convert_to<double>();
		double y = points[i].first->point().y().convert_to<double>();
		double z = depths[i].convert_to<double>();

		Eigen::Vector3d v(x * z, y * z, z);
		//Eigen::Vector3d v(x, y, z);
		os << "v " << v[0] << " " << v[1] << " " << v[2] << "\n";
	}
	os << "### occlude boundaries\n";
	for (auto& i : facets) {
		if (i.outer_indices.size() < 3)
			continue;
		for (int j = 0; j < i.outer_indices.size(); ++j) {
			if (i.outer_type[j] == 2) {
				int next_j = (j + 1) % i.outer_indices.size();
				os << "l " << i.outer_indices[j] + 1
				   << " " << i.outer_indices[next_j] + 1 << "\n";
			}
		}
	}

	for (int i = 0; i < points.size(); ++i) {
		double x = points[i].first->point().x().convert_to<double>();
		double y = points[i].first->point().y().convert_to<double>();
		double z = depths[i].convert_to<double>();

		Eigen::Vector3d v(x * z, y * z, z);
		//Eigen::Vector3d v(x, y, z);
		os << "v " << v[0] << " " << v[1] << " " << v[2] << "\n";
	}
	os << "### sharp edges\n";
	for (auto& i : facets) {
		if (i.outer_indices.size() < 3)
			continue;
		for (int j = 0; j < i.outer_indices.size(); ++j) {
			if (i.outer_type[j] == 1) {
				int next_j = (j + 1) % i.outer_indices.size();
				os << "l " << i.outer_indices[j] + 1
				   << " " << i.outer_indices[next_j] + 1 << "\n";
			}
		}
	}
	os.close();
}

void PostProcess::SaveToSVG(const Mesh& mesh, const char* filename) {
    auto & facets = facets_;
    auto & points = points_;
    auto & face_normals = mesh.GetFaceNormals();
    auto & edge2face = edge2face_;

   	Dimensions dimensions(1000, 1000);
    Document doc(filename, Layout(dimensions, Layout::BottomLeft));

    // Red image border.
    Polygon border(Stroke(1, Color::Red));
    border << Point(0, 0) << Point(dimensions.width, 0)
        << Point(dimensions.width, dimensions.height) << Point(0, dimensions.height);
    doc << border;
    // Render polygon

	for (auto& i : facets) {
		if (i.outer_indices.size() < 3)
			continue;

	    Polygon poly(Color(128,128,128), Stroke(.0, Color(150, 160, 200)));

	    for (auto& e : i.outer_indices) {
	    	auto& p = points[e].first->point();
	    	int x = (p.x().convert_to<double>()+0.5) * 1000;
	    	int y = (p.y().convert_to<double>()+0.5) * 1000;
	    	poly << Point(x, 1000-y);
	    }
	    doc << poly;
	}

	for (int seq = 1; seq <= 2; ++seq) {
		for (auto& i : facets) {
			if (i.outer_indices.size() < 3)
				continue;
			for (int j = 0; j < i.outer_indices.size(); ++j) {
				if (i.outer_type[j] > 0 && i.outer_type[j] == seq) {
			    	Polyline poly(Stroke(2, Color::Blue));
			    	if (i.outer_type[j] == 2)
			    		poly = Polyline(Stroke(2, Color::Red));
				    {
					    auto& p1 = points[i.outer_indices[j]].first->point();
				    	int x = (p1.x().convert_to<double>()+0.5) * 1000;
				    	int y = (p1.y().convert_to<double>()+0.5) * 1000;
				    	poly << Point(x, 1000-y);
				    }
				    {
				    	int next_j = (j + 1) % i.outer_indices.size();
					    auto& p1 = points[i.outer_indices[next_j]].first->point();
				    	int x = (p1.x().convert_to<double>()+0.5) * 1000;
				    	int y = (p1.y().convert_to<double>()+0.5) * 1000;
				    	poly << Point(x, 1000-y);
				    }
				    doc << poly;
			    }
			}
		}
	}

    doc.save();
}

void PostProcess::ComputeDegree()
{
	degrees_.resize(points_.size());

	for (auto& d :degrees_)
		d = 0;
	for (auto& f : facets_) {
		for (auto& e : f.outer_indices) {
			degrees_[e] += 2;
		}
		for (auto& es : f.inner_indices) {
			for (auto& e : es) {
				degrees_[e] += 2;
			}
		}
	}
}

void PostProcess::RemoveRedundantVerticesFromLoop(std::vector<int>& indices, std::vector<int>& types) {
	auto& points = points_;
	auto& degrees = degrees_;

	std::vector<int> mask(indices.size(), 0);
	for (int i = 0; i < indices.size(); ++i) {
		if (degrees[indices[i]] > 2) {
			mask[i] = 1;
			continue;
		}
		if (types[i] != types[(i + indices.size() - 1) % indices.size()]) {
			mask[i] = 1;
			continue;
		}
		int prev_id = indices[(i + indices.size() - 1) % indices.size()];
		int next_id = indices[(i + 1) % indices.size()];

		auto& p1 = points[prev_id];
		auto& p2 = points[indices[i]];
		auto& p3 = points[next_id];

		auto diff1 = p2.first->point() - p1.first->point();
		auto diff2 = p3.first->point() - p2.first->point();

		K a = diff1.x() * diff2.y() - diff1.y() * diff2.x();
		if (a != K(0)) {
			mask[i] = 1;
		}
	}
	int top = 0;
	for (int i = 0; i < indices.size(); ++i) {
		if (mask[i] == 1) {
			indices[top] = indices[i];
			types[top] = types[i];
			top += 1;
		}
	}
	indices.resize(top);
	types.resize(top);

}


#include <boost/math/special_functions/expint.hpp>


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2014, 2015, 2018.
// Modifications copyright (c) 2014-2018, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_UTIL_MATH_HPP
#define BOOST_GEOMETRY_UTIL_MATH_HPP

#include <cmath>
#include <limits>

#include <boost/core/ignore_unused.hpp>

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
//#include <boost/math/special_functions/round.hpp>
#include <boost/numeric/conversion/cast.hpp>
#include <boost/type_traits/is_fundamental.hpp>
#include <boost/type_traits/is_integral.hpp>

#include <boost/geometry/core/cs.hpp>

#include <boost/geometry/util/select_most_precise.hpp>

namespace boost { namespace geometry
{

namespace math
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename T>
inline T const& greatest(T const& v1, T const& v2)
{
    return (std::max)(v1, v2);
}

template <typename T>
inline T const& greatest(T const& v1, T const& v2, T const& v3)
{
    return (std::max)(greatest(v1, v2), v3);
}

template <typename T>
inline T const& greatest(T const& v1, T const& v2, T const& v3, T const& v4)
{
    return (std::max)(greatest(v1, v2, v3), v4);
}

template <typename T>
inline T const& greatest(T const& v1, T const& v2, T const& v3, T const& v4, T const& v5)
{
    return (std::max)(greatest(v1, v2, v3, v4), v5);
}

template <typename T>
inline T bounded(T const& v, T const& lower, T const& upper)
{
    return (std::min)((std::max)(v, lower), upper);
}

template <typename T>
inline T bounded(T const& v, T const& lower)
{
    return (std::max)(v, lower);
}

template <typename T,
          bool IsFloatingPoint = boost::is_floating_point<T>::value>
struct abs
{
    static inline T apply(T const& value)
    {
        T const zero = T();
        return value < zero ? -value : value;
    }
};

template <typename T>
struct abs<T, true>
{
    static inline T apply(T const& value)
    {
        using ::fabs;
        using std::fabs; // for long double

        return fabs(value);
    }
};

struct equals_default_policy
{
    template <typename T>
    static inline T apply(T const& a, T const& b)
    {
        // See http://www.parashift.com/c++-faq-lite/newbie.html#faq-29.17
        return greatest(abs<T>::apply(a), abs<T>::apply(b), T(1));
    }
};

template <typename T,
          bool IsFloatingPoint = boost::is_floating_point<T>::value>
struct equals_factor_policy
{
    equals_factor_policy()
        : factor(1) {}
    explicit equals_factor_policy(T const& v)
        : factor(greatest(abs<T>::apply(v), T(1)))
    {}
    equals_factor_policy(T const& v0, T const& v1, T const& v2, T const& v3)
        : factor(greatest(abs<T>::apply(v0), abs<T>::apply(v1),
                          abs<T>::apply(v2), abs<T>::apply(v3),
                          T(1)))
    {}

    T const& apply(T const&, T const&) const
    {
        return factor;
    }

    T factor;
};

template <typename T>
struct equals_factor_policy<T, false>
{
    equals_factor_policy() {}
    explicit equals_factor_policy(T const&) {}
    equals_factor_policy(T const& , T const& , T const& , T const& ) {}

    static inline T apply(T const&, T const&)
    {
        return T(1);
    }
};

template <typename Type,
          bool IsFloatingPoint = boost::is_floating_point<Type>::value>
struct equals
{
    template <typename Policy>
    static inline bool apply(Type const& a, Type const& b, Policy const&)
    {
        return a == b;
    }
};

template <typename Type>
struct equals<Type, true>
{
    template <typename Policy>
    static inline bool apply(Type const& a, Type const& b, Policy const& policy)
    {
        boost::ignore_unused(policy);

        if (a == b)
        {
            return true;
        }

        if (boost::math::isfinite(a) && boost::math::isfinite(b))
        {
            // If a is INF and b is e.g. 0, the expression below returns true
            // but the values are obviously not equal, hence the condition
            return abs<Type>::apply(a - b)
                <= std::numeric_limits<Type>::epsilon() * policy.apply(a, b);
        }
        else
        {
            return a == b;
        }
    }
};

template <typename T1, typename T2, typename Policy>
inline bool equals_by_policy(T1 const& a, T2 const& b, Policy const& policy)
{
    return detail::equals
        <
            typename select_most_precise<T1, T2>::type
        >::apply(a, b, policy);
}

template <typename Type,
          bool IsFloatingPoint = boost::is_floating_point<Type>::value>
struct smaller
{
    static inline bool apply(Type const& a, Type const& b)
    {
        return a < b;
    }
};

template <typename Type>
struct smaller<Type, true>
{
    static inline bool apply(Type const& a, Type const& b)
    {
        if (!(a < b)) // a >= b
        {
            return false;
        }

        return ! equals<Type, true>::apply(b, a, equals_default_policy());
    }
};

template <typename Type,
          bool IsFloatingPoint = boost::is_floating_point<Type>::value>
struct smaller_or_equals
{
    static inline bool apply(Type const& a, Type const& b)
    {
        return a <= b;
    }
};

template <typename Type>
struct smaller_or_equals<Type, true>
{
    static inline bool apply(Type const& a, Type const& b)
    {
        if (a <= b)
        {
            return true;
        }

        return equals<Type, true>::apply(a, b, equals_default_policy());
    }
};

template <typename Type,
          bool IsFloatingPoint = boost::is_floating_point<Type>::value>
struct equals_with_epsilon
    : public equals<Type, IsFloatingPoint>
{};

template
<
    typename T,
    bool IsFundemantal = boost::is_fundamental<T>::value /* false */
>
struct square_root
{
    typedef T return_type;

    static inline T apply(T const& value)
    {
        // for non-fundamental number types assume that sqrt is
        // defined either:
        // 1) at T's scope, or
        // 2) at global scope, or
        // 3) in namespace std
        using ::sqrt;
        using std::sqrt;

        return sqrt(value);
    }
};

template <typename FundamentalFP>
struct square_root_for_fundamental_fp
{
    typedef FundamentalFP return_type;

    static inline FundamentalFP apply(FundamentalFP const& value)
    {
#ifdef BOOST_GEOMETRY_SQRT_CHECK_FINITENESS
        // This is a workaround for some 32-bit platforms.
        // For some of those platforms it has been reported that
        // std::sqrt(nan) and/or std::sqrt(-nan) returns a finite value.
        // For those platforms we need to define the macro
        // BOOST_GEOMETRY_SQRT_CHECK_FINITENESS so that the argument
        // to std::sqrt is checked appropriately before passed to std::sqrt
        if (boost::math::isfinite(value))
        {
            return std::sqrt(value);
        }
        else if (boost::math::isinf(value) && value < 0)
        {
            return -std::numeric_limits<FundamentalFP>::quiet_NaN();
        }
        return value;
#else
        // for fundamental floating point numbers use std::sqrt
        return std::sqrt(value);
#endif // BOOST_GEOMETRY_SQRT_CHECK_FINITENESS
    }
};

template <>
struct square_root<float, true>
    : square_root_for_fundamental_fp<float>
{
};

template <>
struct square_root<double, true>
    : square_root_for_fundamental_fp<double>
{
};

template <>
struct square_root<long double, true>
    : square_root_for_fundamental_fp<long double>
{
};

template <typename T>
struct square_root<T, true>
{
    typedef double return_type;

    static inline double apply(T const& value)
    {
        // for all other fundamental number types use also std::sqrt
        //
        // Note: in C++98 the only other possibility is double;
        //       in C++11 there are also overloads for integral types;
        //       this specialization works for those as well.
        return square_root_for_fundamental_fp
            <
                double
            >::apply(boost::numeric_cast<double>(value));
    }
};

template
<
    typename T,
    bool IsFundemantal = boost::is_fundamental<T>::value /* false */
>
struct modulo
{
    typedef T return_type;

    static inline T apply(T const& value1, T const& value2)
    {
        // for non-fundamental number types assume that a free
        // function mod() is defined either:
        // 1) at T's scope, or
        // 2) at global scope
        return mod(value1, value2);
    }
};

template
<
    typename Fundamental,
    bool IsIntegral = boost::is_integral<Fundamental>::value
>
struct modulo_for_fundamental
{
    typedef Fundamental return_type;

    static inline Fundamental apply(Fundamental const& value1,
                                    Fundamental const& value2)
    {
        return value1 % value2;
    }
};

// specialization for floating-point numbers
template <typename Fundamental>
struct modulo_for_fundamental<Fundamental, false>
{
    typedef Fundamental return_type;

    static inline Fundamental apply(Fundamental const& value1,
                                    Fundamental const& value2)
    {
        return std::fmod(value1, value2);
    }
};

// specialization for fundamental number type
template <typename Fundamental>
struct modulo<Fundamental, true>
    : modulo_for_fundamental<Fundamental>
{};

/*!
\brief Short constructs to enable partial specialization for PI, 2*PI
       and PI/2, currently not possible in Math.
*/
template <typename T>
struct define_pi
{
    static inline T apply()
    {
        // Default calls Boost.Math
        return boost::math::constants::pi<T>();
    }
};

template <typename T>
struct define_two_pi
{
    static inline T apply()
    {
        // Default calls Boost.Math
        return boost::math::constants::two_pi<T>();
    }
};

template <typename T>
struct define_half_pi
{
    static inline T apply()
    {
        // Default calls Boost.Math
        return boost::math::constants::half_pi<T>();
    }
};

template <typename T>
struct relaxed_epsilon
{
    static inline T apply(const T& factor)
    {
        return factor * std::numeric_limits<T>::epsilon();
    }
};

// This must be consistent with math::equals.
// By default math::equals() scales the error by epsilon using the greater of
// compared values but here is only one value, though it should work the same way.
// (a-a) <= max(a, a) * EPS       -> 0 <= a*EPS
// (a+da-a) <= max(a+da, a) * EPS -> da <= (a+da)*EPS
template <typename T, bool IsFloat = boost::is_floating_point<T>::value>
struct scaled_epsilon
{
    static inline T apply(T const& val)
    {
        return (std::max)(abs<T>::apply(val), T(1))
                    * std::numeric_limits<T>::epsilon();
    }
};

template <typename T>
struct scaled_epsilon<T, false>
{
    static inline T apply(T const&)
    {
        return T(0);
    }
};

// ItoF ItoI FtoF
template <typename Result, typename Source,
          bool ResultIsInteger = std::numeric_limits<Result>::is_integer,
          bool SourceIsInteger = std::numeric_limits<Source>::is_integer>
struct rounding_cast
{
    static inline Result apply(Source const& v)
    {
        return boost::numeric_cast<Result>(v);
    }
};

// TtoT
template <typename Source, bool ResultIsInteger, bool SourceIsInteger>
struct rounding_cast<Source, Source, ResultIsInteger, SourceIsInteger>
{
    static inline Source apply(Source const& v)
    {
        return v;
    }
};

// FtoI
template <typename Result, typename Source>
struct rounding_cast<Result, Source, true, false>
{
    static inline Result apply(Source const& v)
    {
        return boost::numeric_cast<Result>(v < Source(0) ?
                                            v - Source(0.5) :
                                            v + Source(0.5));
    }
};

} // namespace detail
#endif

template <typename T>
inline T pi() { return detail::define_pi<T>::apply(); }

template <typename T>
inline T two_pi() { return detail::define_two_pi<T>::apply(); }

template <typename T>
inline T half_pi() { return detail::define_half_pi<T>::apply(); }

template <typename T>
inline T relaxed_epsilon(T const& factor)
{
    return detail::relaxed_epsilon<T>::apply(factor);
}

template <typename T>
inline T scaled_epsilon(T const& value)
{
    return detail::scaled_epsilon<T>::apply(value);
}

// Maybe replace this by boost equals or so

/*!
    \brief returns true if both arguments are equal.
    \ingroup utility
    \param a first argument
    \param b second argument
    \return true if a == b
    \note If both a and b are of an integral type, comparison is done by ==.
    If one of the types is floating point, comparison is done by abs and
    comparing with epsilon. If one of the types is non-fundamental, it might
    be a high-precision number and comparison is done using the == operator
    of that class.
*/

template <typename T1, typename T2>
inline bool equals(T1 const& a, T2 const& b)
{
    return detail::equals
        <
            typename select_most_precise<T1, T2>::type
        >::apply(a, b, detail::equals_default_policy());
}

template <typename T1, typename T2>
inline bool equals_with_epsilon(T1 const& a, T2 const& b)
{
    return detail::equals_with_epsilon
        <
            typename select_most_precise<T1, T2>::type
        >::apply(a, b, detail::equals_default_policy());
}

template <typename T1, typename T2>
inline bool smaller(T1 const& a, T2 const& b)
{
    return detail::smaller
        <
            typename select_most_precise<T1, T2>::type
        >::apply(a, b);
}

template <typename T1, typename T2>
inline bool larger(T1 const& a, T2 const& b)
{
    return detail::smaller
        <
            typename select_most_precise<T1, T2>::type
        >::apply(b, a);
}

template <typename T1, typename T2>
inline bool smaller_or_equals(T1 const& a, T2 const& b)
{
    return detail::smaller_or_equals
        <
            typename select_most_precise<T1, T2>::type
        >::apply(a, b);
}

template <typename T1, typename T2>
inline bool larger_or_equals(T1 const& a, T2 const& b)
{
    return detail::smaller_or_equals
        <
            typename select_most_precise<T1, T2>::type
        >::apply(b, a);
}

template <typename T>
inline T d2r()
{
    static T const conversion_coefficient = geometry::math::pi<T>() / T(180.0);
    return conversion_coefficient;
}

template <typename T>
inline T r2d()
{
    static T const conversion_coefficient = T(180.0) / geometry::math::pi<T>();
    return conversion_coefficient;
}

#ifndef DOXYGEN_NO_DETAIL
namespace detail {

template <typename DegreeOrRadian>
struct as_radian
{
    template <typename T>
    static inline T apply(T const& value)
    {
        return value;
    }
};

template <>
struct as_radian<degree>
{
    template <typename T>
    static inline T apply(T const& value)
    {
        return value * d2r<T>();
    }
};

template <typename DegreeOrRadian>
struct from_radian
{
    template <typename T>
    static inline T apply(T const& value)
    {
        return value;
    }
};

template <>
struct from_radian<degree>
{
    template <typename T>
    static inline T apply(T const& value)
    {
        return value * r2d<T>();
    }
};

} // namespace detail
#endif

template <typename DegreeOrRadian, typename T>
inline T as_radian(T const& value)
{
    return detail::as_radian<DegreeOrRadian>::apply(value);
}

template <typename DegreeOrRadian, typename T>
inline T from_radian(T const& value)
{
    return detail::from_radian<DegreeOrRadian>::apply(value);
}

/*!
    \brief Calculates the haversine of an angle
    \ingroup utility
    \note See http://en.wikipedia.org/wiki/Haversine_formula
    haversin(alpha) = sin2(alpha/2)
*/
template <typename T>
inline T hav(T const& theta)
{
    T const half = T(0.5);
    T const sn = sin(half * theta);
    return sn * sn;
}

/*!
\brief Short utility to return the square
\ingroup utility
\param value Value to calculate the square from
\return The squared value
*/
template <typename T>
inline T sqr(T const& value)
{
    return value * value;
}

/*!
\brief Short utility to return the square root
\ingroup utility
\param value Value to calculate the square root from
\return The square root value
*/
template <typename T>
inline typename detail::square_root<T>::return_type
sqrt(T const& value)
{
    return detail::square_root
        <
            T, boost::is_fundamental<T>::value
        >::apply(value);
}

/*!
\brief Short utility to return the modulo of two values
\ingroup utility
\param value1 First value
\param value2 Second value
\return The result of the modulo operation on the (ordered) pair
(value1, value2)
*/
template <typename T>
inline typename detail::modulo<T>::return_type
mod(T const& value1, T const& value2)
{
    return detail::modulo
        <
            T, boost::is_fundamental<T>::value
        >::apply(value1, value2);
}

/*!
\brief Short utility to workaround gcc/clang problem that abs is converting to integer
       and that older versions of MSVC does not support abs of long long...
\ingroup utility
*/
template<typename T>
inline T abs(T const& value)
{
    return detail::abs<T>::apply(value);
}

/*!
\brief Short utility to calculate the sign of a number: -1 (negative), 0 (zero), 1 (positive)
\ingroup utility
*/
template <typename T>
inline int sign(T const& value)
{
    T const zero = T();
    return value > zero ? 1 : value < zero ? -1 : 0;
}

/*!
\brief Short utility to cast a value possibly rounding it to the nearest
       integral value.
\ingroup utility
\note If the source T is NOT an integral type and Result is an integral type
      the value is rounded towards the closest integral value. Otherwise it's
      casted without rounding.
*/
template <typename Result, typename T>
inline Result rounding_cast(T const& v)
{
    return detail::rounding_cast<Result, T>::apply(v);
}

/*!
\brief Short utility to calculate the power
\ingroup utility
*/
template <typename T1, typename T2>
inline T1 pow(T1 const& a, T2 const& b)
{
    using std::pow;
    return pow(a, b);
}

} // namespace math

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_UTIL_MATH_HPP


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2016 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2014, 2016.
// Modifications copyright (c) 2014-2016 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_ANDOYER_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_ANDOYER_HPP

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/algorithms/detail/andoyer_inverse.hpp>
#include <boost/geometry/algorithms/detail/flattening.hpp>

#include <boost/geometry/strategies/distance.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Point-point distance approximation taking flattening into account
\ingroup distance
\tparam Spheroid The reference spheroid model
\tparam CalculationType \tparam_calculation
\author After Andoyer, 19xx, republished 1950, republished by Meeus, 1999
\note Although not so well-known, the approximation is very good: in all cases the results
are about the same as Vincenty. In my (Barend's) testcases the results didn't differ more than 6 m
\see http://nacc.upc.es/tierra/node16.html
\see http://sci.tech-archive.net/Archive/sci.geo.satellite-nav/2004-12/2724.html
\see http://home.att.net/~srschmitt/great_circle_route.html (implementation)
\see http://www.codeguru.com/Cpp/Cpp/algorithms/article.php/c5115 (implementation)
\see http://futureboy.homeip.net/frinksamp/navigation.frink (implementation)
\see http://www.voidware.com/earthdist.htm (implementation)
\see http://www.dtic.mil/docs/citations/AD0627893
\see http://www.dtic.mil/docs/citations/AD703541
*/
template
<
    typename Spheroid,
    typename CalculationType = void
>
class andoyer
{
public :
    template <typename Point1, typename Point2>
    struct calculation_type
        : promote_floating_point
          <
              typename select_calculation_type
                  <
                      Point1,
                      Point2,
                      CalculationType
                  >::type
          >
    {};

    typedef Spheroid model_type;

    inline andoyer()
        : m_spheroid()
    {}

    explicit inline andoyer(Spheroid const& spheroid)
        : m_spheroid(spheroid)
    {}

    template <typename Point1, typename Point2>
    inline typename calculation_type<Point1, Point2>::type
    apply(Point1 const& point1, Point2 const& point2) const
    {
        return geometry::detail::andoyer_inverse
            <
                typename calculation_type<Point1, Point2>::type,
                true, false
            >::apply(get_as_radian<0>(point1), get_as_radian<1>(point1),
                     get_as_radian<0>(point2), get_as_radian<1>(point2),
                     m_spheroid).distance;
    }

    inline Spheroid const& model() const
    {
        return m_spheroid;
    }

private :
    Spheroid m_spheroid;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename Spheroid, typename CalculationType>
struct tag<andoyer<Spheroid, CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename Spheroid, typename CalculationType, typename P1, typename P2>
struct return_type<andoyer<Spheroid, CalculationType>, P1, P2>
    : andoyer<Spheroid, CalculationType>::template calculation_type<P1, P2>
{};

template <typename Spheroid, typename CalculationType>
struct comparable_type<andoyer<Spheroid, CalculationType> >
{
    typedef andoyer<Spheroid, CalculationType> type;
};

template <typename Spheroid, typename CalculationType>
struct get_comparable<andoyer<Spheroid, CalculationType> >
{
    static inline andoyer<Spheroid, CalculationType> apply(andoyer<Spheroid, CalculationType> const& input)
    {
        return input;
    }
};

template <typename Spheroid, typename CalculationType, typename P1, typename P2>
struct result_from_distance<andoyer<Spheroid, CalculationType>, P1, P2>
{
    template <typename T>
    static inline typename return_type<andoyer<Spheroid, CalculationType>, P1, P2>::type
        apply(andoyer<Spheroid, CalculationType> const& , T const& value)
    {
        return value;
    }
};

template <typename Point1, typename Point2>
struct default_strategy<point_tag, point_tag, Point1, Point2, geographic_tag, geographic_tag>
{
    typedef strategy::distance::andoyer
                <
                    srs::spheroid
                        <
                            typename select_coordinate_type<Point1, Point2>::type
                        >
                > type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_ANDOYER_HPP