#include <NTL/mat_ZZ.h>

NTL_START_IMPL

void add(mat_ZZ& X, const mat_ZZ& A, const mat_ZZ& 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& X, const mat_ZZ& A, const mat_ZZ& 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));
}

void mul_aux(mat_ZZ& X, const mat_ZZ& A, const mat_ZZ& 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);

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

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

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

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

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

   x.SetLength(n);

   long i, k;
   ZZ acc, tmp;

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

void mul(vec_ZZ& x, const mat_ZZ& A, const vec_ZZ& b)
{
   if (&b == &x || A.alias(x)) {
      vec_ZZ tmp;
      mul_aux(tmp, A, b);
      x = tmp;
   }
   else
      mul_aux(x, A, b);
}

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

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

   x.SetLength(l);

   long i, k;
   ZZ acc, tmp;

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

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

void ident(mat_ZZ& 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));
}

static
long DetBound(const mat_ZZ& a)
{
   long n = a.NumRows();
   long i;
   ZZ res, t1;

   set(res);

   for (i = 0; i < n; i++) {
      InnerProduct(t1, a[i], a[i]);
      if (t1 > 1) {
         SqrRoot(t1, t1);
         add(t1, t1, 1);
      }
      mul(res, res, t1);
   }

   return NumBits(res);
}

void determinant(ZZ& rres, const mat_ZZ& a, long deterministic)
{
   long n = a.NumRows();
   if (a.NumCols() != n)
      LogicError("determinant: nonsquare matrix");

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

   zz_pBak zbak;
   zbak.save();

   ZZ_pBak Zbak;
   Zbak.save();

   long instable = 1;

   long gp_cnt = 0;

   long bound = 2+DetBound(a);

   ZZ res, prod;

   clear(res);
   set(prod);

   long i;
   for (i = 0; ; i++) {
      if (NumBits(prod) > bound)
         break;

      if (!deterministic &&
          !instable && bound > 1000 && NumBits(prod) < 0.25*bound) {
         ZZ P;

         long plen = 90 + NumBits(max(bound, NumBits(res)));
         GenPrime(P, plen, 90 + 2*NumBits(gp_cnt++));

         ZZ_p::init(P);

         mat_ZZ_p A;
         conv(A, a);

         ZZ_p t;
         determinant(t, A);

         if (CRT(res, prod, rep(t), P))
            instable = 1;
         else
            break;
      }

      zz_p::FFTInit(i);
      long p = zz_p::modulus();

      mat_zz_p A;
      conv(A, a);

      zz_p t;
      determinant(t, A);

      instable = CRT(res, prod, rep(t), p);
   }

   rres = res;

   zbak.restore();
   Zbak.restore();
}

void conv(mat_zz_p& x, const mat_ZZ& a)
{
   long n = a.NumRows();
   long m = a.NumCols();
   long i;

   x.SetDims(n, m);
   for (i = 0; i < n; i++)
      conv(x[i], a[i]);
}

void conv(mat_ZZ_p& x, const mat_ZZ& a)
{
   long n = a.NumRows();
   long m = a.NumCols();
   long i;

   x.SetDims(n, m);
   for (i = 0; i < n; i++)
      conv(x[i], a[i]);
}

long IsIdent(const mat_ZZ& 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& X, const mat_ZZ& 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 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);
   }
}

long CRT(mat_ZZ& gg, ZZ& a, const mat_zz_p& G)
{
   long n = gg.NumRows();
   long m = gg.NumCols();

   if (G.NumRows() != n || G.NumCols() != m)
      LogicError("CRT: dimension mismatch");

   long p = zz_p::modulus();

   ZZ new_a;
   mul(new_a, a, p);

   long a_inv;
   a_inv = rem(a, p);
   a_inv = InvMod(a_inv, p);

   long p1;
   p1 = p >> 1;

   ZZ a1;
   RightShift(a1, a, 1);

   long p_odd = (p & 1);

   long modified = 0;

   long h;

   ZZ g;
   long i, j;

   for (i = 0; i < n; i++) {
      for (j = 0; j < m; j++) {
         if (!CRTInRange(gg[i][j], a)) {
            modified = 1;
            rem(g, gg[i][j], a);
            if (g > a1) sub(g, g, a);
         }
         else
            g = gg[i][j];

         h = rem(g, p);
         h = SubMod(rep(G[i][j]), h, p);
         h = MulMod(h, a_inv, p);
         if (h > p1)
            h = h - p;

         if (h != 0) {
            modified = 1;

            if (!p_odd && g > 0 && (h == p1))
               MulSubFrom(g, a, h);
            else
               MulAddTo(g, a, h);

         }

         gg[i][j] = g;
      }
   }

   a = new_a;

   return modified;

}

void mul(mat_ZZ& X, const mat_ZZ& A, const ZZ& b_in)
{
   ZZ 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& X, const mat_ZZ& A, long b)
{
   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);
}

static
void ExactDiv(vec_ZZ& x, const ZZ& d)
{
   long n = x.length();
   long i;

   for (i = 0; i < n; i++)
      if (!divide(x[i], x[i], d))
         ArithmeticError("inexact division");
}

static
void ExactDiv(mat_ZZ& x, const ZZ& d)
{
   long n = x.NumRows();
   long m = x.NumCols();

   long i, j;

   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         if (!divide(x[i][j], x[i][j], d))
            ArithmeticError("inexact division");
}

void diag(mat_ZZ& X, long n, const ZZ& d_in)
{
   ZZ 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& A, long n, const ZZ& 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 solve(ZZ& d_out, vec_ZZ& x_out,
           const mat_ZZ& A, const vec_ZZ& b,
           long deterministic)
{
   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_out);
      x_out.SetLength(0);
      return;
   }

   zz_pBak zbak;
   zbak.save();

   ZZ_pBak Zbak;
   Zbak.save();

   vec_ZZ x(INIT_SIZE, n);
   ZZ d, d1;

   ZZ d_prod, x_prod;
   set(d_prod);
   set(x_prod);

   long d_instable = 1;
   long x_instable = 1;

   long check = 0;

   long gp_cnt = 0;

   vec_ZZ y, b1;

   long i;
   long bound = 2+DetBound(A);

   for (i = 0; ; i++) {
      if ((check || IsZero(d)) && !d_instable) {
         if (NumBits(d_prod) > bound) {
            break;
         }
         else if (!deterministic &&
                  bound > 1000 && NumBits(d_prod) < 0.25*bound) {

            ZZ P;

            long plen = 90 + NumBits(max(bound, NumBits(d)));
            GenPrime(P, plen, 90 + 2*NumBits(gp_cnt++));

            ZZ_p::init(P);

            mat_ZZ_p AA;
            conv(AA, A);

            ZZ_p dd;
            determinant(dd, AA);

            if (CRT(d, d_prod, rep(dd), P))
               d_instable = 1;
            else
               break;
         }
      }

      zz_p::FFTInit(i);
      long p = zz_p::modulus();

      mat_zz_p AA;
      conv(AA, A);

      if (!check) {
         vec_zz_p bb, xx;
         conv(bb, b);

         zz_p dd;

         solve(dd, xx, AA, bb);

         d_instable = CRT(d, d_prod, rep(dd), p);
         if (!IsZero(dd)) {
            mul(xx, xx, dd);
            x_instable = CRT(x, x_prod, xx);
         }
         else
            x_instable = 1;

         if (!d_instable && !x_instable) {
            mul(y, x, A);
            mul(b1, b, d);
            if (y == b1) {
               d1 = d;
               check = 1;
            }
         }
      }
      else {
         zz_p dd;
         determinant(dd, AA);
         d_instable = CRT(d, d_prod, rep(dd), p);
      }
   }

   if (check && d1 != d) {
      mul(x, x, d);
      ExactDiv(x, d1);
   }

   d_out = d;
   if (check) x_out = x;

   zbak.restore();
   Zbak.restore();
}

void inv(ZZ& d_out, mat_ZZ& x_out, const mat_ZZ& A, long deterministic)
{
   long n = A.NumRows();

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

   if (n == 0) {
      set(d_out);
      x_out.SetDims(0, 0);
      return;
   }

   zz_pBak zbak;
   zbak.save();

   ZZ_pBak Zbak;
   Zbak.save();

   mat_ZZ x(INIT_SIZE, n, n);
   ZZ d, d1;

   ZZ d_prod, x_prod;
   set(d_prod);
   set(x_prod);

   long d_instable = 1;
   long x_instable = 1;

   long gp_cnt = 0;

   long check = 0;

   mat_ZZ y;

   long i;
   long bound = 2+DetBound(A);

   for (i = 0; ; i++) {
      if ((check || IsZero(d)) && !d_instable) {
         if (NumBits(d_prod) > bound) {
            break;
         }
         else if (!deterministic &&
                  bound > 1000 && NumBits(d_prod) < 0.25*bound) {

            ZZ P;

            long plen = 90 + NumBits(max(bound, NumBits(d)));
            GenPrime(P, plen, 90 + 2*NumBits(gp_cnt++));

            ZZ_p::init(P);

            mat_ZZ_p AA;
            conv(AA, A);

            ZZ_p dd;
            determinant(dd, AA);

            if (CRT(d, d_prod, rep(dd), P))
               d_instable = 1;
            else
               break;
         }
      }

      zz_p::FFTInit(i);
      long p = zz_p::modulus();

      mat_zz_p AA;
      conv(AA, A);

      if (!check) {
         mat_zz_p xx;

         zz_p dd;

         inv(dd, xx, AA);

         d_instable = CRT(d, d_prod, rep(dd), p);
         if (!IsZero(dd)) {
            mul(xx, xx, dd);
            x_instable = CRT(x, x_prod, xx);
         }
         else
            x_instable = 1;

         if (!d_instable && !x_instable) {
            mul(y, x, A);
            if (IsDiag(y, n, d)) {
               d1 = d;
               check = 1;
            }
         }
      }
      else {
         zz_p dd;
         determinant(dd, AA);
         d_instable = CRT(d, d_prod, rep(dd), p);
      }
   }

   if (check && d1 != d) {
      mul(x, x, d);
      ExactDiv(x, d1);
   }

   d_out = d;
   if (check) x_out = x;

   zbak.restore();
   Zbak.restore();
}

void negate(mat_ZZ& X, const mat_ZZ& 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& 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& x)
{
   long n = x.NumRows();
   long i;
   for (i = 0; i < n; i++)
      clear(x[i]);
}

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

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

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

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

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

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

void inv(mat_ZZ& X, const mat_ZZ& A)
{
   ZZ d;
   inv(d, X, A);
   if (d == -1)
      negate(X, X);
   else if (d != 1)
      ArithmeticError("inv: non-invertible matrix");
}

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

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

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

/***********************************************************

   routines for solving a linear system via Hensel lifting

************************************************************/

static
long MaxBits(const mat_ZZ& A)
{
   long m = 0;
   long i, j;
   for (i = 0; i < A.NumRows(); i++)
      for (j = 0; j < A.NumCols(); j++)
         m = max(m, NumBits(A[i][j]));

   return m;
}

// Computes an upper bound on the numerators and denominators
// to the solution x*A = b using Hadamard's bound and Cramer's rule.
// If A contains a zero row, then sets both bounds to zero.

static
void hadamard(ZZ& num_bound, ZZ& den_bound,
              const mat_ZZ& A, const vec_ZZ& b)
{
   long n = A.NumRows();

   if (n == 0) LogicError("internal error: hadamard with n = 0");

   ZZ b_len, min_A_len, prod, t1;

   InnerProduct(min_A_len, A[0], A[0]);

   prod = min_A_len;

   long i;
   for (i = 1; i < n; i++) {
      InnerProduct(t1, A[i], A[i]);
      if (t1 < min_A_len)
         min_A_len = t1;
      mul(prod, prod, t1);
   }

   if (min_A_len == 0) {
      num_bound = 0;
      den_bound = 0;
      return;
   }

   InnerProduct(b_len, b, b);

   div(t1, prod, min_A_len);
   mul(t1, t1, b_len);

   SqrRoot(num_bound, t1);
   SqrRoot(den_bound, prod);
}

