#include <NTL/mat_lzz_p.h>

#include <NTL/new.h>

#include <NTL/vec_long.h>
#include <NTL/vec_ulong.h>
#include <NTL/vec_double.h>

NTL_START_IMPL

void add(mat_zz_p& X, const mat_zz_p& A, const mat_zz_p& B)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (B.NumRows() != n || B.NumCols() != m)
      LogicError("matrix add: dimension mismatch");

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         add(X(i,j), A(i,j), B(i,j));
}

void sub(mat_zz_p& X, const mat_zz_p& A, const mat_zz_p& B)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (B.NumRows() != n || B.NumCols() != m)
      LogicError("matrix sub: dimension mismatch");

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         sub(X(i,j), A(i,j), B(i,j));
}

// some local buffers

NTL_THREAD_LOCAL static vec_long mul_aux_vec;
NTL_THREAD_LOCAL static NTL_SPMM_VEC_T precon_vec;

static
void mul_aux(mat_zz_p& X, const mat_zz_p& A, const mat_zz_p& B)
{
   long n = A.NumRows();
   long l = A.NumCols();
   long m = B.NumCols();

   if (l != B.NumRows())
      LogicError("matrix mul: dimension mismatch");

   X.SetDims(n, m);

   if (m > 1) {  // new preconditioning code

      long p = zz_p::modulus();
      double pinv = zz_p::ModulusInverse();

      vec_long::Watcher watch_mul_aux_vec(mul_aux_vec);
      mul_aux_vec.SetLength(m);
      long *acc = mul_aux_vec.elts();

      long i, j, k;

      for (i = 0; i < n; i++) {
         const zz_p* ap = A[i].elts();

         for (j = 0; j < m; j++) acc[j] = 0;

         for (k = 0;  k < l; k++) {
            long aa = rep(ap[k]);
            if (aa != 0) {
               const zz_p* bp = B[k].elts();
               long T1;
               mulmod_precon_t aapinv = PrepMulModPrecon(aa, p, pinv);

               for (j = 0; j < m; j++) {
        	  T1 = MulModPrecon(rep(bp[j]), aa, p, aapinv);
        	  acc[j] = AddMod(acc[j], T1, p);
               }
            }
         }

         zz_p *xp = X[i].elts();
         for (j = 0; j < m; j++)
            xp[j].LoopHole() = acc[j];
      }
   }
   else {  // just use the old code, w/o preconditioning

      long p = zz_p::modulus();
      double pinv = zz_p::ModulusInverse();

      long i, j, k;
      long acc, tmp;

      for (i = 1; i <= n; i++) {
	 for (j = 1; j <= m; j++) {
            acc = 0;
            for(k = 1; k <= l; k++) {
               tmp = MulMod(rep(A(i,k)), rep(B(k,j)), p, pinv);
               acc = AddMod(acc, tmp, p);
            }
            X(i,j).LoopHole() = acc;
	 }
      }

   }
}

void mul(mat_zz_p& X, const mat_zz_p& A, const mat_zz_p& B)
{
   if (&X == &A || &X == &B) {
      mat_zz_p tmp;
      mul_aux(tmp, A, B);
      X = tmp;
   }
   else
      mul_aux(X, A, B);
}

void mul(vec_zz_p& x, const vec_zz_p& a, const mat_zz_p& B)
{
   long l = a.length();
   long m = B.NumCols();

   if (l != B.NumRows())
      LogicError("matrix mul: dimension mismatch");

   if (m == 0) {

      x.SetLength(0);

   }
   else if (m == 1) {

      long p = zz_p::modulus();
      double pinv = zz_p::ModulusInverse();

      long acc, tmp;
      long k;

      acc = 0;
      for(k = 1; k <= l; k++) {
         tmp = MulMod(rep(a(k)), rep(B(k,1)), p, pinv);
         acc = AddMod(acc, tmp, p);
      }

      x.SetLength(1);
      x(1).LoopHole()  = acc;

   }
   else {  // m > 1.  precondition

      long p = zz_p::modulus();
      double pinv = zz_p::ModulusInverse();

      vec_long::Watcher watch_mul_aux_vec(mul_aux_vec);
      mul_aux_vec.SetLength(m);
      long *acc = mul_aux_vec.elts();

      long j, k;

      const zz_p* ap = a.elts();

      for (j = 0; j < m; j++) acc[j] = 0;

      for (k = 0;  k < l; k++) {
         long aa = rep(ap[k]);
         if (aa != 0) {
            const zz_p* bp = B[k].elts();
            long T1;
            mulmod_precon_t aapinv = PrepMulModPrecon(aa, p, pinv);

            for (j = 0; j < m; j++) {
               T1 = MulModPrecon(rep(bp[j]), aa, p, aapinv);
               acc[j] = AddMod(acc[j], T1, p);
            }
         }
      }

      x.SetLength(m);
      zz_p *xp = x.elts();
      for (j = 0; j < m; j++)
         xp[j].LoopHole() = acc[j];
   }
}

void mul_aux(vec_zz_p& x, const mat_zz_p& A, const vec_zz_p& b)
{
   long n = A.NumRows();
   long l = A.NumCols();

   if (l != b.length())
      LogicError("matrix mul: dimension mismatch");

   x.SetLength(n);
   zz_p* xp = x.elts();

   long p = zz_p::modulus();
   double pinv = zz_p::ModulusInverse();

   long i, k;
   long acc, tmp;

   const zz_p* bp = b.elts();

   if (n <= 1) {

      for (i = 0; i < n; i++) {
	 acc = 0;
	 const zz_p* ap = A[i].elts();

	 for (k = 0; k < l; k++) {
            tmp = MulMod(rep(ap[k]), rep(bp[k]), p, pinv);
            acc = AddMod(acc, tmp, p);
	 }

	 xp[i].LoopHole() = acc;
      }

   }
   else {

      NTL_SPMM_VEC_T::Watcher watch_precon_vec(precon_vec);
      precon_vec.SetLength(l);
      mulmod_precon_t *bpinv = precon_vec.elts();

      for (k = 0; k < l; k++)
         bpinv[k] = PrepMulModPrecon(rep(bp[k]), p, pinv);

      for (i = 0; i < n; i++) {
	 acc = 0;
	 const zz_p* ap = A[i].elts();

	 for (k = 0; k < l; k++) {
            tmp = MulModPrecon(rep(ap[k]), rep(bp[k]), p, bpinv[k]);
            acc = AddMod(acc, tmp, p);
	 }

	 xp[i].LoopHole() = acc;
      }
   }
}

void mul(vec_zz_p& x, const mat_zz_p& A, const vec_zz_p& b)
{
   if (&b == &x || A.position1(x) != -1) {
      vec_zz_p tmp;
      mul_aux(tmp, A, b);
      x = tmp;
   }
   else
      mul_aux(x, A, b);

}

void mul(mat_zz_p& X, const mat_zz_p& A, zz_p b)
{
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;

   if (n == 0 || m == 0 || (n == 1 && m == 1)) {

      for (i = 0; i < n; i++)
	 for (j = 0; j < m; j++)
            mul(X[i][j], A[i][j], b);

   }
   else {

      long p = zz_p::modulus();
      double pinv = zz_p::ModulusInverse();
      long bb = rep(b);
      mulmod_precon_t bpinv = PrepMulModPrecon(bb, p, pinv);

      for (i = 0; i < n; i++) {
         const zz_p *ap = A[i].elts();
         zz_p *xp = X[i].elts();

	 for (j = 0; j < m; j++)
            xp[j].LoopHole() = MulModPrecon(rep(ap[j]), bb, p, bpinv);
      }

   }
}

void mul(mat_zz_p& X, const mat_zz_p& A, long b_in)
{
   zz_p b;
   b = b_in;
   mul(X, A, b);
}

void ident(mat_zz_p& X, long n)
{
   X.SetDims(n, n);
   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i == j)
            set(X(i, j));
         else
            clear(X(i, j));
}

void determinant(zz_p& d, const mat_zz_p& M_in)
{
   long k, n;
   long i, j;
   long pos;
   zz_p t1, t2, t3;
   zz_p *x, *y;

   mat_zz_p M;
   M = M_in;

   n = M.NumRows();

   if (M.NumCols() != n)
      LogicError("determinant: nonsquare matrix");

   if (n == 0) {
      set(d);
      return;
   }

   zz_p det;

   set(det);

   long p = zz_p::modulus();
   double pinv = zz_p::ModulusInverse();

   for (k = 0; k < n; k++) {
      pos = -1;
      for (i = k; i < n; i++) {
         if (!IsZero(M[i][k])) {
            pos = i;
            break;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         mul(det, det, M[k][k]);

         inv(t3, M[k][k]);

         for (i = k+1; i < n; i++) {
            // M[i] = M[i] - M[k]*M[i,k]*t3

            mul(t1, M[i][k], t3);
            negate(t1, t1);

            x = M[i].elts() + (k+1);
            y = M[k].elts() + (k+1);

            long T1 = rep(t1);
            mulmod_precon_t t1pinv = PrepMulModPrecon(T1, p, pinv); // T1*pinv;
            long T2;

            for (j = k+1; j < n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               T2 = MulModPrecon(rep(*y), T1, p, t1pinv);
               x->LoopHole() = AddMod(rep(*x), T2, p);
            }
         }
      }
      else {
         clear(d);
         return;
      }
   }

   d = det;
}

long IsIdent(const mat_zz_p& A, long n)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i != j) {
            if (!IsZero(A(i, j))) return 0;
         }
         else {
            if (!IsOne(A(i, j))) return 0;
         }

   return 1;
}

void transpose(mat_zz_p& X, const mat_zz_p& A)
{
   long n = A.NumRows();
   long m = A.NumCols();

   long i, j;

   if (&X == & A) {
      if (n == m)
         for (i = 1; i <= n; i++)
            for (j = i+1; j <= n; j++)
               swap(X(i, j), X(j, i));
      else {
         mat_zz_p tmp;
         tmp.SetDims(m, n);
         for (i = 1; i <= n; i++)
            for (j = 1; j <= m; j++)
               tmp(j, i) = A(i, j);
         X.kill();
         X = tmp;
      }
   }
   else {
      X.SetDims(m, n);
      for (i = 1; i <= n; i++)
         for (j = 1; j <= m; j++)
            X(j, i) = A(i, j);
   }
}

void solve(zz_p& d, vec_zz_p& X,
           const mat_zz_p& A, const vec_zz_p& b)

