#include <NTL/mat_ZZ_pE.h>

#include <NTL/new.h>

NTL_START_IMPL

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

   if (B.NumRows() != n || B.NumCols() != m)
      Error("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_pE& X, const mat_ZZ_pE& A, const mat_ZZ_pE& B)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (B.NumRows() != n || B.NumCols() != m)
      Error("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));
}

void negate(mat_ZZ_pE& X, const mat_ZZ_pE& 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));
}

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

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

   X.SetDims(n, m);

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

   for (i = 1; i <= n; i++) {
      for (j = 1; j <= m; j++) {
         clear(acc);
         for(k = 1; k <= l; k++) {
            mul(tmp, rep(A(i,k)), rep(B(k,j)));
            add(acc, acc, tmp);
         }
         conv(X(i,j), acc);
      }
   }
}

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

static
void mul_aux(vec_ZZ_pE& x, const mat_ZZ_pE& A, const vec_ZZ_pE& b)
{
   long n = A.NumRows();
   long l = A.NumCols();

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

   x.SetLength(n);

   long i, k;
   ZZ_pX acc, tmp;

   for (i = 1; i <= n; i++) {
      clear(acc);
      for (k = 1; k <= l; k++) {
         mul(tmp, rep(A(i,k)), rep(b(k)));
         add(acc, acc, tmp);
      }
      conv(x(i), acc);
   }
}

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

static
void mul_aux(vec_ZZ_pE& x, const vec_ZZ_pE& a, const mat_ZZ_pE& B)
{
   long n = B.NumRows();
   long l = B.NumCols();

   if (n != a.length())
      Error("matrix mul: dimension mismatch");

   x.SetLength(l);

   long i, k;
   ZZ_pX acc, tmp;

   for (i = 1; i <= l; i++) {
      clear(acc);
      for (k = 1; k <= n; k++) {
         mul(tmp, rep(a(k)), rep(B(k,i)));
         add(acc, acc, tmp);
      }
      conv(x(i), acc);
   }
}

void mul(vec_ZZ_pE& x, const vec_ZZ_pE& a, const mat_ZZ_pE& B)
{
   if (&a == &x) {
      vec_ZZ_pE tmp;
      mul_aux(tmp, a, B);
      x = tmp;
   }
   else
      mul_aux(x, a, B);

}

void ident(mat_ZZ_pE& 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_pE& d, const mat_ZZ_pE& M_in)
{
   long k, n;
   long i, j;
   long pos;
   ZZ_pX t1, t2;
   ZZ_pX *x, *y;

   const ZZ_pXModulus& p = ZZ_pE::modulus();

   n = M_in.NumRows();

   if (M_in.NumCols() != n)
      Error("determinant: nonsquare matrix");

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

   vec_ZZ_pX *M = NTL_NEW_OP vec_ZZ_pX[n];

   for (i = 0; i < n; i++) {
      M[i].SetLength(n);
      for (j = 0; j < n; j++) {
         M[i][j].rep.SetMaxLength(2*deg(p)-1);
         M[i][j] = rep(M_in[i][j]);
      }
   }

   ZZ_pX det;
   set(det);

   for (k = 0; k < n; k++) {
      pos = -1;
      for (i = k; i < n; i++) {
         rem(t1, M[i][k], p);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1))
            pos = i;
      }

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

         MulMod(det, det, M[k][k], p);

         // make M[k, k] == -1 mod p, and make row k reduced

         InvMod(t1, M[k][k], p);
         negate(t1, t1);
         for (j = k+1; j < n; j++) {
            rem(t2, M[k][j], p);
            MulMod(M[k][j], t2, t1, p);
         }

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

            t1 = M[i][k];   // this is already reduced

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

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

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }
      }
      else {
         clear(d);
         goto done;
      }
   }

   conv(d, det);

done:
   delete[] M;
}

long IsIdent(const mat_ZZ_pE& 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_pE& X, const mat_ZZ_pE& 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_pE 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_pE& d, vec_ZZ_pE& X,
           const mat_ZZ_pE& A, const vec_ZZ_pE& b)

{
   long n = A.NumRows();
   if (A.NumCols() != n)
      Error("solve: nonsquare matrix");

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

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

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

   const ZZ_pXModulus& p = ZZ_pE::modulus();

   vec_ZZ_pX *M = NTL_NEW_OP vec_ZZ_pX[n];

   for (i = 0; i < n; i++) {
      M[i].SetLength(n+1);
      for (j = 0; j < n; j++) {
         M[i][j].rep.SetMaxLength(2*deg(p)-1);
         M[i][j] = rep(A[j][i]);
      }
      M[i][n].rep.SetMaxLength(2*deg(p)-1);
      M[i][n] = rep(b[i]);
   }

   ZZ_pX det;
   set(det);

   for (k = 0; k < n; k++) {
      pos = -1;
      for (i = k; i < n; i++) {
         rem(t1, M[i][k], p);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1)) {
            pos = i;
         }
      }

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

         MulMod(det, det, M[k][k], p);

         // make M[k, k] == -1 mod p, and make row k reduced

         InvMod(t1, M[k][k], p);
         negate(t1, t1);
         for (j = k+1; j <= n; j++) {
            rem(t2, M[k][j], p);
            MulMod(M[k][j], t2, t1, p);
         }

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

            t1 = M[i][k];   // this is already reduced

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

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

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }
      }
      else {
         clear(d);
         goto done;
      }
   }

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

   conv(d, det);

done:
   delete[] M;
}

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

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

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

   const ZZ_pXModulus& p = ZZ_pE::modulus();

   vec_ZZ_pX *M = NTL_NEW_OP vec_ZZ_pX[n];

   for (i = 0; i < n; i++) {
      M[i].SetLength(2*n);
      for (j = 0; j < n; j++) {
         M[i][j].rep.SetMaxLength(2*deg(p)-1);
         M[i][j] = rep(A[i][j]);
         M[i][n+j].rep.SetMaxLength(2*deg(p)-1);
         clear(M[i][n+j]);
      }
      set(M[i][n+i]);
   }

   ZZ_pX det;
   set(det);

   for (k = 0; k < n; k++) {
      pos = -1;
      for (i = k; i < n; i++) {
         rem(t1, M[i][k], p);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1)) {
            pos = i;
         }
      }

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

         MulMod(det, det, M[k][k], p);

         // make M[k, k] == -1 mod p, and make row k reduced

         InvMod(t1, M[k][k], p);
         negate(t1, t1);
         for (j = k+1; j < 2*n; j++) {
            rem(t2, M[k][j], p);
            MulMod(M[k][j], t2, t1, p);
         }

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

            t1 = M[i][k];   // this is already reduced

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

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

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }
      }
      else {
         clear(d);
         goto done;
      }
   }

   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, rep(X[j][k]), M[i][j]);
            add(t1, t1, t2);
         }
         sub(t1, t1, M[i][n+k]);
         conv(X[i][k], t1);
      }
   }

   conv(d, det);

done:
   delete[] M;
}

long gauss(mat_ZZ_pE& M_in, long w)
{
   long k, l;
   long i, j;
   long pos;
   ZZ_pX t1, t2, t3;
   ZZ_pX *x, *y;

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

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

   const ZZ_pXModulus& p = ZZ_pE::modulus();

   vec_ZZ_pX *M = NTL_NEW_OP vec_ZZ_pX[n];

   for (i = 0; i < n; i++) {
      M[i].SetLength(m);
      for (j = 0; j < m; j++) {
         M[i][j].rep.SetMaxLength(2*deg(p)-1);
         M[i][j] = rep(M_in[i][j]);
      }
   }

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

      pos = -1;
      for (i = l; i < n; i++) {
         rem(t1, M[i][k], p);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1)) {
            pos = i;
         }
      }

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

         InvMod(t3, M[l][k], p);
         negate(t3, t3);

         for (j = k+1; j < m; j++) {
            rem(M[l][j], M[l][j], p);
         }

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

            MulMod(t1, M[i][k], t3, p);

            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

               mul(t2, *y, t1);
               add(t2, t2, *x);
               *x = t2;
            }
         }

         l++;
      }
   }

   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         conv(M_in[i][j], M[i][j]);

   delete [] M;

   return l;
}

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

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

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

   mat_ZZ_pE M;
   long r;

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

   X.SetDims(m-r, m);

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

   ZZ_pE T3;

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

   vec_ZZ_pE 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_pE& 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, rep(v[s]), rep(M[i][s]));
               add(t1, t1, t2);
            }

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