static
void MixedMul(vec_ZZ& x, const vec_zz_p& a, const mat_ZZ& B)
{
   long n = B.NumRows();
   long l = B.NumCols();

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

   x.SetLength(l);

   long i, k;
   ZZ acc, tmp;

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

static
void SubDiv(vec_ZZ& e, const vec_ZZ& t, long p)
{
   long n = e.length();
   if (t.length() != n) LogicError("SubDiv: dimension mismatch");

   ZZ s;
   long i;

   for (i = 0; i < n; i++) {
      sub(s, e[i], t[i]);
      div(e[i], s, p);
   }
}

static
void MulAdd(vec_ZZ& x, const ZZ& prod, const vec_zz_p& h)
{
   long n = x.length();
   if (h.length() != n) LogicError("MulAdd: dimension mismatch");

   ZZ t;
   long i;

   for (i = 0; i < n; i++) {
      mul(t, prod, rep(h[i]));
      add(x[i], x[i], t);
   }
}

static
void double_MixedMul1(vec_ZZ& x, double *a, double **B, long n)
{
   long i, k;
   double acc;

   for (i = 0; i < n; i++) {
      double *bp = B[i];
      acc = 0;
      for (k = 0; k < n; k++) {
         acc += bp[k] * a[k];
      }
      conv(x[i], acc);
    }
}

static
void double_MixedMul2(vec_ZZ& x, double *a, double **B, long n, long limit)
{
   long i, k;
   double acc;
   ZZ acc1, t;
   long j;

   for (i = 0; i < n; i++) {
      double *bp = B[i];

      clear(acc1);
      acc = 0;
      j = 0;

      for (k = 0; k < n; k++) {
         acc += bp[k] * a[k];
         j++;
         if (j == limit) {
            conv(t, acc);
            add(acc1, acc1, t);
            acc = 0;
            j = 0;
         }
      }

      if (j > 0) {
         conv(t, acc);
         add(acc1, acc1, t);
      }

      x[i] = acc1;
    }
}

static
void long_MixedMul1(vec_ZZ& x, long *a, long **B, long n)
{
   long i, k;
   long acc;

   for (i = 0; i < n; i++) {
      long *bp = B[i];
      acc = 0;
      for (k = 0; k < n; k++) {
         acc += bp[k] * a[k];
      }
      conv(x[i], acc);
    }
}

static
void long_MixedMul2(vec_ZZ& x, long *a, long **B, long n, long limit)
{
   long i, k;
   long acc;
   ZZ acc1, t;
   long j;

   for (i = 0; i < n; i++) {
      long *bp = B[i];

      clear(acc1);
      acc = 0;
      j = 0;

      for (k = 0; k < n; k++) {
         acc += bp[k] * a[k];
         j++;
         if (j == limit) {
            conv(t, acc);
            add(acc1, acc1, t);
            acc = 0;
            j = 0;
         }
      }

      if (j > 0) {
         conv(t, acc);
         add(acc1, acc1, t);
      }

      x[i] = acc1;
    }
}

void solve1(ZZ& d_out, vec_ZZ& x_out, const mat_ZZ& A, const vec_ZZ& b)
{
   long n = A.NumRows();

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

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

   if (n == 0) {
      set(d_out);
      x_out.SetLength(0);
      return;
   }

   ZZ num_bound, den_bound;

   hadamard(num_bound, den_bound, A, b);

   if (den_bound == 0) {
      clear(d_out);
      return;
   }

   zz_pBak zbak;
   zbak.save();

   long i;
   long j;

   ZZ prod;
   prod = 1;

   mat_zz_p B;

   for (i = 0; ; i++) {
      zz_p::FFTInit(i);

      mat_zz_p AA, BB;
      zz_p dd;

      conv(AA, A);
      inv(dd, BB, AA);

      if (dd != 0) {
         transpose(B, BB);
         break;
      }

      mul(prod, prod, zz_p::modulus());

      if (prod > den_bound) {
         d_out = 0;
         return;
      }
   }

   long max_A_len = MaxBits(A);

   long use_double_mul1 = 0;
   long use_double_mul2 = 0;
   long double_limit = 0;

   if (max_A_len + NTL_SP_NBITS + NumBits(n) <= NTL_DOUBLE_PRECISION-1)
      use_double_mul1 = 1;

   if (!use_double_mul1 && max_A_len+NTL_SP_NBITS+2 <= NTL_DOUBLE_PRECISION-1) {
      use_double_mul2 = 1;
      double_limit = (1L << (NTL_DOUBLE_PRECISION-1-max_A_len-NTL_SP_NBITS));
   }

   long use_long_mul1 = 0;
   long use_long_mul2 = 0;
   long long_limit = 0;

   if (max_A_len + NTL_SP_NBITS + NumBits(n) <= NTL_BITS_PER_LONG-1)
      use_long_mul1 = 1;

   if (!use_long_mul1 && max_A_len+NTL_SP_NBITS+2 <= NTL_BITS_PER_LONG-1) {
      use_long_mul2 = 1;
      long_limit = (1L << (NTL_BITS_PER_LONG-1-max_A_len-NTL_SP_NBITS));
   }

   if (use_double_mul1 && use_long_mul1)
      use_long_mul1 = 0;
   else if (use_double_mul1 && use_long_mul2)
      use_long_mul2 = 0;
   else if (use_double_mul2 && use_long_mul1)
      use_double_mul2 = 0;
   else if (use_double_mul2 && use_long_mul2) {
      if (long_limit > double_limit)
         use_double_mul2 = 0;
      else
         use_long_mul2 = 0;
   }

   double **double_A=0;
   double *double_h=0;

   Unique2DArray<double> double_A_store;
   UniqueArray<double> double_h_store;

   if (use_double_mul1 || use_double_mul2) {
      double_h_store.SetLength(n);
      double_h = double_h_store.get();

      double_A_store.SetDims(n, n);
      double_A = double_A_store.get();

      for (i = 0; i < n; i++)
         for (j = 0; j < n; j++)
            double_A[j][i] = to_double(A[i][j]);
   }

   long **long_A=0;
   long *long_h=0;

   Unique2DArray<long> long_A_store;
   UniqueArray<long> long_h_store;

   if (use_long_mul1 || use_long_mul2) {
      long_h_store.SetLength(n);
      long_h = long_h_store.get();

      long_A_store.SetDims(n, n);
      long_A = long_A_store.get();

      for (i = 0; i < n; i++)
         for (j = 0; j < n; j++)
            long_A[j][i] = to_long(A[i][j]);
   }

   vec_ZZ x;
   x.SetLength(n);

   vec_zz_p h;
   h.SetLength(n);

   vec_ZZ e;
   e = b;

   vec_zz_p ee;

   vec_ZZ t;
   t.SetLength(n);

   prod = 1;

   ZZ bound1;
   mul(bound1, num_bound, den_bound);
   mul(bound1, bound1, 2);

   while (prod <= bound1) {
      conv(ee, e);

      mul(h, B, ee);

      if (use_double_mul1) {
         for (i = 0; i < n; i++)
            double_h[i] = to_double(rep(h[i]));

         double_MixedMul1(t, double_h, double_A, n);
      }
      else if (use_double_mul2) {
         for (i = 0; i < n; i++)
            double_h[i] = to_double(rep(h[i]));

         double_MixedMul2(t, double_h, double_A, n, double_limit);
      }
      else if (use_long_mul1) {
         for (i = 0; i < n; i++)
            long_h[i] = to_long(rep(h[i]));

         long_MixedMul1(t, long_h, long_A, n);
      }
      else if (use_long_mul2) {
         for (i = 0; i < n; i++)
            long_h[i] = to_long(rep(h[i]));

         long_MixedMul2(t, long_h, long_A, n, long_limit);
      }
      else
         MixedMul(t, h, A); // t = h*A

      SubDiv(e, t, zz_p::modulus()); // e = (e-t)/p
      MulAdd(x, prod, h);  // x = x + prod*h

      mul(prod, prod, zz_p::modulus());
   }

   vec_ZZ num, denom;
   ZZ d, d_mod_prod, tmp1;

   num.SetLength(n);
   denom.SetLength(n);

   d = 1;
   d_mod_prod = 1;

   for (i = 0; i < n; i++) {
      rem(x[i], x[i], prod);
      MulMod(x[i], x[i], d_mod_prod, prod);

      if (!ReconstructRational(num[i], denom[i], x[i], prod,
           num_bound, den_bound))
          LogicError("solve1 internal error: rat recon failed!");

      mul(d, d, denom[i]);

      if (i != n-1) {
         if (denom[i] != 1) {
            div(den_bound, den_bound, denom[i]);
            mul(bound1, num_bound, den_bound);
            mul(bound1, bound1, 2);

            div(tmp1, prod, zz_p::modulus());
            while (tmp1 > bound1) {
               prod = tmp1;
               div(tmp1, prod, zz_p::modulus());
            }

            rem(tmp1, denom[i], prod);
            rem(d_mod_prod, d_mod_prod, prod);
            MulMod(d_mod_prod, d_mod_prod, tmp1, prod);
         }
      }
   }

   tmp1 = 1;
   for (i = n-1; i >= 0; i--) {
      mul(num[i], num[i], tmp1);
      mul(tmp1, tmp1, denom[i]);
   }

   x_out.SetLength(n);

   for (i = 0; i < n; i++) {
      x_out[i] = num[i];
   }

   d_out = d;
}

NTL_END_IMPL


#include <mex.h>
#include <Eigen/Eigen>
#include "localize.h"

// Takes an mxArray and returns its contents as an eigen matrix by reference
void input_matrix(const mxArray* p, Eigen::MatrixXd& mat) {
    int m = mxGetM(p);
    int n = mxGetN(p);

    mat.resize(m,n);

    double *data = mxGetPr(p);

    for(int j = 0; j < n; j++) {
        for(int i = 0; i < m; i++) {
            mat(i,j) = data[m*j + i];
        }
    }
}

// Should be called as:
// [P, inliers] = CameraPoseRANSAC_Mex(X,x,tol,nIters), where X is a 3xN matrix containing the 3D points, and
// x is a 3xN matrix containing the image points as unit vectors and tol is the largest allowed
// angular deviation (in radians) for a correspondence to be considered an inlier, and nIters is the max number of iterations.
// The output P is the calculated camera matrix as well as an integer designating the number of inliers.
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    // Validate the input
    if(nrhs != 4)
        mexErrMsgTxt("Invalid input! The function takes three inputs: \n\t3d structure X as a 3xN matrix, \n\ta 3xN matrix representing image points as unit vectors, \n\tthe maximum allowed angle deviation for a correspondence to be considered an inlier,\n\tnumber of RANSAC iterations.");
    if(nlhs != 2)
        mexErrMsgTxt("Invalid output! The function returns two outputs: P representing the triangulated camera matrix and an integer designating the number of inliers for P. ");

    Eigen::MatrixXd x;
    Eigen::MatrixXd X;
    double tol = mxGetScalar(prhs[2]);
    int nIter = int(mxGetScalar(prhs[3]));

    input_matrix(prhs[0],X);
    input_matrix(prhs[1],x);

    if(x.rows() != 3 || X.rows() != 3 || x.cols() != X.cols())
        mexErrMsgTxt("Input matrices must both have three rows and the same number of columns!");

    // Inputs and outputs are ok! Calculate camera pose!
    int numInliers = 0;
    Eigen::Matrix<double,3,4> P = Eigen::MatrixXd::Zero(3,4);
    bool success = cameraPoseRANSAC(x,X,nIter,tol,P,numInliers);
    if (!success)
    {
        numInliers = 0;
    }

    // Create output
    plhs[0] = mxCreateDoubleMatrix(P.rows(),4,mxREAL);
    double* outputMatrix = mxGetPr(plhs[0]);
    int index = 0;
    for (int j = 0; j < P.cols(); j++) {
        for (int i = 0; i < P.rows(); i++) {
            outputMatrix[index] = P(i,j);
            index++;
        }
    }

    plhs[1] = mxCreateDoubleMatrix(1,1,mxREAL);
    double* output = mxGetPr(plhs[1]);
    output[0] = static_cast<double>(numInliers);
}


// Copyright 2018 Hans Dembinski
//
// 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_HISTOGRAM_ACCUMULATORS_WEIGHTED_MEAN_HPP
#define BOOST_HISTOGRAM_ACCUMULATORS_WEIGHTED_MEAN_HPP

#include <boost/histogram/fwd.hpp>
#include <type_traits>

namespace boost {
namespace histogram {
namespace accumulators {

/**
  Calculates mean and variance of weighted sample.

  Uses West's incremental algorithm to improve numerical stability
  of mean and variance computation.
*/
template <typename RealType>
class weighted_mean {
public:
  weighted_mean() = default;
  weighted_mean(const RealType& wsum, const RealType& wsum2, const RealType& mean,
                const RealType& variance)
      : sum_of_weights_(wsum)
      , sum_of_weights_squared_(wsum2)
      , weighted_mean_(mean)
      , sum_of_weighted_deltas_squared_(
            variance * (sum_of_weights_ - sum_of_weights_squared_ / sum_of_weights_)) {}

  void operator()(const RealType& x) { operator()(1, x); }

  void operator()(const RealType& w, const RealType& x) {
    sum_of_weights_ += w;
    sum_of_weights_squared_ += w * w;
    const auto delta = x - weighted_mean_;
    weighted_mean_ += w * delta / sum_of_weights_;
    sum_of_weighted_deltas_squared_ += w * delta * (x - weighted_mean_);
  }