{
   long n = A.NumRows();

   if (A.NumCols() != n)
      LogicError("solve: nonsquare matrix");

   if (b.length() != n)
      LogicError("solve: dimension mismatch");

   if (n == 0) {
      set(d);
      X.SetLength(0);
      return;
   }

   long i, j, k, pos;
   zz_p t1, t2, t3;
   zz_p *x, *y;

   mat_zz_p M;
   M.SetDims(n, n+1);
   for (i = 0; i < n; i++) {
      for (j = 0; j < n; j++)
         M[i][j] = A[j][i];
      M[i][n] = b[i];
   }

   zz_p det;
   set(det);

   long p = zz_p::modulus();
   double pinv = zz_p::ModulusInverse();

   for (k = 0; k < n; k++) {
      pos = -1;
      for (i = k; i < n; i++) {
         if (!IsZero(M[i][k])) {
            pos = i;
            break;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         mul(det, det, M[k][k]);

         inv(t3, M[k][k]);
         M[k][k] = t3;

         for (i = k+1; i < n; i++) {
            // M[i] = M[i] - M[k]*M[i,k]*t3

            mul(t1, M[i][k], t3);
            negate(t1, t1);

            x = M[i].elts() + (k+1);
            y = M[k].elts() + (k+1);

            long T1 = rep(t1);
            mulmod_precon_t  t1pinv = PrepMulModPrecon(T1, p, pinv); // T1*pinv;
            long T2;

            for (j = k+1; j <= n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               T2 = MulModPrecon(rep(*y), T1, p, t1pinv);
               x->LoopHole() = AddMod(rep(*x), T2, p);
            }
         }
      }
      else {
         clear(d);
         return;
      }
   }

   X.SetLength(n);
   for (i = n-1; i >= 0; i--) {
      clear(t1);
      for (j = i+1; j < n; j++) {
         mul(t2, X[j], M[i][j]);
         add(t1, t1, t2);
      }
      sub(t1, M[i][n], t1);
      mul(X[i], t1, M[i][i]);
   }

   d = det;
}

void inv(zz_p& d, mat_zz_p& X, const mat_zz_p& A)
{
   long n = A.NumRows();
   if (A.NumCols() != n)
      LogicError("inv: nonsquare matrix");

   if (n == 0) {
      set(d);
      X.SetDims(0, 0);
      return;
   }

   long i, j, k, pos;
   zz_p t1, t2, t3;
   zz_p *x, *y;

   mat_zz_p M;
   M.SetDims(n, 2*n);
   for (i = 0; i < n; i++) {
      for (j = 0; j < n; j++) {
         M[i][j] = A[i][j];
         clear(M[i][n+j]);
      }
      set(M[i][n+i]);
   }

   zz_p det;
   set(det);

   long p = zz_p::modulus();
   double pinv = zz_p::ModulusInverse();

   for (k = 0; k < n; k++) {
      pos = -1;
      for (i = k; i < n; i++) {
         if (!IsZero(M[i][k])) {
            pos = i;
            break;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         mul(det, det, M[k][k]);

         inv(t3, M[k][k]);
         M[k][k] = t3;

         for (i = k+1; i < n; i++) {
            // M[i] = M[i] - M[k]*M[i,k]*t3

            mul(t1, M[i][k], t3);
            negate(t1, t1);

            x = M[i].elts() + (k+1);
            y = M[k].elts() + (k+1);

            long T1 = rep(t1);
            mulmod_precon_t t1pinv = PrepMulModPrecon(T1, p, pinv); // T1*pinv;
            long T2;

            for (j = k+1; j < 2*n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               T2 = MulModPrecon(rep(*y), T1, p, t1pinv);
               x->LoopHole() = AddMod(rep(*x), T2, p);
            }
         }
      }
      else {
         clear(d);
         return;
      }
   }

   X.SetDims(n, n);
   for (k = 0; k < n; k++) {
      for (i = n-1; i >= 0; i--) {
         clear(t1);
         for (j = i+1; j < n; j++) {
            mul(t2, X[j][k], M[i][j]);
            add(t1, t1, t2);
         }
         sub(t1, M[i][n+k], t1);
         mul(X[i][k], t1, M[i][i]);
      }
   }

   d = det;
}

long gauss(mat_zz_p& M, long w)
{
   long k, l;
   long i, j;
   long pos;
   zz_p t1, t2, t3;
   zz_p *x, *y;

   long n = M.NumRows();
   long m = M.NumCols();

   if (w < 0 || w > m)
      LogicError("gauss: bad args");

   long p = zz_p::modulus();
   double pinv = zz_p::ModulusInverse();
   long T1, T2;

   l = 0;
   for (k = 0; k < w && l < n; k++) {

      pos = -1;
      for (i = l; i < n; i++) {
         if (!IsZero(M[i][k])) {
            pos = i;
            break;
         }
      }

      if (pos != -1) {
         swap(M[pos], M[l]);

         inv(t3, M[l][k]);
         negate(t3, t3);

         for (i = l+1; i < n; i++) {
            // M[i] = M[i] + M[l]*M[i,k]*t3

            mul(t1, M[i][k], t3);

            T1 = rep(t1);
            mulmod_precon_t T1pinv = PrepMulModPrecon(T1, p, pinv); // ((double) T1)*pinv;

            clear(M[i][k]);

            x = M[i].elts() + (k+1);
            y = M[l].elts() + (k+1);

            for (j = k+1; j < m; j++, x++, y++) {
               // *x = *x + (*y)*t1

               T2 = MulModPrecon(rep(*y), T1, p, T1pinv);
               T2 = AddMod(T2, rep(*x), p);
               (*x).LoopHole() = T2;
            }
         }

         l++;
      }
   }

   return l;
}

long gauss(mat_zz_p& M)
{
   return gauss(M, M.NumCols());
}

void image(mat_zz_p& X, const mat_zz_p& A)
{
   mat_zz_p M;
   M = A;
   long r = gauss(M);
   M.SetDims(r, M.NumCols());
   X = M;
}

void kernel(mat_zz_p& X, const mat_zz_p& A)
{
   long m = A.NumRows();
   long n = A.NumCols();

   mat_zz_p M;
   long r;

   transpose(M, A);
   r = gauss(M);

   X.SetDims(m-r, m);

   long i, j, k, s;
   zz_p t1, t2;

   vec_long D;
   D.SetLength(m);
   for (j = 0; j < m; j++) D[j] = -1;

   vec_zz_p inverses;
   inverses.SetLength(m);

   j = -1;
   for (i = 0; i < r; i++) {
      do {
         j++;
      } while (IsZero(M[i][j]));

      D[j] = i;
      inv(inverses[j], M[i][j]);
   }

   for (k = 0; k < m-r; k++) {
      vec_zz_p& v = X[k];
      long pos = 0;
      for (j = m-1; j >= 0; j--) {
         if (D[j] == -1) {
            if (pos == k)
               set(v[j]);
            else
               clear(v[j]);
            pos++;
         }
         else {
            i = D[j];

            clear(t1);

            for (s = j+1; s < m; s++) {
               mul(t2, v[s], M[i][s]);
               add(t1, t1, t2);
            }

            mul(t1, t1, inverses[j]);
            negate(v[j], t1);
         }
      }
   }
}

void diag(mat_zz_p& X, long n, zz_p d)
{
   X.SetDims(n, n);
   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i == j)
            X(i, j) = d;
         else
            clear(X(i, j));
}

long IsDiag(const mat_zz_p& A, long n, zz_p d)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i != j) {
            if (!IsZero(A(i, j))) return 0;
         }
         else {
            if (A(i, j) != d) return 0;
         }

   return 1;
}

void negate(mat_zz_p& X, const mat_zz_p& A)
{
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         negate(X(i,j), A(i,j));
}

long IsZero(const mat_zz_p& a)
{
   long n = a.NumRows();
   long i;

   for (i = 0; i < n; i++)
      if (!IsZero(a[i]))
         return 0;

   return 1;
}

void clear(mat_zz_p& x)
{
   long n = x.NumRows();
   long i;
   for (i = 0; i < n; i++)
      clear(x[i]);
}

mat_zz_p operator+(const mat_zz_p& a, const mat_zz_p& b)
{
   mat_zz_p res;
   add(res, a, b);
   NTL_OPT_RETURN(mat_zz_p, res);
}

mat_zz_p operator*(const mat_zz_p& a, const mat_zz_p& b)
{
   mat_zz_p res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(mat_zz_p, res);
}

mat_zz_p operator-(const mat_zz_p& a, const mat_zz_p& b)
{
   mat_zz_p res;
   sub(res, a, b);
   NTL_OPT_RETURN(mat_zz_p, res);
}

mat_zz_p operator-(const mat_zz_p& a)
{
   mat_zz_p res;
   negate(res, a);
   NTL_OPT_RETURN(mat_zz_p, res);
}

vec_zz_p operator*(const mat_zz_p& a, const vec_zz_p& b)
{
   vec_zz_p res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_zz_p, res);
}

vec_zz_p operator*(const vec_zz_p& a, const mat_zz_p& b)
{
   vec_zz_p res;
   mul(res, a, b);
   NTL_OPT_RETURN(vec_zz_p, res);
}

void inv(mat_zz_p& X, const mat_zz_p& A)
{
   zz_p d;
   inv(d, X, A);
   if (d == 0) ArithmeticError("inv: non-invertible matrix");
}

void power(mat_zz_p& X, const mat_zz_p& A, const ZZ& e)
{
   if (A.NumRows() != A.NumCols()) LogicError("power: non-square matrix");

   if (e == 0) {
      ident(X, A.NumRows());
      return;
   }

   mat_zz_p T1, T2;
   long i, k;

   k = NumBits(e);
   T1 = A;

   for (i = k-2; i >= 0; i--) {
      sqr(T2, T1);
      if (bit(e, i))
         mul(T1, T2, A);
      else
         T1 = T2;
   }

   if (e < 0)
      inv(X, T1);
   else
      X = T1;
}

NTL_END_IMPL