void mul(mat_ZZ_pE& X, const mat_ZZ_pE& A, const ZZ_pE& b_in)
{
   ZZ_pE b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

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

void mul(mat_ZZ_pE& X, const mat_ZZ_pE& A, const ZZ_p& b_in)
{
   NTL_ZZ_pRegister(b);
   b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

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

void mul(mat_ZZ_pE& X, const mat_ZZ_pE& A, long b_in)
{
   NTL_ZZ_pRegister(b);
   b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

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

void diag(mat_ZZ_pE& X, long n, const ZZ_pE& d_in)
{
   ZZ_pE d = d_in;
   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_pE& A, long n, const ZZ_pE& 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;
}

long IsZero(const mat_ZZ_pE& 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_pE& x)
{
   long n = x.NumRows();
   long i;
   for (i = 0; i < n; i++)
      clear(x[i]);
}

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

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

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

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

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

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

void inv(mat_ZZ_pE& X, const mat_ZZ_pE& A)
{
   ZZ_pE d;
   inv(d, X, A);
   if (d == 0) Error("inv: non-invertible matrix");
}

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

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

   mat_ZZ_pE 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


#include <boost/test/unit_test.hpp>

#include "../include/calcutron.h"

using namespace calcutron;
using namespace std;

BOOST_AUTO_TEST_CASE(ReturnNumberTest)
{
	BOOST_CHECK(calculate("1") == 1);
	BOOST_CHECK(calculate("   1") == 1);
	BOOST_CHECK(calculate("1   ") == 1);}

BOOST_AUTO_TEST_CASE(SumTest)
{
	BOOST_CHECK(calculate("1+1") == 2);
}

BOOST_AUTO_TEST_CASE(LeadingMinusTest)
{
	BOOST_CHECK(calculate("-1+1") == 0);
}

BOOST_AUTO_TEST_CASE(SumWithSpacesTest)
{
	BOOST_CHECK(calculate("1 + 1") == 2);
}

BOOST_AUTO_TEST_CASE(SumFloatTest)
{
	BOOST_CHECK(calculate("1.5 + 0,5") == 2);
}

BOOST_AUTO_TEST_CASE(MinusTest)
{
	BOOST_CHECK(calculate("3-1") == 2);
}

BOOST_AUTO_TEST_CASE(MinusWithSpacesTest)
{
	BOOST_CHECK(calculate("3 - 1") == 2);
}

BOOST_AUTO_TEST_CASE(MultTest)
{
	BOOST_CHECK(calculate("3*2") == 6);
}

BOOST_AUTO_TEST_CASE(DivTest)
{
	BOOST_CHECK(calculate("4/2") == 2);
}

BOOST_AUTO_TEST_CASE(DivFloatTest)
{
	BOOST_CHECK(calculate("3 / 1.5") == 2);
}

BOOST_AUTO_TEST_CASE(ParenthesesSimpleTest)
{
	BOOST_CHECK(calculate("(1+1)") == 2);
	BOOST_CHECK(calculate("[1+1]") == 2);
}

BOOST_AUTO_TEST_CASE(ParenthesesComplexTest)
{
	BOOST_CHECK(calculate("-1*(4+2*(1-2))+3") == 1);
	BOOST_CHECK(calculate("-1*(1+1)+3") == 1);
}

BOOST_AUTO_TEST_CASE(MultipleOperatorsTest)
{
	BOOST_CHECK(calculate("- - 1") == 1);
	BOOST_CHECK(calculate("1 - - 1") == 2);
	BOOST_CHECK(calculate("1 * - 1") == -1);	// да, такое теперь разрешено
	BOOST_CHECK(calculate("1 / - 1 + 2") == 1);	// да, такое теперь разрешено
}

// примеры выражений из тех задания
BOOST_AUTO_TEST_CASE(FinalTest)
{
	BOOST_CHECK(calculate("-1 + 5 - 3") == 1);
	BOOST_CHECK(calculate("-10 + (8 * 2.5) - (3 / 1,5)") == 8);
	BOOST_CHECK(calculate("1 + (2 * (2.5 + 2.5 + (3 - 2))) - (3 / 1.5)") == 11);
}


#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <Eigen/Core>
#include <cmath>
#include <iostream>

using namespace Eigen;

double deg2rad(const double degree) { return degree * M_PI / 180.0; }

double rad2deg(const double radian) { return radian * 180.0 / M_PI; }

double angle_limit_pi(double angle)
{
  while (angle >= M_PI) { angle -= 2 * M_PI; }
  while (angle <= -M_PI) { angle += 2 * M_PI; }
  return angle;
}

double sdlab_uniform()
{
  double ret = ((double)rand() + 1.0) / ((double)RAND_MAX + 2.0);
  return ret;
}

// gauss noise
double gauss(double mu, double sigma)
{
  double z = std::sqrt(-2.0 * std::log(sdlab_uniform())) * std::sin(2.0 * M_PI * sdlab_uniform());
  return mu + sigma * z;
}

class FakeSensorPublisher
{
public:
  FakeSensorPublisher()
  : grund_truth(Matrix<double, 3, 1>::Zero()),
    odom(Matrix<double, 3, 1>::Zero()),
    gps(Matrix<double, 3, 1>::Zero())
  {
    Q << 0.1, 0, 0, deg2rad(30);
    Q = Q * Q;
    R << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, deg2rad(5);
    R = R * R;

    ground_truth_pub_ = pnh_.advertise<nav_msgs::Odometry>("grund_truth", 10);
    odom_pub_ = pnh_.advertise<nav_msgs::Odometry>("odom", 10);
    gps_pub_ = pnh_.advertise<nav_msgs::Odometry>("gps", 10);

    previous_stamp_ = ros::Time::now();

    timer_ = nh_.createTimer(ros::Duration(0.01), &FakeSensorPublisher::timerCallback, this);
  }
  ~FakeSensorPublisher() {}
  nav_msgs::Odometry inputToNavMsgs(Matrix<double, 3, 1> pose_2d, Matrix<double, 2, 1> input)
  {
    nav_msgs::Odometry msg;
    msg.header.frame_id = "world";
    msg.child_frame_id = "odom";
    msg.pose.pose.position.x = pose_2d[0];
    msg.pose.pose.position.y = pose_2d[1];

    tf2::Quaternion quat;
    quat.setRPY(0.0, 0.0, angle_limit_pi(pose_2d[2]));
    msg.pose.pose.orientation.w = quat.w();
    msg.pose.pose.orientation.x = quat.x();
    msg.pose.pose.orientation.y = quat.y();
    msg.pose.pose.orientation.z = quat.z();
    msg.twist.twist.linear.x = input[0];
    msg.twist.twist.angular.z = input[1];

    return msg;
  }
  void timerCallback(const ros::TimerEvent & e)
  {
    ros::Time current_stamp = ros::Time::now();

    nav_msgs::Odometry grund_truth_msg;
    nav_msgs::Odometry odom_msg;
    nav_msgs::Odometry gps_msgs;

    Matrix<double, 2, 1> u(1.0, deg2rad(5));

    const double dt = current_stamp.toSec() - previous_stamp_.toSec();

    // ground truth
    ROS_INFO("delta time: %f", dt);
    grund_truth = motionModel(grund_truth, u, dt);
    grund_truth_msg = inputToNavMsgs(grund_truth, u);

    // dead recogning
    Matrix<double, 2, 1> ud = motionNoise(u, Q);
    odom = motionModel(odom, ud, dt);
    odom_msg = inputToNavMsgs(odom, ud);

    // observation
    Matrix<double, 3, 1> gps = observationNoise(grund_truth, R);
    gps_msgs = inputToNavMsgs(gps, Matrix<double, 2, 1>::Zero());

    ground_truth_pub_.publish(grund_truth_msg);
    odom_pub_.publish(odom_msg);
    gps_pub_.publish(gps_msgs);

    previous_stamp_ = current_stamp;
  }
  Matrix<double, 3, 1> motionModel(Matrix<double, 3, 1> x, Matrix<double, 2, 1> u, double dt)
  {
    Matrix<double, 3, 3> F = Matrix<double, 3, 3>::Identity();
    Matrix<double, 3, 2> B;
    B << dt * std::cos(x[2]), 0, dt * std::sin(x[2]), 0, 0, dt;

    x = F * x + B * u;
    x[2] = angle_limit_pi(x[2]);
    return x;
  }
  Matrix<double, 2, 1> motionNoise(Matrix<double, 2, 1> u, Matrix<double, 2, 2> Q)
  {
    Matrix<double, 2, 1> uw(gauss(0.0, Q(0, 0)), gauss(0.0, Q(1, 1)));
    return u + uw;
  }
  Matrix<double, 3, 1> observationNoise(Matrix<double, 3, 1> x, Matrix<double, 3, 3> R)
  {
    Matrix<double, 3, 1> xw(gauss(0.0, R(0, 0)), gauss(0.0, R(1, 1)), gauss(0.0, R(2, 2)));
    return x + xw;
  }

private:
  ros::NodeHandle nh_{};
  ros::NodeHandle pnh_{"~"};
  ros::Timer timer_;
  ros::Time previous_stamp_;

  ros::Publisher ground_truth_pub_;
  ros::Publisher odom_pub_;
  ros::Publisher gps_pub_;

  Matrix<double, 2, 2> Q;
  Matrix<double, 3, 3> R;
  Matrix<double, 3, 1> grund_truth;
  Matrix<double, 3, 1> odom;
  Matrix<double, 3, 1> gps;
};

int main(int argc, char ** argv)
{
  ros::init(argc, argv, "fake_sensor_publisher_node");
  FakeSensorPublisher fake_sensor_publisher;
  ros::spin();
  return 0;
}


#include "sampler.h"
#include "params.h"
#include <NTL/ZZ_pX.h>
#include <NTL/mat_ZZ_p.h>
#include <cassert>

void Sampler::get_ternary_vector(vector<int>& vec)
{

    for(int i=0; i<vec.size(); i++)
        vec[i] = ternary_sampler(rand_engine);
}

void Sampler::get_ternary_matrix(vector<vector<int>>& mat)
{

    for(int i=0; i<mat.size(); i++)
    {
        vector<int>& row = mat[i];
        get_ternary_vector(row);
    }
}

void Sampler::get_binary_vector(vector<int>& vec)
{
    for(int i=0; i<vec.size(); i++)
        vec[i] = binary_sampler(rand_engine);
}

void Sampler::get_uniform_vector(vector<int>& vec)
{
    for(int i=0; i<vec.size(); i++)
        vec[i] = mod_q_base_sampler(rand_engine);
}

void Sampler::get_uniform_matrix(vector<vector<int>>& mat)
{
    for(int i=0; i<mat.size(); i++)
    {
        vector<int>& row = mat[i];
        get_uniform_vector(row);
    }
}

void Sampler::get_gaussian_vector(vector<int>& vec, double st_dev)
{
    normal_distribution<double> gaussian_sampler(0.0, st_dev);
    for(size_t i=0; i<vec.size(); i++)
        vec[i] = static_cast<int>(round(gaussian_sampler(rand_engine)));
}

void Sampler::get_gaussian_matrix(vector<vector<int>>& mat, double st_dev)
{
    for(size_t i=0; i<mat.size(); i++)
    {
        vector<int>& row = mat[i];
        get_gaussian_vector(row, st_dev);
    }
}

void Sampler::get_invertible_vector(vector<int>& vec, vector<int>& vec_inv, int scale, int shift)
{
    //polynomial with the coefficient vector vec (will be generated later)
    ZZ_pX poly;
    //element of Z_(q_boot)
    ZZ_p coef;
    coef.init(ZZ(q_boot));
    //the inverse of poly modulo poly_mod (will be generated later)
    ZZ_pX inv_poly;
    //random sampling
    while (true)
    {
        //create the polynomial with the coefficient vector of the desired form
        SetCoeff(poly, 0, ternary_sampler(rand_engine)*scale + shift);
        for (size_t i = 1; i < vec.size(); i++)
        {
            coef = ternary_sampler(rand_engine)*scale;
            SetCoeff(poly, i, coef);
        }
        //test invertibility
        try
        {
            InvMod(inv_poly, poly, Param::get_def_poly());
            break;
        }
        catch(...)
        {
            cout << "Polynomial " << poly << " isn't a unit" << endl;
            continue;
        }
    }
    //cout << "Poly: " << poly << endl;
    //cout << "Poly inverse: " << inv_poly << endl;
    //extract the coefficient vector of poly
    int tmp_coef;
    for (int i = 0; i <= deg(poly); i++)
    {
        tmp_coef = conv<long>(poly[i]);
        if (tmp_coef > half_q_boot)
            tmp_coef -= q_boot;
        vec[i] = tmp_coef;
    }

    for (int i = 0; i <= deg(inv_poly); i++)
    {
        tmp_coef = conv<long>(inv_poly[i]);
        if (tmp_coef > half_q_boot)
            tmp_coef -= q_boot;
        vec_inv[i] = tmp_coef;
    }

    //cout << "Vector:" << vec << endl;
    //cout << "Inverse vector:" << vec_inv << endl;
}

void Sampler::get_invertible_matrix(vector<vector<int>>& mat, vector<vector<int>>& mat_inv, int scale, int shift)
{
    //check that the input matrices are squares
    assert(mat[0].size() == mat.size());
    assert(mat_inv[0].size() == mat_inv.size());
    //check that both input matrices have the same dimension
    assert(mat.size() == mat_inv.size());

    //number of rows of the input matrix
    int dim = mat.size();

    //element of Z_(q_boot)
    ZZ_p coef;
    coef.init(ZZ(param.q_base));

    //candidate matrix
    mat_ZZ_p tmp_mat(INIT_SIZE, dim, dim);

    //candidate inverse matrix
    mat_ZZ_p tmp_mat_inv(INIT_SIZE, dim, dim);

    //sampling and testing
    while (true)
    {
        //sampling
        for (int i = 0; i < dim; i++)
        {
            Vec<ZZ_p>& row = tmp_mat[i];
            for (int j = 0; j < dim; j++)
            {
                coef = ternary_sampler(rand_engine)*scale;
                if (i==j)
                    coef += ZZ_p(shift);
                row[j] = coef;
            }
        }
        //test invertibility
        try
        {
            inv(tmp_mat_inv, tmp_mat);
            break;
        }
        catch(...)
        {
            cout << "Matrix " << tmp_mat << " is singular" << endl;
            continue;
        }
    }
    //lift mod q representation to integers
    int tmp_coef;
    for (int i = 0; i < dim; i++)
    {
        Vec<ZZ_p>& tmp_row = tmp_mat[i];
        Vec<ZZ_p>& tmp_row_inv = tmp_mat_inv[i];
        vector<int>& row = mat[i];
        vector<int>& row_inv = mat_inv[i];
        for (int j = 0; j < dim; j++)
        {
            tmp_coef = conv<long>(tmp_row[j]);
            if (tmp_coef > param.half_q_base)
                tmp_coef -= param.q_base;
            row[j] = tmp_coef;

            tmp_coef = conv<long>(tmp_row_inv[j]);
            if (tmp_coef > param.half_q_base)
                tmp_coef -= param.q_base;
            row_inv[j] = tmp_coef;
        }
    }
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/make_shared.hpp>

#include <boost/math/distributions/lognormal.hpp>

#include "tudat/math/statistics/multiVariateGaussianProbabilityDistributions.h"

namespace tudat
{
namespace unit_tests
{

// Test function for Gaussian cupola distribution (computation of pdf).
// NOTE: This manner of testing the Gaussian cupola is not ideal, but no usable test data has been identified.
double gaussianCopulaProbabilityDensity(
        const Eigen::VectorXd& independentVariables , int dimension_ , Eigen::MatrixXd correlationMatrix_ )
{
    Eigen::MatrixXd inverseCorrelationMatrix_ = correlationMatrix_.inverse() ;
    double determinant_ = correlationMatrix_.determinant( );

    double probabilityDensity = 0.0 ;
    Eigen::VectorXd y( dimension_ ) ;
    boost::math::normal distribution( 0.0 , 1.0 );

    for( int i = 0 ; i < dimension_; i++ )
    {
        y( i ) = boost::math::quantile( distribution , independentVariables( i ) ); // Inverse cdf
    }

    // Calculate probability density
    Eigen::MatrixXd location = -0.5 *
            ( y.transpose( )  * ( inverseCorrelationMatrix_ - Eigen::MatrixXd::Identity( dimension_ , dimension_ ) ) * y ) ;

    probabilityDensity = ( ( 1.0 / ( std::sqrt( determinant_ ) ) ) * std::exp( location( 0, 0 ) ) ) ;
    return probabilityDensity;
}

// Exact solution of 2D Gaussian distribution
// Montgomery, D. C. & Runger, G. C. Applied statistics and Probability for engineers Wiley, 2014
double computeBiGaussianPdf(
        const double standardDeviationX, const double standardDeviationY, const double correlation,
        const double meanX, const double meanY, const Eigen::VectorXd independentVariables )
{
    using tudat::mathematical_constants::PI;
    using std::pow;
    using std::exp;

    return ( 1.0 / ( 2.0 * PI*standardDeviationX*standardDeviationY*sqrt( 1.0 - pow( correlation, 2.0 ) ) ) ) *
            exp( ( -1.0 / ( 2.0 * ( 1.0 - pow( correlation, 2.0) ) ) ) * (
                     ( pow( independentVariables( 0 ) - meanX, 2.0 ) ) /( pow( standardDeviationX, 2.0 ) )
                     - ( 2.0 * correlation * ( independentVariables( 0 ) - meanX ) *
                         ( independentVariables( 1 )- meanY ) ) / ( standardDeviationX * standardDeviationY )
                     + ( pow( independentVariables( 1 ) - meanY, 2.0 ) ) / ( pow( standardDeviationY, 2.0 ) ) ) );
}

BOOST_AUTO_TEST_SUITE( test_probability_distributions )

//! Test if Multi-dimensional Gaussian distribution class works correctly for 2 dimensions, using analytical solution for
//! bigaussian distribution.
BOOST_AUTO_TEST_CASE( testMultiDimensionalGaussianDistribution )
{
    using namespace tudat::statistics;

    // Defined mean and covariance.
    Eigen::VectorXd mean( 2 );
    mean << 0.0 , 1.0;

    Eigen::MatrixXd covariance( 2, 2 );
    covariance << 3.0, -1.0, -1.0, 3.0;

    // Create distribution
    GaussianDistributionXd distribution( mean, covariance );

    // Defined new mean and covariance.
    Eigen::VectorXd mean2( 2 );
    mean2 << -1.0 , 2.0;

    Eigen::MatrixXd covariance2( 2, 2 );
    covariance2 << 4.0, 1.5, 1.5, 4.0;

    // Create second distribution
    GaussianDistributionXd distribution2( mean2,covariance2 );

    // Define test independent variables.
    Eigen::VectorXd independentVariables( 2 );

    // Test distributions for range of independent variables.
    for( unsigned int i = 0; i < 81; i++ )
    {
        for( unsigned int j = 0; j < 81; j++ )
        {
            independentVariables( 0 ) = -4.0 + 0.1 * static_cast< double >( i );
            independentVariables( 1 ) = -4.0 + 0.1 * static_cast< double >( j );

            {
                // Compute exact solution of pdf
                double standardDeviationX = std::sqrt( covariance( 0, 0 ) );
                double standardDeviationY = std::sqrt( covariance( 1, 1 ) );
                double meanX = mean( 0 );
                double meanY = mean( 1 );
                double correlation = covariance( 0 , 1 ) / ( standardDeviationX * standardDeviationY );

                double exactSolution = computeBiGaussianPdf(
                            standardDeviationX, standardDeviationY, correlation, meanX, meanY, independentVariables );

                // Test pdf value
                BOOST_CHECK_SMALL( std::fabs( distribution.evaluatePdf( independentVariables) - exactSolution ),
                                   std::numeric_limits< double >::epsilon( ) );
            }

            {
                // Compute exact solution of pdf
                double standardDeviationX = std::sqrt( covariance2( 0, 0 ) );
                double standardDeviationY = std::sqrt( covariance2( 1, 1 ) );
                double meanX = mean2( 0 );
                double meanY = mean2( 1 );
                double correlation = covariance2( 0 , 1 ) / ( standardDeviationX * standardDeviationY );

                double exactSolution = computeBiGaussianPdf(
                            standardDeviationX, standardDeviationY, correlation, meanX, meanY, independentVariables );

                // Test pdf value
                BOOST_CHECK_SMALL( std::fabs( distribution2.evaluatePdf( independentVariables ) - exactSolution ) ,
                                   std::numeric_limits<double>::epsilon() ) ;
            }
        }
    }
}

//! Test if Gaussian Copula distribution class works correctly.
BOOST_AUTO_TEST_CASE( testGaussianCopula )
{
    using namespace tudat::statistics;
    using tudat::mathematical_constants::PI;

    // Define properties of distribution.
    int dimension = 2 ;
    Eigen::MatrixXd correlationMatrix( dimension , dimension ) ;
    correlationMatrix << 1.0, 0.3, 0.3, 1.0 ;

    // Create distribution
    GaussianCopulaDistributionXd distribution( correlationMatrix );

    Eigen::VectorXd independentVariables( 2 );
    independentVariables << 0.5 , 0.3 ;

    for( unsigned int i = 1; i < 40; i++ )
    {
        for( unsigned int j = 1; j < 40; j++ )
        {
            independentVariables( 0 ) = 0.025 * static_cast< double >( i );
            independentVariables( 1 ) = 0.025 * static_cast< double >( j );

            // Test Gaussian cupola at current independentVariables/
            // NOTE: This manner of testing the Gaussian cupola (similar computation in source and test file) is not ideal,
            // but no usable test data has been identified.
            BOOST_CHECK_SMALL( std::fabs( gaussianCopulaProbabilityDensity(
                                              independentVariables , dimension , correlationMatrix )
                                          - distribution.evaluatePdf( independentVariables ) ), 1E-15 );

        }
    }

    // Test out of bounds for distribution (pdf equals 0).
    independentVariables << 1.1 , 0.5 ;
    BOOST_CHECK_SMALL( std::fabs( distribution.evaluatePdf( independentVariables ) - 0.0 ) , 1E-15 );

    independentVariables << 0.1 , -0.5 ;
    BOOST_CHECK_SMALL( std::fabs( distribution.evaluatePdf( independentVariables ) - 0.0 ) , 1E-15 );

    independentVariables << -0.1 , -0.5 ;
    BOOST_CHECK_SMALL( std::fabs( distribution.evaluatePdf( independentVariables ) - 0.0 ) , 1E-15 );

    independentVariables << 0.1 , 1.5 ;
    BOOST_CHECK_SMALL( std::fabs( distribution.evaluatePdf( independentVariables ) - 0.0 ) , 1E-15 );

}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/* Boost interval/arith2.hpp template implementation file
 *
 * This header provides some auxiliary arithmetic
 * functions: fmod, sqrt, square, pov, inverse and
 * a multi-interval division.
 *
 * Copyright 2002-2003 Hervé Brönnimann, Guillaume Melquiond, Sylvain Pion
 *
 * 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_INTERVAL_ARITH2_HPP
#define BOOST_NUMERIC_INTERVAL_ARITH2_HPP

#include <boost/config.hpp>
#include <boost/numeric/interval/detail/interval_prototype.hpp>
#include <boost/numeric/interval/detail/test_input.hpp>
#include <boost/numeric/interval/detail/bugs.hpp>
#include <boost/numeric/interval/detail/division.hpp>
#include <boost/numeric/interval/arith.hpp>
#include <boost/numeric/interval/policies.hpp>
#include <algorithm>
#include <cassert>
#include <boost/config/no_tr1/cmath.hpp>

namespace boost {
namespace numeric {

template<class T, class Policies> inline
interval<T, Policies> fmod(const interval<T, Policies>& x,
                           const interval<T, Policies>& y)
{
  if (interval_lib::detail::test_input(x, y))
    return interval<T, Policies>::empty();
  typename Policies::rounding rnd;
  typedef typename interval_lib::unprotect<interval<T, Policies> >::type I;
  T const &yb = interval_lib::user::is_neg(x.lower()) ? y.lower() : y.upper();
  T n = rnd.int_down(rnd.div_down(x.lower(), yb));
  return (const I&)x - n * (const I&)y;
}

template<class T, class Policies> inline
interval<T, Policies> fmod(const interval<T, Policies>& x, const T& y)
{
  if (interval_lib::detail::test_input(x, y))
    return interval<T, Policies>::empty();
  typename Policies::rounding rnd;
  typedef typename interval_lib::unprotect<interval<T, Policies> >::type I;
  T n = rnd.int_down(rnd.div_down(x.lower(), y));
  return (const I&)x - n * I(y);
}

template<class T, class Policies> inline
interval<T, Policies> fmod(const T& x, const interval<T, Policies>& y)
{
  if (interval_lib::detail::test_input(x, y))
    return interval<T, Policies>::empty();
  typename Policies::rounding rnd;
  typedef typename interval_lib::unprotect<interval<T, Policies> >::type I;
  T const &yb = interval_lib::user::is_neg(x) ? y.lower() : y.upper();
  T n = rnd.int_down(rnd.div_down(x, yb));
  return x - n * (const I&)y;
}

namespace interval_lib {

template<class T, class Policies> inline
interval<T, Policies> division_part1(const interval<T, Policies>& x,
                                     const interval<T, Policies>& y, bool& b)
{
  typedef interval<T, Policies> I;
  b = false;
  if (detail::test_input(x, y))
    return I::empty();
  if (zero_in(y))
    if (!user::is_zero(y.lower()))
      if (!user::is_zero(y.upper()))
        return detail::div_zero_part1(x, y, b);
      else
        return detail::div_negative(x, y.lower());
    else
      if (!user::is_zero(y.upper()))
        return detail::div_positive(x, y.upper());
      else
        return I::empty();
  else
    return detail::div_non_zero(x, y);
}

template<class T, class Policies> inline
interval<T, Policies> division_part2(const interval<T, Policies>& x,
                                     const interval<T, Policies>& y, bool b = true)
{
  if (!b) return interval<T, Policies>::empty();
  return detail::div_zero_part2(x, y);
}

template<class T, class Policies> inline
interval<T, Policies> multiplicative_inverse(const interval<T, Policies>& x)
{
  typedef interval<T, Policies> I;
  if (detail::test_input(x))
    return I::empty();
  T one = static_cast<T>(1);
  typename Policies::rounding rnd;
  if (zero_in(x)) {
    typedef typename Policies::checking checking;
    if (!user::is_zero(x.lower()))
      if (!user::is_zero(x.upper()))
        return I::whole();
      else
        return I(checking::neg_inf(), rnd.div_up(one, x.lower()), true);
    else
      if (!user::is_zero(x.upper()))
        return I(rnd.div_down(one, x.upper()), checking::pos_inf(), true);
      else
        return I::empty();
  } else
    return I(rnd.div_down(one, x.upper()), rnd.div_up(one, x.lower()), true);
}

namespace detail {

template<class T, class Rounding> inline
T pow_dn(const T& x_, int pwr, Rounding& rnd) // x and pwr are positive
{
  T x = x_;
  T y = (pwr & 1) ? x_ : static_cast<T>(1);
  pwr >>= 1;
  while (pwr > 0) {
    x = rnd.mul_down(x, x);
    if (pwr & 1) y = rnd.mul_down(x, y);
    pwr >>= 1;
  }
  return y;
}

template<class T, class Rounding> inline
T pow_up(const T& x_, int pwr, Rounding& rnd) // x and pwr are positive
{
  T x = x_;
  T y = (pwr & 1) ? x_ : static_cast<T>(1);
  pwr >>= 1;
  while (pwr > 0) {
    x = rnd.mul_up(x, x);
    if (pwr & 1) y = rnd.mul_up(x, y);
    pwr >>= 1;
  }
  return y;
}

} // namespace detail
} // namespace interval_lib

template<class T, class Policies> inline
interval<T, Policies> pow(const interval<T, Policies>& x, int pwr)
{
  BOOST_USING_STD_MAX();
  using interval_lib::detail::pow_dn;
  using interval_lib::detail::pow_up;
  typedef interval<T, Policies> I;

  if (interval_lib::detail::test_input(x))
    return I::empty();

  if (pwr == 0)
    if (interval_lib::user::is_zero(x.lower())
        && interval_lib::user::is_zero(x.upper()))
      return I::empty();
    else
      return I(static_cast<T>(1));
  else if (pwr < 0)
    return interval_lib::multiplicative_inverse(pow(x, -pwr));

  typename Policies::rounding rnd;

  if (interval_lib::user::is_neg(x.upper())) {        // [-2,-1]
    T yl = pow_dn(static_cast<T>(-x.upper()), pwr, rnd);
    T yu = pow_up(static_cast<T>(-x.lower()), pwr, rnd);
    if (pwr & 1)     // [-2,-1]^1
      return I(-yu, -yl, true);
    else             // [-2,-1]^2
      return I(yl, yu, true);
  } else if (interval_lib::user::is_neg(x.lower())) { // [-1,1]
    if (pwr & 1) {   // [-1,1]^1
      return I(-pow_up(static_cast<T>(-x.lower()), pwr, rnd), pow_up(x.upper(), pwr, rnd), true);
    } else {         // [-1,1]^2
      return I(static_cast<T>(0), pow_up(max BOOST_PREVENT_MACRO_SUBSTITUTION(static_cast<T>(-x.lower()), x.upper()), pwr, rnd), true);
    }
  } else {                                // [1,2]
    return I(pow_dn(x.lower(), pwr, rnd), pow_up(x.upper(), pwr, rnd), true);
  }
}

template<class T, class Policies> inline
interval<T, Policies> sqrt(const interval<T, Policies>& x)
{
  typedef interval<T, Policies> I;
  if (interval_lib::detail::test_input(x) || interval_lib::user::is_neg(x.upper()))
    return I::empty();
  typename Policies::rounding rnd;
  T l = !interval_lib::user::is_pos(x.lower()) ? static_cast<T>(0) : rnd.sqrt_down(x.lower());
  return I(l, rnd.sqrt_up(x.upper()), true);
}

template<class T, class Policies> inline
interval<T, Policies> square(const interval<T, Policies>& x)
{
  typedef interval<T, Policies> I;
  if (interval_lib::detail::test_input(x))
    return I::empty();
  typename Policies::rounding rnd;
  const T& xl = x.lower();
  const T& xu = x.upper();
  if (interval_lib::user::is_neg(xu))
    return I(rnd.mul_down(xu, xu), rnd.mul_up(xl, xl), true);
  else if (interval_lib::user::is_pos(x.lower()))
    return I(rnd.mul_down(xl, xl), rnd.mul_up(xu, xu), true);
  else
    return I(static_cast<T>(0), (-xl > xu ? rnd.mul_up(xl, xl) : rnd.mul_up(xu, xu)), true);
}

namespace interval_lib {
namespace detail {

template< class I > inline
I root_aux(typename I::base_type const &x, int k) // x and k are bigger than one
{
  typedef typename I::base_type T;
  T tk(k);
  I y(static_cast<T>(1), x, true);
  for(;;) {
    T y0 = median(y);
    I yy = intersect(y, y0 - (pow(I(y0, y0, true), k) - x) / (tk * pow(y, k - 1)));
    if (equal(y, yy)) return y;
    y = yy;
  }
}

template< class I > inline // x is positive and k bigger than one
typename I::base_type root_aux_dn(typename I::base_type const &x, int k)
{
  typedef typename I::base_type T;
  typedef typename I::traits_type Policies;
  typename Policies::rounding rnd;
  T one(1);
  if (x > one) return root_aux<I>(x, k).lower();
  if (x == one) return one;
  return rnd.div_down(one, root_aux<I>(rnd.div_up(one, x), k).upper());
}

template< class I > inline // x is positive and k bigger than one
typename I::base_type root_aux_up(typename I::base_type const &x, int k)
{
  typedef typename I::base_type T;
  typedef typename I::traits_type Policies;
  typename Policies::rounding rnd;
  T one(1);
  if (x > one) return root_aux<I>(x, k).upper();
  if (x == one) return one;
  return rnd.div_up(one, root_aux<I>(rnd.div_down(one, x), k).lower());
}

} // namespace detail
} // namespace interval_lib

template< class T, class Policies > inline
interval<T, Policies> nth_root(interval<T, Policies> const &x, int k)
{
  typedef interval<T, Policies> I;
  if (interval_lib::detail::test_input(x)) return I::empty();
  assert(k > 0);
  if (k == 1) return x;
  typename Policies::rounding rnd;
  typedef typename interval_lib::unprotect<I>::type R;
  if (!interval_lib::user::is_pos(x.upper())) {
    if (interval_lib::user::is_zero(x.upper())) {
      T zero(0);
      if (!(k & 1) || interval_lib::user::is_zero(x.lower())) // [-1,0]^/2 or [0,0]
        return I(zero, zero, true);
      else               // [-1,0]^/3
        return I(-interval_lib::detail::root_aux_up<R>(-x.lower(), k), zero, true);
    } else if (!(k & 1)) // [-2,-1]^/2
      return I::empty();
    else {               // [-2,-1]^/3
      return I(-interval_lib::detail::root_aux_up<R>(-x.lower(), k),
               -interval_lib::detail::root_aux_dn<R>(-x.upper(), k), true);
    }
  }
  T u = interval_lib::detail::root_aux_up<R>(x.upper(), k);
  if (!interval_lib::user::is_pos(x.lower()))
    if (!(k & 1) || interval_lib::user::is_zero(x.lower())) // [-1,1]^/2 or [0,1]
      return I(static_cast<T>(0), u, true);
    else                 // [-1,1]^/3
      return I(-interval_lib::detail::root_aux_up<R>(-x.lower(), k), u, true);
  else                   // [1,2]
    return I(interval_lib::detail::root_aux_dn<R>(x.lower(), k), u, true);
}

} // namespace numeric
} // namespace boost

#endif // BOOST_NUMERIC_INTERVAL_ARITH2_HPP


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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
//==============================================================================
#ifndef NT2_EXPONENTIAL_FUNCTIONS_SIMD_COMMON_LOGSPACE_ADD_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_SIMD_COMMON_LOGSPACE_ADD_HPP_INCLUDED

#include <nt2/exponential/functions/logspace_add.hpp>
#include <boost/simd/sdk/config.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/exp.hpp>
#include <nt2/include/functions/simd/log1p.hpp>
#include <nt2/include/functions/simd/max.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/plus.hpp>
#include <nt2/include/functions/simd/unary_minus.hpp>

#ifndef BOOST_SIMD_NO_NANS
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/is_nan.hpp>
#endif

namespace nt2 { namespace ext
{

  BOOST_DISPATCH_IMPLEMENT  ( logspace_add_, tag::cpu_
                            , (A0)(X)
                            , ((simd_< floating_<A0>, X >))
                              ((simd_< floating_<A0>, X >))
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL_REPEAT(2)
    {
      A0 tmp = -nt2::abs(a0-a1);
      A0 r = nt2::max(a0,a1)+nt2::log1p(nt2::exp(tmp));
      #ifndef BOOST_SIMD_NO_NANS
      r = if_else(is_nan(tmp), a0+a1, r);
      #endif
      return r;
    }
  };
} }

#endif


#pragma once

//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
// edited: Simon Pintarelli

#include <boost/math/constants/constants.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/tools/config.hpp>
#include <cmath>

#include "mpfr/import_std_math.hpp"

namespace boost {
namespace math {

// Recurrance relation for Hermite polynomials:
template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type
hermiten_next(unsigned n, T1 x, T2 Hn, T3 Hnm1)
{
  typedef T1 numeric_t;

  const numeric_t fn = 2 / numeric_t(n + 1);
  const numeric_t fnm = numeric_t(n) / (n + 1);
  return ::math::sqrt(fn) * x * Hn - ::math::sqrt(fnm) * Hnm1;
}

namespace detail {

// Implement Hermite polynomials via recurrance:
template <class T>
T
hermiten_imp(unsigned n, T x)
{
  static const T pi = boost::math::constants::pi<T>();
  static const T pif = ::math::pow(pi, (T)-0.25);
  T p0 = pif;

  if (n == 0) return p0;

  T p1 = sqrt(T(2)) * x * pif;

  unsigned c = 1;

  while (c < n) {
    std::swap(p0, p1);
    p1 = hermiten_next(c, x, p0, p1);
    ++c;
  }
  return p1;
}

}  // namespace detail

template <class T, class Policy>
inline typename tools::promote_args<T>::type
hermiten(unsigned n, T x, const Policy&)
{
  typedef typename tools::promote_args<T>::type result_type;
  typedef typename policies::evaluation<result_type, Policy>::type value_type;
  return policies::checked_narrowing_cast<result_type, Policy>(
      detail::hermiten_imp(n, static_cast<value_type>(x)),
      "boost::math::hermiten<%1%>(unsigned, %1%)");
}

template <class T>
inline typename tools::promote_args<T>::type
hermiten(unsigned n, T x)
{
  return boost::math::hermiten(n, x, policies::policy<>());
}

}  // namespace math
}  // namespace boost


////////////////////////////////////////////////////////////////////////////////
//  Copyright (c) 2019 Prashant K. Jha
//
//  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 UTILS_RANDOM_DIST_H
#define UTILS_RANDOM_DIST_H

#include <boost/random.hpp>
#include <random>

#include "libmesh/parallel.h"

#include "utilIO.hpp"

// boost method
//typedef boost::mt19937 RandGen;
//typedef boost::lognormal_distribution<> LogNormalDist;
//typedef boost::uniform_real<> UniformDist;
//typedef boost::normal_distribution<> NormalDist;

typedef std::mt19937 RandGenerator;
typedef std::lognormal_distribution<> LogNormalDistribution;
typedef std::uniform_real_distribution<> UniformDistribution;
typedef std::normal_distribution<> NormalDistribution;

namespace util {

inline RandGenerator get_rd_gen(int &seed) {

  //return RandGenerator();

  if (seed < 0) {
    std::random_device rd;
    seed = rd();
  }

  return RandGenerator(seed);
}

inline std::default_random_engine get_rd_engine(int &seed) {

  //return std::default_random_engine();

  if (seed < 0) {
    std::random_device rd;
    seed = rd();
  }

  return std::default_random_engine(seed);
}

inline double transform_to_normal_dist(double mean, double std, double sample) {
  return std * sample + mean;
}

inline double transform_to_uniform_dist(double min, double max, double sample) {
  return min + sample * (max - min);
}

template<class T>
class DistributionSample {
public:
  DistributionSample(double arg1, double arg2, int seed = -1)
      : d_seed(seed), d_gen(get_rd_gen(seed)), d_dist(arg1, arg2){};

  DistributionSample() : d_seed(-1){};

  void init(double arg1, double arg2, int seed = -1) {

    d_seed = seed;
    d_gen = RandGenerator(get_rd_gen(seed));
    d_dist = T(arg1, arg2);
  }

  double operator()() {
    return d_dist(d_gen);
  }

  // not implemented for this
  void debug_out(const std::string &filename) {
    return;
  }

  int d_seed;
  RandGenerator d_gen;
  T d_dist;
};

template<class T>
class DistributionSampleParallel {
public:
  DistributionSampleParallel(double arg1, double arg2,
                             libMesh::Parallel::Communicator *comm,
                             int seed = -1,
                             unsigned int sample_size = 1000)
      : d_seed(seed), d_curSample(0), d_sampleSize(sample_size),
        d_gen(get_rd_gen(seed)), d_dist(arg1, arg2), d_procRank(0),
        d_procSize(0), d_comm_p(comm) {

    if (comm) {
      d_procRank = d_comm_p->rank();
      d_procSize = d_comm_p->size();
    }

    d_samples.reserve(d_sampleSize);
    if (d_procRank == 0)
      d_samples.resize(d_sampleSize);

    update();
  };

  DistributionSampleParallel()
      : d_seed(-1), d_curSample(0), d_sampleSize(0), d_procRank(0),
        d_procSize(0), d_comm_p(nullptr){};

  void init(double arg1, double arg2,
            libMesh::Parallel::Communicator *comm,
            int seed = -1,
            unsigned int sample_size = 1000) {

    d_seed = seed;
    d_curSample = 0;
    d_sampleSize = sample_size;
    d_gen = RandGenerator(get_rd_gen(seed));
    d_dist = T(arg1, arg2);
    d_comm_p = comm;

    if (comm) {
      d_procRank = d_comm_p->rank();
      d_procSize = d_comm_p->size();
    }

    d_samples.reserve(d_sampleSize);
    if (d_procRank == 0)
      d_samples.resize(d_sampleSize);

    update();
  }

  double operator()() {
    if (d_curSample < d_sampleSize) {
      return d_samples[d_curSample++];
    } else {
      update();
      return d_samples[d_curSample++];
    }
  }

  void update() {

    d_curSample = 0;
    if (d_procRank == 0) {
      for (unsigned int i = 0; i < d_sampleSize; i++)
        d_samples[i] = d_dist(d_gen);
    } else {
      d_samples.resize(0);
    }

    d_comm_p->allgather(d_samples);
    if (d_samples.size() != d_sampleSize)
      libmesh_error_msg("Error collecting samples");
  }

  void debug_out(const std::string &filename) {
    util::io::printFile(filename + "_" + std::to_string(d_procRank) + ".txt", d_samples, 0, " ");
  }

  int d_seed;
  unsigned int d_curSample;
  unsigned int d_sampleSize;
  RandGenerator d_gen;
  T d_dist;
  unsigned int d_procRank;
  unsigned int d_procSize;
  libMesh::Parallel::Communicator *d_comm_p;
  std::vector<double> d_samples;
};

} // namespace util

#endif // UTILS_RANDOM_DIST_H


#pragma once

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

DFG_ROOT_NS_BEGIN { DFG_SUB_NS(math) {

// Returns sign of a value: 1 if value is positive, 0 if value is 0, -1 if value is < 0.
// Note: Behaviour for NaN's is unspecified.
// See also: std::signbit, std::copysign (C++11)
template <class T>
int sign(const T& val)
{
	return boost::math::sign(val);
	/*
	if (val > 0)
		return 1;
	else if (val == 0)
		return 0;
	else
		return -1;
		*/
}

template <class T>
auto signBit(const T& val) -> decltype(boost::math::signbit(val))
{
	return boost::math::signbit(val);
}

// Returns value whose absolute value is from val0 and sign from val1.
template <class T0, class T1>
auto signCopied(const T0& val0, const T1& val1) -> decltype(boost::math::copysign(val0, val1))
{
	return boost::math::copysign(val0, val1);
}

// Returns val with sign changed.
template <class T>
auto signChanged(const T& val) -> decltype(boost::math::changesign(val))
{
	return boost::math::changesign(val);
}

}} // module namespace


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/interval/interval_boost_interval_type_traits.h>
#include <OpenTissue/core/math/interval/interval.h>
#include <OpenTissue/core/math/math_vector3.h>
#include <OpenTissue/core/math/math_matrix3x3.h>
#include <OpenTissue/utility/utility_timer.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

#include <iostream>

using namespace boost::numeric;
using namespace OpenTissue;

// Performance Timings: Centrino Duo, 2GB RAM, .NET 2005
//
//    interval demo started
//    OT type
//        test took 1.74157 seconds
//    boost default type
//        test took 19.701 seconds
//    boost default type with unprotect
//        test took 0.940387 seconds
//    boost smart type
//        test took 0.794767 seconds
//    boost smart type with unprotect
//        test took 0.796035 seconds
//
//
// Linux performance numbers:
//
//  henrik@blackmonster ~/Work/OpenTissue/optimized_test/demos/opengl/interval $ g++ -DHAVE_CONFIG_H -I. -I../../../../demos/opengl/interval -I../../..  -I../../../.. -I../../../../externals/include -O3 -march=pentium4 -fomit-frame-pointer -fwhole-program -pipe -DNDEBUG -o interval ../../../../demos/opengl/interval/src/main.cpp
//  henrik@blackmonster ~/Work/OpenTissue/optimized_test/demos/opengl/interval $ ./interval
//  interval demo started
//  OT type
//        test took 2.68147 seconds
//  boost default type
//        test took 6.14975 seconds
//  boost default type with unprotect
//        test took 4.89632 seconds
//  boost smart type
//        test took 4.12788 seconds
//  boost smart type with unprotect
//        test took 4.12735 seconds
//
//  henrik@blackmonster ~/Work/OpenTissue/optimized_test/demos/opengl/interval $ uname -a
//  Linux blackmonster 2.6.17-gentoo-r8 #3 SMP Mon Nov 6 22:01:53 CET 2006 i686 Intel(R) Pentium(R) 4 CPU 1.60GHz GenuineIntel GNU/Linux
//

template<typename interval_type>
void interval_compile_testing()
{
  typedef typename interval_type::base_type    real_type;
  typedef OpenTissue::math::Vector3<real_type>       vector3_type;
  typedef OpenTissue::math::Vector3<interval_type>   vector3_interval_type;
  typedef OpenTissue::math::Matrix3x3<interval_type> matrix3x3_interval_type;

  //--- implicit type conversions
  interval_type i1;
  interval_type i2(5);
  interval_type i3(5.0);
  interval_type i4(5.0f);
  interval_type i5(5,6);
  interval_type i6(5.0,6.0);
  interval_type i7(5.0f,6.0f);
  interval_type A(-1.0,1.0);
  interval_type B(0.0,1.0);
  interval_type C(-1.0,0.0);
  double s = 1.0;
  //--- arithmetic testing
  A += B;
  A = B + C;
  A -= B;
  A = B - C;
  B = -A;
  A *= B;
  A = B*C;
  A = C*B;
  A /= B;
  A = B/C;
  A = C/B;
  A *= s;
  A = B*s;
  //A = s*B; // do not exist!!!
  //--- only OpenTissue::interval
  //A.clear();
  //A.is_valid();
  //  A.get_abs_lower();
  //  A.get_abs_upper();
  //  A[-2];
  //  A[0];
  //  A[1];
  //  A[2];
  //  A(-2);
  //  A(0);
  //  A(1);
  //  A(2);
  //--- assignment testing
  A.assign( -1.0,  1.0);
  B.assign( -3.0, -2.0);
  C.assign(  2.0,  4.0);
  //--- logical/comparison testing
  bool cmp1 = (A==B);
  bool cmp2 = (A!=B);
  bool cmp3 = (C<B) ;
  bool cmp4 = (C>B) ;
  bool cmp5 = (C<=B);
  bool cmp6 = (C>=B);

  cmp1 = cmp2 | cmp3 | cmp4 | cmp5 | cmp6; // To get rid of compiler warning: unused variable

  //--- intersect testing
  A = intersect(B,C);
  B = interval_type(-2,-1);
  C = interval_type(1,2);
  A = intersect(B,C);
  A = intersect(C,B);
  B = interval_type(-2,1);
  C = interval_type(-1,2);
  A = intersect(B,C);
  A = intersect(C,B);
  B = interval_type(1,-1);
  C = interval_type(1,1);
  A = intersect(B,C);
  A = intersect(C,B);
}

template<typename interval_type>
void performance_test(interval_type const & i)
{
  using namespace OpenTissue::math::interval;

  OpenTissue::utility::Timer<double> watch;
  watch.start();
  typedef double                               real_type;
  typedef OpenTissue::math::Vector3<real_type>       vector3_type;
  typedef OpenTissue::math::Vector3<interval_type>   vector3_interval_type;
  typedef OpenTissue::math::Matrix3x3<interval_type> matrix3x3_interval_type;
  interval_type A(-1,1);
  matrix3x3_interval_type IM(A,A,A,A,A,A,A,A,A);
  vector3_interval_type IV(A,A,A);
  vector3_type V(1,2,3);
  for(unsigned int i=0u;i<10000000u;++i)
    IV = IM * V + (IV*0.5);
  watch.stop();
  BOOST_TEST_MESSAGE( typeid(interval_type).name() << " took " << watch() << " sec." );
}

BOOST_AUTO_TEST_SUITE(opentissue_math_interval);

  BOOST_AUTO_TEST_CASE(opentissue_testing)
  {
    typedef OpenTissue::math::interval::Interval<double> interval_type;
    performance_test(interval_type());
    void (*ptr) () = 0;
    ptr = &interval_compile_testing< interval_type >;
  }

  BOOST_AUTO_TEST_CASE(boost_default_testing)
  {
    typedef boost::numeric::interval<double >      interval_type;
    performance_test(interval_type());
    void (*ptr) () = 0;
    ptr = &interval_compile_testing< interval_type >;
  }

  BOOST_AUTO_TEST_CASE(boost_default_with_unprotect_testing)
  {
    typedef boost::numeric::interval<double >      interval_type;
    interval_type::traits_type::rounding rnd;
    typedef  boost::numeric::interval_lib::unprotect<interval_type>::type R;
    performance_test(R());
    void (*ptr) () = 0;
    ptr = &interval_compile_testing< interval_type >;
  }

  BOOST_AUTO_TEST_CASE(boost_smart_type_testing)
  {
    typedef OpenTissue::math::interval::BoostIntervalTypeTraits<double>::interval_type interval_type;
    performance_test(interval_type());
    void (*ptr) () = 0;
    ptr = &interval_compile_testing< interval_type >;
  }

  BOOST_AUTO_TEST_CASE(boost_smart_type_with_unprotect_testing)
  {
    typedef OpenTissue::math::interval::BoostIntervalTypeTraits<double>::interval_type interval_type;
    interval_type::traits_type::rounding rnd;

    rnd;

    typedef  boost::numeric::interval_lib::unprotect<interval_type>::type R;
    performance_test(R());
    void (*ptr) () = 0;
    ptr = &interval_compile_testing< interval_type >;
  }

BOOST_AUTO_TEST_SUITE_END();


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

// Copyright (c) 2010-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

#include <iostream>
#include <string>

#include <geometry_test_common.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <boost/geometry/algorithms/length.hpp>
#include <boost/geometry/algorithms/num_points.hpp>
#include <boost/geometry/algorithms/unique.hpp>
#include <boost/geometry/extensions/algorithms/offset.hpp>
#include <boost/geometry/multi/io/wkt/read.hpp>

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

#if defined(TEST_WITH_SVG)
#  include <boost/geometry/multi/algorithms/envelope.hpp>
#  include <boost/geometry/io/svg/svg_mapper.hpp>
#endif

template <typename GeometryOut, typename Geometry>
void test_offset(std::string const& caseid, Geometry const& geometry,
        double distance,
        double expected_length, double percentage)
{
    typedef typename bg::coordinate_type<Geometry>::type coordinate_type;
    typedef typename bg::point_type<Geometry>::type point_type;

    // TODO: also make tests for miter
    typedef bg::strategy::buffer::join_round
        <
            point_type,
            point_type
        > join_strategy;

    GeometryOut moved_by_offset;
    bg::offset(geometry, moved_by_offset, join_strategy(), distance);

    typename bg::default_length_result<Geometry>::type length
                    = bg::length(moved_by_offset);

    /*
    std::size_t count = bg::num_points(moved_by_offset);
    BOOST_CHECK_MESSAGE(count == expected_point_count,
            "offset: " << caseid
            << " #points expected: " << expected_point_count
            << " detected: " << count
            << " type: " << string_from_type<coordinate_type>::name()
            );
    */

    //BOOST_CHECK_EQUAL(holes, expected_hole_count);
    BOOST_CHECK_CLOSE(length, expected_length, percentage);

#if defined(TEST_WITH_SVG)
    {
        std::ostringstream filename;
        filename << "offset_"
            << caseid << "_"
            << string_from_type<coordinate_type>::name()
            << ".svg";

        std::ofstream svg(filename.str().c_str());

        bg::svg_mapper
            <
                typename bg::point_type<Geometry>::type
            > mapper(svg, 500, 500);
        mapper.add(geometry);
        mapper.add(moved_by_offset);

        mapper.map(geometry, "opacity:0.6;fill:rgb(0,0,255);stroke:rgb(0,0,0);stroke-width:1");
        mapper.map(moved_by_offset, "opacity:0.6;fill:none;stroke:rgb(255,0,0);stroke-width:5");
    }
#endif
}

template <typename Geometry>
void test_one(std::string const& caseid, std::string const& wkt, double distance,
        double expected_length_plus, double expected_length_minus, bool do_plus, bool do_min)
{
    Geometry geometry;
    bg::read_wkt(wkt, geometry);

    double percentage = 0.01;
    if (do_plus) test_offset<Geometry>(caseid + "_a", geometry, distance, expected_length_plus, percentage);
    if (do_min) test_offset<Geometry>(caseid + "_b", geometry, -distance, expected_length_minus, percentage);
}

template <typename P>
void test_all()
{

    typedef bg::model::linestring<P> linestring;

    static std::string const simplex = "LINESTRING(0 0,1 1)";
    static std::string const one_bend = "LINESTRING(0 0,4 5,7 4)";
    static std::string const two_bends = "LINESTRING(0 0,4 5,7 4,10 6)";
    static std::string const overlapping = "LINESTRING(0 0,4 5,7 4,10 6, 10 2,2 2)";
    static std::string const curve = "LINESTRING(2 7,3 5,5 4,7 5,8 7)";
    static std::string const reallife1 = "LINESTRING(76396.40464822574 410095.6795147947,76397.85016212701 410095.211865792,76401.30666443033 410095.0466387949,76405.05892643372 410096.1007777959,76409.45103273794 410098.257640797,76412.96309264141 410101.6522238015)";

    test_one<linestring>("ls_simplex", simplex, 0.5, std::sqrt(2.0), std::sqrt(2.0), true, true);
    test_one<linestring>("one_bend", one_bend, 0.5, 10.17328, 8.8681, true, false);

    // Most of the tests below fail because the internal implementation of buffer is changed in the meantime (on purpose).
    // The offset now contains knots which should be removed separately, apart from the buffer algorithm.
    // The offset algorithm is therefore hardly usable now (only convex pieces are handled correctly...)

    // TODO: decide about this / implement this correctly.

    //test_one<linestring>("two_bends", two_bends, 0.5, 13.2898, 12.92811);
    //test_one<linestring>("overlapping", overlapping, 0.5, 27.1466, 22.0596);
    test_one<linestring>("curve", curve, 0.5, 7.7776,  10.0507, false, true);
    //test_one<linestring>("reallife1", reallife1, 16.5, 5.4654, 36.4943);
}

int test_main(int, char* [])
{
    test_all<bg::model::d2::point_xy<double> >();

    return 0;
}


/**
 *  @copyright Copyright 2021 The J-PET Framework Authors. All rights reserved.
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may find a copy of the License in the LICENCE file.
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *  @file CalibrationToolsTest.cpp
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE EventCategorizerToolsTests
#include "../calibrationProgram/CalibrationTools.h"
#include <boost/test/unit_test.hpp>
#include <cstdlib>
#include <chrono>
#include <random>
#include <ctime>

/// Accuracy for BOOST_REQUIRE_CLOSE comparisons
const double kEpsilon = 0.01;

BOOST_AUTO_TEST_SUITE(CalibrationProgramCheckSuite)

BOOST_AUTO_TEST_CASE(checkEstimateExtremumBin) {
  CalibrationTools calibTools;
  std::vector<double> values;
  unsigned numberOfPoints = 100;
  double mean1 = -2000, mean2 = 2000;
  double normalization = 1000000;
  double sigma = 200;
  double stepSize = 100;
  double tempValue;
  for (unsigned i=0; i<numberOfPoints; i++) {
    tempValue = 0;
    tempValue = normalization*exp(-pow(-0.5*stepSize*numberOfPoints + i*stepSize - mean1,2)/(2*pow(sigma,2)));
    tempValue -= normalization*exp(-pow(-0.5*stepSize*numberOfPoints + i*stepSize - mean2,2)/(2*pow(sigma,2)));
    values.push_back(tempValue);
  }
  unsigned extremumLeft = calibTools.EstimateExtremumBin(values, 0, 1, Side::Left);
  unsigned extremumRight = calibTools.EstimateExtremumBin(values, 0, 1, Side::Right);
  BOOST_REQUIRE_EQUAL((float)extremumLeft, 31);
  BOOST_REQUIRE_EQUAL((float)extremumRight, 69);
}

BOOST_AUTO_TEST_CASE(checkFindingPeak) {
  CalibrationTools calibTools;
  std::vector<double> values = {-2, -1, 0, 1, 2};
  std::vector<double> arguments = {0, 1, 2, 3, 4};
  Parameter result = calibTools.FindPeak(arguments, values, 0, 4);
  BOOST_REQUIRE_CLOSE(result.Value, 2, kEpsilon);
}

BOOST_AUTO_TEST_CASE(checkFindingMiddleAB) {
  CalibrationTools calibTools;
  TH1D* histo = new TH1D("TestDistributionAB", "Test distribution of AB", 2500, -24990, 25010);
  unsigned numberOfEvents = 10000000;
  double uniformDistributionWidth = 5000; // 5 ns
  double resolutionSigma = 150; // 150 ps
  double tdiffAB, smearing, tempNumber2, tempNumber3;

  std::uniform_real_distribution<float> distribution(0.0, 1.0);
  std::random_device rd;
  std::default_random_engine generator(rd());
  for (unsigned i=0; i<numberOfEvents; i++) {
    tdiffAB = uniformDistributionWidth*distribution(generator);
    tempNumber2 = distribution(generator);
    tempNumber3 = distribution(generator);
    smearing = sqrt(-2*log(tempNumber2))*cos(2*M_PI*tempNumber3);
//Box-muller transformation
    histo -> Fill(tdiffAB + smearing*resolutionSigma - 0.5*uniformDistributionWidth);
  }
  double meanTemp = histo->GetMean(1);
  double rangeParameter = 4*histo->GetStdDev();

  Parameter resultLeft = calibTools.FindMiddle(histo, meanTemp - rangeParameter, meanTemp, Side::Left, "");
  Parameter resultRight = calibTools.FindMiddle(histo, meanTemp, meanTemp + rangeParameter, Side::Right, "");
  double acceptedRange = 2*kEpsilon*resolutionSigma;
  BOOST_REQUIRE_CLOSE(resultLeft.Value, -0.5*uniformDistributionWidth, acceptedRange);
  BOOST_REQUIRE_CLOSE(resultRight.Value, 0.5*uniformDistributionWidth, acceptedRange);

  delete histo;
}

BOOST_AUTO_TEST_CASE(checkFindingMiddlePALS) {
  CalibrationTools calibTools;
  TH1D* histo = new TH1D("TestDistributionPALS", "Test PALS distribution", 2500, -24990, 25010);
  unsigned numberOfEvents = 10000000;
  double exponentialMean = 2000;
  double resolutionSigma = 150; // 150 ps
  srand((unsigned)time(NULL));
  double tdiff, smearing, tempNumber2, tempNumber3;

  std::uniform_real_distribution<float> distribution(0.0, 1.0);
  std::random_device rd;
  std::default_random_engine generator(rd());
  for (unsigned i=0; i<numberOfEvents; i++) {
    tdiff = -log(distribution(generator))/exponentialMean;
    tempNumber2 = distribution(generator);
    tempNumber3 = distribution(generator);
    smearing = sqrt(-2*log(tempNumber2))*cos(2*M_PI*tempNumber3);
//Box-muller transformation
    histo -> Fill(tdiff + smearing*resolutionSigma);
  }
  double meanTemp = histo->GetMean(1);
  double rangeParameter = 4*histo->GetStdDev();

  Parameter results = calibTools.FindMiddle(histo, meanTemp - rangeParameter, meanTemp + rangeParameter, Side::MaxAnni, "");
// Proper mode of the exponentially modified gaussian is estimated by erfcinvx function which is quite complicated. Instead a value
// for 150ps is estimated here. Form is left in order someone want to expand it in future
  double acceptedValue = pow(resolutionSigma, 2)/exponentialMean - sqrt(2.5)*resolutionSigma*kEpsilon;
  double acceptedRange = 3*kEpsilon*resolutionSigma;
  BOOST_REQUIRE_CLOSE(results.Value, acceptedValue, acceptedRange);

  delete histo;
}

BOOST_AUTO_TEST_CASE(checkFindingMaximum) {
  std::vector<double> values = {1, 2, 3, 2, 1, 4, 5, 4, 3, 2, 1};
  std::vector<double> arguments = {-5, -4, -3, -2, -1, 0, 1, 2, 3, 4, 5};
  unsigned maximum = FindMaximum(values, arguments, -3, 3);
  BOOST_REQUIRE_EQUAL(maximum, 6);

  unsigned maximum2 = FindMaximum(values, arguments, -6, -1);
  BOOST_REQUIRE_EQUAL(maximum2, 2);
}

BOOST_AUTO_TEST_CASE(checkCalculationsMeanDifference) {
  std::vector<double> vector1 = {1, 2, 3, 2, 1, 4, 5, 4, 3, 2, 1};
  double diff = CalcMeanDiff(vector1, vector1);
  BOOST_REQUIRE_CLOSE(diff, 0, kEpsilon);

  std::vector<double> vector2 = {0, 1, 2, 1, 0, 3, 4, 3, 2, 1, 0};
  diff = CalcMeanDiff(vector1, vector2);
  BOOST_REQUIRE_CLOSE(diff, 1, kEpsilon);
}

BOOST_AUTO_TEST_CASE(checkCalculationsMeanCorrection) {
  Parameter par1, par2;
  par1.Value = 2;
  par2.Value = 1;
  std::vector<Parameter> vector1 = {par1, par1, par1, par1, par1};
  std::vector<std::vector<Parameter>> vector1_2 = {vector1, vector1, vector1};
  Parameter diff = CalculateMeanCorrection(vector1_2, vector1_2);
  BOOST_REQUIRE_CLOSE(diff.Value, 0, kEpsilon);

  std::vector<Parameter> vector2 = {par2, par2, par2, par2, par2};
  std::vector<std::vector<Parameter>> vector2_2 = {vector2, vector2, vector2};
  diff = CalculateMeanCorrection(vector1_2, vector2_2);
  BOOST_REQUIRE_CLOSE(diff.Value, 1, kEpsilon);
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * position3.hpp
 *
 *  Created on: Dec 9, 2013
 *      Author: joost
 */

#ifndef POSITION3_HPP_
#define POSITION3_HPP_

#include <math.h>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/utility.hpp>
#include <sferes/dbg/dbg.hpp>

namespace sferes
{
  namespace gen
  {
    namespace spatial
    {
      class Pos
      {
        public:
          Pos() {
          }
          Pos(float x, float y, float z) : _x(x), _y(y), _z(z) {
          }
          float dist(const Pos& p) const
          {
            float x = _x - p._x;
            float y = _y - p._y;
            float z = _z - p._z;
            return sqrt(x * x + y * y + z * z);
          }
          float x() const { return _x; }
          float y() const { return _y; }
          float z() const { return _z; }

          void setX(const float& x){_x = x;};
          void setY(const float& y){_y = y;};
          void setZ(const float& z){_z = z;};

          void moveX(const float& dx){_x += dx;};
          void moveY(const float& dy){_y += dy;};
          void moveZ(const float& dz){_z += dz;};

          void translate(const float& dx, const float& dy, const float& dz){
        	  moveX(dx);
        	  moveY(dy);
        	  moveZ(dz);
          }

          float& operator [] (const size_t& index){
        	  switch(index){
        	  case 0:
        		  return _x;
        	  case 1:
        		  return _y;
        	  case 2:
        		  return _z;
        	  default: dbg::sentinel(DBG_HERE);
        	  }
        	  dbg::sentinel(DBG_HERE);
        	  throw 0;
          }

          Pos& operator *= (const float& scalar){
        	  _x*=scalar;
        	  _y*=scalar;
        	  _z*=scalar;
        	  return *this;
          }

          Pos& operator += (const float& scalar){
        	  _x+=scalar;
        	  _y+=scalar;
        	  _z+=scalar;
        	  return *this;
          }

          Pos& operator += (const Pos& other){
        	  _x+=other.x();
        	  _y+=other.y();
        	  _z+=other.z();
        	  return *this;
          }

          template<class Archive>
          void serialize(Archive& ar, const unsigned int version)
          {
            ar& BOOST_SERIALIZATION_NVP(_x);
            ar& BOOST_SERIALIZATION_NVP(_y);
            ar& BOOST_SERIALIZATION_NVP(_z);
          }
          bool operator == (const Pos &p)
          { return _x == p._x && _y == p._y && _z == p._z; }
        protected:
          float _x, _y, _z;
      };

      Pos operator+(const Pos& lhs, const Pos& rhs){
    	Pos result (lhs);
      	result+=rhs;
      	return result;
      }

      Pos operator*(const Pos& lhs, const float& rhs){
    	Pos result (lhs);
      	result*=rhs;
      	return result;
      }

      Pos operator*(const float& rhs, const Pos& lhs){
      	return lhs*rhs;
      }

      std::ostream& operator<<(std::ostream& is, const Pos& obj){
          is << obj.x() << " " << obj.y() << " " << obj.z();
          return is;
      }
    }
  }
}

#endif /* POSITION3_HPP_ */


// Filename: matrix_free_3.cpp (part of MTL4)

#include <iostream>
#include <cassert>
#include <boost/numeric/mtl/mtl.hpp>

struct poisson2D_dirichlet
{
    poisson2D_dirichlet(int m, int n) : m(m), n(n), s(m * n) {}

    template <typename VectorIn, typename VectorOut, typename Assign>
    void mult(const VectorIn& v, VectorOut& w, Assign) const
    {
	assert(int(size(v)) == m * n);
	assert(size(v) == size(w));

	// Inner domain
	for (int i= 1; i < m-1; i++)
	    for (int j= 1, k= i * n + j; j < n-1; j++, k++)
		Assign::apply(w[k], 4 * v[k] - v[k-n] - v[k+n] - v[k-1] - v[k+1]);

	// Upper border
	for (int j= 1; j < n-1; j++)
	    Assign::apply(w[j], 4 * v[j] - v[j+n] - v[j-1] - v[j+1]);

	// Lower border
	for (int j= 1, k= (m-1) * n + j; j < n-1; j++, k++)
	    Assign::apply(w[k], 4 * v[k] - v[k-n] - v[k-1] - v[k+1]);

	// Left border
	for (int i= 1, k= n; i < m-1; i++, k+= n)
	    Assign::apply(w[k], 4 * v[k] - v[k-n] - v[k+n] - v[k+1]);

	// Right border
	for (int i= 1, k= n+n-1; i < m-1; i++, k+= n)
	    Assign::apply(w[k], 4 * v[k] - v[k-n] - v[k+n] - v[k-1]);

	// Corners
	Assign::apply(w[0], 4 * v[0] - v[1] - v[n]);
	Assign::apply(w[n-1], 4 * v[n-1] - v[n-2] - v[2*n - 1]);
	Assign::apply(w[(m-1)*n], 4 * v[(m-1)*n] - v[(m-2)*n] - v[(m-1)*n+1]);
	Assign::apply(w[m*n-1], 4 * v[m*n-1] - v[m*n-2] - v[m*n-n-1]);
    }

    template <typename VectorIn>
    mtl::vec::mat_cvec_multiplier<poisson2D_dirichlet, VectorIn> operator*(const VectorIn& v) const
    {	return mtl::vec::mat_cvec_multiplier<poisson2D_dirichlet, VectorIn>(*this, v);    }

    int m, n, s;
};

inline std::size_t size(const poisson2D_dirichlet& A) { return A.s * A.s; }
inline std::size_t num_rows(const poisson2D_dirichlet& A) { return A.s; }
inline std::size_t num_cols(const poisson2D_dirichlet& A) { return A.s; }

namespace mtl {

    template <>
    struct Collection<poisson2D_dirichlet>
    {
	typedef double value_type;
	typedef int    size_type;
    };

    namespace ashape {
	template <> struct ashape_aux<poisson2D_dirichlet>
	{	typedef nonscal type;    };
    }
}

int main(int, char**)
{
    using namespace std;
    typedef mtl::dense_vector<double> vt;

    vt v(20);
    iota(v);
    cout << "v is " << v << endl;

    poisson2D_dirichlet A(4, 5);
    vt                  w2(20);

    w2= A * v;
    cout << "A * v is " << w2 << endl;

    w2+= A * v;
    cout << "w2+= A * v is " << w2 << endl;

    w2-= A * v;
    cout << "w2-= A * v is " << w2 << endl;

    return 0;
}


#include <benchmark/benchmark.h>

#include <tnt/core/core.hpp>
#include <tnt/math/math.hpp>

#include <opencv2/core.hpp>
#include <benchmark/opencv_utils.hpp>

#include <Eigen/Dense>
#include <blas/cblas.hpp>

template <typename DataType>
static void multiply_TNT(benchmark::State& state, int size)
{
    while (state.KeepRunning()) {
        state.PauseTiming(); // We don't count tensor creation
        tnt::Shape shape{size, size};

        tnt::Tensor<DataType> left(shape, 3.f);
        tnt::Tensor<DataType> right(shape, 4.f);

        state.ResumeTiming();
        tnt::multiply(left, right);
    }
}

template <typename DataType>
static void multiply_OCV(benchmark::State& state, int size)
{
    while (state.KeepRunning()) {
        state.PauseTiming();

        cv::Mat left  = tnt::create_cv_mat<DataType>(size, size, cv::Scalar(3));
        cv::Mat right = tnt::create_cv_mat<DataType>(size, size, cv::Scalar(4));

        state.ResumeTiming();
        cv::Mat dst = left * right;
    }
}

template <typename DataType>
static void multiply_EIG(benchmark::State& state, int size)
{
    using MatType = Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic>;

    while (state.KeepRunning()) {
        state.PauseTiming();

        MatType left = MatType::Constant(size, size, 3);
        MatType right = MatType::Constant(size, size, 4);

        state.ResumeTiming();
        MatType dst = left * right;
    }
}

template <typename DataType>
struct BLASMultiply {};

template <> struct BLASMultiply<float>
{
    static void run(int size, float* left, float* right, float* dst)
    {
        /* See https://www.christophlassner.de/using-blas-from-c-with-row-major-data.html */
        /* For trick to use fortran order (col-major) */
        cblas_sgemm(CblasColMajor, /* Memory order */
                    CblasNoTrans,  /* Left is transposed? */
                    CblasNoTrans,  /* Right is transposed? */
                    size,          /* Rows of left */
                    size,          /* Columns of right */
                    size,          /* Columns of left and rows of right */
                    1.,            /* Alpha (scale after mult) */
                    right,         /* Left data */
                    size,          /* lda, this is really for 2D mats */
                    left,          /* right data */
                    size,          /* ldb, this is really for 2D mats */
                    0.,            /* beta (scale on result before addition) */
                    dst,           /* result data */
                    size);         /* ldc, this is really for 2D mats */
    }
};

template <> struct BLASMultiply<double>
{
    static void run(int size, double* left, double* right, double* dst)
    {
        /* See https://www.christophlassner.de/using-blas-from-c-with-row-major-data.html */
        /* For trick to use fortran order (col-major) */
        cblas_dgemm(CblasColMajor, /* Memory order */
                    CblasNoTrans,  /* Left is transposed? */
                    CblasNoTrans,  /* Right is transposed? */
                    size,          /* Rows of left */
                    size,          /* Columns of right */
                    size,          /* Columns of left and rows of right */
                    1.,            /* Alpha (scale after mult) */
                    right,         /* Left data */
                    size,          /* lda, this is really for 2D mats */
                    left,          /* right data */
                    size,          /* ldb, this is really for 2D mats */
                    0.,            /* beta (scale on result before addition) */
                    dst,           /* result data */
                    size);         /* ldc, this is really for 2D mats */
    }
};

template <typename DataType>
static void multiply_BLAS(benchmark::State& state, int size)
{
    while (state.KeepRunning()) {
        state.PauseTiming();

        DataType* left  = new DataType[size * size];
        DataType* right = new DataType[size * size];

        for (int i = 0; i < size * size; ++i) {
            left[i] = 3;
            right[i] = 4;
        }

        state.ResumeTiming();

        // For fairness allocation happens during timing
        DataType* dst = new DataType[size * size];

        BLASMultiply<DataType>::run(size, left, right, dst);
    }
}

template <typename T>
class RegisterMatrixMultiplyBenchmark
{
public:
    RegisterMatrixMultiplyBenchmark(const std::string& type)
    {
        std::vector<int> sizes{4, 16, 64, 512, 2048, 3121};
        for (int size : sizes) {
            std::string suffix = type + ">[" + std::to_string(size) + "x" + std::to_string(size) + "]";
            benchmark::RegisterBenchmark(("MatrixMultiply:TNT <" + suffix).c_str(), multiply_TNT<T>, size);
            benchmark::RegisterBenchmark(("MatrixMultiply:OCV <" + suffix).c_str(), multiply_OCV<T>, size);
            benchmark::RegisterBenchmark(("MatrixMultiply:EIG <" + suffix).c_str(), multiply_EIG<T>, size);
            benchmark::RegisterBenchmark(("MatrixMultiply:BLAS<" + suffix).c_str(), multiply_BLAS<T>, size);
        }
    }
};

//static RegisterMultiplyBenchmark<int>    multiply_benchmark_int("int");
static RegisterMatrixMultiplyBenchmark<float>  matrix_multiply_benchmark_float("float");
static RegisterMatrixMultiplyBenchmark<double> matrix_multiply_benchmark_double("double");