  template <typename T>
  weighted_mean& operator+=(const weighted_mean<T>& rhs) {
    const auto tmp = weighted_mean_ * sum_of_weights_ +
                     static_cast<RealType>(rhs.weighted_mean_ * rhs.sum_of_weights_);
    sum_of_weights_ += static_cast<RealType>(rhs.sum_of_weights_);
    sum_of_weights_squared_ += static_cast<RealType>(rhs.sum_of_weights_squared_);
    weighted_mean_ = tmp / sum_of_weights_;
    sum_of_weighted_deltas_squared_ +=
        static_cast<RealType>(rhs.sum_of_weighted_deltas_squared_);
    return *this;
  }

  weighted_mean& operator*=(const RealType& s) {
    weighted_mean_ *= s;
    sum_of_weighted_deltas_squared_ *= s * s;
    return *this;
  }

  template <typename T>
  bool operator==(const weighted_mean<T>& rhs) const noexcept {
    return sum_of_weights_ == rhs.sum_of_weights_ &&
           sum_of_weights_squared_ == rhs.sum_of_weights_squared_ &&
           weighted_mean_ == rhs.weighted_mean_ &&
           sum_of_weighted_deltas_squared_ == rhs.sum_of_weighted_deltas_squared_;
  }

  template <typename T>
  bool operator!=(const T& rhs) const noexcept {
    return !operator==(rhs);
  }

  const RealType& sum_of_weights() const noexcept { return sum_of_weights_; }
  const RealType& value() const noexcept { return weighted_mean_; }
  RealType variance() const {
    return sum_of_weighted_deltas_squared_ /
           (sum_of_weights_ - sum_of_weights_squared_ / sum_of_weights_);
  }

  template <class Archive>
  void serialize(Archive&, unsigned /* version */);

private:
  RealType sum_of_weights_ = RealType(), sum_of_weights_squared_ = RealType(),
           weighted_mean_ = RealType(), sum_of_weighted_deltas_squared_ = RealType();
};

} // namespace accumulators
} // namespace histogram
} // namespace boost

#ifndef BOOST_HISTOGRAM_DOXYGEN_INVOKED
namespace std {
template <class T, class U>
/// Specialization for boost::histogram::accumulators::weighted_mean.
struct common_type<boost::histogram::accumulators::weighted_mean<T>,
                   boost::histogram::accumulators::weighted_mean<U>> {
  using type = boost::histogram::accumulators::weighted_mean<common_type_t<T, U>>;
};
} // namespace std
#endif

#endif


// Copyright (c) 2013-2016. This code was produced by the
// Australian Centre for Field Robotics, The University of Sydney under
// the Future Flight Planning project, University Reference 13996, contract
// NSW-CPS-2011-015264, Work Orders 5, 7 and 8. The intellectual property
// ownership is as set out in these contracts, as registered with
// Commercial Development and Industry Partnerships.

#include <iostream>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <comma/application/command_line_options.h>
#include <comma/csv/stream.h>
#include <comma/visiting/traits.h>
#include "../../../math/range_bearing_elevation.h"
#include "../coordinates.h"
#include "../sample.h"
#include "../traits.h"

static const std::string app_name = "sphere-calc";

static void usage( bool verbose )
{
    std::cerr << std::endl;
    std::cerr << "generates a sample ever the whole sphere" << std::endl;
    std::cerr << std::endl;
    std::cerr << "usage: sphere-calc <options> > sample.csv" << std::endl;
    std::cerr << std::endl;
    std::cerr << "options" << std::endl;
    std::cerr << "    -h|--help                       Show this help (-v|--verbose to show csv options)" << std::endl;
    std::cerr << std::endl;
    std::cerr << "sample type options" << std::endl;
    std::cerr << "    --random                        Random uniform sample" << std::endl;
    std::cerr << "    --regular                       Regular grid sample" << std::endl;
    std::cerr << "    --regular-uniform,--uniform     Regular, but pretty uniform" << std::endl;
    std::cerr << std::endl;
    std::cerr << "random seed options" << std::endl;
    std::cerr << "    -s|--seed=<random-seed>         seed for the random sample" << std::endl;
    std::cerr << "    -t|--seed-time                  use current time as random number seed" << std::endl;
    std::cerr << std::endl;
    std::cerr << "sample options" << std::endl;
    std::cerr << "    --begin,--from=<latitude,longitude>; default: -90, -180" << std::endl;
    std::cerr << "    --end,--to=<latitude,longitude>; end not included, default: 90.00000001, 180" << std::endl;
    std::cerr << "    --resolution,-r=<value>         Sample resolution in degrees" << std::endl;
    if( verbose ) { std::cerr << std::endl << "csv options" << std::endl << comma::csv::options::usage() << std::endl; }
    std::cerr << std::endl;
    exit( 0 );
}

template< typename T > T get_option( comma::command_line_options &options, const std::string option_name, const T &default_val )
{
    if( !options.exists( option_name ) ) { return default_val; }
    T result = default_val;
    result = comma::csv::ascii< T >().get( options.value< std::string >( option_name ) );
    return result;
}

using namespace snark;

int main( int ac, char** av )
{
    try
    {
        using snark::spherical::coordinates;

        comma::command_line_options options( ac, av, usage );
        if( options.exists( "--verbose,-v" ) ) { std::cerr << "sphere-calc: called as: " << options.string() << std::endl; }
        double resolution = options.value< double >( "--resolution,-r" ) * M_PI / 180;
        comma::csv::output_stream< coordinates > ostream( std::cout, comma::csv::options( options ) );
        options.assert_mutually_exclusive( "--random,--regular" );
        bool regular = options.exists( "--regular" );
        bool random = options.exists( "--random" );
        bool regular_uniform = options.exists( "--regular-uniform,--uniform" );
        if( !regular && !random && !regular_uniform ) { std::cerr << "sphere-calc: expected sample type (--random, --regular, or --uniform)" << std::endl; return 1; }
        coordinates begin = get_option< coordinates >( options, "--begin", coordinates( -M_PI / 2, -M_PI ) );
        static const double epsilon = 0.00000001;
        coordinates end = get_option< coordinates >( options, "--end", coordinates( M_PI / 2 + epsilon, M_PI - epsilon ) );
        if( begin.latitude < -M_PI / 2 ) { begin.latitude = -M_PI / 2; }
        if( begin.longitude < -M_PI ) { begin.longitude = -M_PI; }
        if( end.latitude > M_PI / 2 ) { end.latitude = M_PI / 2; }
        if( end.longitude > M_PI ) { end.longitude = M_PI; }
        if( regular )
        {
            for( coordinates c = begin; comma::math::less( c.latitude, end.latitude ); c.latitude += resolution )
            {
                for( c.longitude = begin.longitude; comma::math::less( c.longitude, end.longitude ); c.longitude += resolution )
                {
                    ostream.write( c );
                }
            }
        }
        else if( random || regular_uniform )
        {
            options.assert_mutually_exclusive( "--seed,-s,--seed-time,-t" );
            boost::optional< unsigned long > seed = options.optional< unsigned long >( "--seed,-s" );
            if( options.exists( "--seed-time,-t" ) ) { seed = static_cast< unsigned long >( std::time( 0 ) ); }
            boost::mt19937 generator;
            if( seed ) { generator.seed( *seed ); }
            boost::uniform_real< double > distribution( 0, 1 );
            boost::variate_generator< boost::mt19937&, boost::uniform_real< double > > r( generator, distribution );
            for( coordinates c = begin; comma::math::less( c.latitude, end.latitude ); c.latitude += resolution )
            {
                double radius = std::abs( std::cos( c.latitude ) );
                if( radius == 0 ) { continue; }
                double step = resolution / radius;
                double offset = random ? ( r() * 2 - 1 ) * step : 0.0;
                for( c.longitude = begin.longitude + offset; comma::math::less( c.longitude, end.longitude + offset ); c.longitude += step )
                {
                    ostream.write( regular_uniform ? c : pretty_uniform_sample( c, resolution / 2 ) );
                }
            }
        }
        return 0;
    }
    catch( std::exception& ex ) { std::cerr << "sphere-calc: " << ex.what() << std::endl; }
    catch( ... ) { std::cerr << "sphere-calc: unknown exception" << std::endl; }
    return 1;
}