/*
  [auto_generated]
  boost/numeric/odeint/stepper/bulirsch_stoer.hpp

  [begin_description]
  Implementation of the Burlish-Stoer method. As described in
  Ernst Hairer, Syvert Paul Norsett, Gerhard Wanner
  Solving Ordinary Differential Equations I. Nonstiff Problems.
  Springer Series in Comput. Mathematics, Vol. 8, Springer-Verlag 1987, Second revised edition 1993.
  [end_description]

  Copyright 2011-2013 Mario Mulansky
  Copyright 2011-2013 Karsten Ahnert
  Copyright 2012 Christoph Koke

  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 BOOST_NUMERIC_ODEINT_STEPPER_BULIRSCH_STOER_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_BULIRSCH_STOER_HPP_INCLUDED

#include <iostream>

#include <algorithm>

#include <boost/config.hpp> // for min/max guidelines

#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>

#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/modified_midpoint.hpp>
#include <boost/numeric/odeint/stepper/controlled_step_result.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>

#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/util/unit_helper.hpp>
#include <boost/numeric/odeint/util/detail/less_with_sign.hpp>

namespace boost {
namespace numeric {
namespace odeint {

template<
    class State ,
    class Value = double ,
    class Deriv = State ,
    class Time = Value ,
    class Algebra = typename algebra_dispatcher< State >::algebra_type ,
    class Operations = typename operations_dispatcher< State >::operations_type ,
    class Resizer = initially_resizer
    >
class bulirsch_stoer {

public:

    typedef State state_type;
    typedef Value value_type;
    typedef Deriv deriv_type;
    typedef Time time_type;
    typedef Algebra algebra_type;
    typedef Operations operations_type;
    typedef Resizer resizer_type;
#ifndef DOXYGEN_SKIP
    typedef state_wrapper< state_type > wrapped_state_type;
    typedef state_wrapper< deriv_type > wrapped_deriv_type;
    typedef controlled_stepper_tag stepper_category;

    typedef bulirsch_stoer< State , Value , Deriv , Time , Algebra , Operations , Resizer > controlled_error_bs_type;

    typedef typename inverse_time< time_type >::type inv_time_type;

    typedef std::vector< value_type > value_vector;
    typedef std::vector< time_type > time_vector;
    typedef std::vector< inv_time_type > inv_time_vector;  //should be 1/time_type for boost.units
    typedef std::vector< value_vector > value_matrix;
    typedef std::vector< size_t > int_vector;
    typedef std::vector< wrapped_state_type > state_table_type;
#endif //DOXYGEN_SKIP
    const static size_t m_k_max = 8;

    bulirsch_stoer(
        value_type eps_abs = 1E-6 , value_type eps_rel = 1E-6 ,
        value_type factor_x = 1.0 , value_type factor_dxdt = 1.0 ,
        time_type max_dt = static_cast<time_type>(0))
        : m_error_checker( eps_abs , eps_rel , factor_x, factor_dxdt ) , m_midpoint() ,
          m_last_step_rejected( false ) , m_first( true ) ,
          m_max_dt(max_dt) ,
          m_interval_sequence( m_k_max+1 ) ,
          m_coeff( m_k_max+1 ) ,
          m_cost( m_k_max+1 ) ,
          m_facmin_table( m_k_max+1 ) ,
          m_table( m_k_max ) ,
          STEPFAC1( 0.65 ) , STEPFAC2( 0.94 ) , STEPFAC3( 0.02 ) , STEPFAC4( 4.0 ) , KFAC1( 0.8 ) , KFAC2( 0.9 )
    {
        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();
        /* initialize sequence of stage numbers and work */
        for( unsigned short i = 0; i < m_k_max+1; i++ )
        {
            m_interval_sequence[i] = 2 * (i+1);
            if( i == 0 )
                m_cost[i] = m_interval_sequence[i];
            else
                m_cost[i] = m_cost[i-1] + m_interval_sequence[i];
            m_coeff[i].resize(i);
            m_facmin_table[i] = pow BOOST_PREVENT_MACRO_SUBSTITUTION( STEPFAC3 , static_cast< value_type >(1) / static_cast< value_type >( 2*i+1 ) );
            for( size_t k = 0 ; k < i ; ++k  )
            {
                const value_type r = static_cast< value_type >( m_interval_sequence[i] ) / static_cast< value_type >( m_interval_sequence[k] );
                m_coeff[i][k] = 1.0 / ( r*r - static_cast< value_type >( 1.0 ) ); // coefficients for extrapolation
            }
        }
        reset();
    }

    /*
     * Version 1 : try_step( sys , x , t , dt )
     *
     * The overloads are needed to solve the forwarding problem
     */
    template< class System , class StateInOut >
    controlled_step_result try_step( System system , StateInOut &x , time_type &t , time_type &dt )
    {
        return try_step_v1( system , x , t, dt );
    }

    /**
     * \brief Second version to solve the forwarding problem, can be used with Boost.Range as StateInOut.
     */
    template< class System , class StateInOut >
    controlled_step_result try_step( System system , const StateInOut &x , time_type &t , time_type &dt )
    {
        return try_step_v1( system , x , t, dt );
    }

    /*
     * Version 2 : try_step( sys , x , dxdt , t , dt )
     *
     * this version does not solve the forwarding problem, boost.range can not be used
     */
    template< class System , class StateInOut , class DerivIn >
    controlled_step_result try_step( System system , StateInOut &x , const DerivIn &dxdt , time_type &t , time_type &dt )
    {
        m_xnew_resizer.adjust_size( x , detail::bind( &controlled_error_bs_type::template resize_m_xnew< StateInOut > , detail::ref( *this ) , detail::_1 ) );
        controlled_step_result res = try_step( system , x , dxdt , t , m_xnew.m_v , dt );
        if( res == success )
        {
            boost::numeric::odeint::copy( m_xnew.m_v , x );
        }
        return res;
    }

    /*
     * Version 3 : try_step( sys , in , t , out , dt )
     *
     * this version does not solve the forwarding problem, boost.range can not be used
     */
    template< class System , class StateIn , class StateOut >
    typename boost::disable_if< boost::is_same< StateIn , time_type > , controlled_step_result >::type
    try_step( System system , const StateIn &in , time_type &t , StateOut &out , time_type &dt )
    {
        typename odeint::unwrap_reference< System >::type &sys = system;
        m_dxdt_resizer.adjust_size( in , detail::bind( &controlled_error_bs_type::template resize_m_dxdt< StateIn > , detail::ref( *this ) , detail::_1 ) );
        sys( in , m_dxdt.m_v , t );
        return try_step( system , in , m_dxdt.m_v , t , out , dt );
    }

    /*
     * Full version : try_step( sys , in , dxdt_in , t , out , dt )
     *
     * contains the actual implementation
     */
    template< class System , class StateIn , class DerivIn , class StateOut >
    controlled_step_result try_step( System system , const StateIn &in , const DerivIn &dxdt , time_type &t , StateOut &out , time_type &dt )
    {
        if( m_max_dt != static_cast<time_type>(0) && detail::less_with_sign(m_max_dt, dt, dt) )
        {
            // given step size is bigger then max_dt
            // set limit and return fail
            dt = m_max_dt;
            return fail;
        }

        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();

        static const value_type val1( 1.0 );

        if( m_resizer.adjust_size( in , detail::bind( &controlled_error_bs_type::template resize_impl< StateIn > , detail::ref( *this ) , detail::_1 ) ) )
        {
            reset(); // system resized -> reset
        }

        if( dt != m_dt_last )
        {
            reset(); // step size changed from outside -> reset
        }

        bool reject( true );

        time_vector h_opt( m_k_max+1 );
        inv_time_vector work( m_k_max+1 );

        time_type new_h = dt;

        /* m_current_k_opt is the estimated current optimal stage number */
        for( size_t k = 0 ; k <= m_current_k_opt+1 ; k++ )
        {
            /* the stage counts are stored in m_interval_sequence */
            m_midpoint.set_steps( m_interval_sequence[k] );
            if( k == 0 )
            {
                m_midpoint.do_step( system , in , dxdt , t , out , dt );
                /* the first step, nothing more to do */
            }
            else
            {
                m_midpoint.do_step( system , in , dxdt , t , m_table[k-1].m_v , dt );
                extrapolate( k , m_table , m_coeff , out );
                // get error estimate
                m_algebra.for_each3( m_err.m_v , out , m_table[0].m_v ,
                                     typename operations_type::template scale_sum2< value_type , value_type >( val1 , -val1 ) );
                const value_type error = m_error_checker.error( m_algebra , in , dxdt , m_err.m_v , dt );
                h_opt[k] = calc_h_opt( dt , error , k );
                work[k] = static_cast<value_type>( m_cost[k] ) / h_opt[k];

                if( (k == m_current_k_opt-1) || m_first )
                { // convergence before k_opt ?
                    if( error < 1.0 )
                    {
                        //convergence
                        reject = false;
                        if( (work[k] < KFAC2*work[k-1]) || (m_current_k_opt <= 2) )
                        {
                            // leave order as is (except we were in first round)
                            m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(k)+1 ) );
                            new_h = h_opt[k];
                            new_h *= static_cast<value_type>( m_cost[k+1] ) / static_cast<value_type>( m_cost[k] );
                        } else {
                            m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(k) ) );
                            new_h = h_opt[k];
                        }
                        break;
                    }
                    else if( should_reject( error , k ) && !m_first )
                    {
                        reject = true;
                        new_h = h_opt[k];
                        break;
                    }
                }
                if( k == m_current_k_opt )
                { // convergence at k_opt ?
                    if( error < 1.0 )
                    {
                        //convergence
                        reject = false;
                        if( (work[k-1] < KFAC2*work[k]) )
                        {
                            m_current_k_opt = max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(m_current_k_opt)-1 );
                            new_h = h_opt[m_current_k_opt];
                        }
                        else if( (work[k] < KFAC2*work[k-1]) && !m_last_step_rejected )
                        {
                            m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max-1) , static_cast<int>(m_current_k_opt)+1 );
                            new_h = h_opt[k];
                            new_h *= static_cast<value_type>(m_cost[m_current_k_opt])/static_cast<value_type>(m_cost[k]);
                        } else
                            new_h = h_opt[m_current_k_opt];
                        break;
                    }
                    else if( should_reject( error , k ) )
                    {
                        reject = true;
                        new_h = h_opt[m_current_k_opt];
                        break;
                    }
                }
                if( k == m_current_k_opt+1 )
                { // convergence at k_opt+1 ?
                    if( error < 1.0 )
                    {   //convergence
                        reject = false;
                        if( work[k-2] < KFAC2*work[k-1] )
                            m_current_k_opt = max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(m_current_k_opt)-1 );
                        if( (work[k] < KFAC2*work[m_current_k_opt]) && !m_last_step_rejected )
                            m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , static_cast<int>(k) );
                        new_h = h_opt[m_current_k_opt];
                    } else
                    {
                        reject = true;
                        new_h = h_opt[m_current_k_opt];
                    }
                    break;
                }
            }
        }

        if( !reject )
        {
            t += dt;
        }

        if( !m_last_step_rejected || boost::numeric::odeint::detail::less_with_sign(new_h, dt, dt) )
        {
            // limit step size
            if( m_max_dt != static_cast<time_type>(0) )
            {
                new_h = detail::min_abs(m_max_dt, new_h);
            }
            m_dt_last = new_h;
            dt = new_h;
        }

        m_last_step_rejected = reject;
        m_first = false;

        if( reject )
            return fail;
        else
            return success;
    }

    /** \brief Resets the internal state of the stepper */
    void reset()
    {
        m_first = true;
        m_last_step_rejected = false;
        // crude estimate of optimal order
        m_current_k_opt = 4;
        /* no calculation because log10 might not exist for value_type!
        const value_type logfact( -log10( max BOOST_PREVENT_MACRO_SUBSTITUTION( eps_rel , static_cast< value_type >(1.0E-12) ) ) * 0.6 + 0.5 );
        m_current_k_opt = max BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<value_type>( 1 ) , min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<value_type>( m_k_max-1 ) , logfact ));
        */
    }

    /* Resizer methods */

    template< class StateIn >
    void adjust_size( const StateIn &x )
    {
        resize_m_dxdt( x );
        resize_m_xnew( x );
        resize_impl( x );
        m_midpoint.adjust_size( x );
    }

private:

    template< class StateIn >
    bool resize_m_dxdt( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
    }

    template< class StateIn >
    bool resize_m_xnew( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_xnew , x , typename is_resizeable<state_type>::type() );
    }

    template< class StateIn >
    bool resize_impl( const StateIn &x )
    {
        bool resized( false );
        for( size_t i = 0 ; i < m_k_max ; ++i )
            resized |= adjust_size_by_resizeability( m_table[i] , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_err , x , typename is_resizeable<state_type>::type() );
        return resized;
    }

    template< class System , class StateInOut >
    controlled_step_result try_step_v1( System system , StateInOut &x , time_type &t , time_type &dt )
    {
        typename odeint::unwrap_reference< System >::type &sys = system;
        m_dxdt_resizer.adjust_size( x , detail::bind( &controlled_error_bs_type::template resize_m_dxdt< StateInOut > , detail::ref( *this ) , detail::_1 ) );
        sys( x , m_dxdt.m_v ,t );
        return try_step( system , x , m_dxdt.m_v , t , dt );
    }

    template< class StateInOut >
    void extrapolate( size_t k , state_table_type &table , const value_matrix &coeff , StateInOut &xest )
    /* polynomial extrapolation, see http://www.nr.com/webnotes/nr3web21.pdf
       uses the obtained intermediate results to extrapolate to dt->0
    */
    {
        static const value_type val1 = static_cast< value_type >( 1.0 );
        for( int j=k-1 ; j>0 ; --j )
        {
            m_algebra.for_each3( table[j-1].m_v , table[j].m_v , table[j-1].m_v ,
                                 typename operations_type::template scale_sum2< value_type , value_type >( val1 + coeff[k][j] , -coeff[k][j] ) );
        }
        m_algebra.for_each3( xest , table[0].m_v , xest ,
                             typename operations_type::template scale_sum2< value_type , value_type >( val1 + coeff[k][0] , -coeff[k][0]) );
    }

    time_type calc_h_opt( time_type h , value_type error , size_t k ) const
    /* calculates the optimal step size for a given error and stage number */
    {
        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();
        using std::pow;
        value_type expo( 1.0/(2*k+1) );
        value_type facmin = m_facmin_table[k];
        value_type fac;
        if (error == 0.0)
            fac=1.0/facmin;
        else
        {
            fac = STEPFAC2 / pow BOOST_PREVENT_MACRO_SUBSTITUTION( error / STEPFAC1 , expo );
            fac = max BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<value_type>(facmin/STEPFAC4) , min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<value_type>(1.0/facmin) , fac ) );
        }
        return h*fac;
    }

    controlled_step_result set_k_opt( size_t k , const inv_time_vector &work , const time_vector &h_opt , time_type &dt )
    /* calculates the optimal stage number */
    {
        if( k == 1 )
        {
            m_current_k_opt = 2;
            return success;
        }
        if( (work[k-1] < KFAC1*work[k]) || (k == m_k_max) )
        {   // order decrease
            m_current_k_opt = k-1;
            dt = h_opt[ m_current_k_opt ];
            return success;
        }
        else if( (work[k] < KFAC2*work[k-1]) || m_last_step_rejected || (k == m_k_max-1) )
        {   // same order - also do this if last step got rejected
            m_current_k_opt = k;
            dt = h_opt[ m_current_k_opt ];
            return success;
        }
        else
        {   // order increase - only if last step was not rejected
            m_current_k_opt = k+1;
            dt = h_opt[ m_current_k_opt-1 ] * m_cost[ m_current_k_opt ] / m_cost[ m_current_k_opt-1 ] ;
            return success;
        }
    }

    bool in_convergence_window( size_t k ) const
    {
        if( (k == m_current_k_opt-1) && !m_last_step_rejected )
            return true; // decrease stepsize only if last step was not rejected
        return ( (k == m_current_k_opt) || (k == m_current_k_opt+1) );
    }

    bool should_reject( value_type error , size_t k ) const
    {
        if( k == m_current_k_opt-1 )
        {
            const value_type d = m_interval_sequence[m_current_k_opt] * m_interval_sequence[m_current_k_opt+1] /
                (m_interval_sequence[0]*m_interval_sequence[0]);
            //step will fail, criterion 17.3.17 in NR
            return ( error > d*d );
        }
        else if( k == m_current_k_opt )
        {
            const value_type d = m_interval_sequence[m_current_k_opt] / m_interval_sequence[0];
            return ( error > d*d );
        } else
            return error > 1.0;
    }

    default_error_checker< value_type, algebra_type , operations_type > m_error_checker;
    modified_midpoint< state_type , value_type , deriv_type , time_type , algebra_type , operations_type , resizer_type > m_midpoint;

    bool m_last_step_rejected;
    bool m_first;

    time_type m_dt_last;
    time_type m_t_last;
    time_type m_max_dt;

    size_t m_current_k_opt;

    algebra_type m_algebra;

    resizer_type m_dxdt_resizer;
    resizer_type m_xnew_resizer;
    resizer_type m_resizer;

    wrapped_state_type m_xnew;
    wrapped_state_type m_err;
    wrapped_deriv_type m_dxdt;

    int_vector m_interval_sequence; // stores the successive interval counts
    value_matrix m_coeff;
    int_vector m_cost; // costs for interval count
    value_vector m_facmin_table; // for precomputed facmin to save pow calls

    state_table_type m_table; // sequence of states for extrapolation

    value_type STEPFAC1 , STEPFAC2 , STEPFAC3 , STEPFAC4 , KFAC1 , KFAC2;
};