/*
Copyright (c) 2015 Jeff Epler

This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any damages
arising from the use of this software.

Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely, subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not
   claim that you wrote the original software. If you use this software
   in a product, an acknowledgement in the product documentation would be
   appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be
   misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#ifndef DASHING_H
#define DASHING_H

#include <cmath>
#include <cassert>
#include <vector>
#include <string>
#include <fstream>
#include <stdexcept>
#include <boost/algorithm/string/replace.hpp>
#include <boost/algorithm/string/trim.hpp>

namespace dashing
{

struct PSMatrix {
    double a, b, c, d, e, f;

    PSMatrix inverse() const;
    double determinant() const { return a * d - b * c; }
};

PSMatrix Translation(double x, double y);
PSMatrix Rotation(double theta);
PSMatrix XSkew(double xk);
PSMatrix YScale(double ys);

struct Point { double x, y; };
inline Point operator*(const Point &p, const PSMatrix &m) {
    return Point{ p.x*m.a + p.y*m.c + m.e,
                  p.x*m.b + p.y*m.d + m.f };
}
inline Point operator*(const Point &p, double d)
{
    return Point{ p.x * d, p.y * d };
}
inline Point operator*(double d, const Point &p)
{
    return Point{ p.x * d, p.y * d };
}
inline Point operator+(const Point &p, const Point &q)
{
    return Point{ p.x + q.x, p.y * q.y };
}

PSMatrix operator*(const PSMatrix &m1, const PSMatrix m2);

struct Dash {
    PSMatrix tr, tf;
    std::vector<double> dash, sum;

    Dash(double th, double x0, double y0, double dx, double dy,
            const std::vector<double>::const_iterator dbegin,
            const std::vector<double>::const_iterator dend);

    static Dash FromString(const std::string &line, double scale);
};

struct Segment { Point p, q; bool swapped; };
struct Intersection { double u; bool positive; };
inline bool operator<(const Intersection &a, const Intersection &b)
{
    return a.u < b.u;
}

// "sort" a segment so that its first component has the lower y-value
inline void ysort(Segment &s) {
    if(s.p.y < s.q.y) return;
    s.swapped = ! s.swapped;
    std::swap(s.p, s.q);
}

inline double intceil(double x) { return int(ceil(x)); }
inline double intfloor(double x) { return int(floor(x)); }

inline double pythonmod(double a, double b) {
    auto r = a - floor(a / b) * b;
    if(r == b) return 0;
    return r;
}

inline size_t utoidx(const Dash &d, double u, double &o) {
    u = pythonmod(u, d.sum.back());
    for(size_t i = 1; i != d.sum.size(); i++) {
        if(u < d.sum[i]) { o = u - d.sum[i-1]; return i-1; }
    }
    abort(); // should be unreachable
}

template<class Cb>
void uvdraw(const Dash &pattern, double v, double u1, double u2, Cb cb) {
    if(pattern.dash.empty()) { cb(v, u1, u2); return;  }
    double o;
    auto i = utoidx(pattern, u1, o);
    const auto &pi = pattern.dash[i];
    if(pi >= 0) { cb(v, u1, std::min(u2, u1+pi-o)); u1 += pi-o; }
    else { u1 -= pi+o; }
    i = i + 1;
    if(i == pattern.dash.size()) i = 0;
    for(auto u = u1; u < u2;) {
        const auto &pi = pattern.dash[i];
        if(pi >= 0) { cb(v, u, std::min(u2, u+pi)); u += pi; }
        else { u -= pi; }
        i = i + 1;
        if(i == pattern.dash.size()) i = 0;
    }
}

template<class Cb, class Wr>
void uvspans(const Dash &pattern, std::vector<Segment> && segments, Cb cb, std::vector<Intersection> &uu, Wr wr) {
    if(segments.empty()) return; // no segments

    for(auto &s : segments) ysort(s);
    std::sort(segments.begin(), segments.end(),
        [](const Segment &a, const Segment &b) {
            return a.p.y < b.p.y; // sort in increasing p.y
        });

    // we want to maintain the heap condition in such a way that we can always
    // quickly pop items that our span has moved past.
    // C++ heaps are max-heaps, so we need a decreasing sort.
    auto heapcmp = [](const Segment &a, const Segment &b) {
            return b.q.y < a.q.y; // sort in decreasing q.y;
        };

    auto segments_begin = segments.begin();
    auto heap_begin = segments.begin(), heap_end = segments.begin();

    auto vstart = intfloor(segments.front().p.y);
    auto vend = intceil(std::max_element(segments.begin(), segments.end(),
                [](const Segment &a, const Segment &b) {
                        return a.q.y < b.q.y; // sort in increasing q.y;
                })->q.y);

    // sweep-line algorithm to intersects spans with segments
    // "active" holds segments that may intersect with this span;
    // when v moves below an active segment, drop it from the active heap.
    // when v moves into a remaining segment, move it from segments to active.
    for(auto v = vstart; v != vend; v++) {
        uu.clear();

        while(heap_begin != heap_end && heap_begin->q.y < v)
        {
            std::pop_heap(heap_begin, heap_end, heapcmp);
            heap_end --;
        }
        while(segments_begin != segments.end() && segments_begin->p.y < v) {
            const auto &s = *segments_begin;
            if(s.q.y >= v) {
                *heap_end++ = s;
                std::push_heap(heap_begin, heap_end, heapcmp);
            }
            segments_begin ++;
        }

        for(const auto &s : boost::make_iterator_range(heap_begin, heap_end)) {
            auto du = s.q.x - s.p.x;
            auto dv = s.q.y - s.p.y;
            assert(dv);
            if(dv) uu.push_back(
                    Intersection{s.p.x + du * (v - s.p.y) / dv,s.swapped});
        }
        std::sort(uu.begin(), uu.end());
        int winding = 0;
        double old_u = -std::numeric_limits<double>::infinity();
        for(const auto &isect : uu) {
            if(wr(winding)) uvdraw(pattern, v, old_u, isect.u, cb);
            winding += 2*isect.positive - 1;
            old_u = isect.u;
        }
    }
}

struct HatchPattern {
    std::vector<Dash> d;
    static HatchPattern FromFile(std::istream &fi, double scale) {
        HatchPattern result;

        std::string line;
        while(getline(fi, line)) {
            auto i = line.find(";");
            if(i != line.npos) line.erase(i, line.npos);
            boost::algorithm::trim(line);
            if(line.empty()) continue;
            if(line[0] == '*') continue;
            result.d.push_back(Dash::FromString(line, scale));
        }
        return result;
    }

    static HatchPattern FromFile(const char *filename, double scale) {
        std::ifstream fi(filename);
        return FromFile(fi, scale);
    }
};

template<class It, class Cb, class Wr>
void xyhatch(const Dash &pattern, It start, It end, Cb cb, std::vector<Segment> &uvsegments, std::vector<Intersection> &uu, Wr wr) {
    uvsegments.clear();
    bool swapped = pattern.tf.determinant() < 0;
    std::transform(start, end, std::back_inserter(uvsegments),
        [&](const Segment &s)
        { return Segment{s.p * pattern.tf, s.q * pattern.tf, swapped != s.swapped };
    });
    uvspans(pattern, std::move(uvsegments), [&](double v, double u1, double u2) {
        Point p{u1, v}, q{u2, v};
        Segment xy{ p * pattern.tr, q * pattern.tr, false };
        cb(xy);
    }, uu, wr);
}

template<class It, class Cb, class Wr>
void xyhatch(const HatchPattern &pattern, It start, It end, Cb cb, Wr wr) {
    std::vector<Segment> uvsegments;
    uvsegments.reserve(end-start);
    std::vector<Intersection> uu;
    uu.reserve(8);
    for(const auto &i : pattern.d) xyhatch(i, start, end, cb, uvsegments, uu, wr);
}

template<class C, class Cb, class Wr>
void xyhatch(const HatchPattern &pattern, const C &c, Cb cb, Wr wr) {
    xyhatch(pattern, c.begin(), c.end(), cb, wr);
}

}
#endif


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

// Copyright Paul A. Bristow 2015 - 2016.
// Copyright Christopher Kormanyos 2015 - 2016.

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

// This file also includes Doxygen-style documentation about the function of the code.
// See http://www.doxygen.org for details.

//! \file

// Below are snippets of code that can be included into a Quickbook file.

#include <exception>
#include <iomanip>
#include <iostream>
#include <limits>
#include <typeinfo>

//[fixed_point_include_1
#include <boost/fixed_point/fixed_point.hpp>
//] [/fixed_point_include_1]

//[fixed_point_typedef_1
typedef boost::fixed_point::negatable<15, -16> fixed_point_type;
//] [/fixed_point_typedef_1]

int main()
{
  try
  {
    std::cout.precision(std::numeric_limits<fixed_point_type>::digits10 + 1);

//[show_numeric_limits_1

    std::cout << "Numeric_limits for type"                                                 << std::endl
              << typeid(fixed_point_type).name()                                           << std::endl
              << "digits10     = " <<  std::numeric_limits<fixed_point_type>::digits10     << std::endl
              << "max_digits10 = " <<  std::numeric_limits<fixed_point_type>::max_digits10 << std::endl
              << "radix        = " <<  std::numeric_limits<fixed_point_type>::radix        << std::endl
              << "epsilon      = " <<  std::numeric_limits<fixed_point_type>::epsilon()    << std::endl
              << "max          = " << (std::numeric_limits<fixed_point_type>::max)()       << std::endl
              << "min          = " << (std::numeric_limits<fixed_point_type>::min)()       << std::endl
              << "lowest       = " <<  std::numeric_limits<fixed_point_type>::lowest()     << std::endl
              ;

    if(std::numeric_limits<fixed_point_type>::has_infinity)
    {
      std::cout << "infinity = " << std::numeric_limits<fixed_point_type>::infinity() << std::endl;
    }
    else
    {
      std::cout << "Type does not have an infinity" << std::endl;
    }

    if (std::numeric_limits<fixed_point_type>::has_quiet_NaN)
    {
      std::cout << "NaN = " << std::numeric_limits<fixed_point_type>::quiet_NaN() << std::endl;
    }
    else
    {
      std::cout << "Type does not have a NaN" << std::endl;
    }
//] [/show_numeric_limits_1]

    std::cout.setf(std::ios::boolalpha | std::ios::showpoint); // Show any trailing zeros.
    std::cout << std::endl;

//[fixed_example_1

    fixed_point_type x = fixed_point_type(123) /   100;
    fixed_point_type y = fixed_point_type(456) / 10000;

    // Show all the significant digits for this particular floating-point type.
    std::cout.precision(std::numeric_limits<fixed_point_type>::digits10);

    std::cout << "x = fixed_point_type(123) /   100 = "
              << x // 1.22999573 is the nearest representation of decimal digit string 1.23.
              << std::endl;

    std::cout << "y = fixed_point_type(456) / 10000 = "
              << y // 0.0455932617 is the nearest representation of decimal digit string 0.0456
              << std::endl;

    x = -x / 2; // Do some trivial arithmetic.

    std::cout <<"x = -x / 2 = "
              << x // -0.614990234  is the nearest representation of decimal digit string -0.615
              << std::endl;

//] [/fixed_example_1]

//[fixed_example_functions

    int exponential;

    x = frexp(x, &exponential);

    std::cout << "x = frexp(x, &exponential) = "
              << x // 0.614990234
              << " exponential = "
              << exponential
              << std::endl; // 0

    exponential++; // double the value.

    std::cout << "double the value" << std::endl;

    x = ldexp(x, exponential);

    // Show the fraction and exponent parts after changing the exponent.
    std::cout << "x = frexp(x, &exponential) = "
              << frexp(x, &exponential) // -0.614990234
              << " exponential = "
              << exponential
              << std::endl; // 1

    // Other C numeric math functions (cmath) are available, of course, for example:
    std::cout << "ldexp(x, exponential); = "
              << x
              << std::endl; // -1.22998047

    std::cout << "abs  (x)       = "
              <<  abs(x)
              << std::endl; // 1.22998047

    std::cout << "fabs (x)       = " << fabs(x) << std::endl; // 1.22998047

    std::cout << "sqrt (fabs(x)) = "
              <<  sqrt(fabs(x))
              << std::endl; //  = 0.

    std::cout << "sqrt (y)       = "
              <<  sqrt(y)
              << std::endl; //  = 0.213516235

//] [/fixed_example_functions]
  }
  catch(const std::exception& ex)
  {
    std::cout << ex.what() << std::endl;
  }
}

/*
//[numeric_limits_output_1
Numeric_limits for type
class boost::fixed_point::negatable<15,-16,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined>
digits10     = 9
max_digits10 = 11
radix        = 2
epsilon      = 3.051757813e-005
max          = 32767.99998
min          = 1.525878906e-005
lowest       = -32768
Type does not have an infinity
Type does not have a NaN

x = fixed_point_type(123) /   100 = 1.22999573
y = fixed_point_type(456) / 10000 = 0.0455932617
x = -x / 2 = -0.614990234
//] [/numeric_limits_output_1]

//[fixed_example_functions_output
x = frexp(x, &exponential) = -0.614990234 exponential = 0
double the value
x = frexp(x, &exponential) = -0.614990234 exponential = 1
ldexp(x, exponential); = -1.22998047
abs  (x)       = 1.22998047
fabs (x)       = 1.22998047
sqrt (fabs(x)) = 1.10902405
sqrt (y)       = 0.213516235
//] [/fixed_example_functions_output]

*/


#include "consts.hpp"
#include "gamma.hpp"

#include <math.h>
#include <limits>
#include <boost/numeric/conversion/cast.hpp>

namespace cephes {

constexpr double ASYMP_FACTOR = 1e6;

double lbeta_asymp(double a, double b, int *sgn);
double lbeta_negint(int a, double b);
double beta_negint(int a, double b);

double beta(double a, double b)
{
    double y;
    int sign = 1;

    if (a <= 0.0) {
        if (a == std::floor(a)) {
            if (a == boost::numeric_cast<int>(a)) {
                return beta_negint(boost::numeric_cast<int>(a), b);
            }
            else {
                goto overflow;
            }
        }
    }

    if (b <= 0.0) {
        if (b == std::floor(b)) {
            if (b == boost::numeric_cast<int>(b)) {
                return beta_negint(boost::numeric_cast<int>(b), a);
            }
            else {
                goto overflow;
            }
        }
    }

    if (std::fabs(a) < std::fabs(b)) {
        y = a; a = b; b = y;
    }

    if (std::fabs(a) > ASYMP_FACTOR * std::fabs(b) && a > ASYMP_FACTOR) {
        /* Avoid loss of precision in lgam(a + b) - lgam(a) */
        y = lbeta_asymp(a, b, &sign);
        return sign * std::exp(y);
    }

    y = a + b;
    if (std::fabs(y) > MAXGAM || std::fabs(a) > MAXGAM || std::fabs(b) > MAXGAM) {
	int sgngam;
	y = lgam_sgn(y, &sgngam);
	sign *= sgngam;		/* keep track of the sign */
	y = lgam_sgn(b, &sgngam) - y;
	sign *= sgngam;
	y = lgam_sgn(a, &sgngam) + y;
	sign *= sgngam;
	if (y > MAXLOG) {
	    goto overflow;
	}
	return (sign * std::exp(y));
    }

    y = Gamma(y);
    a = Gamma(a);
    b = Gamma(b);
    if (y == 0.0)
        goto overflow;

    if (std::fabs(std::fabs(a) - std::fabs(y)) > std::fabs(std::fabs(b) - std::fabs(y))) {
        y = b / y;
        y *= a;
    }
    else {
        y = a / y;
        y *= b;
    }

    return (y);

overflow:
    return (sign * std::numeric_limits<double>::infinity());
}

/* Natural log of |beta|. */

double lbeta(double a, double b)
{
    double y;
    int sign;

    sign = 1;

    if (a <= 0.0) {
        if (a == std::floor(a)) {
            if (a == boost::numeric_cast<int>(a)) {
                return lbeta_negint(boost::numeric_cast<int>(a), b);
            }
            else {
                goto over;
            }
        }
    }

    if (b <= 0.0) {
        if (b == std::floor(b)) {
            if (b == boost::numeric_cast<int>(b)) {
                return lbeta_negint(boost::numeric_cast<int>(b), a);
            }
            else {
                goto over;
            }
        }
    }

    if (std::fabs(a) < std::fabs(b)) {
        y = a; a = b; b = y;
    }

    if (std::fabs(a) > ASYMP_FACTOR * std::fabs(b) && a > ASYMP_FACTOR) {
        /* Avoid loss of precision in lgam(a + b) - lgam(a) */
        y = lbeta_asymp(a, b, &sign);
        return y;
    }

    y = a + b;
    if (std::fabs(y) > MAXGAM || std::fabs(a) > MAXGAM || std::fabs(b) > MAXGAM) {
	int sgngam;
	y = lgam_sgn(y, &sgngam);
	sign *= sgngam;		/* keep track of the sign */
	y = lgam_sgn(b, &sgngam) - y;
	sign *= sgngam;
	y = lgam_sgn(a, &sgngam) + y;
	sign *= sgngam;
	return (y);
    }

    y = Gamma(y);
    a = Gamma(a);
    b = Gamma(b);
    if (y == 0.0) {
      over:
        return (sign * std::numeric_limits<double>::infinity());
    }

    if (std::fabs(std::fabs(a) - std::fabs(y)) > std::fabs(std::fabs(b) - std::fabs(y))) {
        y = b / y;
        y *= a;
    }
    else {
        y = a / y;
        y *= b;
    }

    if (y < 0) {
	y = -y;
    }

    return std::log(y);
}

/*
 * Asymptotic expansion for  ln(|B(a, b)|) for a > ASYMP_FACTOR*max(|b|, 1).
 */
double lbeta_asymp(double a, double b, int *sgn)
{
    double r = lgam_sgn(b, sgn);
    r -= b * std::log(a);

    r += b*(1-b)/(2*a);
    r += b*(1-b)*(1-2*b)/(12*a*a);
    r += - b*b*(1-b)*(1-b)/(12*a*a*a);

    return r;
}

/*
 * Special case for a negative integer argument
 */

double beta_negint(int a, double b)
{
    int sgn;
    if (b == boost::numeric_cast<int>(b) && 1 - a - b > 0) {
        sgn = (boost::numeric_cast<int>(b) % 2 == 0) ? 1 : -1;
        return sgn * beta(1 - a - b, b);
    }
    else {
        return std::numeric_limits<double>::infinity();
    }
}

double lbeta_negint(int a, double b)
{
    double r;
    if (b == boost::numeric_cast<int>(b) && 1 - a - b > 0) {
        r = lbeta(1 - a - b, b);
        return r;
    }
    else {
        return std::numeric_limits<double>::infinity();
    }
}

}


#include "joint_tracker/RevoluteJointFilter.h"

#include "omip_common/OMIPUtils.h"

#include <Eigen/Geometry>

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

#include "geometry_msgs/PoseWithCovarianceStamped.h"

#include "std_msgs/Float64MultiArray.h"

using namespace omip;
using namespace MatrixWrapper;
using namespace BFL;

// Dimensions of the system state of the filter that tracks a revolute joint: orientation (2 values), position (3 values), joint variable, and joint velocity
#define REV_STATE_DIM 7
#define MEAS_DIM 6

/**
 * EKF internal state:
 *
 * x(1) =  RevJointOrientation_phi
 * x(2) =  RevJointOrientation_theta
 * RevJointOrientation is represented in spherical coords
 * x(3) = RevJointPosition_x
 * x(4) = RevJointPosition_y
 * x(5) = RevJointPosition_z
 * x(6) = RevJointVariable
 * x(7) = RevJointVariable_d
 *
 * EKF measurement:
 *
 * m(1) = TwistLinearPart_x
 * m(2) = TwistLinearPart_y
 * m(3) = TwistLinearPart_z
 * m(4) = TwistAngularPart_x
 * m(5) = TwistAngularPart_y
 * m(6) = TwistAngularPart_z
 */

using namespace omip;

RevoluteJointFilter::RevoluteJointFilter() :
    JointFilter(),
    _sys_PDF(NULL),
    _sys_MODEL(NULL),
    _meas_PDF(NULL),
    _meas_MODEL(NULL),
    _ekf(NULL),
    _sigma_delta_meas_uncertainty_angular(-1),
    _accumulated_rotation(0.0)
{
}

void RevoluteJointFilter::setCovarianceDeltaMeasurementAngular(double sigma_delta_meas_uncertainty_angular)
{
    this->_sigma_delta_meas_uncertainty_angular = sigma_delta_meas_uncertainty_angular;
}

void RevoluteJointFilter::initialize()
{
    JointFilter::initialize();
    this->_joint_orientation = Eigen::Vector3d( this->_current_delta_pose_in_rrbf.rx(),
                                                    this->_current_delta_pose_in_rrbf.ry(),
                                                    this->_current_delta_pose_in_rrbf.rz());
    this->_joint_state = this->_joint_orientation.norm();
    //this->_joint_velocity = this->_joint_state/(this->_loop_period_ns/1e9);
    // Setting it to 0 is better.
    // The best approximation would be to (this->_joint_state/num_steps_to_joint_state)/(this->_loop_period_ns/1e9)
    // but we don't know how many steps passed since we estimated the first time the joint variable
    this->_joint_velocity = 0.0;

    this->_joint_states_all.push_back(this->_joint_state);

    Eigen::Vector3d linear_part = Eigen::Vector3d( this->_current_delta_pose_in_rrbf.vx(),
                                                   this->_current_delta_pose_in_rrbf.vy(),
                                                   this->_current_delta_pose_in_rrbf.vz());
// Alternative
//    Eigen::Matrix4d ht;
//    Twist2TransformMatrix(_current_delta_pose_in_rrbf, ht);

//    this->_joint_position = ht.block<3,1>(0,3)
//            - ht.block<3,3>(0,0)*ht.block<3,1>(0,3);

    this->_joint_position = (1.0 / (pow(this->_joint_state, 2))) * this->_joint_orientation.cross(linear_part);
    this->_joint_orientation.normalize();

    this->_initializeSystemModel();
    this->_initializeMeasurementModel();
    this->_initializeEKF();
}

void RevoluteJointFilter::setMinRotationRevolute(const double& value)
{
    _rev_min_rot_for_ee = value;
}

void RevoluteJointFilter::setMaxRadiusDistanceRevolute(const double& value)
{
    _rev_max_joint_distance_for_ee = value;
}

void RevoluteJointFilter::_initializeSystemModel()
{
    // create SYSTEM MODEL
    Matrix A(REV_STATE_DIM, REV_STATE_DIM);
    A = 0.;
    for (unsigned int i = 1; i <= REV_STATE_DIM; i++)
    {
        A(i, i) = 1.0;
    }
    A(6, 7) = this->_loop_period_ns/1e9; //Adding the velocity to the position of the joint variable

    ColumnVector sys_noise_MU(REV_STATE_DIM);
    sys_noise_MU = 0;

    SymmetricMatrix sys_noise_COV(REV_STATE_DIM);
    sys_noise_COV = 0.0;
    sys_noise_COV(1, 1) = this->_sigma_sys_noise_phi* (std::pow((this->_loop_period_ns/1e9),3) / 3.0); // PHI
    sys_noise_COV(2, 2) = this->_sigma_sys_noise_theta* (std::pow((this->_loop_period_ns/1e9),3) / 3.0); // THETA
    sys_noise_COV(3, 3) = this->_sigma_sys_noise_px* (std::pow((this->_loop_period_ns/1e9),3) / 3.0); // Px
    sys_noise_COV(4, 4) = this->_sigma_sys_noise_py* (std::pow((this->_loop_period_ns/1e9),3) / 3.0); // Py
    sys_noise_COV(5, 5) = this->_sigma_sys_noise_pz* (std::pow((this->_loop_period_ns/1e9),3) / 3.0); // Pz
    sys_noise_COV(6, 6) = this->_sigma_sys_noise_jv* (std::pow((this->_loop_period_ns/1e9),3) / 3.0); // pv
    sys_noise_COV(7, 7) = this->_sigma_sys_noise_jvd*(this->_loop_period_ns/1e9); // d(pv)/dt

    // Initialize System Model
    Gaussian system_uncertainty_PDF(sys_noise_MU, sys_noise_COV);
    this->_sys_PDF = new LinearAnalyticConditionalGaussian( A, system_uncertainty_PDF);
    this->_sys_MODEL = new LinearAnalyticSystemModelGaussianUncertainty( this->_sys_PDF);
}

void RevoluteJointFilter::_initializeMeasurementModel()
{
    // create MEASUREMENT MODEL
    ColumnVector meas_noise_MU(MEAS_DIM);
    meas_noise_MU = 0.0;
    SymmetricMatrix meas_noise_COV(MEAS_DIM);
    meas_noise_COV = 0.0;
    for (unsigned int i = 1; i <= MEAS_DIM; i++)
        meas_noise_COV(i, i) = this->_sigma_meas_noise;

    Gaussian meas_uncertainty_PDF(meas_noise_MU, meas_noise_COV);

    this->_meas_PDF = new NonLinearRevoluteMeasurementPdf(meas_uncertainty_PDF);
    this->_meas_MODEL = new AnalyticMeasurementModelGaussianUncertainty(this->_meas_PDF);
}

void RevoluteJointFilter::_initializeEKF()
{
    ColumnVector prior_MU(REV_STATE_DIM);
    prior_MU = 0.0;

    SymmetricMatrix prior_COV(REV_STATE_DIM);
    prior_COV = 0.0;

    // This is weird but happens when working with synthetic data
    // We don't want to divide by 0!
    if(this->_joint_orientation.x() == 0.0)
    {
        this->_joint_orientation.x() = 1e-6;
    }

    prior_MU(1) = atan2( this->_joint_orientation.y() , this->_joint_orientation.x());
    prior_MU(2) = acos(this->_joint_orientation.z());
    prior_MU(3) = this->_joint_position.x();
    prior_MU(4) = this->_joint_position.y();
    prior_MU(5) = this->_joint_position.z();
    prior_MU(6) = this->_joint_state;
    prior_MU(7) = this->_joint_velocity;

    for (int i = 1; i <= REV_STATE_DIM; i++)
    {
        prior_COV(i, i) = _prior_cov_vel;
    }

    Gaussian prior_PDF(prior_MU, prior_COV);

    this->_ekf = new ExtendedKalmanFilter(&prior_PDF);
}

RevoluteJointFilter::~RevoluteJointFilter()
{
    if (this->_sys_PDF)
    {
        delete this->_sys_PDF;
        this->_sys_PDF = NULL;
    }
    if (this->_sys_MODEL)
    {
        delete this->_sys_MODEL;
        this->_sys_MODEL = NULL;
    }
    if (this->_meas_PDF)
    {
        delete this->_meas_PDF;
        this->_meas_PDF = NULL;
    }
    if (this->_meas_MODEL)
    {
        delete this->_meas_MODEL;
        this->_meas_MODEL = NULL;
    }
    if (this->_ekf)
    {
        delete this->_ekf;
        this->_ekf = NULL;
    }
}

RevoluteJointFilter::RevoluteJointFilter(const RevoluteJointFilter &rev_joint) :
    JointFilter(rev_joint)
{
    this->_sigma_delta_meas_uncertainty_angular = rev_joint._sigma_delta_meas_uncertainty_angular;
    this->_sys_PDF = new LinearAnalyticConditionalGaussian(*(rev_joint._sys_PDF));
    this->_sys_MODEL = new LinearAnalyticSystemModelGaussianUncertainty( *(rev_joint._sys_MODEL));
    this->_meas_PDF = new NonLinearRevoluteMeasurementPdf(*(rev_joint._meas_PDF));
    this->_meas_MODEL = new AnalyticMeasurementModelGaussianUncertainty( *(rev_joint._meas_MODEL));
    this->_ekf = new ExtendedKalmanFilter(*(rev_joint._ekf));
}

void RevoluteJointFilter::predictState(double time_interval_ns)
{
    // Estimate the new cov matrix depending on the time elapsed between the previous and the current measurement
    SymmetricMatrix sys_noise_COV(REV_STATE_DIM);
    sys_noise_COV = 0.0;
    sys_noise_COV(1, 1) = this->_sigma_sys_noise_phi* (std::pow((time_interval_ns/1e9),3) / 3.0); // PHI
    sys_noise_COV(2, 2) = this->_sigma_sys_noise_theta* (std::pow((time_interval_ns/1e9),3) / 3.0); // THETA
    sys_noise_COV(3, 3) = this->_sigma_sys_noise_px* (std::pow((time_interval_ns/1e9),3) / 3.0); // Px
    sys_noise_COV(4, 4) = this->_sigma_sys_noise_py* (std::pow((time_interval_ns/1e9),3) / 3.0); // Py
    sys_noise_COV(5, 5) = this->_sigma_sys_noise_pz* (std::pow((time_interval_ns/1e9),3) / 3.0); // Pz
    sys_noise_COV(6, 6) = this->_sigma_sys_noise_jv* (std::pow((time_interval_ns/1e9),3) / 3.0); // pv
    sys_noise_COV(7, 7) = this->_sigma_sys_noise_jvd*(time_interval_ns/1e9); // d(pv)/dt

    // Estimate the new updating matrix which also depends on the time elapsed between the previous and the current measurement
    // x(t+1) = x(t) + v(t) * delta_t
    Matrix A(REV_STATE_DIM, REV_STATE_DIM);
    A = 0.;
    for (unsigned int i = 1; i <= REV_STATE_DIM; i++)
    {
        A(i, i) = 1.0;
    }
    A(6, 7) = time_interval_ns/1e9;; //Adding the velocity (times the time) to the position of the joint variable

    this->_sys_PDF->MatrixSet(0, A);
    this->_sys_PDF->AdditiveNoiseSigmaSet(sys_noise_COV);
    //The system update
    this->_ekf->Update(this->_sys_MODEL);
}