/******** DOXYGEN ********/
/**
 * \class bulirsch_stoer
 * \brief The Bulirsch-Stoer algorithm.
 *
 * The Bulirsch-Stoer is a controlled stepper that adjusts both step size
 * and order of the method. The algorithm uses the modified midpoint and
 * a polynomial extrapolation compute the solution.
 *
 * \tparam State The state type.
 * \tparam Value The value type.
 * \tparam Deriv The type representing the time derivative of the state.
 * \tparam Time The time representing the independent variable - the time.
 * \tparam Algebra The algebra type.
 * \tparam Operations The operations type.
 * \tparam Resizer The resizer policy type.
 */

    /**
     * \fn bulirsch_stoer::bulirsch_stoer( value_type eps_abs , value_type eps_rel , value_type factor_x , value_type factor_dxdt )
     * \brief Constructs the bulirsch_stoer class, including initialization of
     * the error bounds.
     *
     * \param eps_abs Absolute tolerance level.
     * \param eps_rel Relative tolerance level.
     * \param factor_x Factor for the weight of the state.
     * \param factor_dxdt Factor for the weight of the derivative.
     */

    /**
     * \fn bulirsch_stoer::try_step( System system , StateInOut &x , time_type &t , time_type &dt )
     * \brief Tries to perform one step.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed. Also, the internal order of the stepper is adjusted if required.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE.
     * It must fulfill the Simple System concept.
     * \param x The state of the ODE which should be solved. Overwritten if
     * the step is successful.
     * \param t The value of the time. Updated if the step is successful.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */

    /**
     * \fn bulirsch_stoer::try_step( System system , StateInOut &x , const DerivIn &dxdt , time_type &t , time_type &dt )
     * \brief Tries to perform one step.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed. Also, the internal order of the stepper is adjusted if required.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE.
     * It must fulfill the Simple System concept.
     * \param x The state of the ODE which should be solved. Overwritten if
     * the step is successful.
     * \param dxdt The derivative of state.
     * \param t The value of the time. Updated if the step is successful.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */

    /**
     * \fn bulirsch_stoer::try_step( System system , const StateIn &in , time_type &t , StateOut &out , time_type &dt )
     * \brief Tries to perform one step.
     *
     * \note This method is disabled if state_type=time_type to avoid ambiguity.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed. Also, the internal order of the stepper is adjusted if required.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE.
     * It must fulfill the Simple System concept.
     * \param in The state of the ODE which should be solved.
     * \param t The value of the time. Updated if the step is successful.
     * \param out Used to store the result of the step.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */

    /**
     * \fn bulirsch_stoer::try_step( System system , const StateIn &in , const DerivIn &dxdt , time_type &t , StateOut &out , time_type &dt )
     * \brief Tries to perform one step.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed. Also, the internal order of the stepper is adjusted if required.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE.
     * It must fulfill the Simple System concept.
     * \param in The state of the ODE which should be solved.
     * \param dxdt The derivative of state.
     * \param t The value of the time. Updated if the step is successful.
     * \param out Used to store the result of the step.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */

    /**
     * \fn bulirsch_stoer::adjust_size( const StateIn &x )
     * \brief Adjust the size of all temporaries in the stepper manually.
     * \param x A state from which the size of the temporaries to be resized is deduced.
     */

}
}
}

#endif // BOOST_NUMERIC_ODEINT_STEPPER_BULIRSCH_STOER_HPP_INCLUDED


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_FOUROPI_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_FOUROPI_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Constant \f$\frac4\pi\f$.

    @par Header <boost/simd/constant/fouropi.hpp>

    @par Semantic:

    For type T:

    @code
    T r = Fouropi<T>();
    @endcode

    is similar to:

    @code
    T r = Four<T>()/Pi<T>();
    @endcode

    @return a value of type T

**/
  template<typename T> T Fouropi();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Constant \f$\frac4\pi\f$.

      Generate the  constant fouropi.

      @return The Fouropi constant for the proper type
    **/
    Value Fouropi();
  }
} }
#endif

#include <boost/simd/constant/scalar/fouropi.hpp>
#include <boost/simd/constant/simd/fouropi.hpp>

#endif


#pragma once

#include <Eigen/Dense>

#include <utils/types.hpp>

template<int Dimension>
struct Ray
{
    Eigen::Matrix<double, Dimension, 1> origin;
    Eigen::Matrix<double, Dimension, 1> direction;

    /*
     * Ray constructor
    **/
    Ray(const Eigen::Matrix<double, Dimension, 1>& o = Eigen::Matrix<double, Dimension, 1>::Zero(),
        const Eigen::Matrix<double, Dimension, 1>& d = Eigen::Matrix<double, Dimension, 1>::Zero())
    : origin(o), direction(d)
    {}

    /*
     * Ray destructor
    **/
    ~Ray(){};
};

using Ray2D = Ray<2>;
using Ray3D = Ray<3>;

using Rays2D = AlignedVector<Ray2D>;
using Rays3D = AlignedVector<Ray3D>;

/*
 * the custom operator<< for Ray
**/
template<int Dimension>
std::ostream& operator<<(std::ostream& os, const Ray<Dimension>& r)
{
    os << "origin: " << r.origin << "\n";
    os << "direction: " << r.direction;

    return os;
}


//# /Users/arnold/Documents/prg/clang/bin/clang++ -U__STRICT_ANSI__ -std=c++14 -I/Users/arnold/Documents/prg/libraries/boost_1_56_0/ -I/Developer/SDKs/MacOSX10.6.sdk/usr/include -I/Users/arnold/Documents/prg/libraries/cryptopp563/include -Wall -O2 -L/Users/arnold/Documents/prg/libraries/boost_1_56_0/stage/lib -L/Users/arnold/Documents/prg/libraries/cryptopp563/lib -o apfel2 apfel2.cpp -lboost_system -lboost_filesystem -lboost_iostreams -lcryptopp

#include <iostream>
#include <boost/filesystem.hpp>
#include <vector>
#include <string>
#include <array>
#include <algorithm>
#include <complex>
#include <boost/format.hpp>
#include <future>
#include <utility>

using namespace std;

template<typename T>
struct window {
	window(const T& xmin, const T& ymin, const T& xmax, const T& ymax)
		: xmin_{xmin}, ymin_{ymin}, xmax_{xmax}, ymax_{ymax}
	{
		if (xmin_ > xmax_)
			swap(xmin_, xmax_);
		if (ymin_ > ymax_)
			swap(ymin_, ymax_);
	}
	window() = default;
	T width() const { return xmax_ - xmin_; }
	T height() const { return ymax_ - ymin_; }
	void zoom(const double& fx, const double& fy) {
		//double ffx = 1.0/fx;
		//double ffy = 1.0/fy;
		double dx = width() * fx/2.0;
		double dy = height() * fy/2.0;
		xmin_ += dx;
		xmax_ -= dx;
		ymin_ += dy;
		ymax_ -= dy;
	}
	void zoom(const double& f) {
		zoom(f, f);
/*
		double ff = 1.0/f;
		double dx = width() * ff/2.0;
		double dy = height() * ff/2.0;
		xmin_ += dx;
		xmax_ -= dx;
		ymin_ += dy;
		ymax_ -= dy;*/
	}
	void move(const double& h, const double& v) {
		double dx = width() * h;
		double dy = height() * v;
		xmin_ += dx;
		xmax_ += dx;
		ymin_ += dy;
		ymax_ += dy;
	}
	T xmin_ {-2};
	T ymin_ {-1.5};
	T xmax_ {1};
	T ymax_ {1.5};
};

struct fract {
	using it_t = unsigned;
	window<double>     imgrange_;
	window<unsigned>   dim_;

};

template<typename T>
ostream& operator<<(ostream& o, const window<T>& w) {
	o << boost::format{"(%1.15f, %1.15f)-(%1.15f, %1.15f)"} % w.xmin_ % w.ymin_ % w.xmax_ % w.ymax_;
	return o;
}

struct pxmap {
	pxmap(unsigned maxit, unsigned width, unsigned height)
		: maxit_{maxit+1}, width_{width}, height_{height}, px_(width_ * height_)
	{}
	unsigned maxit_;
	unsigned width_;
	unsigned height_;
	vector<unsigned> px_;
	static const array<char, 14> ch_; // = {{ ' ', '.', ',', ':', ';', '+', 'i', 'I', '%', 'H', '8', 'M', '#'}};
	void print() {
		unsigned pn = 0;
		for(const auto& p : px_) {
			cout << ch_[static_cast<double>(p)/maxit_ * pxmap::ch_.size()];
			if (++pn % width_ == 0) {
				cout << endl;
			}
		}
		cout << endl;
	}
};

constexpr const array<char, 14> pxmap::ch_ = {{ ' ', '.', ',', ':', ';', '+', 'i', 'I', '%', 'H', '8', 'M', '#', '*'}};