void RevoluteJointFilter::predictMeasurement()
{
    ColumnVector empty;
    ColumnVector state_updated_state = this->_ekf->PostGet()->ExpectedValueGet();

    ColumnVector predicted_delta_pose_in_rrbf = this->_meas_MODEL->PredictionGet(empty, state_updated_state);

    this->_predicted_delta_pose_in_rrbf = Eigen::Twistd( predicted_delta_pose_in_rrbf(4), predicted_delta_pose_in_rrbf(5),
                                                         predicted_delta_pose_in_rrbf(6), predicted_delta_pose_in_rrbf(1),
                                                         predicted_delta_pose_in_rrbf(2), predicted_delta_pose_in_rrbf(3));

    Eigen::Displacementd predicted_delta = this->_predicted_delta_pose_in_rrbf.exp(1e-20);
    Eigen::Displacementd T_rrbf_srbf_t0 = this->_srb_initial_pose_in_rrbf.exp(1.0e-20);
    Eigen::Displacementd T_rrbf_srbf_t_next = predicted_delta * T_rrbf_srbf_t0;

    this->_srb_predicted_pose_in_rrbf = T_rrbf_srbf_t_next.log(1.0e-20);

    bool change = false;
    this->_srb_predicted_pose_in_rrbf = unwrapTwist(this->_srb_predicted_pose_in_rrbf, T_rrbf_srbf_t_next, this->_srb_previous_predicted_pose_in_rrbf, change);

    this->_srb_previous_predicted_pose_in_rrbf = this->_srb_predicted_pose_in_rrbf;
}

void RevoluteJointFilter::correctState()
{
    ColumnVector updated_state = this->_ekf->PostGet()->ExpectedValueGet();
    ColumnVector rb2_measured_delta_relative_pose_cv(MEAS_DIM);
    rb2_measured_delta_relative_pose_cv = 0.;
    rb2_measured_delta_relative_pose_cv(1) = this->_current_delta_pose_in_rrbf.vx();
    rb2_measured_delta_relative_pose_cv(2) = this->_current_delta_pose_in_rrbf.vy();
    rb2_measured_delta_relative_pose_cv(3) = this->_current_delta_pose_in_rrbf.vz();
    rb2_measured_delta_relative_pose_cv(4) = this->_current_delta_pose_in_rrbf.rx();
    rb2_measured_delta_relative_pose_cv(5) = this->_current_delta_pose_in_rrbf.ry();
    rb2_measured_delta_relative_pose_cv(6) = this->_current_delta_pose_in_rrbf.rz();

    // Update the uncertainty on the measurement
    // The uncertainty on the measurement (the delta motion of the second rigid body wrt the reference rigid body) will be large if the measurement
    // is small and small if the measurement is large
    // Also at 2PI the uncertainty should be high (rotational periodicity of the exponential map)
    Eigen::Vector3d angular_component(this->_current_delta_pose_in_rrbf.rx(), this->_current_delta_pose_in_rrbf.ry(), this->_current_delta_pose_in_rrbf.rz());
    double meas_uncertainty_factor = 1 / (1.0 - exp(-sin(angular_component.norm()/2.0)/this->_sigma_delta_meas_uncertainty_angular));

    // Truncate the factor
    meas_uncertainty_factor = std::min(meas_uncertainty_factor, 1e6);

    SymmetricMatrix current_delta_pose_cov_in_rrbf(6);
    for (unsigned int i = 0; i < 6; i++)
    {
        for (unsigned int j = 0; j < 6; j++)
        {
            current_delta_pose_cov_in_rrbf(i + 1, j + 1) = _current_delta_pose_cov_in_rrbf(i, j);
        }
    }

    this->_meas_PDF->AdditiveNoiseSigmaSet(current_delta_pose_cov_in_rrbf * meas_uncertainty_factor );

    this->_ekf->Update(this->_meas_MODEL, rb2_measured_delta_relative_pose_cv);

    updated_state = this->_ekf->PostGet()->ExpectedValueGet();

    this->_joint_orientation(0) = sin(updated_state(2)) * cos(updated_state(1));
    this->_joint_orientation(1) = sin(updated_state(2)) * sin(updated_state(1));
    this->_joint_orientation(2) = cos(updated_state(2));

    SymmetricMatrix updated_uncertainty = this->_ekf->PostGet()->CovarianceGet();

    for(int i=0; i<3; i++)
    {
        this->_joint_position(i) = updated_state(i+3);
        for(int j=0; j<3; j++)
        {
            this->_uncertainty_joint_position(i,j) = updated_uncertainty(i+3, j+3);
        }
    }

    double joint_state_before = _joint_state;

    // This jump should happen if we are close to 2PI or -2PI rotation
    if(_from_inverted_to_non_inverted)
    {
        _accumulated_rotation = std::round(joint_state_before/(2*M_PI))*2*M_PI;
    }

    // This jump should happen if we are close to PI or -PI rotation
    if(_from_non_inverted_to_inverted)
    {
        _accumulated_rotation = std::round(joint_state_before/(M_PI))*2*M_PI;
    }

    this->_joint_state = updated_state(6);

    // If we are inverting the twist is because we are in the interval (PI, 2PI) or (-PI, -2PI)
    if(_inverted_delta_srb_pose_in_rrbf)
    {
        _joint_state = _accumulated_rotation - _joint_state;
    }else{
        _joint_state = _accumulated_rotation + _joint_state;
    }

    this->_uncertainty_joint_state = updated_uncertainty(6,6);
    this->_joint_velocity = updated_state(7);
    this->_uncertainty_joint_velocity = updated_uncertainty(7,7);

    this->_joint_orientation_phi = updated_state(1);
    this->_joint_orientation_theta = updated_state(2);
    this->_uncertainty_joint_orientation_phitheta(0,0) = updated_uncertainty(1, 1);
    this->_uncertainty_joint_orientation_phitheta(0,1) = updated_uncertainty(1, 2);
    this->_uncertainty_joint_orientation_phitheta(1,0) = updated_uncertainty(1, 2);
    this->_uncertainty_joint_orientation_phitheta(1,1) = updated_uncertainty(2, 2);
}

void RevoluteJointFilter::estimateMeasurementHistoryLikelihood()
{
    double accumulated_error = 0.;

    double p_one_meas_given_model_params = 0;
    double p_all_meas_given_model_params = 0;

    double sigma_translation = 0.05;
    double sigma_rotation = 0.2;

    ColumnVector updated_state = this->_ekf->PostGet()->ExpectedValueGet();
    double phi = updated_state(1);
    double theta = updated_state(2);
    double sp = sin(phi);
    double cp = cos(phi);
    double st = sin(theta);
    double ct = cos(theta);

    double px = updated_state(3);
    double py = updated_state(4);
    double pz = updated_state(5);

    this->_joint_states_all.push_back(updated_state(6));

    Eigen::Vector3d rev_joint_rotation_unitary = Eigen::Vector3d(cp * st, sp * st, ct);
    rev_joint_rotation_unitary.normalize();

    // Convert the screw attributes (line description) to a twist
    Eigen::Vector3d rev_joint_position = Eigen::Vector3d(px, py, pz);
    Eigen::Vector3d rev_joint_translation_unitary = (-rev_joint_rotation_unitary).cross(rev_joint_position);

    double counter = 0.;
    size_t trajectory_length = this->_delta_poses_in_rrbf.size();
    size_t amount_samples = std::min(trajectory_length, (size_t)this->_likelihood_sample_num);
    double delta_idx_samples = (double)std::max(1., (double)trajectory_length/(double)this->_likelihood_sample_num);
    size_t current_idx = 0;

    double max_norm_of_deltas = 0;

    // Estimation of the quality of the parameters of the revolute joint
    // If the joint is revolute and the parameters are accurate, the joint axis orientation and position should not change over time
    // That means that the current orientation/position, multiplied by the amount of revolute displacement at each time step, should provide the delta in the relative
    // pose between ref and second rb at each time step
    // We test amount_samples of the relative trajectory
    // I check if the joint is too young or if there is too few rotation and the axis is very far away (a prismatic joint can be seen as a revolute joint where the
    // joint is far away)
    // We need to have memory here: if the rotation was once larger than the this->_rev_min_rot_for_ee could be that we returned to the starting relative pose

    for (size_t sample_idx = 0; sample_idx < amount_samples; sample_idx++)
    {
        current_idx = boost::math::round(sample_idx*delta_idx_samples);
        Eigen::Displacementd rb2_last_delta_relative_displ = this->_delta_poses_in_rrbf.at(current_idx).exp(1e-12);

        max_norm_of_deltas = std::max(this->_delta_poses_in_rrbf.at(current_idx).norm(), max_norm_of_deltas);

        Eigen::Vector3d rb2_last_delta_relative_translation = rb2_last_delta_relative_displ.getTranslation();
        Eigen::Quaterniond rb2_last_delta_relative_rotation = Eigen::Quaterniond(rb2_last_delta_relative_displ.qw(),
                                                                                rb2_last_delta_relative_displ.qx(),
                                                                                rb2_last_delta_relative_displ.qy(),
                                                                                rb2_last_delta_relative_displ.qz());

        Eigen::Vector3d rev_joint_translation = this->_joint_states_all.at(current_idx) * rev_joint_translation_unitary;
        Eigen::Vector3d rev_joint_rotation = this->_joint_states_all.at(current_idx) * rev_joint_rotation_unitary;
        Eigen::Displacementd rb2_last_delta_relative_displ_rev_hyp = Eigen::Twistd( rev_joint_rotation.x(),
                                                                                rev_joint_rotation.y(),
                                                                                rev_joint_rotation.z(),
                                                                                rev_joint_translation.x(),
                                                                                rev_joint_translation.y(),
                                                                                rev_joint_translation.z()).exp(1e-12);
        Eigen::Vector3d rb2_last_delta_relative_translation_rev_hyp = rb2_last_delta_relative_displ_rev_hyp.getTranslation();
        Eigen::Quaterniond rb2_last_delta_relative_rotation_rev_hyp = Eigen::Quaterniond(rb2_last_delta_relative_displ_rev_hyp.qw(),
                                                                                rb2_last_delta_relative_displ_rev_hyp.qx(),
                                                                                rb2_last_delta_relative_displ_rev_hyp.qy(),
                                                                                rb2_last_delta_relative_displ_rev_hyp.qz());

        // Distance proposed by park and okamura in "Kinematic calibration using the product of exponentials formula"
        double translation_error = (rb2_last_delta_relative_translation - rb2_last_delta_relative_translation_rev_hyp).norm();
        Eigen::Quaterniond rotation_error = rb2_last_delta_relative_rotation.inverse() * rb2_last_delta_relative_rotation_rev_hyp;
        double rotation_error_angle = Eigen::Displacementd(0., 0., 0., rotation_error.w(), rotation_error.x(), rotation_error.y(), rotation_error.z()).log(1e-12).norm();

        accumulated_error += translation_error + fabs(rotation_error_angle);

        p_one_meas_given_model_params = (1.0/(sigma_translation*sqrt(2.0*M_PI)))*exp((-1.0/2.0)*pow(translation_error/sigma_translation, 2)) *
                (1.0/(sigma_rotation*sqrt(2.0*M_PI)))*exp((-1.0/2.0)*pow(rotation_error_angle/sigma_rotation, 2));

        p_all_meas_given_model_params += (p_one_meas_given_model_params/(double)amount_samples);

        counter++;
    }

    if(counter != 0)
    {
        this->_measurements_likelihood = p_all_meas_given_model_params;
    }else{
        this->_measurements_likelihood = 1e-5;
    }
}

void RevoluteJointFilter::estimateUnnormalizedModelProbability()
{
    Eigen::Vector3d point1_in_rrbf = this->_joint_position + 100*this->_joint_orientation;
    Eigen::Vector3d point2_in_rrbf = this->_joint_position - 100*this->_joint_orientation;

    // The joint position and orientation are in rrb frame.

    double p_params_given_model = 1;

    // We only measure the distance to the reference rb if it is not the static environment!
    double distance_to_rrb = 0;
    if(_rrb_id != 0)
    {
        distance_to_rrb = (point2_in_rrbf- point1_in_rrbf).cross(point1_in_rrbf).norm()/((point2_in_rrbf - point1_in_rrbf).norm());
        p_params_given_model *= (1.0/(_rev_max_joint_distance_for_ee*sqrt(2.0*M_PI)))*exp((-1.0/2.0)*pow(distance_to_rrb/_rev_max_joint_distance_for_ee, 2));
    }

    Eigen::Vector4d point1_in_rrbf_homo(point1_in_rrbf.x(), point1_in_rrbf.y(), point1_in_rrbf.z(), 1.0);
    Eigen::Vector4d point1_in_sf_homo = _rrb_current_pose_in_sf.exp(1e-12).toHomogeneousMatrix()*point1_in_rrbf_homo;
    Eigen::Vector3d point1_in_sf(point1_in_sf_homo[0],point1_in_sf_homo[1],point1_in_sf_homo[2]);

    Eigen::Vector4d point2_in_rrbf_homo(point2_in_rrbf.x(), point2_in_rrbf.y(), point2_in_rrbf.z(), 1.0);
    Eigen::Vector4d point2_in_sf_homo = _rrb_current_pose_in_sf.exp(1e-12).toHomogeneousMatrix()*point2_in_rrbf_homo;
    Eigen::Vector3d point2_in_sf(point2_in_sf_homo[0],point2_in_sf_homo[1],point2_in_sf_homo[2]);

    double distance_to_srb = ((point2_in_sf - point1_in_sf).cross(point1_in_sf - _srb_centroid_in_sf)).norm()/((point2_in_sf - point1_in_sf).norm());

    p_params_given_model *= (1.0/(_rev_max_joint_distance_for_ee*sqrt(2.0*M_PI)))*exp((-1.0/2.0)*pow(distance_to_srb/_rev_max_joint_distance_for_ee, 2));

    this->_unnormalized_model_probability = _model_prior_probability*_measurements_likelihood*p_params_given_model;
}

geometry_msgs::TwistWithCovariance RevoluteJointFilter::getPredictedSRBDeltaPoseWithCovInSensorFrame()
{
    Eigen::Matrix<double, 6, 6> adjoint;
    computeAdjoint(this->_rrb_current_pose_in_sf, adjoint);
    Eigen::Twistd predicted_delta_pose_in_sf = adjoint*this->_predicted_delta_pose_in_rrbf;

    geometry_msgs::TwistWithCovariance hypothesis;

    hypothesis.twist.linear.x = predicted_delta_pose_in_sf.vx();
    hypothesis.twist.linear.y = predicted_delta_pose_in_sf.vy();
    hypothesis.twist.linear.z = predicted_delta_pose_in_sf.vz();
    hypothesis.twist.angular.x = predicted_delta_pose_in_sf.rx();
    hypothesis.twist.angular.y = predicted_delta_pose_in_sf.ry();
    hypothesis.twist.angular.z = predicted_delta_pose_in_sf.rz();

    // This call gives me the covariance of the predicted measurement: the relative pose between RBs
    ColumnVector empty;
    ColumnVector state_updated_state = this->_ekf->PostGet()->ExpectedValueGet();
    SymmetricMatrix measurement_cov = this->_meas_MODEL->CovarianceGet(empty, state_updated_state);
    for(int i=0; i<6; i++)
    {
        for(int j=0; j<6; j++)
        {
             hypothesis.covariance[6 * i + j] = measurement_cov(i+1,j+1);
        }
    }

    return hypothesis;
}

geometry_msgs::TwistWithCovariance RevoluteJointFilter::getPredictedSRBVelocityWithCovInSensorFrame()
{
    Eigen::Matrix<double, 6, 6> adjoint;
    computeAdjoint(this->_rrb_current_pose_in_sf, adjoint);
    Eigen::Twistd predicted_delta_pose_in_sf = adjoint*(this->_predicted_delta_pose_in_rrbf/(_loop_period_ns/1e9));

    geometry_msgs::TwistWithCovariance hypothesis;

    hypothesis.twist.linear.x = predicted_delta_pose_in_sf.vx();
    hypothesis.twist.linear.y = predicted_delta_pose_in_sf.vy();
    hypothesis.twist.linear.z = predicted_delta_pose_in_sf.vz();
    hypothesis.twist.angular.x = predicted_delta_pose_in_sf.rx();
    hypothesis.twist.angular.y = predicted_delta_pose_in_sf.ry();
    hypothesis.twist.angular.z = predicted_delta_pose_in_sf.rz();

    // This call gives me the covariance of the predicted measurement: the relative pose between RBs
    ColumnVector empty;
    ColumnVector state_updated_state = this->_ekf->PostGet()->ExpectedValueGet();
    SymmetricMatrix measurement_cov = this->_meas_MODEL->CovarianceGet(empty, state_updated_state);
    for(int i=0; i<6; i++)
    {
        for(int j=0; j<6; j++)
        {
             hypothesis.covariance[6 * i + j] = measurement_cov(i+1,j+1);
        }
    }

    return hypothesis;
}

geometry_msgs::TwistWithCovariance RevoluteJointFilter::getPredictedSRBPoseWithCovInSensorFrame()
{
    Eigen::Twistd delta_rrb_in_sf = this->_rrb_current_vel_in_sf*(this->_loop_period_ns/1e9);
    Eigen::Twistd rrb_next_pose_in_sf = (delta_rrb_in_sf.exp(1e-12)*this->_rrb_current_pose_in_sf.exp(1e-12)).log(1e-12);

    Eigen::Displacementd T_sf_rrbf_next = rrb_next_pose_in_sf.exp(1e-12);
    Eigen::Displacementd T_rrbf_srbf_next = this->_srb_predicted_pose_in_rrbf.exp(1e-12);

    Eigen::Displacementd T_sf_srbf_next = T_rrbf_srbf_next*T_sf_rrbf_next;

    Eigen::Twistd srb_next_pose_in_sf = T_sf_srbf_next.log(1e-12);

    geometry_msgs::TwistWithCovariance hypothesis;

    hypothesis.twist.linear.x = srb_next_pose_in_sf.vx();
    hypothesis.twist.linear.y = srb_next_pose_in_sf.vy();
    hypothesis.twist.linear.z = srb_next_pose_in_sf.vz();
    hypothesis.twist.angular.x = srb_next_pose_in_sf.rx();
    hypothesis.twist.angular.y = srb_next_pose_in_sf.ry();
    hypothesis.twist.angular.z = srb_next_pose_in_sf.rz();

    // This call gives me the covariance of the predicted measurement: the relative pose between RBs
    ColumnVector empty;
    ColumnVector state_updated_state = this->_ekf->PostGet()->ExpectedValueGet();
    SymmetricMatrix measurement_cov = this->_meas_MODEL->CovarianceGet(empty, state_updated_state);
    Eigen::Matrix<double,6,6> measurement_cov_eigen;
    for(int i=0; i<6; i++)
    {
        for(int j=0; j<6; j++)
        {
            measurement_cov_eigen(i,j) = measurement_cov(i+1,j+1);
        }
    }
    // I need the covariance of the absolute pose of the second RB, so I add the cov of the relative pose to the
    // cov of the reference pose. I need to "move" the second covariance to align it to the reference frame (see Barfoot)
    Eigen::Matrix<double,6,6> tranformed_cov;
    adjointXcovXadjointT(_rrb_current_pose_in_sf, measurement_cov_eigen, tranformed_cov);
    Eigen::Matrix<double,6,6> new_pose_covariance = this->_rrb_pose_cov_in_sf + tranformed_cov;
    for (unsigned int i = 0; i < 6; i++)
    {
        for (unsigned int j = 0; j < 6; j++)
        {
            hypothesis.covariance[6 * i + j] = new_pose_covariance(i, j);
        }
    }

#ifdef PUBLISH_PREDICTED_POSE_AS_PWC
    // This is used to visualize the predictions based on the joint hypothesis
    geometry_msgs::PoseWithCovarianceStamped pose_with_cov_stamped;
    pose_with_cov_stamped.header.stamp = ros::Time::now();
    pose_with_cov_stamped.header.frame_id = "camera_rgb_optical_frame";

    Eigen::Displacementd displ_from_twist = srb_next_pose_in_sf.exp(1e-12);
    pose_with_cov_stamped.pose.pose.position.x = displ_from_twist.x();
    pose_with_cov_stamped.pose.pose.position.y = displ_from_twist.y();
    pose_with_cov_stamped.pose.pose.position.z = displ_from_twist.z();
    pose_with_cov_stamped.pose.pose.orientation.x = displ_from_twist.qx();
    pose_with_cov_stamped.pose.pose.orientation.y = displ_from_twist.qy();
    pose_with_cov_stamped.pose.pose.orientation.z = displ_from_twist.qz();
    pose_with_cov_stamped.pose.pose.orientation.w = displ_from_twist.qw();

    for (unsigned int i = 0; i < 6; i++)
        for (unsigned int j = 0; j < 6; j++)
            pose_with_cov_stamped.pose.covariance[6 * i + j] = new_pose_covariance(i, j);

    _predicted_next_pose_publisher.publish(pose_with_cov_stamped);
#endif

    return hypothesis;
}