int main (int argc, char const *argv[])
{
	using cpt = complex<double>;
	double escval = 4.0;
	vector<string> params(argv, argv + argc);
//	cin >> noskipws;
	try {
//		window<unsigned> p(0, 100, 0, 20);
		cpt ca(1,0.5);
/*		cout << ca << endl;
		cout << ca * ca * ca << endl;
		cout << pow(ca,3) << endl;*/
		window<double> w;//(-1.5,0,0.5,1);
		unsigned maxit = 100;
		unsigned rows = 100;
		unsigned lines = rows * 1.0/2.0;
		double exp = 2.0;
		for(unsigned it = 0; true; ++it) {
			if (it > 0) {
				cout << w << " " << maxit << " esc:" << escval << " exp:" << exp << " zoom In, zoom Out, Left, Right, Up, Down, +iterate, -iterate, +Esc, -esc, +Wide, -wide, +Pow, -pow, Quit: ";
				string cc;
				getline(cin, cc);
				char c;
				c = cc.at(0);
				cout << endl;
				switch(c) {
					case 'q' :
						exit(0);
						break;
					case 'l':
						w.move(-0.25, 0);
						break;
					case 'r':
						w.move(0.25, 0);
						break;
					case 'u':
						w.move(0,0.25);
						break;
					case 'd':
						w.move(0,-0.25);
						break;
					case 'i':
						w.zoom(0.5);
						break;
					case 'o':
						w.zoom(-1);
						break;
					case '+':
						maxit *= 2.0;
						maxit = min(1u<<16,maxit);
						break;
					case '-':
						maxit /= 2.0;
						maxit = max(1u,maxit);
						break;
					case 'e':
						escval /= 2.0;
						escval = max(1.0, escval);
						break;
					case 'E':
						escval *= 2.0;
						escval = min(256.0, escval);
						break;
					case 'W':
						rows += rows * 0.25;
						w.zoom(-0.25, 0);
						break;
					case 'w':
						rows -= rows/4.0;
						w.zoom(0.25, 0);
						break;
					case 'P':
						exp += 0.1;
						exp = min(10.0, exp);
						break;
					case 'p':
						exp -= 0.1;
						exp = max(0.1, exp);
						break;
				}
			}
			double stepx = w.width() / static_cast<double>(rows);
			double stepy = w.height() / static_cast<double>(lines);
			cout << stepx << ", " << stepy << endl;
			//cout << endl;
			pxmap pm(maxit, rows, lines);
			auto pmit = pm.px_.begin();
			for(unsigned iy = 0; iy < lines; ++iy) {
				double y = w.ymax_ - stepy * iy;
			//for(double y = w.ymax_; y > w.ymin_; y -= stepy) {
				for(unsigned ix = 0; ix < rows; ++ix) {
					double x = w.xmin_ + stepx * ix;
				//for(double x = w.xmin_; x < w.xmax_; x += stepx) {
					unsigned i = 0;
					//bool isin = true;
					cpt c(0,0);
					cpt cc(x,y);
					for(; i < maxit; ++i) {
						//c = c * c + cc;
						c = pow(c, exp) + cc;
						if (c.real() * c.real() + c.imag() * c.imag() > escval) {
							//isin = false;
							break;
						}
					}
					*pmit = i;
					++pmit;
				}
				cout << endl;
			}
//			cout << "********************" << endl;
			pm.print();
		}
	} catch(...) {
		cerr << "Unknown exception" << endl;
	}
	return 0;
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      120203    B. Tong Minh      Copied RungeKutta4Stepsize unit test.
 *      120207    K. Kumar          Adapted to use modified benchmark functions in Tudat Core.
 *      120213    K. Kumar          Modified getCurrentInterval() to getIndependentVariable();
 *                                  transferred to Boost unit test framework.
 *      120321    K. Kumar          Updated (Burden and Faires, 2011) benchmark function call.
 *      120328    K. Kumar          Removed specific integrator tests; added compiler tests; added
 *                                  test of runtime error when minimm step size is exceeded.
 *      120331    B. Tong Minh      Added typeid check for RungeKuttaVariableStepSizeIntegratorXd
 *                                  typedef; modified minimum step size exceeded unit tests to use
 *                                  custom exception object.
 *      120321    D.Dirkx           Added unit test for getCurrentStateDerivatives function.
 *
 *    References
 *      Burden, R.L., Faires, J.D. Numerical Analysis, 7th Edition, Books/Cole, 2001.
 *
 *    Notes
 *      This file doesn't test any specific Runge-Kutta-type integrators, but rather some general
 *      functionality adopted in the the RungeKuttaVariableStepSizeIntegrator class, applicable to
 *      all Runge-Kutta-type integrators.
 *
 *      It should be noted that the getCurrentStateDerivatives() member function is not tested in a
 *      fully generic manner at the moment; the test is setup specifically based on the
 *      Runge-Kutta-Fehlberg 4(5) (RKF45) integrator. A more comprehensive test should be designed
 *      to ensure that the member function performs as desired regardless of the coefficient set
 *      chosen.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/exception/all.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaVariableStepSizeIntegrator.h"
#include "Tudat/Mathematics/NumericalIntegrators/rungeKutta4Integrator.h"
#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h"
#include "Tudat/Basics/testMacros.h"
#include "Tudat/Mathematics/NumericalIntegrators/UnitTests/numericalIntegratorTestFunctions.h"

#include <limits>
#include <string>
#include <typeinfo>

namespace tudat
{
namespace unit_tests
{

using numerical_integrators::RungeKuttaCoefficients;
using numerical_integrators::RungeKuttaVariableStepSizeIntegrator;
using numerical_integrators::RungeKuttaVariableStepSizeIntegratorXd;
using numerical_integrator_test_functions::computeZeroStateDerivative;

BOOST_AUTO_TEST_SUITE( test_runge_kutta_variable_step_size_integrator )

//! Test different types of states and state derivatives.
BOOST_AUTO_TEST_CASE( testCompilerErrors )
{
    // Case 1: test the VectorXd typdef. There is no need to explicitly test anything, since we're
    //         looking for compiler errors.
    {
        RungeKuttaVariableStepSizeIntegratorXd integrator(
                    RungeKuttaCoefficients( ),
                    &computeZeroStateDerivative,
                    0.0,
                    Eigen::Vector3d::Zero( ),
                    0.01,
                    std::numeric_limits< double >::infinity( ),
                    10.0 * std::numeric_limits< double >::epsilon( ),
                    10.0 * std::numeric_limits< double >::epsilon( ) );

        // Test integrateTo() function.
        integrator.integrateTo( 10.0, 0.1 );

        // Test performIntegrationStep() function.
        integrator.performIntegrationStep( 0.1 );

        // Test rollbackToPreviousState() function.
        integrator.rollbackToPreviousState( );

        // Check if the default types are correct.
        BOOST_CHECK_EQUAL(
                    std::string( typeid( integrator.getCurrentIndependentVariable( ) ).name( ) ),
                    std::string( typeid( double ).name( ) ) );
        BOOST_CHECK_EQUAL( std::string( typeid( integrator.getCurrentState( ) ).name( ) ),
                           std::string( typeid( Eigen::VectorXd ).name( ) ) );
    }

    // Case 2: test different types of states and state derivatives. There is no need to explicitly
    //         test anything, since we're looking for compiler errors.
    {
        RungeKuttaVariableStepSizeIntegrator < double, Eigen::Vector3d, Eigen::VectorXd >
                integrator( RungeKuttaCoefficients( ),
                            &computeZeroStateDerivative,
                            0.0,
                            Eigen::Vector3d::Zero( ),
                            0.01,
                            std::numeric_limits< double >::infinity( ),
                            10.0 * std::numeric_limits< double >::epsilon( ),
                            10.0 * std::numeric_limits< double >::epsilon( ) );

        // Test integrateTo() function.
        integrator.integrateTo( 10.0, 0.1 );

        // Test performIntegrationStep() function.
        integrator.performIntegrationStep( 0.1 );

        // Test rollbackToPreviousState() function.
        integrator.rollbackToPreviousState( );
    }

    // Case 3: test same types of states and state derivatives (MatrixXd). There is no need to
    //         explicitly test anything, since we're looking for compiler errors.
    {
        RungeKuttaVariableStepSizeIntegrator < double, Eigen::MatrixXd, Eigen::MatrixXd >
                integrator( RungeKuttaCoefficients( ),
                            &computeZeroStateDerivative,
                            0.0,
                            Eigen::Vector3d::Zero( ),
                            0.01,
                            std::numeric_limits< double >::infinity( ),
                            10.0 * std::numeric_limits< double >::epsilon( ),
                            10.0 * std::numeric_limits< double >::epsilon( ) );

        // Test integrateTo() function.
        integrator.integrateTo( 10.0, 0.1 );

        // Test performIntegrationStep() function.
        integrator.performIntegrationStep( 0.1 );

        // Test rollbackToPreviousState() function.
        integrator.rollbackToPreviousState( );
    }
}

//! Test that exceeding minimum step size throws a runtime error.
BOOST_AUTO_TEST_CASE( testMinimumStepSizeRuntimeError )
{
    RungeKuttaVariableStepSizeIntegratorXd integrator(
                RungeKuttaCoefficients( ),
                &computeZeroStateDerivative,
                0.0,
                Eigen::Vector3d::Zero( ),
                100.0,
                std::numeric_limits< double >::infinity( ),
                std::numeric_limits< double >::epsilon( ),
                std::numeric_limits< double >::epsilon( ) );

    // Case 1: test that minimum step size is exceeded when using integrateTo().
    {
        // Declare boolean flag to test if minimum step size is exceed for integrateTo().
        bool isMinimumStepSizeExceededForIntegrateTo = false;

        // Try integrateTo(), which should result in a runtime error.
        try
        {
            // Test integrateTo() function.
            integrator.integrateTo( 10.0, 0.1 );
        }

        // Catch the expected runtime error, and set the boolean flag to true.
        catch ( RungeKuttaVariableStepSizeIntegratorXd::MinimumStepSizeExceededError
                minimumStepSizeExceededError )
        {
            isMinimumStepSizeExceededForIntegrateTo = true;
            BOOST_CHECK_EQUAL( minimumStepSizeExceededError.minimumStepSize, 100.0 );
        }

        // Check that the minimum step size was indeed exceeded.
        BOOST_CHECK( isMinimumStepSizeExceededForIntegrateTo );
    }

    // Case 2: test that minimum step size is exceeded when using performIntegrationStep().
    {
        // Declare boolean flag to test if minimum step size is exceed for
        // performIntegrationStep().
        bool isMinimumStepSizeExceededForPerformIntegrationStep = false;

        // Try performIntegrationStep(), which should result in a runtime error.
        try
        {
            // Test performIntegrationStep() function.
            integrator.performIntegrationStep( 0.1 );
        }

        // Catch the expected runtime error, and set the boolean flag to true.
        catch ( RungeKuttaVariableStepSizeIntegratorXd::MinimumStepSizeExceededError
                minimumStepSizeExceededError )
        {
            isMinimumStepSizeExceededForPerformIntegrationStep = true;
            BOOST_CHECK_EQUAL( minimumStepSizeExceededError.minimumStepSize, 100.0 );
        }

        // Check that the minimum step size was indeed exceeded.
        BOOST_CHECK( isMinimumStepSizeExceededForPerformIntegrationStep );
    }
}

//! Test if the state derivative evaliations are properly returned.
BOOST_AUTO_TEST_CASE( testStateDerivativeRetrievalFunction )
{
    using namespace numerical_integrators;
    using namespace unit_tests::numerical_integrator_test_functions;

    // This test is based on the Runge-Kutta-Fehlberg 4(5) coefficient set, hence the test does not
    // robustly ensure that the getCurrentStateDerivatives() works correctly for any given
    // coefficient set currently. This test is more of an preliminary check that the function
    // performs as required, with the extrapolation that it is likely to perform consistently in
    // this manner for any Runge-Kutta-type coefficient set.

    // Create test integrator.
    RungeKuttaVariableStepSizeIntegratorXd integrator(
                RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg45 ),
                &computeVanDerPolStateDerivative,
                0.0,
                ( Eigen::VectorXd( 2 ) << 1.0, 2.0 ).finished( ),
                0.0, 10.0, 1.0E-8, 1.0E-8 );

    // Perform first integration step.
    integrator.performIntegrationStep( 1.0 );

    // Retrieve time and state after first integration step.
    const double previousTime = integrator.getCurrentIndependentVariable( );
    const Eigen::VectorXd previousState = integrator.getCurrentState( );

    // Perform additional integration step to verify that there are no problems with
    // getCurrentStateDerivatives after >1 iterations (i.e not being reset etc.).
    integrator.performIntegrationStep( integrator.getNextStepSize( ) );

    // Get current time and previous time step.
    const double currentTime = integrator.getCurrentIndependentVariable( );
    const double stepSize = currentTime - previousTime;

    // Retrieve state derivative values used in previous time step.
    std::vector< Eigen::VectorXd > stateDerivatives = integrator.getCurrentStateDerivatives( );

    // Check size of state derivative vector. This test is specifically set up to test the for the
    // number of stages in the RKF45 integrator.
    BOOST_CHECK_EQUAL( stateDerivatives.size( ), 6 );

    // Perform manual state derivative evaluations at previous time step using RKF45 method
    RungeKuttaCoefficients rkf45Coefficients = RungeKuttaCoefficients::get(
                RungeKuttaCoefficients::rungeKuttaFehlberg45 );
    std::vector< Eigen::VectorXd > directStateDerivativeValues;

    for ( int stage = 0; stage < rkf45Coefficients.cCoefficients.rows( ); stage++ )
    {
        // Compute the intermediate state to pass to the state derivative for this stage.
        Eigen::VectorXd intermediateState( previousState );

        // Compute the intermediate state.
        for ( int column = 0; column < stage; column++ )
        {
            intermediateState += stepSize * rkf45Coefficients.aCoefficients( stage, column )
                    * directStateDerivativeValues[ column ];
        }

        // Compute state derivative.
        directStateDerivativeValues.push_back(
                    computeVanDerPolStateDerivative(
                        previousTime +
                        rkf45Coefficients.cCoefficients( stage ) * stepSize,
                        intermediateState ) );

        // Check if manual result matched result from NumericalIntegrator.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( directStateDerivativeValues.at( stage ),
                                           stateDerivatives.at( stage ),
                                           1.0E-15 );
    }
}

//! Test if integtrateTo function works for variable step size where last step is modified.
BOOST_AUTO_TEST_CASE( testVariableStepIntegrateToFunction )
{
    using namespace numerical_integrators;
    using namespace unit_tests::numerical_integrator_test_functions;

    // In this test, the integrateTo function is used with variable step size integrator, where the
    // step size is modified by the last step size, to ensure the correct operation of the
    // integrator in this case.

    // Create variable step size integrator.
    RungeKuttaVariableStepSizeIntegratorXd integrator(
                RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg45 ),
                &computeSecondNonAutonomousModelStateDerivative,
                0.0,
                ( Eigen::VectorXd( 1 ) << 1.0 ).finished( ),
                0.0, 10.0, 1.0E-10, 1.0E-10 );

    // Use integrateTo function
    Eigen::VectorXd integratedValue = integrator.integrateTo( 0.5, 1.0 );
    double currentTime = integrator.getCurrentIndependentVariable( );

    // Check if current time is correct.
    BOOST_CHECK_CLOSE_FRACTION( currentTime, 0.5, std::numeric_limits< double >::epsilon( ) );

    // Create integrator to see if performing a single step with the given settings will indeed
    // result in the step size being adapted.
    RungeKuttaVariableStepSizeIntegratorXd verificationIntegrator(
                RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg45 ),
                &computeSecondNonAutonomousModelStateDerivative,
                0.0,
                ( Eigen::VectorXd( 1 ) << 1.0 ).finished( ),
                0.0, 10.0, 1.0E-10, 1.0E-10 );

    // Check if single step size of 0.5 will be adapted.
    verificationIntegrator.performIntegrationStep( 0.5 );
    BOOST_CHECK_EQUAL( ( verificationIntegrator.getCurrentIndependentVariable( ) < 0.5 *
                         ( 1.0 - 10.0 * std::numeric_limits< double >::epsilon( ) ) ), true );

    // Use a fixed step size integrator to check the original result of integrateTo
    RungeKutta4IntegratorXd fixedStepSizeIntegrator(
                &computeSecondNonAutonomousModelStateDerivative,0.0,
                ( Eigen::VectorXd( 1 ) << 1.0 ).finished( ) );
    Eigen::VectorXd fixedStepIntegratedValue = integrator.integrateTo( 0.5, 0.01 );
    BOOST_CHECK_CLOSE_FRACTION( fixedStepIntegratedValue.x( ), integratedValue.x( ), 1.0E-10 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/arithmetic/include/functions/muls.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/two.hpp>
#include <boost/simd/include/constants/mtwo.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/functions/make.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>

NT2_TEST_CASE_TPL ( muls_signed_int,  BOOST_SIMD_SIMD_INTEGRAL_SIGNED_TYPES)
{
  using boost::simd::muls;
  using boost::simd::tag::muls_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;

  // specific values tests
  NT2_TEST_EQUAL(muls(boost::simd::Mone<vT>(), boost::simd::Mone<vT>()), boost::simd::One<vT>());
  NT2_TEST_EQUAL(muls(boost::simd::One<vT>(), boost::simd::One<vT>()), boost::simd::One<vT>());
  NT2_TEST_EQUAL(muls(boost::simd::Valmax<vT>(), boost::simd::Valmax<vT>()), boost::simd::Valmax<vT>());
  NT2_TEST_EQUAL(muls(boost::simd::Valmax<vT>(),boost::simd::splat<vT>(2)), boost::simd::Valmax<vT>());
  NT2_TEST_EQUAL(muls(boost::simd::Valmax<vT>(),boost::simd::Mone<vT>()), boost::simd::Valmin<vT>()+boost::simd::One<vT>());
  NT2_TEST_EQUAL(muls(boost::simd::Valmax<vT>(),boost::simd::One<vT>()), boost::simd::Valmax<vT>());
  NT2_TEST_EQUAL(muls(boost::simd::Valmin<vT>(),boost::simd::Mone<vT>()), boost::simd::Valmax<vT>());
  NT2_TEST_EQUAL(muls(boost::simd::Valmin<vT>(),boost::simd::Valmin<vT>()), boost::simd::Valmax<vT>());
  NT2_TEST_EQUAL(muls(boost::simd::Zero<vT>(), boost::simd::Zero<vT>()), boost::simd::Zero<vT>());
} // end of test for signed_int_

NT2_TEST_CASE_TPL ( muls_unsigned_int,  BOOST_SIMD_SIMD_UNSIGNED_TYPES)
{
  using boost::simd::muls;
  using boost::simd::tag::muls_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename boost::dispatch::meta::call<muls_(vT,vT)>::type r_t;

  // specific values tests
  NT2_TEST_EQUAL(muls(boost::simd::One<vT>(), boost::simd::One<vT>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(muls(boost::simd::Valmax<vT>(),boost::simd::splat<vT>(2)), boost::simd::Valmax<r_t>());
  NT2_TEST_EQUAL(muls(boost::simd::Zero<vT>(), boost::simd::Zero<vT>()), boost::simd::Zero<r_t>());
} // end of test for unsigned_int_

NT2_TEST_CASE(muls_special)
{
  using boost::simd::muls;
  using boost::simd::splat;
  using boost::simd::make;
  using boost::simd::Valmin;
  using boost::simd::Valmax;

#ifndef BOOST_SIMD_HAS_MIC_SUPPORT
  typedef boost::simd::native<short int, BOOST_SIMD_DEFAULT_EXTENSION> vT1;
  NT2_TEST_EQUAL(muls(splat<vT1>(-5165), splat<vT1>(23258)), Valmin<vT1>());
#endif

  typedef boost::simd::native<int, BOOST_SIMD_DEFAULT_EXTENSION> vT2;
  NT2_TEST_EQUAL(muls(splat<vT2>(-1306766858), splat<vT2>(1550772331)), Valmin<vT2>());
  NT2_TEST_EQUAL(muls(splat<vT2>(1467238299), splat<vT2>(-900961598)), Valmin<vT2>());

#ifdef BOOST_SIMD_HAS_AVX_SUPPORT
  typedef int T2;
  vT2 a = make<vT2>(853350212, 191584584, 1467238299, -1306766858, 991230901, 146415451, 154742226, 1320298211);
  vT2 b = make<vT2>(1557885369, 1394765115, -900961598, 1550772331, 1563251902, -50470159, -76281765, 405234440);

  vT2 c = make<vT2>(Valmax<T2>(), Valmax<T2>(), Valmin<T2>(), Valmin<T2>(), Valmax<T2>(), Valmin<T2>(), Valmin<T2>(), Valmax<T2>());
  NT2_TEST_EQUAL(muls(a, b), c);
#endif
}


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// 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)
//=======================================================================
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/ref.hpp>
#include <vector>

#include <boost/graph/planar_face_traversal.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>

using namespace boost;

// Some planar face traversal visitors that will
// print the vertices and edges on the faces

struct output_visitor : public planar_face_traversal_visitor
{
    void begin_face() { std::cout << "New face: "; }
    void end_face() { std::cout << std::endl; }
};

struct vertex_output_visitor : public output_visitor
{
    template < typename Vertex > void next_vertex(Vertex v)
    {
        std::cout << v << " ";
    }
};

struct edge_output_visitor : public output_visitor
{
    template < typename Edge > void next_edge(Edge e) { std::cout << e << " "; }
};

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

    typedef adjacency_list< vecS, vecS, undirectedS,
        property< vertex_index_t, int >, property< edge_index_t, int > >
        graph;

    // Create a graph - this is a biconnected, 3 x 3 grid.
    // It should have four small (four vertex/four edge) faces and
    // one large face that contains all but the interior vertex
    graph g(9);

    add_edge(0, 1, g);
    add_edge(1, 2, g);

    add_edge(3, 4, g);
    add_edge(4, 5, g);

    add_edge(6, 7, g);
    add_edge(7, 8, g);

    add_edge(0, 3, g);
    add_edge(3, 6, g);

    add_edge(1, 4, g);
    add_edge(4, 7, g);

    add_edge(2, 5, g);
    add_edge(5, 8, g);

    // Initialize the interior edge index
    property_map< graph, edge_index_t >::type e_index = get(edge_index, g);
    graph_traits< graph >::edges_size_type edge_count = 0;
    graph_traits< graph >::edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
        put(e_index, *ei, edge_count++);

    // Test for planarity - we know it is planar, we just want to
    // compute the planar embedding as a side-effect
    typedef std::vector< graph_traits< graph >::edge_descriptor > vec_t;
    std::vector< vec_t > embedding(num_vertices(g));
    if (boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
            boyer_myrvold_params::embedding = &embedding[0]))
        std::cout << "Input graph is planar" << std::endl;
    else
        std::cout << "Input graph is not planar" << std::endl;

    std::cout << std::endl << "Vertices on the faces: " << std::endl;
    vertex_output_visitor v_vis;
    planar_face_traversal(g, &embedding[0], v_vis);

    std::cout << std::endl << "Edges on the faces: " << std::endl;
    edge_output_visitor e_vis;
    planar_face_traversal(g, &embedding[0], e_vis);

    return 0;
}


//!
//! Contains the implementation of an adaptive proposal function
//!
//! \file infer/adaptive.hpp
//! \author Darren Shen
//! \date 2014
//! \license Affero General Public License version 3 or later
//! \copyright (c) 2014, NICTA
//!

#pragma once

#include <random>
#include <functional>
#include <Eigen/Core>

namespace stateline
{
  namespace mcmc
  {

    //! A function to bounce the MCMC proposal off the hard boundaries.
    //! This allows the proposal to always move around without getting stuck at
    //! 'walls'
    //!
    //! \param val The proposed value
    //! \param min The minimum bound of theta
    //! \param max The maximum bound of theta
    //!  \returns The new bounced theta definitely in the bounds
    //!
    Eigen::VectorXd bouncyBounds(const Eigen::VectorXd& val,const Eigen::VectorXd& min, const Eigen::VectorXd& max)
    {
      Eigen::VectorXd delta = max - min;
      Eigen::VectorXd result = val;
      Eigen::Matrix<bool, Eigen::Dynamic, 1> tooBig = (val.array() > max.array());
      Eigen::Matrix<bool, Eigen::Dynamic, 1> tooSmall = (val.array() < min.array());
      for (uint i=0; i< result.size(); i++)
      {
        bool big = tooBig(i);
        bool small = tooSmall(i);
        if (big)
        {
          double overstep = val(i)-max(i);
          int nSteps = (int)(overstep /  delta(i));
          double stillToGo = overstep - nSteps*delta(i);
          if (nSteps % 2 == 0)
            result(i) = max(i) - stillToGo;
          else
            result(i) = min(i) + stillToGo;
        }
        if (small)
        {
          double understep = min(i) - val(i);
          int nSteps = (int)(understep / delta(i));
          double stillToGo = understep - nSteps*delta(i);
          if (nSteps % 2 == 0)
            result(i) = min(i) + stillToGo;
          else
            result(i) = max(i) - stillToGo;
        }
      }
      return result;
    }

    //! An adaptive Gaussian proposal function. It randomly varies each value in
    //! the state according to a Gaussian distribution whose variance changes
    //! depending on the acceptance ratio of a chain. It also bounces of the
    //! walls of the hard boundaries given so as not to get stuck in corners.
    //!
    //! \param state The current state of the chain
    //! \param sigma The standard deviation of the distribution (step size of the proposal)
    //! \param min The minimum bound of theta
    //! \param max The maximum bound of theta
    //! \returns The new proposed theta
    //!
    Eigen::VectorXd adaptiveGaussianProposal(const Eigen::VectorXd &state, double sigma,
        const Eigen::VectorXd& min, const Eigen::VectorXd& max)
    {
      // Random number generators
      static std::random_device rd;
      static std::mt19937 generator(rd());
      static std::normal_distribution<> rand; // Standard normal

      // Vary each paramater according to a Gaussian distribution
      Eigen::VectorXd proposal(state.rows());
      for (int i = 0; i < proposal.rows(); i++)
        proposal(i) = state(i) + rand(generator) * sigma;

      return bouncyBounds(proposal, min, max);
    };

  }
}


#include <iostream>
#include <igl/readPLY.h>
#include <igl/grad.h>
#include <igl/opengl/glfw/Viewer.h>
#include <igl/heat_geodesics.h>
#include <igl/triangle_triangle_adjacency.h>
#include <Eigen/Dense>
#include <cstdlib>
#include <random>
#include <unordered_map>
#include <chrono>

std::random_device rd;
std::mt19937 e2(rd());
std::uniform_real_distribution<> rand01(0, 1);

using Vec3 = Eigen::Vector3d;
Eigen::MatrixXd V;
Eigen::MatrixXi F;
Eigen::MatrixXi TT;
Eigen::SparseMatrix<double> G;
igl::HeatGeodesicsData<double> heat_data;
std::unordered_map<int, std::set<int>> neighbours;

void ComputeNeighbours() {
  for(int f = 0; f< F.rows(); f++) {
    const auto f0 = F(f, 0);
    const auto f1 = F(f, 1);
    const auto f2 = F(f, 2);

    neighbours[f0].insert(f1);
    neighbours[f0].insert(f2);
    neighbours[f1].insert(f0);
    neighbours[f1].insert(f2);
    neighbours[f2].insert(f0);
    neighbours[f2].insert(f1);
  }
}

bool is_in_ring(int f_a, int f_b, int ring) {
  std::vector<int> q;
  std::set<int> visited;

  q.push_back(f_b);
  for(int i=0; i<ring; i++) {
    if(q.size() == 0) {
      break;
    }

    std::vector<int> new_q;
    for(auto &f : q) {
      if(visited.find(f) != visited.end()) {
        continue;
      }

      visited.insert(f);
      int n0 = TT(f, 0);
      int n1 = TT(f, 1);
      int n2 = TT(f, 2);
      if(n0 == f_a || n1 == f_a || n2 == f_a) {
        return true;
      }
      new_q.push_back(n0);
      new_q.push_back(n1);
      new_q.push_back(n2);
    }
    q = std::move(new_q);
  }
  return false;
}

void sample_point(Vec3& out, Vec3& bary, int& f) {
  f = rand() % F.rows();
  const auto v0 = F(f, 0);
  const auto v1 = F(f, 1);
  const auto v2 = F(f, 2);

  const double b0 = rand01(e2);
  const double b1 = rand01(e2) * (1.0 - b0);
  const double b2 = 1.0 - b1 - b0;

  out = b0*V.row(v0) + b1*V.row(v1) + b2*V.row(v2);
  bary = {b0, b1, b2};
}

// doesn't consider TWO POINTS ON SAME TRIANGLE
void sample_point_close_to(int f_orig, Vec3& out, Vec3& bary, int& f) {
  const int hops = rand() % 5;
  f = f_orig;
  for(int i=0; i<hops || f==f_orig; i++) {
    int new_f = -1;
    while(new_f == -1) {
      new_f = TT(f, rand()%3);
    }
    f = new_f;
  }

  const auto v0 = F(f, 0);
  const auto v1 = F(f, 1);
  const auto v2 = F(f, 2);

  const double b0 = rand01(e2);
  const double b1 = rand01(e2) * (1.0 - b0);
  const double b2 = 1.0 - b1 - b0;

  out = b0*V.row(v0) + b1*V.row(v1) + b2*V.row(v2);
  bary = {b0, b1, b2};
}

const Eigen::VectorXd& all_geodesics_from_point(int v1) {
  static std::unordered_map<int, Eigen::VectorXd> cache_v;

  const auto entry = cache_v.find(v1);
  if(entry != cache_v.end()) {
    return entry->second;
  }

  Eigen::VectorXi gamma;
  Eigen::VectorXd D;
  gamma.resize(1); gamma << v1;
  igl::heat_geodesics_solve(heat_data, gamma, D);
  cache_v[v1] = std::move(D);

  return cache_v[v1];
}

const Eigen::VectorXd all_euclidean_from_point(const Vec3& src) {
  Eigen::VectorXd D;

  D = (V.array().rowwise() - src.transpose().array()).matrix().rowwise().norm();

  return D;
}

double geodesic_distance(int v1, int v2) {
  static std::unordered_map<int, Eigen::VectorXd> cache_v;
  if(v1 > v2) {
    std::swap(v1, v2);
  }

  const auto entry = cache_v.find(v1);
  if(entry != cache_v.end()) {
    return entry->second(v2);
  }

  Eigen::VectorXi gamma;
  Eigen::VectorXd D;
  gamma.resize(1); gamma << v1;
  igl::heat_geodesics_solve(heat_data, gamma, D);

  const double d = D(v2);
  cache_v[v1] = std::move(D);

  return d;
}

double geodesic_distance_from_face(int f, int v) {
  static std::unordered_map<int, Eigen::VectorXd> cache_f;
  const auto entry = cache_f.find(f);
  if(entry != cache_f.end()) {
    return entry->second(v);
  }

  Eigen::VectorXi gamma;
  Eigen::VectorXd D;
  gamma.resize(1); gamma << F(f, 0), F(f, 1), F(f, 2);
  igl::heat_geodesics_solve(heat_data, gamma, D);

  const double d = D(v);
  cache_f[f] = std::move(D);

  return d;
}

// consider 9 possible paths based on triangles and chose the shortest
double geodesic_distance_bo9(
        const Vec3& pt_a, const Vec3& bary_a, int f_a,
        const Vec3& pt_b, const Vec3& bary_b, int f_b,
        Vec3* grad_out=nullptr
        ) {
  if(f_a == f_b) {
    if(grad_out != nullptr) {
      *grad_out = pt_a - pt_b;
    }
    return (pt_a - pt_b).norm();
  }

  int best_i, best_j;
  double best_dist = 1e9;
  for(int i=0; i<3; i++) {
    // geodesic(==euclidean) distance between point a and face i
    const double d_ai = (pt_a - V.row(F(f_a, i)).transpose()).norm();

    for(int j=0; j<3; j++) {
      // geodesic distance between the two vertices on the mesh
      const double g_ij = geodesic_distance(F(f_a,i), F(f_b,j));

      // geodesic(==euclidean) distance between point b and face j
      const double d_bi = (pt_b - V.row(F(f_b, j)).transpose()).norm();

      // total geodesic distance
      const double g = d_ai + g_ij + d_bi;

      // std::cout << "(" << i << "," << j <<") " << d_ai << " + " << g_ij << " + " << d_bi << " == " << g << "\n";

      if(g < best_dist) {
        best_dist = g;
        best_i = i;
        best_j = j;
      }
    }
  }

  if(grad_out != nullptr) {
    // const auto& D = all_geodesics_from_point(F(f_b, best_j));
    const auto D = all_euclidean_from_point(pt_b);
    const Eigen::MatrixXd GD_all = Eigen::Map<const Eigen::MatrixXd>((G*D).eval().data(), F.rows(), 3);
    const Eigen::MatrixXd GD = GD_all.row(f_a);

    (*grad_out)[0] = GD(0, 0);
    (*grad_out)[1] = GD(0, 1);
    (*grad_out)[2] = GD(0, 2);
  }

  return best_dist;
}

double geodesic_distance_barydatar(
        const Vec3& pt_a, const Vec3& bary_a, int f_a,
        const Vec3& pt_b, const Vec3& bary_b, int f_b,
        Vec3 *grad_out
) {
  // ref: https://www.ncbi.nlm.nih.gov/pmc/articles/PMC3346950/
  if(f_a == f_b) {
    if(grad_out != nullptr) {
      *grad_out = pt_a - pt_b;
    }
    return (pt_a - pt_b).norm();
  }

  double res = 0.0;

  for(int i=0; i<3; i++) {
    const double g_i0 = geodesic_distance(F(f_a,i), F(f_b,0));
    const double g_i1 = geodesic_distance(F(f_a,i), F(f_b,1));
    const double g_i2 = geodesic_distance(F(f_a,i), F(f_b,2));
    const double g_iy = bary_b[0]*g_i0 + bary_b[1]*g_i1 + bary_b[2]*g_i2;
    res += bary_a[i] * g_iy;
  }

  if(grad_out != nullptr) {
    if(is_in_ring(f_a, f_b, 1)) {
      *grad_out = pt_a - pt_b;
    } else {
      *grad_out = Vec3(0.0, 0.0, 0.0);
      const auto D = all_euclidean_from_point(pt_b);
      const Eigen::MatrixXd GD_all = Eigen::Map<const Eigen::MatrixXd>((G*D).eval().data(), F.rows(), 3);

      Eigen::MatrixXd GD_all_pervertex;
      igl::per_vertex_normals(V, F, GD_all, GD_all_pervertex);

      for(int i=0; i<3; i++) {
        const Vec3 g = GD_all_pervertex.row(F(f_a, i));
        *grad_out += bary_a[i] * g;
      }
    }
  }

  return res;
}

double geodesic_distance_testmeshfimgrad(
        const Vec3& pt_a, const Vec3& bary_a, int f_a,
        const Vec3& pt_b, const Vec3& bary_b, int f_b,
        Vec3 *grad_out
) {
  if(grad_out != nullptr) {
    *grad_out = {0.0, 0.0, 0.0};

    const auto D = all_euclidean_from_point(pt_b);
    const auto grad_v = [&](int v) {
      Vec3 df = {0.0, 0.0, 0.0};
      for(int o : neighbours[v]) {
        const Vec3 pt_diff = V.row(o) - V.row(v);
        const double value_diff = D[o] - D[v];

        df[0] += value_diff / (pt_diff[0] + 0.0001);
        df[1] += value_diff / (pt_diff[1] + 0.0001);
        df[2] += value_diff / (pt_diff[2] + 0.0001);
      }
      df /= (double) neighbours[v].size();
      return df;
    };

    for(int i=0; i<3; i++) {
      *grad_out += bary_a[i] * grad_v(F(f_a, i));
    }

  }

  return (pt_a - pt_b).norm();
}

// barycentric, but my own interpretation?
double geodesic_distance_barykk(
  const Vec3& pt_a, const Vec3& bary_a, int f_a,
  const Vec3& pt_b, const Vec3& bary_b, int f_b
  ) {
  if(f_a == f_b) {
    return (pt_a - pt_b).norm();
  }

  const Vec3 distances = {
          geodesic_distance_from_face(f_b, F(f_a, 0)),
          geodesic_distance_from_face(f_b, F(f_a, 1)),
          geodesic_distance_from_face(f_b, F(f_a, 2)),
  };
  return distances.dot(bary_a);
}

int main() {
  srand(time(nullptr));

  const auto mesh_filepath = "/scratch/karthik/projects/ShapeWorks/Examples/Python/Output/plane/ply/plane_highres.ply";
  igl::readPLY(mesh_filepath, V, F);

  std::cerr << "Mesh:\n";
  std::cerr << "V: " << V.rows() << "x" << V.cols() << "\n";
  std::cerr << "F: " << F.rows() << "x" << F.cols() << "\n";
  std::cerr << "Bounds: [" << V.minCoeff() << "] -> [" << V.maxCoeff() << "]\n\n";

  igl::grad(V, F, G);
  igl::triangle_triangle_adjacency(F, TT);
  ComputeNeighbours();

  igl::heat_geodesics_precompute(V, F, heat_data);

  Vec3 pt_a, bary_a; int f_a;
  Vec3 pt_b, bary_b; int f_b;

  // compare geodesic distance
  for(int i=0; i<10000; i++) {
    sample_point(pt_a, bary_a, f_a);

    if(i < 5000) {
      sample_point_close_to(f_a, pt_b, bary_b, f_b);
    } else {
      sample_point(pt_b, bary_b, f_b);
    }

    Vec3 geo_grad;
    // const double geo = geodesic_distance_barykk(pt_a, bary_a, f_a, pt_b, bary_b, f_b);
    // const double geo = geodesic_distance_testmeshfimgrad(pt_a, bary_a, f_a, pt_b, bary_b, f_b, &geo_grad);
    // const double geo = geodesic_distance_bo9(pt_a, bary_a, f_a, pt_b, bary_b, f_b, &geo_grad);
    const double geo = geodesic_distance_barydatar(pt_a, bary_a, f_a, pt_b, bary_b, f_b, &geo_grad);

    /*
    using namespace std::chrono;
    const auto start = high_resolution_clock::now();
    const auto end = high_resolution_clock::now();
    const auto elapsed = duration_cast<nanoseconds>(end - start).count();
    std::cout << "time: " << elapsed <<"\n";
    continue;
    */

    const double euc = (pt_a - pt_b).norm();
    const Vec3 euc_grad = pt_a - pt_b;

    const double geo_angle = std::atan2(geo_grad.y(), geo_grad.x());
    const double euc_angle = std::atan2(euc_grad.y(), euc_grad.x());
    const double geo_mag = geo_grad.norm();
    const double euc_mag = euc_grad.norm();

    std::cout << "distance: " << geo <<       " | " << euc <<       " (diff: " << geo - euc <<             ") ";
    std::cout << "| angle:  " << geo_angle << " | " << euc_angle << " (diff: " << geo_angle - euc_angle << ") ";
    std::cout << "|   mag:  " << geo_mag   << " | " << euc_mag   << " (diff: " << geo_mag   - euc_mag   << ") ";
    std::cout << "\n";
  }

  return 0;

  Eigen::MatrixXd T1(2, 3);
  T1.row(0) = pt_a;
  T1.row(1) = pt_b;

  Eigen::MatrixXd T2(1, 3); T2 << 1.0, 0.0, 0.0;
  igl::opengl::glfw::Viewer viewer;
  viewer.data().set_mesh(V, F);
  viewer.data().set_points(T1, T2);
  // viewer.data().show_lines = false;
  // viewer.data().line_width = 2.0;
  viewer.launch();

  return 0;
}


#pragma once
#include <vizkit3d_debug_drawings/commands/DrawCommand.hpp>
#include <string>
#include <Eigen/Core>
#include <vizkit3d_debug_drawings/commands/BoostSerializationHelpers.hpp>

namespace osg
{
    class Node;
}

namespace vizkit3dDebugDrawings
{
class DrawAABBCommand : public DrawCommand
{
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive & ar, const unsigned int version)
    {
        // serialize base class information
        ar & boost::serialization::base_object<DrawCommand>(*this);
        ar & min;
        ar & max;
        ar & colorRGBA;
    }

public:
    DrawAABBCommand();

    DrawAABBCommand(const std::string& drawingChannel, const Eigen::Vector3d& min,
                    const Eigen::Vector3d& max, const Eigen::Vector4d& colorRGBA);

    virtual osg::ref_ptr<osgviz::Object> createPrimitive() const;

    virtual DrawAABBCommand* clone() const;

private:
    Eigen::Vector3d min;
    Eigen::Vector3d max;
    Eigen::Vector4d colorRGBA;
};
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <list>
#include <algorithm>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/topological_sort.hpp>
#include <iterator>
#include <utility>

typedef std::pair<std::size_t,std::size_t> Pair;

/*
  Topological sort example

  The topological sort algorithm creates a linear ordering
  of the vertices such that if edge (u,v) appears in the graph,
  then u comes before v in the ordering.

  Sample output:

  A topological ordering: 2 5 0 1 4 3

*/

int
main(int , char* [])
{
  //begin
  using namespace boost;

  /* Topological sort will need to color the graph.  Here we use an
     internal decorator, so we "property" the color to the graph.
     */
  typedef adjacency_list<vecS, vecS, directedS,
    property<vertex_color_t, default_color_type> > Graph;

  typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
  Pair edges[6] = { Pair(0,1), Pair(2,4),
                    Pair(2,5),
                    Pair(0,3), Pair(1,4),
                    Pair(4,3) };
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  // VC++ can't handle the iterator constructor
  Graph G(6);
  for (std::size_t j = 0; j < 6; ++j)
    add_edge(edges[j].first, edges[j].second, G);
#else
  Graph G(edges, edges + 6, 6);
#endif

  boost::property_map<Graph, vertex_index_t>::type id = get(vertex_index, G);

  typedef std::vector< Vertex > container;
  container c;
  topological_sort(G, std::back_inserter(c));

  std::cout << "A topological ordering: ";
  for (container::reverse_iterator ii = c.rbegin();
       ii != c.rend(); ++ii)
    std::cout << id[*ii] << " ";
  std::cout << std::endl;

  return 0;
}


#pragma once

#include <Eigen/Dense>

namespace ipc::rigid {

///
/// Generate a canonical triangle/quad subdivided from a regular grid
///
/// @param[in]  n  			 { n grid quads }
/// @param[in]  tri			 { is a tri or a quad }
/// @param[out] V            { #V x 2 output vertices positions }
/// @param[out] F            { #F x 3 output triangle indices }
///
void regular_2d_grid(
    const int n, const bool tri, Eigen::MatrixXd& V, Eigen::MatrixXi& F);

void regular_2d_grid(
    const int num_cols,
    const int num_rows,
    Eigen::MatrixXd& V,
    Eigen::MatrixXi& F);

} // namespace ipc::rigid


// Copyright (c) 2005 - 2015 Marc de Kamps
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
//    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation
//      and/or other materials provided with the distribution.
//    * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software
//      without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
// USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <TwoDLib.hpp>
#include <vector>
#include <iostream>

using namespace std;
using namespace TwoDLib;

BOOST_AUTO_TEST_CASE(CellCreationTest)
{
	vector<double> vec_v(4);
	vector<double> vec_w(4);

	vec_v[0] = 0.;
	vec_v[1] = 1.;
	vec_v[2] = 1.;
	vec_v[3] = 0.;

	vec_w[0] = 0.;
	vec_w[1] = 0.;
	vec_w[2] = 1.;
	vec_w[3] = 1.;

	Quadrilateral quad(vec_v, vec_w);
	BOOST_REQUIRE( quad.SignedArea()  ==  1);
	BOOST_REQUIRE( quad.IsClockwise() == -1);

	vec_v[0] = 0.;
	vec_v[1] = 0.;
	vec_v[2] = 1.;
	vec_v[3] = 1.;

	vec_w[0] = 0.;
	vec_w[1] = 1.;
	vec_w[2] = 1.;
	vec_w[3] = 0.;

	Quadrilateral quad2(vec_v, vec_w);

	BOOST_REQUIRE( quad2.SignedArea() == -1);
	BOOST_REQUIRE( quad2.IsClockwise() ==  1);

}

BOOST_AUTO_TEST_CASE(InsideClockWise){
	vector<double>  vec_v(4);
	vector<double>  vec_w(4);

	vec_v[0] = 0.;
	vec_v[1] = 0.;
	vec_v[2] = 1.;
	vec_v[3] = 1.;

	vec_w[0] = 0.;
	vec_w[1] = 1.;
	vec_w[2] = 1.;
	vec_w[3] = 0.;

	Quadrilateral quad(vec_v, vec_w);

	Point p(0.5,0.5);

	BOOST_REQUIRE( quad.IsInside(p) == true );

	Point p1(-0.5,0.5);
	BOOST_REQUIRE(quad.IsInside(p1) == false);
	Point p2(0.5,1.5);
	BOOST_REQUIRE(quad.IsInside(p2) == false);
	Point p3(1.5,0.5);
	BOOST_REQUIRE(quad.IsInside(p3) == false);
	Point p4(0.5,-0.5);
	BOOST_REQUIRE(quad.IsInside(p4) == false);

}

BOOST_AUTO_TEST_CASE(InsideAntiClockWise){
	vector<double>  vec_v(4);
	vector<double>  vec_w(4);

	vec_v[0] = 0.;
	vec_v[1] = 1.;
	vec_v[2] = 1.;
	vec_v[3] = 0.;

	vec_w[0] = 0.;
	vec_w[1] = 0.;
	vec_w[2] = 1.;
	vec_w[3] = 1.;

	Quadrilateral quad(vec_v, vec_w);

	Point p(0.5,0.5);

	BOOST_REQUIRE( quad.IsInside(p) == true );

	Point p1(-0.5,0.5);
	BOOST_REQUIRE(quad.IsInside(p1) == false);
	Point p2(0.5,1.5);
	BOOST_REQUIRE(quad.IsInside(p2) == false);
	Point p3(1.5,0.5);
	BOOST_REQUIRE(quad.IsInside(p3) == false);
	Point p4(0.5,-0.5);
	BOOST_REQUIRE(quad.IsInside(p4) == false);
}

BOOST_AUTO_TEST_CASE(InsideConcave){
	vector<double>  vec_v(4);
	vector<double>  vec_w(4);

	vec_v[0] = -1.;
	vec_v[1] =  0.;
	vec_v[2] =  1.;
	vec_v[3] =  0.;

	vec_w[0] = -0.5;
	vec_w[1] =  0.;
	vec_w[2] = -0.5;
	vec_w[3] =  1.;

	Quadrilateral quad(vec_v, vec_w);

	Point p(0.0,0.5);
	BOOST_REQUIRE( quad.IsInside(p) == true );

	Point p1(-1.,0.);
	BOOST_REQUIRE(quad.IsInside(p1) == false);
	Point p2( 1.,0.);
	BOOST_REQUIRE(quad.IsInside(p2) == false);
	Point p3(0.,-0.1);
	BOOST_REQUIRE(quad.IsInside(p3) == false);
	Point p4(0., 1.1);
	BOOST_REQUIRE(quad.IsInside(p4) == false);

}

BOOST_AUTO_TEST_CASE(Split){
	vector<double> v(Quadrilateral::_nr_points);
	v[0] = 0;
	v[1] = 1;
	v[2] = 0;
	v[3] = -1;

	vector<double> w(Quadrilateral::_nr_points);
	w[0] = 0;
	w[1] = -0.5;
	w[2] =  1.0;
	w[3] = -0.5;

	Quadrilateral quad(v,w);

	pair<Triangle,Triangle> ts = quad.Split();

	Triangle t1 = ts.first;

	Point p1 = t1.Points()[0];
	Point p2 = t1.Points()[1];
	Point p3 = t1.Points()[2];

	BOOST_REQUIRE( p1[0] == 0 && p1[1] == 0);
	BOOST_REQUIRE( p2[0] == 0 && p2[1] == 1);
	BOOST_REQUIRE( p3[0] == 1 && p3[1] == -0.5);

	Triangle t2 = ts.second;

	p1 = t2.Points()[0];
	p2 = t2.Points()[1];
	p3 = t2.Points()[2];

	BOOST_REQUIRE( p1[0] == 0 && p1[1] == 0);
	BOOST_REQUIRE( p2[0] == 0 && p2[1] == 1);
	BOOST_REQUIRE( p3[0] == -1 && p3[1] == -0.5);

// Now rotate the points. Same triangles should be produced

	v[1] = 0;
	v[2] = 1;
	v[3] = 0;
	v[0] = -1;

	w[1] = 0;
	w[2] = -0.5;
	w[3] =  1.0;
	w[0] = -0.5;

	Quadrilateral quad2(v,w);
	pair<Triangle,Triangle> ts2 = quad2.Split();

	Triangle t3 = ts2.first;

	p1 = t3.Points()[0];
	p2 = t3.Points()[1];
	p3 = t3.Points()[2];

	BOOST_REQUIRE( p1[0] == 0 && p1[1] == 0);
	BOOST_REQUIRE( p2[0] == 0 && p2[1] == 1);
	BOOST_REQUIRE( p3[0] == -1 && p3[1] == -0.5);

	Triangle t4 = ts2.second;

	p1 = t4.Points()[0];
	p2 = t4.Points()[1];
	p3 = t4.Points()[2];

	BOOST_REQUIRE( p1[0] == 0 && p1[1] == 0);
	BOOST_REQUIRE( p2[0] == 0 && p2[1] == 1);
	BOOST_REQUIRE( p3[0] == 1 && p3[1] == -0.5);

}

BOOST_AUTO_TEST_CASE(CentroidTest){
	vector<double>  vec_v(4);
	vector<double>  vec_w(4);

	vec_v[0] = 0.;
	vec_v[1] = 0.;
	vec_v[2] = 1.;
	vec_v[3] = 1.;

	vec_w[0] = 0.;
	vec_w[1] = 1.;
	vec_w[2] = 1.;
	vec_w[3] = 0.;

	Quadrilateral quad(vec_v, vec_w);

	Point centre = quad.Centroid();
	BOOST_REQUIRE(centre[0] == 0.5);
	BOOST_REQUIRE(centre[1] == 0.5);
}