std::vector<visualization_msgs::Marker> RevoluteJointFilter::getJointMarkersInRRBFrame() const
{
    // The class variable _joint_orientation and _rev_joint_posi (also _uncertainty_o_phi and _uncertainty_o_theta) are defined in the frame of the
    // ref RB with the initial relative transformation to the second RB
    // We want the variables to be in the ref RB frame, without the initial relative transformation to the second RB
    Eigen::Vector3d rev_joint_ori_in_ref_rb = this->_joint_orientation;
    Eigen::Vector3d rev_joint_posi_in_ref_rb = this->_joint_position;

    std::vector<visualization_msgs::Marker> revolute_markers;
    // AXIS MARKER 1 -> The axis ///////////////////////////////////////////////////////////////////////////////////////////////////////////
    visualization_msgs::Marker axis_orientation_marker;
    axis_orientation_marker.ns = "kinematic_structure";
    axis_orientation_marker.action = visualization_msgs::Marker::ADD;
    axis_orientation_marker.type = visualization_msgs::Marker::ARROW;

    // Define the joint parameters relative to the REFERENCE RIGID BODY and the marker will be also
    // defined wrt to the frame of the REFERENCE RIGID BODY!
    // The frame name will be assigned in the MultiJointTrackerNode because we don't know the rb ids here
    //axis_orientation_marker.header.frame_id = "camera_rgb_optical_frame";
    axis_orientation_marker.id = 3 * this->_joint_id;
    axis_orientation_marker.scale.x = JOINT_AXIS_AND_VARIABLE_MARKER_RADIUS;
    axis_orientation_marker.scale.y = 0.f;
    axis_orientation_marker.scale.z = 0.f;
    axis_orientation_marker.color.r = 1.f;
    axis_orientation_marker.color.g = 0.f;
    axis_orientation_marker.color.b = 0.f;
    axis_orientation_marker.color.a = 1.f;
    Eigen::Vector3d point_on_rot_axis_1 = rev_joint_posi_in_ref_rb - this->_joint_state * rev_joint_ori_in_ref_rb;
    geometry_msgs::Point pt1;
    pt1.x = point_on_rot_axis_1.x();
    pt1.y = point_on_rot_axis_1.y();
    pt1.z = point_on_rot_axis_1.z();
    axis_orientation_marker.points.push_back(pt1);
    Eigen::Vector3d point_on_rot_axis_2 = rev_joint_posi_in_ref_rb + this->_joint_state * rev_joint_ori_in_ref_rb;
    geometry_msgs::Point pt2;
    pt2.x = point_on_rot_axis_2.x();
    pt2.y = point_on_rot_axis_2.y();
    pt2.z = point_on_rot_axis_2.z();
    axis_orientation_marker.points.push_back(pt2);
    revolute_markers.push_back(axis_orientation_marker);

    // AXIS MARKER 2 -> Proportional to the joint state///////////////////////////////////////////////////////////////////////////////////////////////////////////
    visualization_msgs::Marker axis_orientation_markerb;
    axis_orientation_markerb.ns = "kinematic_structure";
    axis_orientation_markerb.action = visualization_msgs::Marker::ADD;
    axis_orientation_markerb.type = visualization_msgs::Marker::ARROW;
    // Define the joint parameters relative to the REFERENCE RIGID BODY and the marker will be also
    // defined wrt to the frame of the REFERENCE RIGID BODY!
    // The frame name will be assigned in the MultiJointTrackerNode because we don't know the rb ids here
    //axis_orientation_markerb.header.frame_id = "camera_rgb_optical_frame";
    axis_orientation_markerb.id = 3 * this->_joint_id + 1;
    axis_orientation_markerb.scale.x = JOINT_AXIS_MARKER_RADIUS;
    axis_orientation_markerb.scale.y = 0.f;
    axis_orientation_markerb.scale.z = 0.f;
    axis_orientation_markerb.color.r = 1.f;
    axis_orientation_markerb.color.g = 0.f;
    axis_orientation_markerb.color.b = 0.f;
    axis_orientation_markerb.color.a = 1.f;
    Eigen::Vector3d point_on_rot_axis_1b = rev_joint_posi_in_ref_rb - 100 * rev_joint_ori_in_ref_rb;
    geometry_msgs::Point pt1b;
    pt1b.x = point_on_rot_axis_1b.x();
    pt1b.y = point_on_rot_axis_1b.y();
    pt1b.z = point_on_rot_axis_1b.z();
    axis_orientation_markerb.points.push_back(pt1b);
    Eigen::Vector3d point_on_rot_axis_2b = rev_joint_posi_in_ref_rb  + 100 * rev_joint_ori_in_ref_rb;
    geometry_msgs::Point pt2b;
    pt2b.x = point_on_rot_axis_2b.x();
    pt2b.y = point_on_rot_axis_2b.y();
    pt2b.z = point_on_rot_axis_2b.z();
    axis_orientation_markerb.points.push_back(pt2b);

    revolute_markers.push_back(axis_orientation_markerb);

    // AXIS MARKER 3 -> Text with the joint state ///////////////////////////////////////////////////////////////////////////////////////////////////////////
    axis_orientation_markerb.points.clear();
    axis_orientation_markerb.id = 3 * this->_joint_id + 2;
    axis_orientation_markerb.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    axis_orientation_markerb.scale.z = JOINT_VALUE_TEXT_SIZE;
    std::ostringstream oss_joint_value;
    oss_joint_value << std::fixed<< std::setprecision(0) << (180/M_PI)*this->_joint_state;
    axis_orientation_markerb.text = oss_joint_value.str() + std::string(" deg");
    axis_orientation_markerb.pose.position.x = rev_joint_posi_in_ref_rb.x();
    axis_orientation_markerb.pose.position.y = rev_joint_posi_in_ref_rb.y();
    axis_orientation_markerb.pose.position.z = rev_joint_posi_in_ref_rb.z();
    axis_orientation_markerb.pose.orientation.x = 0;
    axis_orientation_markerb.pose.orientation.y = 0;
    axis_orientation_markerb.pose.orientation.z = 0;
    axis_orientation_markerb.pose.orientation.w = 1;

    revolute_markers.push_back(axis_orientation_markerb);

    // UNCERTAINTY MARKERS ///////////////////////////////////////////////////////////////////////////////////////////////////////

    visualization_msgs::Marker axis_position_uncertainty_marker;
    axis_position_uncertainty_marker.pose.position.x = rev_joint_posi_in_ref_rb.x();
    axis_position_uncertainty_marker.pose.position.y = rev_joint_posi_in_ref_rb.y();
    axis_position_uncertainty_marker.pose.position.z = rev_joint_posi_in_ref_rb.z();
    // Define the joint parameters relative to the REFERENCE RIGID BODY and the marker will be also
    // defined wrt to the frame of the REFERENCE RIGID BODY!
    // The frame name will be assigned in the MultiJointTrackerNode because we don't know the rb ids here
    //axis_position_uncertainty_marker.header.frame_id = "camera_rgb_optical_frame";
    axis_position_uncertainty_marker.ns = "kinematic_structure_uncertainty";
    axis_position_uncertainty_marker.type = visualization_msgs::Marker::SPHERE;
    axis_position_uncertainty_marker.action = visualization_msgs::Marker::ADD;
    axis_position_uncertainty_marker.id = 3 * this->_joint_id ;
    // Reliability = 1 --> position_sphere_uncertainty = 0
    // Reliability = 0 --> position_sphere_uncertainty = 1m
    axis_position_uncertainty_marker.scale.x = this->_uncertainty_joint_position(0,0); //Using start and end points, scale.x is the radius of the array body
    axis_position_uncertainty_marker.scale.y = this->_uncertainty_joint_position(1,1); //Using start and end points, scale.y is the radius of the array head
    axis_position_uncertainty_marker.scale.z = this->_uncertainty_joint_position(2,2); //Using start and end points, scale.y is the radius of the array head
    axis_position_uncertainty_marker.color.a = 0.3;
    axis_position_uncertainty_marker.color.r = 1.0;
    axis_position_uncertainty_marker.color.g = 0.0;
    axis_position_uncertainty_marker.color.b = 0.0;
    revolute_markers.push_back(axis_position_uncertainty_marker);

    visualization_msgs::Marker rev_axis_unc_cone1;
    rev_axis_unc_cone1.type = visualization_msgs::Marker::MESH_RESOURCE;
    rev_axis_unc_cone1.action = visualization_msgs::Marker::ADD;
    rev_axis_unc_cone1.mesh_resource = "package://joint_tracker/meshes/cone.stl";
    rev_axis_unc_cone1.pose.position.x = rev_joint_posi_in_ref_rb.x();
    rev_axis_unc_cone1.pose.position.y = rev_joint_posi_in_ref_rb.y();
    rev_axis_unc_cone1.pose.position.z = rev_joint_posi_in_ref_rb.z();

    // NOTE:
    // Estimation of the uncertainty cones -----------------------------------------------
    // We estimate the orientation of the revolute axis in spherical coordinates (r=1 always)
    // We estimate phi: angle from the x axis to the projection of the revolute joint axis to the xy plane
    // We estimate theta: angle from the z axis to the revolute joint axis
    // [TODO: phi and theta are in the reference rigid body. Do we need to transform it (adding uncertainty) to the reference frame?]
    // The covariance of phi and theta (a 2x2 matrix) gives us the uncertainty of the orientation of the joint
    // If we look from the joint axis, we would see an ellipse given by this covariance matrix [http://www.visiondummy.com/2014/04/draw-error-ellipse-representing-covariance-matrix/]
    // But in RVIZ we can only set the scale of our cone mesh in x and y, not in a different axis
    // The first thing is then to estimate the direction of the major and minor axis of the ellipse and their size
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(this->_uncertainty_joint_orientation_phitheta);

    // The sizes of the major and minor axes of the ellipse are given by the eigenvalues and the chi square distribution P(x<critical_value) = confidence_value
    // For 50% of confidence on the cones shown
    double confidence_value = 0.5;
    boost::math::chi_squared chi_sq_dist(2);
    double critical_value = boost::math::quantile(chi_sq_dist, confidence_value);
    double major_axis_length = 2*eigensolver.eigenvalues()[1]*std::sqrt(critical_value);
    double minor_axis_length = 2*eigensolver.eigenvalues()[0]*std::sqrt(critical_value);

    // If z is pointing in the direction of the joint, the angle between the x axis and the largest axis of the ellipse is arctg(v1_y/v1_x) where v1 is the eigenvector of
    // largest eigenvalue (the last column in the matrix returned by eigenvectors() in eigen library):
    double alpha = atan2(eigensolver.eigenvectors().col(1)[1],eigensolver.eigenvectors().col(1)[0]);
    // We create a rotation around the z axis to align the ellipse to have the major axis aligned to the x-axis (we UNDO the rotation of the ellipse):
    Eigen::AngleAxisd init_rot(-alpha, Eigen::Vector3d::UnitZ());

    // Now I need to rotate the mesh so that:
    // 1) The z axis of the mesh points in the direction of the joint
    // 2) The x axis of the mesh is contained in the x-y plane of the reference frame

    // To get the z axis of the mesh to point in the direction of the joint
    Eigen::Quaterniond ori_quat;
    ori_quat.setFromTwoVectors(Eigen::Vector3d::UnitZ(), rev_joint_ori_in_ref_rb);

    // To get the x axis of the mesh to be contained in the x-y plane of the reference frame
    // First, find a vector that is orthogonal to the joint orientation and also to the z_axis (this latter implies to be contained in the xy plane)
    Eigen::Vector3d coplanar_xy_orthogonal_to_joint_ori = rev_joint_ori_in_ref_rb.cross(Eigen::Vector3d::UnitZ());
    // Normalize it -> Gives me the desired x axis after the rotation
    coplanar_xy_orthogonal_to_joint_ori.normalize();

    // Then find the corresponding y axis after the rotation as the cross product of the z axis after rotation (orientation of the joint)
    // and the x axis after rotation
    Eigen::Vector3d y_pos = rev_joint_ori_in_ref_rb.cross(coplanar_xy_orthogonal_to_joint_ori);

    // Create a matrix with the values of the vectors after rotation
    Eigen::Matrix3d rotation_pos;
    rotation_pos << coplanar_xy_orthogonal_to_joint_ori.x(),y_pos.x(),rev_joint_ori_in_ref_rb.x(),
            coplanar_xy_orthogonal_to_joint_ori.y(),y_pos.y(),rev_joint_ori_in_ref_rb.y(),
            coplanar_xy_orthogonal_to_joint_ori.z(),y_pos.z(),rev_joint_ori_in_ref_rb.z();

    // Create a quaternion with the matrix
    Eigen::Quaterniond ori_quat_final(rotation_pos);

    Eigen::Quaterniond ori_quat_final_ellipse(ori_quat_final.toRotationMatrix()*init_rot.toRotationMatrix());

    rev_axis_unc_cone1.pose.orientation.x = ori_quat_final_ellipse.x();
    rev_axis_unc_cone1.pose.orientation.y = ori_quat_final_ellipse.y();
    rev_axis_unc_cone1.pose.orientation.z = ori_quat_final_ellipse.z();
    rev_axis_unc_cone1.pose.orientation.w = ori_quat_final_ellipse.w();
    // Define the joint parameters relative to the REFERENCE RIGID BODY and the marker will be also
    // defined wrt to the frame of the REFERENCE RIGID BODY!
    // The frame name will be assigned in the MultiJointTrackerNode because we don't know the rb ids here
    //rev_axis_unc_cone1.header.frame_id = "camera_rgb_optical_frame";
    rev_axis_unc_cone1.ns = "kinematic_structure_uncertainty";
    rev_axis_unc_cone1.id = 3 * this->_joint_id + 1;
    rev_axis_unc_cone1.color.a = 0.4;
    rev_axis_unc_cone1.color.r = 1.0;
    rev_axis_unc_cone1.color.g = 0.0;
    rev_axis_unc_cone1.color.b = 0.0;

    // If the uncertainty is pi/6 (30 degrees) the scale in this direction should be 1
    // If the uncertainty is pi/12 (15 degrees) the scale in this direction should be 0.5
    // If the uncertainty is close to 0 the scale in this direction should be 0
    // If the uncertainty is close to pi the scale in this direction should be inf

    rev_axis_unc_cone1.scale.x = major_axis_length / (M_PI / 6.0);
    rev_axis_unc_cone1.scale.y = minor_axis_length / (M_PI / 6.0);
    rev_axis_unc_cone1.scale.z = 1.;
    revolute_markers.push_back(rev_axis_unc_cone1);

    // We repeat the process for the cone in the other direction
    // To get the z axis of the mesh to point in the direction of the joint (negative)
    Eigen::Vector3d rev_joint_ori_in_ref_rb_neg = -rev_joint_ori_in_ref_rb;
    Eigen::Quaterniond ori_quat_neg;
    ori_quat_neg.setFromTwoVectors(Eigen::Vector3d::UnitZ(), rev_joint_ori_in_ref_rb_neg);

    // To get the x axis of the mesh to be contained in the x-y plane of the reference frame
    // First, find a vector that is orthogonal to the joint orientation and also to the z_axis (this latter implies to be contained in the xy plane)
    Eigen::Vector3d coplanar_xy_orthogonal_to_joint_ori_neg = rev_joint_ori_in_ref_rb_neg.cross(Eigen::Vector3d::UnitZ());
    // Normalize it -> Gives me the desired x axis after the rotation
    coplanar_xy_orthogonal_to_joint_ori_neg.normalize();

    // Then find the corresponding y axis after the rotation as the cross product of the z axis after rotation (orientation of the joint)
    // and the x axis after rotation
    Eigen::Vector3d y_neg = rev_joint_ori_in_ref_rb_neg.cross(coplanar_xy_orthogonal_to_joint_ori_neg);

    // Create a matrix with the values of the vectors after rotation
    Eigen::Matrix3d rotation_neg;
    rotation_neg << coplanar_xy_orthogonal_to_joint_ori_neg.x(),y_neg.x(),rev_joint_ori_in_ref_rb_neg.x(),
            coplanar_xy_orthogonal_to_joint_ori_neg.y(),y_neg.y(),rev_joint_ori_in_ref_rb_neg.y(),
            coplanar_xy_orthogonal_to_joint_ori_neg.z(),y_neg.z(),rev_joint_ori_in_ref_rb_neg.z();

    // Create a quaternion with the matrix
    Eigen::Quaterniond ori_quat_neg_final(rotation_neg);

    // We undo the rotation of the ellipse (but negative!):
    Eigen::AngleAxisd init_rot_neg(alpha, Eigen::Vector3d::UnitZ());
    Eigen::Quaterniond ori_quat_neg_final_ellipse(ori_quat_neg_final.toRotationMatrix()*init_rot_neg.toRotationMatrix());

    rev_axis_unc_cone1.pose.orientation.x = ori_quat_neg_final_ellipse.x();
    rev_axis_unc_cone1.pose.orientation.y = ori_quat_neg_final_ellipse.y();
    rev_axis_unc_cone1.pose.orientation.z = ori_quat_neg_final_ellipse.z();
    rev_axis_unc_cone1.pose.orientation.w = ori_quat_neg_final_ellipse.w();
    rev_axis_unc_cone1.scale.x = major_axis_length / (M_PI / 6.0);
    rev_axis_unc_cone1.scale.y = minor_axis_length / (M_PI / 6.0);
    rev_axis_unc_cone1.scale.z = 1.;
    rev_axis_unc_cone1.id = 3 * this->_joint_id + 2;
    revolute_markers.push_back(rev_axis_unc_cone1);

    return revolute_markers;
}

JointFilterType RevoluteJointFilter::getJointFilterType() const
{
    return REVOLUTE_JOINT;
}

std::string RevoluteJointFilter::getJointFilterTypeStr() const
{
    return std::string("RevoluteJointFilter");
}