#include <NTL/LLL.h>
#include <NTL/fileio.h>
#include <NTL/vec_double.h>

NTL_START_IMPL

static inline
void CheckFinite(double *p)
{
   if (!IsFinite(p)) ResourceError("LLL_FP: numbers too big...use LLL_XD");
}

static double InnerProduct(double *a, double *b, long n)
{
   double s;
   long i;

   s = 0;
   for (i = 1; i <= n; i++)
      s += a[i]*b[i];

   return s;
}

static void RowTransform(vec_ZZ& A, vec_ZZ& B, const ZZ& MU1)
// x = x - y*MU
{
   NTL_ZZRegister(T);
   NTL_ZZRegister(MU);
   long k;

   long n = A.length();
   long i;

   MU = MU1;

   if (MU == 1) {
      for (i = 1; i <= n; i++)
         sub(A(i), A(i), B(i));

      return;
   }

   if (MU == -1) {
      for (i = 1; i <= n; i++)
         add(A(i), A(i), B(i));

      return;
   }

   if (MU == 0) return;

   if (NumTwos(MU) >= NTL_ZZ_NBITS)
      k = MakeOdd(MU);
   else
      k = 0;

   if (MU.WideSinglePrecision()) {
      long mu1;
      conv(mu1, MU);

      if (k > 0) {

         for (i = 1; i <= n; i++) {
            mul(T, B(i), mu1);
            LeftShift(T, T, k);
            sub(A(i), A(i), T);
         }

      }
      else {

         for (i = 1; i <= n; i++) {
            MulSubFrom(A(i), B(i), mu1);
         }

      }
   }
   else {
      for (i = 1; i <= n; i++) {
         mul(T, B(i), MU);
         if (k > 0) LeftShift(T, T, k);
         sub(A(i), A(i), T);
      }
   }
}

#define TR_BND (NTL_FDOUBLE_PRECISION/2.0)
// Just to be safe!!

static double max_abs(double *v, long n)
{
   long i;
   double res, t;

   res = 0;

   for (i = 1; i <= n; i++) {
      t = fabs(v[i]);
      if (t > res) res = t;
   }

   return res;
}

static void RowTransformStart(double *a, long *in_a, long& in_float, long n)
{
   long i;
   long inf = 1;

   for (i = 1; i <= n; i++) {
      in_a[i] = (a[i] < TR_BND && a[i] > -TR_BND);
      inf = inf & in_a[i];
   }

   in_float = inf;
}

static void RowTransformFinish(vec_ZZ& A, double *a, long *in_a)
{
   long n = A.length();
   long i;

   for (i = 1; i <= n; i++) {
      if (in_a[i])  {
         conv(A(i), a[i]);
      }
      else {
         conv(a[i], A(i));
         CheckFinite(&a[i]);
      }
   }
}

static void RowTransform(vec_ZZ& A, vec_ZZ& B, const ZZ& MU1,
                         double *a, double *b, long *in_a,
                         double& max_a, double max_b, long& in_float)
// x = x - y*MU
{
   NTL_ZZRegister(T);
   NTL_ZZRegister(MU);
   long k;
   double mu;

   conv(mu, MU1);
   CheckFinite(&mu);

   long n = A.length();
   long i;

   if (in_float) {
      double mu_abs = fabs(mu);
      if (mu_abs > 0 && max_b > 0 && (mu_abs >= TR_BND || max_b >= TR_BND)) {
         in_float = 0;
      }
      else {
         max_a += mu_abs*max_b;
         if (max_a >= TR_BND)
            in_float = 0;
      }
   }

   if (in_float) {
      if (mu == 1) {
         for (i = 1; i <= n; i++)
            a[i] -= b[i];

         return;
      }

      if (mu == -1) {
         for (i = 1; i <= n; i++)
            a[i] += b[i];

         return;
      }

      if (mu == 0) return;

      for (i = 1; i <= n; i++)
         a[i] -= mu*b[i];

      return;
   }

   MU = MU1;

   if (MU == 1) {
      for (i = 1; i <= n; i++) {
         if (in_a[i] && a[i] < TR_BND && a[i] > -TR_BND &&
             b[i] < TR_BND && b[i] > -TR_BND) {

            a[i] -= b[i];
         }
         else {
            if (in_a[i]) {
               conv(A(i), a[i]);
               in_a[i] = 0;
            }

            sub(A(i), A(i), B(i));
         }
      }
      return;
   }

   if (MU == -1) {
      for (i = 1; i <= n; i++) {
         if (in_a[i] && a[i] < TR_BND && a[i] > -TR_BND &&
             b[i] < TR_BND && b[i] > -TR_BND) {

            a[i] += b[i];
         }
         else {
            if (in_a[i]) {
               conv(A(i), a[i]);
               in_a[i] = 0;
            }

            add(A(i), A(i), B(i));
         }
      }
      return;
   }

   if (MU == 0) return;

   double b_bnd = fabs(TR_BND/mu) - 1;
   if (b_bnd < 0) b_bnd = 0;

   if (NumTwos(MU) >= NTL_ZZ_NBITS)
      k = MakeOdd(MU);
   else
      k = 0;

   if (MU.WideSinglePrecision()) {
      long mu1;
      conv(mu1, MU);

      if (k > 0) {
         for (i = 1; i <= n; i++) {
            if (in_a[i]) {
               conv(A(i), a[i]);
               in_a[i] = 0;
            }

            mul(T, B(i), mu1);
            LeftShift(T, T, k);
            sub(A(i), A(i), T);
         }
      }
      else {
         for (i = 1; i <= n; i++) {
            if (in_a[i] && a[i] < TR_BND && a[i] > -TR_BND &&
                b[i] < b_bnd && b[i] > -b_bnd) {

               a[i] -= b[i]*mu;
            }
            else {
               if (in_a[i]) {
                  conv(A(i), a[i]);
                  in_a[i] = 0;
               }
               MulSubFrom(A(i), B(i), mu1);
            }
         }
      }
   }
   else {
      for (i = 1; i <= n; i++) {
         if (in_a[i]) {
            conv(A(i), a[i]);
            in_a[i] = 0;
         }
         mul(T, B(i), MU);
         if (k > 0) LeftShift(T, T, k);
         sub(A(i), A(i), T);
      }
   }
}

static void RowTransform2(vec_ZZ& A, vec_ZZ& B, const ZZ& MU1)
// x = x + y*MU

{
   NTL_ZZRegister(T);
   NTL_ZZRegister(MU);
   long k;

   long n = A.length();
   long i;

   MU = MU1;

   if (MU == 1) {
      for (i = 1; i <= n; i++)
         add(A(i), A(i), B(i));

      return;
   }

   if (MU == -1) {
      for (i = 1; i <= n; i++)
         sub(A(i), A(i), B(i));

      return;
   }

   if (MU == 0) return;

   if (NumTwos(MU) >= NTL_ZZ_NBITS)
      k = MakeOdd(MU);
   else
      k = 0;

   if (MU.WideSinglePrecision()) {
      long mu1;
      conv(mu1, MU);

      for (i = 1; i <= n; i++) {
         mul(T, B(i), mu1);
         if (k > 0) LeftShift(T, T, k);
         add(A(i), A(i), T);
      }
   }
   else {
      for (i = 1; i <= n; i++) {
         mul(T, B(i), MU);
         if (k > 0) LeftShift(T, T, k);
         add(A(i), A(i), T);
      }
   }
}

static
void ComputeGS(mat_ZZ& B, double **B1, double **mu, double *b,
               double *c, long k, double bound, long st, double *buf)

{
   long n = B.NumCols();
   long i, j;
   double s, t1, y, t;

   ZZ T1;
   long test;

   double *mu_k = mu[k];

   if (st < k) {
      for (i = 1; i < st; i++)
         buf[i] = mu_k[i]*c[i];
   }

   for (j = st; j <= k-1; j++) {
      s = InnerProduct(B1[k], B1[j], n);

      // test = b[k]*b[j] >= NTL_FDOUBLE_PRECISION^2

      test = (b[k]/NTL_FDOUBLE_PRECISION >= NTL_FDOUBLE_PRECISION/b[j]);

      // test = test && s^2 <= b[k]*b[j]/bound,
      // but we compute it in a strange way to avoid overflow

      if (test && (y = fabs(s)) != 0) {
         t = y/b[j];
         t1 = b[k]/y;
         if (t <= 1)
            test = (t*bound <= t1);
         else if (t1 >= 1)
            test = (t <= t1/bound);
         else
            test = 0;
      }

      if (test) {
         InnerProduct(T1, B(k), B(j));
         conv(s, T1);
      }

      double *mu_j = mu[j];

      t1 = 0;
      for (i = 1; i <= j-1; i++) {
         t1 += mu_j[i]*buf[i];
      }

      mu_k[j] = (buf[j] = (s - t1))/c[j];
   }

#if (!NTL_EXT_DOUBLE)

   // Kahan summation

   double c1;

   s = c1 = 0;
   for (j = 1; j <= k-1; j++) {
      y = mu_k[j]*buf[j] - c1;
      t = s+y;
      c1 = t-s;
      c1 = c1-y;
      s = t;
   }

#else

   s = 0;
   for (j = 1; j <= k-1; j++)
      s += mu_k[j]*buf[j];

#endif

   c[k] = b[k] - s;
}

NTL_CHEAP_THREAD_LOCAL double LLLStatusInterval = 900.0;
NTL_CHEAP_THREAD_LOCAL char *LLLDumpFile = 0;

static NTL_CHEAP_THREAD_LOCAL double red_fudge = 0;
static NTL_CHEAP_THREAD_LOCAL long log_red = 0;
static NTL_CHEAP_THREAD_LOCAL long verbose = 0;

static NTL_CHEAP_THREAD_LOCAL unsigned long NumSwaps = 0;
static NTL_CHEAP_THREAD_LOCAL double RR_GS_time = 0;
static NTL_CHEAP_THREAD_LOCAL double StartTime = 0;
static NTL_CHEAP_THREAD_LOCAL double LastTime = 0;

static void LLLStatus(long max_k, double t, long m, const mat_ZZ& B)
{
   cerr << "---- LLL_FP status ----\n";
   cerr << "elapsed time: ";
   PrintTime(cerr, t-StartTime);
   cerr << ", stage: " << max_k;
   cerr << ", rank: " << m;
   cerr << ", swaps: " << NumSwaps << "\n";

   ZZ t1;
   long i;
   double prodlen = 0;

   for (i = 1; i <= m; i++) {
      InnerProduct(t1, B(i), B(i));
      if (!IsZero(t1))
         prodlen += log(t1);
   }

   cerr << "log of prod of lengths: " << prodlen/(2.0*log(2.0)) << "\n";

   if (LLLDumpFile) {
      cerr << "dumping to " << LLLDumpFile << "...";

      ofstream f;
      OpenWrite(f, LLLDumpFile);

      f << "[";
      for (i = 1; i <= m; i++) {
         f << B(i) << "\n";
      }
      f << "]\n";

      f.close();

      cerr << "\n";
   }

   LastTime = t;

}

static void init_red_fudge()
{
   long i;

   log_red = long(0.50*NTL_DOUBLE_PRECISION);
   red_fudge = 1;

   for (i = log_red; i > 0; i--)
      red_fudge = red_fudge*0.5;
}

static void inc_red_fudge()
{

   red_fudge = red_fudge * 2;
   log_red--;

   cerr << "LLL_FP: warning--relaxing reduction (" << log_red << ")\n";

   if (log_red < 4)
      ResourceError("LLL_FP: too much loss of precision...stop!");
}

#if 0

static void print_mus(double **mu, long k)
{
   long i;

   for (i = k-1; i >= 1; i--)
      cerr << mu[k][i] << " ";
   cerr << "\n";
}

#endif

void ComputeGS(const mat_ZZ& B, mat_RR& B1,
               mat_RR& mu, vec_RR& b,
               vec_RR& c, long k, const RR& bound, long st,
               vec_RR& buf, const RR& bound2);

static void RR_GS(mat_ZZ& B, double **B1, double **mu,
                  double *b, double *c, double *buf, long prec,
                  long rr_st, long k, long m_orig,
                  mat_RR& rr_B1, mat_RR& rr_mu,
                  vec_RR& rr_b, vec_RR& rr_c)
{
   double tt;

   cerr << "LLL_FP: RR refresh " << rr_st << "..." << k << "...";
   tt = GetTime();

   if (rr_st > k) ResourceError("LLL_FP: can not continue!!!");

   RRPush push;
   RR::SetPrecision(prec);

   long n = B.NumCols();

   rr_B1.SetDims(k, n);
   rr_mu.SetDims(k, m_orig);
   rr_b.SetLength(k);
   rr_c.SetLength(k);

   vec_RR rr_buf;
   rr_buf.SetLength(k);

   long i, j;

   for (i = rr_st; i <= k; i++)
      for (j = 1; j <= n; j++)
         conv(rr_B1(i, j), B(i, j));

   for (i = rr_st; i <= k; i++)
      InnerProduct(rr_b(i), rr_B1(i), rr_B1(i));

   RR bound;
   power2(bound, 2*long(0.15*RR::precision()));

   RR bound2;
   power2(bound2, 2*RR::precision());

   for (i = rr_st; i <= k; i++)
      ComputeGS(B, rr_B1, rr_mu, rr_b, rr_c, i, bound, 1, rr_buf, bound2);

   for (i = rr_st; i <= k; i++)
      for (j = 1; j <= n; j++) {
         conv(B1[i][j], rr_B1(i,j));
         CheckFinite(&B1[i][j]);
      }

   for (i = rr_st; i <= k; i++)
      for (j = 1; j <= i-1; j++) {
         conv(mu[i][j], rr_mu(i,j));
      }

   for (i = rr_st; i <= k; i++) {
      conv(b[i], rr_b(i));
      CheckFinite(&b[i]);
   }

   for (i = rr_st; i <= k; i++) {
      conv(c[i], rr_c(i));
      CheckFinite(&c[i]);
   }

   for (i = 1; i <= k-1; i++) {
      conv(buf[i], rr_buf[i]);
   }

   tt = GetTime()-tt;
   RR_GS_time += tt;
   cerr << tt << " (" << RR_GS_time << ")\n";
}

void ComputeGS(const mat_ZZ& B, mat_RR& mu, vec_RR& c)
{
   long n = B.NumCols();
   long k = B.NumRows();

   mat_RR B1;
   vec_RR b;

   B1.SetDims(k, n);
   mu.SetDims(k, k);
   b.SetLength(k);
   c.SetLength(k);

   vec_RR buf;
   buf.SetLength(k);

   long i, j;

   for (i = 1; i <= k; i++)
      for (j = 1; j <= n; j++)
         conv(B1(i, j), B(i, j));

   for (i = 1; i <= k; i++)
      InnerProduct(b(i), B1(i), B1(i));

   RR bound;
   power2(bound, 2*long(0.15*RR::precision()));

   RR bound2;
   power2(bound2, 2*RR::precision());

   for (i = 1; i <= k; i++)
      ComputeGS(B, B1, mu, b, c, i, bound, 1, buf, bound2);

}

static
long ll_LLL_FP(mat_ZZ& B, mat_ZZ* U, double delta, long deep,
           LLLCheckFct check, double **B1, double **mu,
           double *b, double *c,
           long m, long init_k, long &quit)
{
   long n = B.NumCols();

   long i, j, k, Fc1;
   ZZ MU;
   double mu1;

   double t1;
   ZZ T1;
   double *tp;

   static double bound = 0;

   if (bound == 0) {
      // we tolerate a 15% loss of precision in computing
      // inner products in ComputeGS.

      bound = 1;
      for (i = 2*long(0.15*NTL_DOUBLE_PRECISION); i > 0; i--)
         bound = bound * 2;
   }

   double half_plus_fudge = 0.5 + red_fudge;

   quit = 0;
   k = init_k;

   vec_long st_mem;
   st_mem.SetLength(m+2);
   long *st = st_mem.elts();

   for (i = 1; i < k; i++)
      st[i] = i;

   for (i = k; i <= m+1; i++)
      st[i] = 1;

   UniqueArray<double> buf_store;
   buf_store.SetLength(m+1);
   double *buf = buf_store.get();

   vec_long in_vec_mem;
   in_vec_mem.SetLength(n+1);
   long *in_vec = in_vec_mem.elts();

   UniqueArray<double> max_b_store;
   max_b_store.SetLength(m+1);
   double *max_b = max_b_store.get();

   for (i = 1; i <= m; i++)
      max_b[i] = max_abs(B1[i], n);

   long in_float;

   long rst;
   long counter;
   long start_over;

   long trigger_index;
   long small_trigger;
   long cnt;

   mat_RR rr_B1;
   mat_RR rr_mu;
   vec_RR rr_c;
   vec_RR rr_b;

   long m_orig = m;

   long rr_st = 1;

   long max_k = 0;

   long prec = RR::precision();

   double tt;

   long swap_cnt = 0;

   while (k <= m) {

      if (k > max_k) {
         max_k = k;
         swap_cnt = 0;
      }

      if (verbose) {
         tt = GetTime();

         if (tt > LastTime + LLLStatusInterval)
            LLLStatus(max_k, tt, m, B);
      }

      if (k < rr_st) rr_st = k;

      if (st[k] == k)
         rst = 1;
      else
         rst = k;

      if (st[k] < st[k+1]) st[k+1] = st[k];
      ComputeGS(B, B1, mu, b, c, k, bound, st[k], buf);
      CheckFinite(&c[k]);
      st[k] = k;

      if (swap_cnt > 200000) {
         cerr << "LLL_FP: swap loop?\n";
         RR_GS(B, B1, mu, b, c, buf, prec,
               rr_st, k, m_orig, rr_B1, rr_mu, rr_b, rr_c);
         if (rr_st < st[k+1]) st[k+1] = rr_st;
         rr_st = k+1;
         rst = k;
         swap_cnt = 0;
      }

      counter = 0;
      trigger_index = k;
      small_trigger = 0;
      cnt = 0;

      long thresh = 10;
      long sz=0, new_sz;

      long did_rr_gs = 0;

      do {
         // size reduction

         counter++;
         if ((counter & 127) == 0) {

            new_sz = 0;
            for (j = 1; j <= n; j++)
               new_sz += NumBits(B(k,j));

            if ((counter >> 7) == 1 || new_sz < sz) {
               sz = new_sz;
            }
            else {
               cerr << "LLL_FP: warning--infinite loop?\n";
            }
         }

         Fc1 = 0;
         start_over = 0;

         for (j = rst-1; j >= 1; j--) {
            t1 = fabs(mu[k][j]);
            if (t1 > half_plus_fudge) {

               if (!Fc1) {
                  if (j > trigger_index ||
                      (j == trigger_index && small_trigger)) {

                     cnt++;

                     if (cnt > thresh) {
                        if (log_red <= 15) {

                           while (log_red > 10)
                              inc_red_fudge();

                           half_plus_fudge = 0.5 + red_fudge;

                           if (!did_rr_gs) {
                              RR_GS(B, B1, mu, b, c, buf, prec,
                                    rr_st, k, m_orig, rr_B1, rr_mu, rr_b, rr_c);
                              if (rr_st < st[k+1]) st[k+1] = rr_st;
                              rr_st = k+1;
                              did_rr_gs = 1;
                              rst = k;
                              trigger_index = k;
                              small_trigger = 0;
                              start_over = 1;
                              break;
                           }
                        }
                        else {
                           inc_red_fudge();
                           half_plus_fudge = 0.5 + red_fudge;
                           cnt = 0;
                        }
                     }
                  }

                  trigger_index = j;
                  small_trigger = (t1 < 4);

                  Fc1 = 1;
                  if (k < rr_st) rr_st = k;
                  RowTransformStart(B1[k], in_vec, in_float, n);
               }

               mu1 = mu[k][j];
               if (mu1 >= 0)
                  mu1 = ceil(mu1-0.5);
               else
                  mu1 = floor(mu1+0.5);

               double *mu_k = mu[k];
               double *mu_j = mu[j];

               if (mu1 == 1) {
                  for (i = 1; i <= j-1; i++)
                     mu_k[i] -= mu_j[i];
               }
               else if (mu1 == -1) {
                  for (i = 1; i <= j-1; i++)
                     mu_k[i] += mu_j[i];
               }
               else {
                  for (i = 1; i <= j-1; i++)
                     mu_k[i] -= mu1*mu_j[i];
               }

               mu_k[j] -= mu1;

               conv(MU, mu1);

               RowTransform(B(k), B(j), MU, B1[k], B1[j], in_vec,
                            max_b[k], max_b[j], in_float);
               if (U) RowTransform((*U)(k), (*U)(j), MU);
            }
         }

         if (Fc1) {
            RowTransformFinish(B(k), B1[k], in_vec);
            max_b[k] = max_abs(B1[k], n);

            if (!did_rr_gs) {
               b[k] = InnerProduct(B1[k], B1[k], n);
               CheckFinite(&b[k]);

               ComputeGS(B, B1, mu, b, c, k, bound, 1, buf);
               CheckFinite(&c[k]);
            }
            else {
               RR_GS(B, B1, mu, b, c, buf, prec,
                     rr_st, k, m_orig, rr_B1, rr_mu, rr_b, rr_c);
               rr_st = k+1;
            }

            rst = k;
         }
      } while (Fc1 || start_over);

      if (check && (*check)(B(k)))
         quit = 1;

      if (b[k] == 0) {
         for (i = k; i < m; i++) {
            // swap i, i+1
            swap(B(i), B(i+1));
            tp = B1[i]; B1[i] = B1[i+1]; B1[i+1] = tp;
            t1 = b[i]; b[i] = b[i+1]; b[i+1] = t1;
            t1 = max_b[i]; max_b[i] = max_b[i+1]; max_b[i+1] = t1;
            if (U) swap((*U)(i), (*U)(i+1));
         }

         for (i = k; i <= m+1; i++) st[i] = 1;
         if (k < rr_st) rr_st = k;

         m--;
         if (quit) break;
         continue;
      }

      if (quit) break;

      if (deep > 0) {
         // deep insertions

         double cc = b[k];
         long l = 1;
         while (l <= k-1 && delta*c[l] <= cc) {
            cc = cc - mu[k][l]*mu[k][l]*c[l];
            l++;
         }

         if (l <= k-1 && (l <= deep || k-l <= deep)) {
            // deep insertion at position l

            for (i = k; i > l; i--) {
               // swap rows i, i-1
               swap(B(i), B(i-1));
               tp = B1[i]; B1[i] = B1[i-1]; B1[i-1] = tp;
               tp = mu[i]; mu[i] = mu[i-1]; mu[i-1] = tp;
               t1 = b[i]; b[i] = b[i-1]; b[i-1] = t1;
               t1 = max_b[i]; max_b[i] = max_b[i-1]; max_b[i-1] = t1;
               if (U) swap((*U)(i), (*U)(i-1));
            }

            k = l;
            NumSwaps++;
            swap_cnt++;
            continue;
         }
      } // end deep insertions

      // test LLL reduction condition

      if (k > 1 && delta*c[k-1] > c[k] + mu[k][k-1]*mu[k][k-1]*c[k-1]) {
         // swap rows k, k-1
         swap(B(k), B(k-1));
         tp = B1[k]; B1[k] = B1[k-1]; B1[k-1] = tp;
         tp = mu[k]; mu[k] = mu[k-1]; mu[k-1] = tp;
         t1 = b[k]; b[k] = b[k-1]; b[k-1] = t1;
         t1 = max_b[k]; max_b[k] = max_b[k-1]; max_b[k-1] = t1;
         if (U) swap((*U)(k), (*U)(k-1));

         k--;
         NumSwaps++;
         swap_cnt++;
         // cout << "-\n";
      }
      else {

         k++;
         // cout << "+\n";
      }

   }

   if (verbose) {
      LLLStatus(m+1, GetTime(), m, B);
   }

   return m;
}

static
long LLL_FP(mat_ZZ& B, mat_ZZ* U, double delta, long deep,
           LLLCheckFct check)
{
   long m = B.NumRows();
   long n = B.NumCols();

   long i, j;
   long new_m, dep, quit;
   ZZ MU;

   ZZ T1;

   init_red_fudge();

   if (U) ident(*U, m);

   Unique2DArray<double> B1_store;
   B1_store.SetDimsFrom1(m+1, n+1);
   double **B1 = B1_store.get();  // approximates B

   Unique2DArray<double> mu_store;
   mu_store.SetDimsFrom1(m+1, m+1);
   double **mu = mu_store.get();

   UniqueArray<double> c_store;
   c_store.SetLength(m+1);
   double *c = c_store.get(); // squared lengths of Gramm-Schmidt basis vectors

   UniqueArray<double> b_store;
   b_store.SetLength(m+1);
   double *b = b_store.get(); // squared lengths of basis vectors

   for (i = 1; i <=m; i++)
      for (j = 1; j <= n; j++) {
         conv(B1[i][j], B(i, j));
         CheckFinite(&B1[i][j]);
      }

   for (i = 1; i <= m; i++) {
      b[i] = InnerProduct(B1[i], B1[i], n);
      CheckFinite(&b[i]);
   }

   new_m = ll_LLL_FP(B, U, delta, deep, check, B1, mu, b, c, m, 1, quit);
   dep = m - new_m;
   m = new_m;

   if (dep > 0) {
      // for consistency, we move all of the zero rows to the front

      for (i = 0; i < m; i++) {
         swap(B(m+dep-i), B(m-i));
         if (U) swap((*U)(m+dep-i), (*U)(m-i));
      }
   }

   return m;
}

long LLL_FP(mat_ZZ& B, double delta, long deep, LLLCheckFct check,
           long verb)
{
   verbose = verb;
   RR_GS_time = 0;
   NumSwaps = 0;
   if (verbose) {
      StartTime = GetTime();
      LastTime = StartTime;
   }

   if (delta < 0.50 || delta >= 1) LogicError("LLL_FP: bad delta");
   if (deep < 0) LogicError("LLL_FP: bad deep");
   return LLL_FP(B, 0, delta, deep, check);
}

long LLL_FP(mat_ZZ& B, mat_ZZ& U, double delta, long deep,
           LLLCheckFct check, long verb)
{
   verbose = verb;
   RR_GS_time = 0;
   NumSwaps = 0;
   if (verbose) {
      StartTime = GetTime();
      LastTime = StartTime;
   }

   if (delta < 0.50 || delta >= 1) LogicError("LLL_FP: bad delta");
   if (deep < 0) LogicError("LLL_FP: bad deep");
   return LLL_FP(B, &U, delta, deep, check);
}

static vec_double BKZConstant;

static
void ComputeBKZConstant(long beta, long p)
{
   const double c_PI = 3.14159265358979323846264338328;
   const double LogPI = 1.14472988584940017414342735135;

   BKZConstant.SetLength(beta-1);

   vec_double Log;
   Log.SetLength(beta);

   long i, j, k;
   double x, y;

   for (j = 1; j <= beta; j++)
      Log(j) = log(double(j));

   for (i = 1; i <= beta-1; i++) {
      // First, we compute x = gamma(i/2)^{2/i}

      k = i/2;

      if ((i & 1) == 0) { // i even
         x = 0;
         for (j = 1; j <= k; j++)
            x = x + Log(j);

         x = x * (1/double(k));

         x = exp(x);
      }
      else { // i odd
         x = 0;
         for (j = k + 2; j <= 2*k + 2; j++)
            x = x + Log(j);

         x = 0.5*LogPI + x - 2*(k+1)*Log(2);

         x = x * (2.0/double(i));

         x = exp(x);
      }

      // Second, we compute y = 2^{2*p/i}

      y = -(2*p/double(i))*Log(2);
      y = exp(y);

      BKZConstant(i) = x*y/c_PI;
   }
}

static vec_double BKZThresh;

static
void ComputeBKZThresh(double *c, long beta)
{
   BKZThresh.SetLength(beta-1);

   long i;
   double x;

   x = 0;

   for (i = 1; i <= beta-1; i++) {
      x += log(c[i-1]);
      BKZThresh(i) = exp(x/double(i))*BKZConstant(i);
      if (!IsFinite(&BKZThresh(i))) BKZThresh(i) = 0;
   }
}

static
void BKZStatus(double tt, double enum_time, unsigned long NumIterations,
               unsigned long NumTrivial, unsigned long NumNonTrivial,
               unsigned long NumNoOps, long m,
               const mat_ZZ& B)
{
   cerr << "---- BKZ_FP status ----\n";
   cerr << "elapsed time: ";
   PrintTime(cerr, tt-StartTime);
   cerr << ", enum time: ";
   PrintTime(cerr, enum_time);
   cerr << ", iter: " << NumIterations << "\n";
   cerr << "triv: " << NumTrivial;
   cerr << ", nontriv: " << NumNonTrivial;
   cerr << ", no ops: " << NumNoOps;
   cerr << ", rank: " << m;
   cerr << ", swaps: " << NumSwaps << "\n";

   ZZ t1;
   long i;
   double prodlen = 0;

   for (i = 1; i <= m; i++) {
      InnerProduct(t1, B(i), B(i));
      if (!IsZero(t1))
         prodlen += log(t1);
   }

   cerr << "log of prod of lengths: " << prodlen/(2.0*log(2.0)) << "\n";

   if (LLLDumpFile) {
      cerr << "dumping to " << LLLDumpFile << "...";

      ofstream f;
      OpenWrite(f, LLLDumpFile);

      f << "[";
      for (i = 1; i <= m; i++) {
         f << B(i) << "\n";
      }
      f << "]\n";

      f.close();

      cerr << "\n";
   }

   LastTime = tt;

}

static
long BKZ_FP(mat_ZZ& BB, mat_ZZ* UU, double delta,
         long beta, long prune, LLLCheckFct check)
{

   long m = BB.NumRows();
   long n = BB.NumCols();
   long m_orig = m;

   long i, j;
   ZZ MU;

   double t1;
   ZZ T1;
   double *tp;

   init_red_fudge();

   mat_ZZ B;
   B = BB;

   B.SetDims(m+1, n);

   Unique2DArray<double> B1_store;
   B1_store.SetDimsFrom1(m+2, n+1);
   double **B1 = B1_store.get();  // approximates B

   Unique2DArray<double> mu_store;
   mu_store.SetDimsFrom1(m+2, m+1);
   double **mu = mu_store.get();

   UniqueArray<double> c_store;
   c_store.SetLength(m+2);
   double *c = c_store.get(); // squared lengths of Gramm-Schmidt basis vectors

   UniqueArray<double> b_store;
   b_store.SetLength(m+2);
   double *b = b_store.get(); // squared lengths of basis vectors

   double cbar;

   UniqueArray<double> ctilda_store;
   ctilda_store.SetLength(m+2);
   double *ctilda = ctilda_store.get();

   UniqueArray<double> vvec_store;
   vvec_store.SetLength(m+2);
   double *vvec = vvec_store.get();

   UniqueArray<double> yvec_store;
   yvec_store.SetLength(m+2);
   double *yvec = yvec_store.get();

   UniqueArray<double> uvec_store;
   uvec_store.SetLength(m+2);
   double *uvec = uvec_store.get();

   UniqueArray<double> utildavec_store;
   utildavec_store.SetLength(m+2);
   double *utildavec = utildavec_store.get();

   UniqueArray<long> Deltavec_store;
   Deltavec_store.SetLength(m+2);
   long *Deltavec = Deltavec_store.get();

   UniqueArray<long> deltavec_store;
   deltavec_store.SetLength(m+2);
   long *deltavec = deltavec_store.get();;

   mat_ZZ Ulocal;
   mat_ZZ *U;

   if (UU) {
      Ulocal.SetDims(m+1, m);
      for (i = 1; i <= m; i++)
         conv(Ulocal(i, i), 1);
      U = &Ulocal;
   }
   else
      U = 0;

   long quit;
   long new_m;
   long z, jj, kk;
   long s, t;
   long h;
   double eta;

   for (i = 1; i <=m; i++)
      for (j = 1; j <= n; j++) {
         conv(B1[i][j], B(i, j));
         CheckFinite(&B1[i][j]);
      }

   for (i = 1; i <= m; i++) {
      b[i] = InnerProduct(B1[i], B1[i], n);
      CheckFinite(&b[i]);
   }

   m = ll_LLL_FP(B, U, delta, 0, check, B1, mu, b, c, m, 1, quit);

   double tt;

   double enum_time = 0;
   unsigned long NumIterations = 0;
   unsigned long NumTrivial = 0;
   unsigned long NumNonTrivial = 0;
   unsigned long NumNoOps = 0;

   long verb = verbose;

   verbose = 0;

   long clean = 1;

   if (m < m_orig) {
      for (i = m_orig+1; i >= m+2; i--) {
         // swap i, i-1

         swap(B(i), B(i-1));
         if (U) swap((*U)(i), (*U)(i-1));
      }
   }

   if (!quit && m > 1) {
      if (beta > m) beta = m;

      if (prune > 0)
         ComputeBKZConstant(beta, prune);

      z = 0;
      jj = 0;

      while (z < m-1) {
         jj++;
         kk = min(jj+beta-1, m);

         if (jj == m) {
            jj = 1;
            kk = beta;
            clean = 1;
         }

         if (verb) {
            tt = GetTime();
            if (tt > LastTime + LLLStatusInterval)
               BKZStatus(tt, enum_time, NumIterations, NumTrivial,
                         NumNonTrivial, NumNoOps, m, B);
         }

         // ENUM

         double tt1;

         if (verb) {
            tt1 = GetTime();
         }

         if (prune > 0)
            ComputeBKZThresh(&c[jj], kk-jj+1);

         cbar = c[jj];
         utildavec[jj] = uvec[jj] = 1;

         yvec[jj] = vvec[jj] = 0;
         Deltavec[jj] = 0;

         s = t = jj;
         deltavec[jj] = 1;

         for (i = jj+1; i <= kk+1; i++) {
            ctilda[i] = uvec[i] = utildavec[i] = yvec[i] = 0;
            Deltavec[i] = 0;
            vvec[i] = 0;
            deltavec[i] = 1;
         }

         long enum_cnt = 0;

         while (t <= kk) {
            if (verb) {
               enum_cnt++;
               if (enum_cnt > 100000) {
                  enum_cnt = 0;
                  tt = GetTime();
                  if (tt > LastTime + LLLStatusInterval) {
                     enum_time += tt - tt1;
                     tt1 = tt;
                     BKZStatus(tt, enum_time, NumIterations, NumTrivial,
                               NumNonTrivial, NumNoOps, m, B);
                  }
               }
            }

            ctilda[t] = ctilda[t+1] +
               (yvec[t]+utildavec[t])*(yvec[t]+utildavec[t])*c[t];

            ForceToMem(&ctilda[t]);  // prevents an infinite loop

            if (prune > 0 && t > jj) {
               eta = BKZThresh(t-jj);
            }
            else
               eta = 0;

            if (ctilda[t] < cbar - eta) {
               if (t > jj) {
                  t--;
                  t1 = 0;
                  for (i = t+1; i <= s; i++)
                     t1 += utildavec[i]*mu[i][t];
                  yvec[t] = t1;
                  t1 = -t1;
                  if (t1 >= 0)
                     t1 = ceil(t1-0.5);
                  else
                     t1 = floor(t1+0.5);
                  utildavec[t] = vvec[t] = t1;
                  Deltavec[t] = 0;
                  if (utildavec[t] > -yvec[t])
                     deltavec[t] = -1;
                  else
                     deltavec[t] = 1;
               }
               else {
                  cbar = ctilda[jj];
                  for (i = jj; i <= kk; i++) {
                     uvec[i] = utildavec[i];
                  }
               }
            }
            else {
               t++;
               s = max(s, t);
               if (t < s) Deltavec[t] = -Deltavec[t];
               if (Deltavec[t]*deltavec[t] >= 0) Deltavec[t] += deltavec[t];
               utildavec[t] = vvec[t] + Deltavec[t];
            }
         }

         if (verb) {
            tt1 = GetTime() - tt1;
            enum_time += tt1;
         }

         NumIterations++;

         h = min(kk+1, m);

         if ((delta - 8*red_fudge)*c[jj] > cbar) {

            clean = 0;

            // we treat the case that the new vector is b_s (jj < s <= kk)
            // as a special case that appears to occur most of the time.

            s = 0;
            for (i = jj+1; i <= kk; i++) {
               if (uvec[i] != 0) {
                  if (s == 0)
                     s = i;
                  else
                     s = -1;
               }
            }

            if (s == 0) LogicError("BKZ_FP: internal error");

            if (s > 0) {
               // special case

               NumTrivial++;

               for (i = s; i > jj; i--) {
                  // swap i, i-1
                  swap(B(i-1), B(i));
                  if (U) swap((*U)(i-1), (*U)(i));
                  tp = B1[i-1]; B1[i-1] = B1[i]; B1[i] = tp;
                  t1 = b[i-1]; b[i-1] = b[i]; b[i] = t1;
               }

               // cerr << "special case\n";
               new_m = ll_LLL_FP(B, U, delta, 0, check,
                                B1, mu, b, c, h, jj, quit);
               if (new_m != h) LogicError("BKZ_FP: internal error");
               if (quit) break;
            }
            else {
               // the general case

               NumNonTrivial++;

               for (i = 1; i <= n; i++) conv(B(m+1, i), 0);

               if (U) {
                  for (i = 1; i <= m_orig; i++)
                     conv((*U)(m+1, i), 0);
               }

               for (i = jj; i <= kk; i++) {
                  if (uvec[i] == 0) continue;
                  conv(MU, uvec[i]);
                  RowTransform2(B(m+1), B(i), MU);
                  if (U) RowTransform2((*U)(m+1), (*U)(i), MU);
               }

               for (i = m+1; i >= jj+1; i--) {
                  // swap i, i-1
                  swap(B(i-1), B(i));
                  if (U) swap((*U)(i-1), (*U)(i));
                  tp = B1[i-1]; B1[i-1] = B1[i]; B1[i] = tp;
                  t1 = b[i-1]; b[i-1] = b[i]; b[i] = t1;
               }

               for (i = 1; i <= n; i++) {
                  conv(B1[jj][i], B(jj, i));
                  CheckFinite(&B1[jj][i]);
               }

               b[jj] = InnerProduct(B1[jj], B1[jj], n);
               CheckFinite(&b[jj]);

               if (b[jj] == 0) LogicError("BKZ_FP: internal error");

               // remove linear dependencies

               // cerr << "general case\n";
               new_m = ll_LLL_FP(B, U, delta, 0, 0, B1, mu, b, c, kk+1, jj, quit);

               if (new_m != kk) LogicError("BKZ_FP: internal error");

               // remove zero vector

               for (i = kk+2; i <= m+1; i++) {
                  // swap i, i-1
                  swap(B(i-1), B(i));
                  if (U) swap((*U)(i-1), (*U)(i));
                  tp = B1[i-1]; B1[i-1] = B1[i]; B1[i] = tp;
                  t1 = b[i-1]; b[i-1] = b[i]; b[i] = t1;
               }

               quit = 0;
               if (check) {
                  for (i = 1; i <= kk; i++)
                     if ((*check)(B(i))) {
                        quit = 1;
                        break;
                     }
               }

               if (quit) break;

               if (h > kk) {
                  // extend reduced basis

                  new_m = ll_LLL_FP(B, U, delta, 0, check,
                                   B1, mu, b, c, h, h, quit);

                  if (new_m != h) LogicError("BKZ_FP: internal error");
                  if (quit) break;
               }
            }

            z = 0;
         }
         else {
            // LLL_FP
            // cerr << "progress\n";

            NumNoOps++;

            if (!clean) {
               new_m =
                  ll_LLL_FP(B, U, delta, 0, check, B1, mu, b, c, h, h, quit);
               if (new_m != h) LogicError("BKZ_FP: internal error");
               if (quit) break;
            }

            z++;
         }
      }
   }

   if (verb) {
      BKZStatus(GetTime(), enum_time, NumIterations, NumTrivial, NumNonTrivial,
                NumNoOps, m, B);
   }

   // clean up

   if (m_orig > m) {
      // for consistency, we move zero vectors to the front

      for (i = m+1; i <= m_orig; i++) {
         swap(B(i), B(i+1));
         if (U) swap((*U)(i), (*U)(i+1));
      }

      for (i = 0; i < m; i++) {
         swap(B(m_orig-i), B(m-i));
         if (U) swap((*U)(m_orig-i), (*U)(m-i));
      }
   }

   B.SetDims(m_orig, n);
   BB = B;

   if (U) {
      U->SetDims(m_orig, m_orig);
      *UU = *U;
   }

   return m;
}

long BKZ_FP(mat_ZZ& BB, mat_ZZ& UU, double delta,
         long beta, long prune, LLLCheckFct check, long verb)
{
   verbose = verb;
   RR_GS_time = 0;
   NumSwaps = 0;
   if (verbose) {
      StartTime = GetTime();
      LastTime = StartTime;
   }

   if (delta < 0.50 || delta >= 1) LogicError("BKZ_FP: bad delta");
   if (beta < 2) LogicError("BKZ_FP: bad block size");

   return BKZ_FP(BB, &UU, delta, beta, prune, check);
}

long BKZ_FP(mat_ZZ& BB, double delta,
         long beta, long prune, LLLCheckFct check, long verb)
{
   verbose = verb;
   RR_GS_time = 0;
   NumSwaps = 0;
   if (verbose) {
      StartTime = GetTime();
      LastTime = StartTime;
   }

   if (delta < 0.50 || delta >= 1) LogicError("BKZ_FP: bad delta");
   if (beta < 2) LogicError("BKZ_FP: bad block size");

   return BKZ_FP(BB, 0, delta, beta, prune, check);
}

NTL_END_IMPL


// Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

// 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 UUID_51968952D30A11DFAFE78CE3DFD72085
#define UUID_51968952D30A11DFAFE78CE3DFD72085

#include <boost/qvm/deduce_vec.hpp>
#include <boost/qvm/enable_if.hpp>
#include <boost/qvm/inline.hpp>
#include <boost/qvm/quat_traits.hpp>

namespace boost {
namespace qvm {
template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<is_quat<A>::value && is_vec<B>::value &&
                                  vec_traits<B>::dim == 3,
                              deduce_vec2<A, B, 3>>::type
    operator*(A const &a, B const &b) {
  typedef typename deduce_vec2<A, B, 3>::type R;
  typedef typename quat_traits<A>::scalar_type TA;
  typedef typename vec_traits<B>::scalar_type TB;
  TA const aa = quat_traits<A>::template read_element<0>(a);
  TA const ab = quat_traits<A>::template read_element<1>(a);
  TA const ac = quat_traits<A>::template read_element<2>(a);
  TA const ad = quat_traits<A>::template read_element<3>(a);
  TA const t2 = aa * ab;
  TA const t3 = aa * ac;
  TA const t4 = aa * ad;
  TA const t5 = -ab * ab;
  TA const t6 = ab * ac;
  TA const t7 = ab * ad;
  TA const t8 = -ac * ac;
  TA const t9 = ac * ad;
  TA const t10 = -ad * ad;
  TB const bx = vec_traits<B>::template read_element<0>(b);
  TB const by = vec_traits<B>::template read_element<1>(b);
  TB const bz = vec_traits<B>::template read_element<2>(b);
  R r;
  vec_traits<R>::template write_element<0>(r) =
      2 * ((t8 + t10) * bx + (t6 - t4) * by + (t3 + t7) * bz) + bx;
  vec_traits<R>::template write_element<1>(r) =
      2 * ((t4 + t6) * bx + (t5 + t10) * by + (t9 - t2) * bz) + by;
  vec_traits<R>::template write_element<2>(r) =
      2 * ((t7 - t3) * bx + (t2 + t9) * by + (t5 + t8) * bz) + bz;
  return r;
}

namespace sfinae {
using ::boost::qvm::operator*;
}
} // namespace qvm
} // namespace boost

#endif


//
// Copyright (c) 2017 CNRS
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

#include <tsid/solvers/solver-HQP-factory.hxx>
#include <tsid/solvers/solver-HQP-eiquadprog.hpp>
#include <tsid/solvers/solver-HQP-eiquadprog-rt.hpp>
#include <tsid/math/utils.hpp>
#include <tsid/math/constraint-equality.hpp>
#include <tsid/math/constraint-inequality.hpp>
#include <tsid/math/constraint-bound.hpp>
#include <tsid/utils/stop-watch.hpp>
#include <tsid/utils/statistics.hpp>

#define CHECK_LESS_THAN(A,B) BOOST_CHECK_MESSAGE(A<B, #A<<": "<<A<<">"<<B)
#define REQUIRE_FINITE(A) BOOST_REQUIRE_MESSAGE(isFinite(A), #A<<": "<<A)

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

//BOOST_AUTO_TEST_CASE ( test_eiquadprog_unconstrained)
//{
//  std::cout << "test_eiquadprog_unconstrained\n";
//  using namespace tsid;
//  using namespace math;
//  using namespace solvers;
//  using namespace std;

//  const unsigned int n = 5;
//  const unsigned int m = 3;
//  const unsigned int neq = 0;
//  const unsigned int nin = 0;
//  const double damping = 1e-4;
//  SolverHQPBase * solver = SolverHQPBase::getNewSolver(SOLVER_HQP_EIQUADPROG, "solver-eiquadprog");
//  solver->resize(n, neq, nin);

//  HQPData HQPData(2);
//  Matrix A = Matrix::Random(m, n);
//  Vector b = Vector::Random(m);
//  ConstraintEquality constraint1("c1", A, b);
//  HQPData[1].push_back(make_pair<double, ConstraintBase*>(1.0, &constraint1));

//  ConstraintEquality constraint2("c2", Matrix::Identity(n,n), Vector::Zero(n));
//  HQPData[1].push_back(make_pair<double, ConstraintBase*>(damping, &constraint2));

//  const HQPOutput & output = solver->solve(HQPData);
//  BOOST_CHECK_MESSAGE(output.status==HQP_STATUS_OPTIMAL, "Status "+toString(output.status));

//  Vector x(n);
//  svdSolveWithDamping(A, b, x, damping);
//  BOOST_CHECK_MESSAGE(x.isApprox(output.x, 1e-3), "Solution error: "+toString(x-output.x));
//}

//BOOST_AUTO_TEST_CASE ( test_eiquadprog_equality_constrained)
//{
//  std::cout << "test_eiquadprog_equality_constrained\n";
//  using namespace tsid;
//  using namespace math;
//  using namespace solvers;
//  using namespace std;

//  const unsigned int n = 5;
//  const unsigned int m = 3;
//  const unsigned int neq = 2;
//  const unsigned int nin = 0;
//  const double damping = 1e-4;
//  SolverHQPBase * solver = SolverHQPBase::getNewSolver(SOLVER_HQP_EIQUADPROG,
//                                                           "solver-eiquadprog");
//  solver->resize(n, neq, nin);

//  HQPData HQPData(2);

//  Matrix A_eq = Matrix::Random(neq, n);
//  Vector b_eq = Vector::Random(neq);
//  ConstraintEquality eq_constraint("eq1", A_eq, b_eq);
//  HQPData[0].push_back(make_pair<double, ConstraintBase*>(1.0, &eq_constraint));

//  Matrix A = Matrix::Random(m, n);
//  Vector b = Vector::Random(m);
//  ConstraintEquality constraint1("c1", A, b);
//  HQPData[1].push_back(make_pair<double, ConstraintBase*>(1.0, &constraint1));

//  ConstraintEquality constraint2("c2", Matrix::Identity(n,n), Vector::Zero(n));
//  HQPData[1].push_back(make_pair<double, ConstraintBase*>(damping, &constraint2));

//  const HQPOutput & output = solver->solve(HQPData);
//  BOOST_REQUIRE_MESSAGE(output.status==HQP_STATUS_OPTIMAL,
//                        "Status "+toString(output.status));
//  BOOST_CHECK_MESSAGE(b_eq.isApprox(A_eq*output.x),
//                      "Constraint error: "+toString(b_eq-A_eq*output.x));
//}

//BOOST_AUTO_TEST_CASE ( test_eiquadprog_inequality_constrained)
//{
//  std::cout << "test_eiquadprog_inequality_constrained\n";
//  using namespace tsid;
//  using namespace math;
//  using namespace solvers;
//  using namespace std;

//  const unsigned int n = 5;
//  const unsigned int m = 3;
//  const unsigned int neq = 0;
//  const unsigned int nin = 3;
//  const double damping = 1e-5;
//  SolverHQPBase * solver = SolverHQPBase::getNewSolver(SOLVER_HQP_EIQUADPROG,
//                                                           "solver-eiquadprog");
//  solver->resize(n, neq, nin);

//  HQPData HQPData(2);

//  Matrix A = Matrix::Random(m, n);
//  Vector b = Vector::Random(m);
//  ConstraintEquality constraint1("c1", A, b);
//  HQPData[1].push_back(make_pair<double, ConstraintBase*>(1.0, &constraint1));

//  ConstraintEquality constraint2("c2", Matrix::Identity(n,n), Vector::Zero(n));
//  HQPData[1].push_back(make_pair<double, ConstraintBase*>(damping, &constraint2));

//  Vector x(n);
//  svdSolveWithDamping(A, b, x, damping);

//  Matrix A_in = Matrix::Random(nin, n);
//  Vector A_lb = A_in*x - Vector::Ones(nin) + Vector::Random(nin);
//  Vector A_ub = A_in*x + Vector::Ones(nin) + Vector::Random(nin);
//  ConstraintInequality in_constraint("in1", A_in, A_lb, A_ub);
//  HQPData[0].push_back(make_pair<double, ConstraintBase*>(1.0, &in_constraint));

//  const HQPOutput & output = solver->solve(HQPData);
//  BOOST_REQUIRE_MESSAGE(output.status==HQP_STATUS_OPTIMAL,
//                        "Status "+toString(output.status));
//  BOOST_CHECK_MESSAGE(((A_in*output.x).array() <= A_ub.array()).all(),
//                      "Upper bound error: "+toString(A_ub - A_in*output.x));
//  BOOST_CHECK_MESSAGE(((A_in*output.x).array() >= A_lb.array()).all(),
//                      "Lower bound error: "+toString(A_in*output.x-A_lb));

//  A_lb[0] += 2.0;
//  in_constraint.setLowerBound(A_lb);
//  const HQPOutput & output2 = solver->solve(HQPData);
//  BOOST_REQUIRE_MESSAGE(output.status==HQP_STATUS_OPTIMAL,
//                        "Status "+toString(output.status));
//  BOOST_CHECK_MESSAGE((A_in.row(0)*output.x).isApproxToConstant(A_lb[0]),
//      "Active constraint error: "+toString(A_in.row(0)*output.x-A_lb.head<1>()));
//}

#define PROFILE_EIQUADPROG "Eiquadprog"
#define PROFILE_EIQUADPROG_RT "Eiquadprog Real Time"
#define PROFILE_EIQUADPROG_FAST "Eiquadprog Fast"

BOOST_AUTO_TEST_CASE ( test_eiquadprog_classic_vs_rt_vs_fast)
{
  std::cout << "test_eiquadprog_classic_vs_rt_vs_fast\n";
  using namespace tsid;
  using namespace math;
  using namespace solvers;

  const double EPS = 1e-8;
#ifdef NDEBUG
  const unsigned int nTest = 1000;
#else
  const unsigned int nTest = 100;
#endif
  const unsigned int n = 60;
  const unsigned int neq = 36;
  const unsigned int nin = 40;
  const double damping = 1e-10;

  // variance of the normal distribution used for generating initial bounds
  const double NORMAL_DISTR_VAR = 10.0;
  // each cycle the gradient is perturbed by a Gaussian random variable with this covariance
  const double GRADIENT_PERTURBATION_VARIANCE = 1e-2;
  // each cycle the Hessian is perturbed by a Gaussian random variable with this covariance
  const double HESSIAN_PERTURBATION_VARIANCE = 1e-1;
  // min margin of activation of the inequality constraints for the first QP
  const double MARGIN_PERC = 1e-3;

  std::cout<<"Gonna perform "<<nTest<<" tests with "<<n<<" variables, "<<
	  neq<<" equalities, "<<nin<<" inequalities\n";

  // CREATE SOLVERS
  SolverHQPBase * solver_rt = SolverHQPFactory::createNewSolver<n,neq,nin>(SOLVER_HQP_EIQUADPROG_RT,
                                                                           "eiquadprog_rt");
  solver_rt->resize(n, neq, nin);

  SolverHQPBase * solver_fast = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST,
                                                                  "eiquadprog_fast");
  solver_fast->resize(n, neq, nin);

  SolverHQPBase * solver = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG,
                                                             "eiquadprog");
  solver->resize(n, neq, nin);

  // CREATE PROBLEM DATA
  HQPData HQPData(2);

  Matrix A1 = Matrix::Random(n, n);
  Vector b1 = Vector::Random(n);
  ConstraintEquality cost("c1", A1, b1);
  HQPData[1].push_back(make_pair<double, ConstraintBase*>(1.0, &cost));

  Vector x(n);
  svdSolveWithDamping(A1, b1, x, damping);

  Matrix A_in = Matrix::Random(nin, n);
  Vector A_lb = Vector::Random(nin)*NORMAL_DISTR_VAR;
  Vector A_ub = Vector::Random(nin)*NORMAL_DISTR_VAR;
  Vector constrVal = A_in*x;
  for(unsigned int i=0; i<nin; i++)
  {
      if(constrVal[i]>A_ub[i])
      {
//        std::cout<<"Inequality constraint "<<i<<" active at first iteration. UB="<<A_ub[i]<<", value="<<constrVal[i]<<endl;
        A_ub[i] = constrVal[i] + MARGIN_PERC*fabs(constrVal[i]);
      }
      if(constrVal[i]<A_lb[i])
      {
//        std::cout<<"Inequality constraint "<<i<<" active at first iteration. LB="<<A_lb[i]<<", value="<<constrVal[i]<<endl;
        A_lb[i] = constrVal[i] - MARGIN_PERC*fabs(constrVal[i]);
      }
  }
  ConstraintInequality in_constraint("in1", A_in, A_lb, A_ub);
  HQPData[0].push_back(make_pair<double, ConstraintBase*>(1.0, &in_constraint));

  Matrix A_eq = Matrix::Random(neq, n);
  Vector b_eq = A_eq*x;
  ConstraintEquality eq_constraint("eq1", A_eq, b_eq);
  HQPData[0].push_back(make_pair<double, ConstraintBase*>(1.0, &eq_constraint));

  // Prepare random data to perturb initial QP
  std::vector<Vector> gradientPerturbations(nTest);
  std::vector<Matrix> hessianPerturbations(nTest);
  for(unsigned int i=0; i<nTest; i++)
  {
    gradientPerturbations[i] = Vector::Random(n)*GRADIENT_PERTURBATION_VARIANCE;
    hessianPerturbations[i] = Matrix::Random(n,n)*HESSIAN_PERTURBATION_VARIANCE;
  }

  // START COMPUTING
  for(unsigned int i=0; i<nTest; i++)
  {
    if(true || i==0)
    {
      cost.matrix() += hessianPerturbations[i];
      cost.vector() += gradientPerturbations[i];
    }

    getProfiler().start(PROFILE_EIQUADPROG_FAST);
    const HQPOutput & output_fast = solver_fast->solve(HQPData);
    getProfiler().stop(PROFILE_EIQUADPROG_FAST);

    getProfiler().start(PROFILE_EIQUADPROG_RT);
    const HQPOutput & output_rt = solver_rt->solve(HQPData);
    getProfiler().stop(PROFILE_EIQUADPROG_RT);

    getProfiler().start(PROFILE_EIQUADPROG);
    const HQPOutput & output    = solver->solve(HQPData);
    getProfiler().stop(PROFILE_EIQUADPROG);

    getStatistics().store("active inequalities", (double)output_rt.activeSet.size());
    getStatistics().store("solver iterations", output_rt.iterations);

    BOOST_REQUIRE_MESSAGE(output.status==output_rt.status,
                          "Status "+SolverHQPBase::HQP_status_string[output.status]+
                          " Status RT "+SolverHQPBase::HQP_status_string[output_rt.status]);
    BOOST_REQUIRE_MESSAGE(output.status==output_fast.status,
                          "Status "+SolverHQPBase::HQP_status_string[output.status]+
                          " Status FAST "+SolverHQPBase::HQP_status_string[output_fast.status]);

    if(output.status==HQP_STATUS_OPTIMAL)
    {
      CHECK_LESS_THAN((A_eq*output.x - b_eq).norm(), EPS);

      BOOST_CHECK_MESSAGE(((A_in*output.x).array() <= A_ub.array() + EPS).all(),
          "Lower bounds violated: "+toString((A_ub-A_in*output.x).transpose()));

      BOOST_CHECK_MESSAGE(((A_in*output.x).array() >= A_lb.array() - EPS).all(),
          "Upper bounds violated: "+toString((A_in*output.x - A_lb).transpose()));

      BOOST_CHECK_MESSAGE(output.x.isApprox(output_rt.x, EPS),
  //                        "Sol "+toString(output.x.transpose())+
  //                        "\nSol RT "+toString(output_rt.x.transpose())+
                          "\nDiff RT: "+toString((output.x-output_rt.x).norm()));

      BOOST_CHECK_MESSAGE(output_rt.x.isApprox(output_fast.x, EPS),
  //                        "Sol RT"+toString(output_rt.x.transpose())+
  //                        "\nSol FAST "+toString(output_fast.x.transpose())+
                          "\nDiff FAST: "+toString((output_rt.x-output_fast.x).norm()));
    }
  }

  std::cout<<"\n### TEST FINISHED ###\n";
  getProfiler().report_all(3, std::cout);
  getStatistics().report_all(1, std::cout);
}

BOOST_AUTO_TEST_SUITE_END ()


/*
 * MutexAddConstraint.cpp
 * @brief Mutually exclusive constraint
 * @date Mar 20, 2022
 * @author Yoonwoo Kim
 */

#include <gtsam/base/Testable.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <cpp/planning/MutexAddConstraint.h>
#include <cpp/planning/NotSingleValueConstraint.h>
#include <cpp/planning/SingleValueConstraint.h>

#include <boost/make_shared.hpp>
using namespace gtsam;
using namespace std;

namespace gtsam_planner {

/* ************************************************************************* */
MutexAddConstraint::MutexAddConstraint(const DiscreteKey& dkey,
  const DiscreteKeys& dkeys, const vector<size_t>& values)
  : DiscreteFactor(dkeys.indices()) {
  for (const DiscreteKey& dkey : dkeys) cardinalities_.insert(dkey);
  for (const size_t& value : values) values_.push_back(value);
  dkey_ = dkey;
  dkeys_ = std::vector<DiscreteKey>(dkeys.begin() + 1, dkeys.end());;
}

/* ************************************************************************* */
void MutexAddConstraint::print(const std::string& s, const KeyFormatter& formatter) const {
  cout << s << "MutexAddConstraint on "
       << "j=" << formatter(dkey_.first) << endl;
}

/* ************************************************************************* */
double MutexAddConstraint::operator()(const DiscreteValues& values) const {
  size_t count = 0;
  for (size_t i=0; i < values_.size(); i++) {
    size_t value = values.at(dkeys_[i].first);
    if (value == values_[i]) count++;
    if (count > 1.0) return 0.0;
  }
  return 1.0;
}

/* ************************************************************************* */
double add_m(const double& a, const double& b) {
    return a + b;
}

/* ************************************************************************* */
DecisionTreeFactor MutexAddConstraint::toDecisionTreeFactor() const {
  // vector of decision tree factors
  vector<DecisionTreeFactor> factors;
  // number of keys
  size_t nrKeys = dkeys_.size();
  for (size_t i = 0; i < nrKeys; i++) {
    SingleValueConstraint single(dkeys_[i], values_[i]);
    DecisionTreeFactor singleTree = single.toDecisionTreeFactor();
    for (size_t j = 0; j < nrKeys; j++) {
      if (i == j) continue;
      NotSingleValueConstraint notSingle(dkeys_[j], values_[j]);
      singleTree = singleTree * notSingle.toDecisionTreeFactor();
    }
    factors.push_back(singleTree);
  }

  SingleValueConstraint single_comb(dkey_, 0);
  DecisionTreeFactor combined = single_comb * factors[0];
  size_t nrFactors = factors.size();
  for (size_t i=1; i<nrFactors; i++) {
    SingleValueConstraint single(dkey_, i);
    DecisionTreeFactor single_m = single * factors[i];
    combined = combined.apply(single_m, &add_m);
  }
  return combined;
}

/* ************************************************************************* */
DecisionTreeFactor MutexAddConstraint::operator*(const DecisionTreeFactor& f) const {
  // TODO: can we do this more efficiently?
  return toDecisionTreeFactor() * f;
}

/* ************************************************************************* */
}  // namespace gtsam


#pragma once
#include <pcl/point_types.h>
#include <Eigen/Core>
#include <vector>
#include "macros.h"

typedef std::vector<int> IntVec;

namespace cloudproc {

TRAJOPT_API void ConvexDecomp1(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud, float thresh,
    /*optional outputs: */ std::vector<IntVec>* indices, std::vector< IntVec >* hull_indices);

TRAJOPT_API void ConvexDecomp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud, const Eigen::MatrixXf& dirs, float thresh,
    /*optional outputs: */ std::vector<IntVec>* indices, std::vector< IntVec >* hull_indices);
}


#include <vector>
#include <armadillo>
#include "Ray.hpp"
#ifndef BOX_HPP
#define	BOX_HPP

using namespace arma;
using namespace std;

class Box {

public:
    vector <vec> bounds;

public:
    Box();

    Box(vec min, vec max){
	this->bounds.push_back(min);
	this->bounds.push_back(max);
    }

    vec getMin() {
        return bounds[0];
    }

    vec getMax() {
        return bounds[1];
    }

    bool intersect(Ray &r);

};

#endif	/* BOX_HPP */


// Copyright (C) 2011-2012 The Trustees of Indiana University.

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

//  Authors: Nick Edmonds

#include <am++/am++.hpp>
#include <am++/mpi_transport.hpp>

#include <boost/graph/use_mpi.hpp>
#include <boost/graph/distributed/compressed_sparse_row_graph.hpp>
#include <boost/graph/small_world_generator.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/lexical_cast.hpp>

using namespace boost;

int main(int argc, char* argv[])
{
  amplusplus::environment env = amplusplus::mpi_environment(argc, argv, true);
  amplusplus::transport trans = env.create_transport();

  int num_threads = 2;

  for (int i = 1; i < argc; ++i) {
    std::string arg = argv[i];

    if (arg == "--threads")
      num_threads = boost::lexical_cast<int>( argv[i+1] );
  }

  typedef compressed_sparse_row_graph<directedS, no_property, no_property, no_property,
                                      distributedS<> >
    Digraph;

  // Build an Erdos-Renyi graph to test with
  typedef small_world_iterator<minstd_rand, Digraph> SWIter;

  int n = 50000, k = 4;
  double prob = 16. / n;

  minstd_rand gen;
  gen.seed(1);
  Digraph g(edges_are_unsorted, SWIter(gen, n, k, prob), SWIter(), n, trans);

  typedef two_bit_color_map<property_map<Digraph, vertex_index_t>::type> ColorMap;
  ColorMap color(num_vertices(g), get(vertex_index, g));

  boost::graph::distributed::breadth_first_search<Digraph>
    bfs(g, make_bfs_visitor(null_visitor()), color);
  bfs.set_source(vertex(0, g));

  // Two threads now
  trans.set_nthreads(num_threads);

  boost::scoped_array<boost::thread> threads(new boost::thread[num_threads]);
  for (int i = 0; i < num_threads; ++i) {
    boost::thread thr(boost::ref(bfs), i);
    threads[i].swap(thr);
  }

  for (int i = 0; i < num_threads; ++i)
    threads[i].join();

  // Back to one thread
  trans.set_nthreads(1);

  typedef property_traits<ColorMap>::value_type ColorValue;
  typedef color_traits<ColorValue> Color;
  int visited = 0;
  BGL_FORALL_VERTICES(v, g, Digraph) {
    if (get(color, v) == Color::black())
      ++visited;
  }

  std::cerr << trans.rank() << ": visited " << visited << " vertices of " << num_vertices(g) << std::endl;

  return 0;
}


//!
//! Datatypes for MCMC.
//!
//! \file infer/mcmctypes.hpp
//! \author Lachlan McCalman
//! \date December 2013
//! \license Affero General Public License version 3 or later
//! \copyright (c) 2014, NICTA
//!

#pragma once

#include <Eigen/Core>

namespace stateline
{
  namespace mcmc
  {
    //! Used for recording swapping of chains.
    //!
    enum class SwapType
    {
      //! No swap was attempted.
      NoAttempt,

      //! A swap was accepted.
      Accept,

      //! A swap was rejected.
      Reject
    };

    //! Used to represent a state in a Markov Chain.
    //!
    struct State
    {
      //! The actual sample that the state represents.
      Eigen::VectorXd sample;

      //! The energy (negative log likelihood) of the state.
      double energy;

      //! The log density ratio of the state evaluated for the proposal distribution.
      double logDensityRatio;

      //! The inverse temperature of the chain when this state was recorded.
      double beta;

      //! Whether this state was an accepted proposal state or a copy of the previous state.
      bool accepted;

      //! The type of swap that occurred when this state was recorded.
      SwapType swapType;
    };

    //! Type representing proposal acceptance functions.
    using PropAcceptFn = std::function<bool(const State&, const State&, double)>;

    //! Type representing swap acceptance functions.
    using SwapAcceptFn = std::function<bool(const State&, const State&, double, double)>;

  }
}


#include <iostream>
#include <stdint.h>

#include <Eigen/StdVector>
#include <Eigen/Core>

#include <unordered_set>
#include <memory>
#include <vector>
#include <stdlib.h>

#include "g2o/stuff/sampler.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/robust_kernel_impl.h"
#include "g2o/core/batch_stats.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/optimization_algorithm_gauss_newton.h"
#include "g2o/core/optimization_algorithm_dogleg.h"

#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/solvers/eigen/linear_solver_eigen.h"
#include "g2o/solvers/pcg/linear_solver_pcg.h"
#include "g2o/solvers/structure_only/structure_only_solver.h"

#include "g2o/types/sba/types_six_dof_expmap.h"

#include "BundleParams.h"
#include "BALProblem.h"
#include "bal_class_g2o.h"

typedef Eigen::Map<Eigen::VectorXd> VectorRef;
typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;
typedef g2o::BlockSolver<g2o::BlockSolverTraits<9, 3>> BalBlockSolver;

/**
 * set up the vertexs and edges for the bundle adjustment
 */
void buildProblem(const BALProblem* bal_problem, g2o::SparseOptimizer* optimizer, const BundleParams& params)
{
    const int num_points = bal_problem->num_points();
    const int num_cameras = bal_problem->num_cameras();
    const int camera_block_size = bal_problem->camera_block_size();
    const int point_block_size = bal_problem->point_block_size();

    /* Set camera vertexs with initial value in the dataset */
    const double* raw_cameras = bal_problem->cameras();
    for (int i = 0; i < num_cameras; ++i) {
        ConstVectorRef temVecCamera(raw_cameras + camera_block_size * i, camera_block_size);
        VertexCameraBAL* pCamera = new VertexCameraBAL();
        pCamera->setEstimate(temVecCamera);     // initial value for the camera i
        pCamera->setId(i);                      // set id for each camera vertex

        /* remeber to add vertex into optimizer */
        optimizer->addVertex(pCamera);
    }

    /* Set point vertex with initial value in the dataset. */
    const double* raw_points = bal_problem->points();
    for (int i = 0; i < num_points; ++i) {
        ConstVectorRef temVecPoint(raw_points + point_block_size * i, point_block_size);
        VertexPointBAL* pPoint = new VertexPointBAL();
        pPoint->setEstimate(temVecPoint);       // initial value for the point i
        pPoint->setId(num_cameras + i);         // each vertex should have an unique id, no matter it is a camera vertex, or a point vertex

        // remeber to add vertex into optimizer.
        pPoint->setMarginalized(true);
        optimizer->addVertex(pPoint);
    }

    /* Set edges for graph */
    const int num_observations = bal_problem->num_observations();
    const double* observations = bal_problem->observations();
    for (int i = 0; i < num_observations; ++i) {
        EdgeObservationBAL* bal_edge = new EdgeObservationBAL();

        const int camera_id = bal_problem->camera_index()[i];               // get id for the camera
        const int point_id = bal_problem->point_index()[i] + num_cameras;   // get id for the point

        if (params.robustify) {
            g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
            rk->setDelta(1.f);
            bal_edge->setRobustKernel(rk);
        }

        /* set the vertex by the ids for an edge observation */
        bal_edge->setVertex(0, dynamic_cast<VertexCameraBAL*>(optimizer->vertex(camera_id)));
        bal_edge->setVertex(1, dynamic_cast<VertexPointBAL*>(optimizer->vertex(point_id)));
        bal_edge->setInformation(Eigen::Matrix2d::Identity());
        bal_edge->setMeasurement(Eigen::Vector2d(observations[2*i +0], observations[2*i +1]));

        optimizer->addEdge(bal_edge);
    }
}

void writeToBALProblem(BALProblem* bal_problem, g2o::SparseOptimizer* optimizer)
{
    const int num_points = bal_problem->num_points();
    const int num_cameras = bal_problem->num_cameras();
    const int camera_block_size = bal_problem->camera_block_size();
    const int point_block_size = bal_problem->point_block_size();

    double* raw_cameras = bal_problem->mutable_cameras();
    for (int i = 0; i < num_cameras; ++i) {
        VertexCameraBAL* pCamera = dynamic_cast<VertexCameraBAL*>(optimizer->vertex(i));
        Eigen::VectorXd newCameraVec = pCamera->estimate();
        memcpy(raw_cameras + i * camera_block_size, newCameraVec.data(), sizeof(double) * camera_block_size);
    }

    double* raw_points = bal_problem->mutable_points();
    for(int i = 0; i < num_points; ++i) {
        VertexPointBAL* pPoint = dynamic_cast<VertexPointBAL*>(optimizer->vertex(i + num_cameras));
        Eigen:: Vector3d newPointVec = pPoint->estimate();
        memcpy(raw_points + i * point_block_size, newPointVec.data(), sizeof(double) * point_block_size);
    }
}

void SetSolverOptionsFromFlags(BALProblem* bal_problem, const BundleParams& params, g2o::SparseOptimizer* optimizer)
{
    BalBlockSolver* solver_ptr;

    g2o::LinearSolver<BalBlockSolver::PoseMatrixType>* linearSolver = nullptr;

    if (params.linear_solver == "dense_schur") {
        linearSolver = new g2o::LinearSolverDense<BalBlockSolver::PoseMatrixType>();
    } else if (params.linear_solver == "sparse_schur") {
        linearSolver = new g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>();
        dynamic_cast<g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>*>(linearSolver)->setBlockOrdering(true);  // AMD ordering, only needed for sparse cholesky solver
    }

    solver_ptr = new BalBlockSolver(std::unique_ptr<g2o::LinearSolver<BalBlockSolver::PoseMatrixType>>(linearSolver));

    g2o::OptimizationAlgorithmWithHessian* solver;

    if (params.trust_region_strategy == "levenberg_marquardt") {
        solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<BalBlockSolver>(solver_ptr));
    } else if (params.trust_region_strategy == "dogleg") {
        solver = new g2o::OptimizationAlgorithmDogleg(std::unique_ptr<BalBlockSolver>(solver_ptr));
    } else {
        std::cout << "Please check your trust_region_strategy parameter again." << std::endl;
        exit(EXIT_FAILURE);
    }

    optimizer->setAlgorithm(solver);
}

void solveProblem(const char* filename, const BundleParams& params)
{
    BALProblem bal_problem(filename);

    /* show some information heer */
    std::cout << "BAL problem file loaded..." << std::endl;
    std::cout << "BAL problem have " << bal_problem.num_cameras() << " cameras and "
              << bal_problem.num_points() << " points." << std::endl;
    std::cout << "Forming " << bal_problem.num_observations() << " observations." << std::endl;

    /* store the initial 3d cloud points camera pose */
    if (!params.initial_ply.empty()) {
        bal_problem.WriteToPLYFile(params.initial_ply);
    }

    std::cout << "beginning problem..." << std::endl;

    /* add some noise for the initial value */
    srand(params.random_seed);
    bal_problem.Normalize();
    bal_problem.Perturb(params.rotation_sigma, params.translation_sigma, params.point_sigma);

    std::cout << "Normalization complete..." << std::endl;

    g2o::SparseOptimizer optimizer;
    SetSolverOptionsFromFlags(&bal_problem, params, &optimizer);
    buildProblem(&bal_problem, &optimizer, params);

    std::cout << "begin optimization ..." << std::endl;

    /* preform the optimization */
    optimizer.initializeOptimization();
    optimizer.setVerbose(true);
    optimizer.optimize(params.num_iterations);

    std::cout << "optimization complete." << std::endl;
    writeToBALProblem(&bal_problem, &optimizer);

    if (!params.final_ply.empty())  {
        bal_problem.WriteToPLYFile(params.final_ply);
    }
}

int main(int argc, char* argv[])
{
    BundleParams params(argc, argv);    // set the parameters here.

    if (params.input.empty()) {
        std::cout << "Usage: bundle_adjuster - input <path for dataset>";
        return 1;
    }

    solveProblem(params.input.c_str(), params);

    return 0;
}


#ifndef SINCFILTER_H
#define SINCFILTER_H
#include <vector>
#include <cassert>
#include "fifoBuffer.hpp"
#include "armaGetter.hpp"
#include <iostream>
#include <armadillo>

namespace visa
{
class LowPassFilter
{
public:
  LowPassFilter(){};

  /** Set the length of the vector to be filtered */
  void setSourceSize( unsigned int lIn ){ sourceSize=lIn; };

  /** Set the length of the vector that should be returned */
  void setTargetSize( unsigned int lOut ){ targetSize=lOut; };

  /** Computes the filter coefficient corresponding to lengthIn and lengthOut */
  template <class kernelType>
  void computeFilterCoefficients( const kernelType &kernel );

  /** Runs the sinc filter through the array. arrayType must implement operator[] */
  template <class elemType, ArmaMatrix_t dir>
  void filterArray( arma::Mat<elemType> &array, visa::ArmaGetter<elemType,dir> &getter ) const;
private:
  unsigned int sourceSize{0};
  unsigned int targetSize{0};
  std::vector<double> filterCoeff;
  double sumFilter{0.0};

  /** Nyquist frequency of the output vector */
  double newNyquistFrequency();

  /** Compute the sum of the filter coefficients */
  double sumFilterCoeff() const;

  static double sinc( double x );
};
}; // namespace

#include "lowPassFilter.tpp"
#endif


#ifndef ALM_CONVERSION_UTILS
#define ALM_CONVERSION_UTILS

#include <Eigen/Dense>
#include <cmath>
#include <iostream>

namespace math_utils{
  void convert(const Eigen::Quaterniond & from, Eigen::AngleAxisd & to);
  void convert(const Eigen::AngleAxisd & from, Eigen::Vector3d & to);
  void convert(const Eigen::Quaterniond & from, Eigen::Vector3d & to);

  void compute_quat_error(const Eigen::Quaterniond & des,
    					  const Eigen::Quaterniond & current,
    					  Eigen::Vector3d & error);

  void printQuat(const Eigen::Quaterniond & quat);

}

#endif


#ifndef MOCHIMOCHI_PA_HPP_
#define MOCHIMOCHI_PA_HPP_

#include <Eigen/Dense>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <fstream>
#include <functional>
#include "../../functions/enumerate.hpp"

class PA {
private :
  const std::size_t kDim;
  const double kC;
  const int kSelect;

private :
  Eigen::VectorXd _weight;
  std::function<double(double, double)> _compute_tau;

public :
  PA(const std::size_t dim, const double C, const int select = 2)
    : kDim(dim),
      kC(C),
      kSelect(select),
      _weight(Eigen::VectorXd::Zero(dim)) {

    static_assert(std::numeric_limits<decltype(dim)>::max() > 0, "Dimension Error. (Dimension > 0)");
    static_assert(std::numeric_limits<decltype(C)>::max() > 0, "Hyper Parameter Error. (C > 0)");

    // int select : switching the PA algorithm
    // 0 : PA
    // 1 : PA-1
    // 2 : PA-2
    switch(kSelect) {
    case 0 :
      _compute_tau = [](const auto value, const auto loss) {
        return loss / std::pow(std::abs(value), 2);
      };
      break;
    case 1 :
      _compute_tau = [=](const auto value, const auto loss) {
        const auto pa = loss / std::pow(std::abs(value), 2);
        return std::min(kC, pa);
      };
      break;
    case 2 :
      _compute_tau = [=](const auto value, const auto loss) {
        return loss / (std::pow(std::abs(value), 2) + 1.0 / 2 * kC );
      };
      break;
    default:
      std::runtime_error("Error in the PA algorithm.");
    }

  }

  virtual ~PA() { }

private :

  double suffer_loss(const Eigen::VectorXd& x, const int y) const {
    return std::max(0.0, 1.0 - y * _weight.dot(x));
  }

  double compute_margin(const Eigen::VectorXd& x) const {
    return _weight.dot(x);
  }

public :

  bool update(const Eigen::VectorXd& feature, const int label) {
    const auto loss = suffer_loss(feature, label);
    functions::enumerate(feature.data(), feature.data() + feature.size(), 0,
                         [&](const std::size_t index, const double value) {
                           const auto tau = _compute_tau(value, loss);
                           _weight[index] += tau * label * value;
                         });

    return true;
  }

  int predict(const Eigen::VectorXd& x) const {
    return compute_margin(x) > 0.0 ? 1 : -1;
  }

  Eigen::VectorXd get_weight(void) const {
    return _weight;
  }

  void save(const std::string& filename) {
    std::ofstream ofs(filename);
    assert(ofs);
    boost::archive::text_oarchive oa(ofs);
    oa << *this;
    ofs.close();
  }

  void load(const std::string& filename) {
    std::ifstream ifs(filename);
    assert(ifs);
    boost::archive::text_iarchive ia(ifs);
    ia >> *this;
    ifs.close();
  }

private :
  friend class boost::serialization::access;
  BOOST_SERIALIZATION_SPLIT_MEMBER();
  template <class Archive>
  void save(Archive& ar, const unsigned int version) const {
    std::vector<double> weight(_weight.data(), _weight.data() + _weight.size());
    ar & boost::serialization::make_nvp("weigth", weight);
    ar & boost::serialization::make_nvp("dimension", const_cast<std::size_t&>(kDim));
    ar & boost::serialization::make_nvp("C", const_cast<double&>(kC));
  }

  template <class Archive>
  void load(Archive& ar, const unsigned int version) {
    std::vector<double> weight;
    ar & boost::serialization::make_nvp("weight", weight);
    ar & boost::serialization::make_nvp("dimension", const_cast<std::size_t&>(kDim));
    ar & boost::serialization::make_nvp("C", const_cast<double&>(kC));
    _weight = Eigen::Map<Eigen::VectorXd>(&weight[0], weight.size());
  }
};

#endif //MOCHIMOCHI_PA_HPP_


// SPDX-License-Identifier: MIT
// SPDX-FileCopyrightText: Copyright 2019-2022 Heal Research

#ifndef OPERON_NNLS_RESIDUAL_EVALUATOR_HPP
#define OPERON_NNLS_RESIDUAL_EVALUATOR_HPP

#include <Eigen/Core>
#include "operon/interpreter/interpreter.hpp"

namespace Operon {
// simple functor that wraps everything together and provides residuals
struct ResidualEvaluator {
    ResidualEvaluator(Interpreter const& interpreter, Tree const& tree, Dataset const& dataset, const Operon::Span<const Operon::Scalar> targetValues, Range const range)
        : interpreter_(interpreter)
        , tree_(tree)
        , dataset_(dataset)
        , range_(range)
        , target_(targetValues)
        , numParameters_(tree_.get().GetCoefficients().size())
    {
    }

    template <typename T>
    auto operator()(T const* parameters, T* residuals) const -> bool
    {
        Operon::Span<T> result(residuals, target_.size());
        GetInterpreter().Evaluate<T>(tree_.get(), dataset_.get(), range_, result, parameters);
        Eigen::Map<Eigen::Array<T, Eigen::Dynamic, 1, Eigen::ColMajor>> resMap(residuals, target_.size());
        Eigen::Map<const Eigen::Array<Operon::Scalar, Eigen::Dynamic, 1, Eigen::ColMajor>> targetMap(target_.data(), static_cast<Eigen::Index>(target_.size()));
        resMap -= targetMap.cast<T>();
        return true;
    }

    [[nodiscard]] auto NumParameters() const -> size_t { return numParameters_; }
    [[nodiscard]] auto NumResiduals() const -> size_t { return target_.size(); }

    [[nodiscard]] auto GetInterpreter() const -> Interpreter const& { return interpreter_.get(); }

private:
    std::reference_wrapper<Interpreter const> interpreter_;
    std::reference_wrapper<Tree const> tree_;
    std::reference_wrapper<Dataset const> dataset_;
    Range range_;
    Operon::Span<const Operon::Scalar> target_;
    size_t numParameters_; // cache the number of parameters in the tree
};
} // namespace Operon

#endif


/* boost validate.cpp
 *
 * Copyright Jens Maurer 2000
 * 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)
 *
 * $Id: instantiate.cpp 58649 2010-01-02 21:23:17Z steven_watanabe $
 */

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
#pragma warning( disable : 4786 )
#endif

#include <iostream>
#include <sstream>
#include <string>
#include <cmath>
#include <iterator>
#include <vector>
#include <boost/random.hpp>
#include <boost/config.hpp>
#include <boost/preprocessor/stringize.hpp>

#include <boost/test/test_tools.hpp>
#include <boost/test/included/test_exec_monitor.hpp>

#ifdef BOOST_NO_STDC_NAMESPACE
  namespace std { using ::abs; using ::fabs; using ::pow; }
#endif

/*
 * General portability note:
 * MSVC mis-compiles explicit function template instantiations.
 * For example, f<A>() and f<B>() are both compiled to call f<A>().
 * BCC is unable to implicitly convert a "const char *" to a std::string
 * when using explicit function template instantiations.
 *
 * Therefore, avoid explicit function template instantiations.
 */

/*
 * Check function signatures
 */

#if BOOST_WORKAROUND( __BORLANDC__, BOOST_TESTED_AT( 0x570) )
#pragma warn -par
#endif
template<class URNG, class Dist>
void instantiate_dist(URNG& urng, const char * name, const Dist& dist)
{
  // this makes a copy of urng
  boost::variate_generator<URNG, Dist> gen(urng, dist);

  // this keeps a reference to urng
  boost::variate_generator<URNG&, Dist> genref(urng, dist);

  BOOST_CHECK(gen.engine() == genref.engine());

#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
  // and here is a pointer to (a copy of) the urng
  URNG copy = urng;
  boost::variate_generator<URNG*, Dist> genptr(&copy, dist);
#endif

  for(int i = 0; i < 1000; ++i) {
    (void) gen();
    (void) genref();
#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
    (void) genptr();
#endif
  }
  // If the values are not exactly equal, we cannot
  // rely on them being close...
  typename Dist::result_type g = gen();
  BOOST_CHECK_EQUAL(g, genref());
#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
  BOOST_CHECK_EQUAL(g, genptr());
#endif

  (void) gen.engine();
  gen.distribution().reset();

  Dist d = dist;            // copy ctor
  d = dist;                 // copy assignment

#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
  {
    std::ostringstream file;
    file << urng << std::endl;
    file << d;
    std::istringstream input(file.str());
    // std::cout << file.str() << std::endl;
    URNG restored_engine;
    input >> restored_engine;
    input >> std::ws;
    Dist restored_dist;
    input >> restored_dist;
#if !defined(BOOST_MSVC) || BOOST_MSVC > 1300 // MSVC brokenness
    boost::variate_generator<URNG, Dist> old(urng, d);
    boost::variate_generator<URNG, Dist> restored(restored_engine, restored_dist);
    // advance some more so that state is exercised
    for(int i = 0; i < 1000; ++i) {
      (void) old();
      (void) restored();
    }
    BOOST_CHECK_MESSAGE((std::abs(old()-restored()) < 0.0001),
                        (std::string(name) + " old == restored_dist"));
#endif // BOOST_MSVC
  }
#endif // BOOST_RANDOM_NO_STREAM_OPERATORS
}

template<class URNG, class RealType>
void instantiate_real_dist(URNG& urng, RealType /* ignored */)
{
  instantiate_dist(urng, "uniform_01",
                   boost::uniform_01<RealType>());
  instantiate_dist(urng, "uniform_real",
                   boost::uniform_real<RealType>(0, 2.1));
  instantiate_dist(urng, "triangle_distribution",
                   boost::triangle_distribution<RealType>(1, 1.5, 7));
  instantiate_dist(urng, "exponential_distribution",
                   boost::exponential_distribution<RealType>(5));
  instantiate_dist(urng, "normal_distribution",
                   boost::normal_distribution<RealType>());
  instantiate_dist(urng, "lognormal_distribution",
                   boost::lognormal_distribution<RealType>(1, 1));
  instantiate_dist(urng, "cauchy_distribution",
                   boost::cauchy_distribution<RealType>(1));
  instantiate_dist(urng, "gamma_distribution",
                   boost::gamma_distribution<RealType>(1));
}

template<class URNG, class T, class Converted>
void test_seed_conversion(URNG & urng, const T & t, const Converted &) {
    Converted c = static_cast<Converted>(t);
    if(static_cast<T>(c) == t) {
        URNG urng2(c);
        std::ostringstream msg;
        msg << "Testing seed: type " << typeid(Converted).name() << ", value " << c;
        BOOST_CHECK_MESSAGE(urng == urng2, msg.str());
        urng2.seed(c);
        BOOST_CHECK_MESSAGE(urng == urng2, msg.str());
    }
}

// rand48 uses non-standard seeding
template<class T, class Converted>
void test_seed_conversion(boost::rand48 & urng, const T & t, const Converted &) {
    boost::rand48 urng2(t);
    urng2.seed(t);
}

template<class URNG, class ResultType>
void test_seed(const URNG &, const ResultType & value) {
    URNG urng(value);

    // integral types
    test_seed_conversion(urng, value, static_cast<char>(0));
    test_seed_conversion(urng, value, static_cast<signed char>(0));
    test_seed_conversion(urng, value, static_cast<unsigned char>(0));
    test_seed_conversion(urng, value, static_cast<short>(0));
    test_seed_conversion(urng, value, static_cast<unsigned short>(0));
    test_seed_conversion(urng, value, static_cast<int>(0));
    test_seed_conversion(urng, value, static_cast<unsigned int>(0));
    test_seed_conversion(urng, value, static_cast<long>(0));
    test_seed_conversion(urng, value, static_cast<unsigned long>(0));
#if !defined(BOOST_NO_INT64_T)
    test_seed_conversion(urng, value, static_cast<boost::int64_t>(0));
    test_seed_conversion(urng, value, static_cast<boost::uint64_t>(0));
#endif

    // floating point types
    test_seed_conversion(urng, value, static_cast<float>(0));
    test_seed_conversion(urng, value, static_cast<double>(0));
    test_seed_conversion(urng, value, static_cast<long double>(0));
}

template<class URNG, class ResultType>
void instantiate_seed(const URNG & urng, const ResultType &) {
    {
        URNG urng;
        URNG urng2;
        urng2.seed();
        BOOST_CHECK(urng == urng2);
    }
    test_seed(urng, static_cast<ResultType>(0));
    test_seed(urng, static_cast<ResultType>(127));
    test_seed(urng, static_cast<ResultType>(539157235));
    test_seed(urng, static_cast<ResultType>(~0u));
}

// ranlux uses int32_t for seeding instead of result_type
template<class ResultType>
void instantiate_seed(const boost::ranlux3 & urng, const ResultType &) {
    instantiate_seed<boost::ranlux3, boost::uint32_t>(urng, ResultType());
}
template<class ResultType>
void instantiate_seed(const boost::ranlux4 & urng, const ResultType &) {
    instantiate_seed<boost::ranlux4, boost::uint32_t>(urng, ResultType());
}
template<class ResultType>
void instantiate_seed(const boost::ranlux3_01 & urng, const ResultType &) {
    instantiate_seed<boost::ranlux3_01, boost::uint32_t>(urng, ResultType());
}
template<class ResultType>
void instantiate_seed(const boost::ranlux4_01 & urng, const ResultType &) {
    instantiate_seed<boost::ranlux4_01, boost::uint32_t>(urng, ResultType());
}
#if !defined(BOOST_NO_INT64_T) && !defined(BOOST_NO_INTEGRAL_INT64_T)
template<class ResultType>
void instantiate_seed(const boost::ranlux64_3 & urng, const ResultType &) {
    instantiate_seed<boost::ranlux64_3, boost::uint32_t>(urng, ResultType());
}
template<class ResultType>
void instantiate_seed(const boost::ranlux64_4 & urng, const ResultType &) {
    instantiate_seed<boost::ranlux64_3, boost::uint32_t>(urng, ResultType());
}
#endif
template<class ResultType>
void instantiate_seed(const boost::ranlux64_3_01 & urng, const ResultType &) {
    instantiate_seed<boost::ranlux64_3_01, boost::uint32_t>(urng, ResultType());
}
template<class ResultType>
void instantiate_seed(const boost::ranlux64_4_01 & urng, const ResultType &) {
    instantiate_seed<boost::ranlux64_4_01, boost::uint32_t>(urng, ResultType());
}

template<class URNG, class ResultType>
void instantiate_urng(const std::string & s, const URNG & u, const ResultType & r)
{
  std::cout << "Basic tests for " << s << std::endl;
  URNG urng;
  instantiate_seed(u, r);                       // seed() member function
  int a[URNG::has_fixed_range ? 5 : 10];        // compile-time constant
  (void) a;   // avoid "unused" warning
  typename URNG::result_type x1 = urng();
  ResultType x2 = x1;
  (void) &x2;           // avoid "unused" warning

  URNG urng2 = urng;             // copy constructor
#if !defined(BOOST_MSVC) || BOOST_MSVC > 1300 // MSVC brokenness
  BOOST_CHECK(urng == urng2);     // operator==
  BOOST_CHECK(!(urng != urng2));  // operator!=
  urng();
  urng2 = urng;                  // copy assignment
  BOOST_CHECK(urng == urng2);
  urng2 = URNG(urng2);           // copy constructor, not templated constructor
  BOOST_CHECK(urng == urng2);
#endif // BOOST_MSVC

  const std::vector<int> v(9999u, 0x41);
  std::vector<int>::const_iterator it = v.begin();
  std::vector<int>::const_iterator it_end = v.end();
  URNG urng3(it, it_end);
  BOOST_CHECK(it != v.begin());
  std::iterator_traits<std::vector<int>::const_iterator>::difference_type n_words = (it - v.begin());
  std::cout << "; seeding uses " << n_words << " words" << std::endl;

  it = v.end();
  BOOST_CHECK_THROW(urng3.seed(it, it_end), std::invalid_argument);

  if(n_words > 1) {
    it = v.end();
    --it;
    BOOST_CHECK_THROW(urng3.seed(it, it_end), std::invalid_argument);
  }

  // check for min/max members
  ResultType min = (urng3.min)();
  (void) &min;
  ResultType max = (urng3.max)();
  (void) &max;

#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
  // Streamable concept not supported for broken compilers

  // advance a little so that state is relatively arbitrary
  for(int i = 0; i < 9307; ++i)
    urng();
  urng2 = urng;

  {
    // narrow stream first
    std::ostringstream file;
    file << urng;
    // move forward
    urng();
    // restore old state
    std::istringstream input(file.str());
    input >> urng;
    // std::cout << file.str() << std::endl;
#if !defined(BOOST_MSVC) || BOOST_MSVC > 1300 // MSVC brokenness
    // advance some more so that state is exercised
    for(int i = 0; i < 10000; ++i) {
      urng();
      urng2();
    }
    BOOST_CHECK(urng == urng2);
#endif // BOOST_MSVC
  }

  urng2 = urng;
#if !defined(BOOST_NO_STD_WSTREAMBUF) && !defined(BOOST_NO_STD_WSTRING)
  {
    // then wide stream
    std::wostringstream file;
    file << urng;
    // move forward
    urng();
    std::wistringstream input(file.str());
    input >> urng;
#if !defined(BOOST_MSVC) || BOOST_MSVC > 1300 // MSVC brokenness
    // advance some more so that state is exercised
    for(int i = 0; i < 10000; ++i) {
      urng();
      urng2();
    }
    BOOST_CHECK(urng == urng2);
#endif // BOOST_MSVC
  }
#endif // BOOST_NO_STD_WSTREAMBUF, BOOST_NO_STD_WSTRING
#endif // BOOST_RANDOM_NO_STREAM_OPERATORS

  // instantiate various distributions with this URNG
  instantiate_dist(urng, "uniform_smallint", boost::uniform_smallint<>(0, 11));
  instantiate_dist(urng, "uniform_int", boost::uniform_int<>(-200, 20000));
  instantiate_dist(urng, "bernoulli_distribution",
                   boost::bernoulli_distribution<>(0.2));
  instantiate_dist(urng, "binomial_distribution",
                   boost::binomial_distribution<>(4, 0.2));
  instantiate_dist(urng, "geometric_distribution",
                   boost::geometric_distribution<>(0.8));
  instantiate_dist(urng, "poisson_distribution",
                   boost::poisson_distribution<>(1));

  instantiate_real_dist(urng, 1.0f);
  instantiate_real_dist(urng, 1.0);
  instantiate_real_dist(urng, 1.0l);

#if 0
  // We cannot compare the outcomes before/after save with std::abs(x-y)
  instantiate_dist("uniform_on_sphere",
                   boost::uniform_on_sphere<URNG>(urng, 2));
#endif
}

template<class T>
void extra_tests(T*)
{
}

#if !defined(BOOST_NO_INT64_T) && !defined(BOOST_NO_INTEGRAL_INT64_T)
void extra_tests(boost::rand48*)
{
  using namespace boost;
  rand48 rnd(boost::int32_t(5));
  rand48 rnd2(boost::uint64_t(0x80000000) * 42);
  rnd.seed(boost::int32_t(17));
  rnd2.seed(boost::uint64_t(0x80000000) * 49);
}
#endif

void extra_tests(boost::minstd_rand*)
{
  using namespace boost;
  minstd_rand mstd(42);
  mstd.seed(17);
}

void extra_tests(boost::mt19937*)
{
  using namespace boost;
  minstd_rand mstd(42);
  mstd.seed(17);

  mt19937 mt(boost::uint32_t(17));  // needs to be an exact type match for MSVC
  int i = 42;
  mt.seed(boost::uint32_t(i));
  mt19937 mt2(mstd);
  mt2.seed(mstd);

  random_number_generator<mt19937> std_rng(mt2);
  (void) std_rng(10);
}

void instantiate_all()
{
  using namespace boost;

  typedef boost::random::lagged_fibonacci<boost::uint32_t, 24, 607, 273> lagged_fibonacci;

  typedef BOOST_RANDOM_URNG_TEST::result_type result_type;
  instantiate_urng(BOOST_PP_STRINGIZE(BOOST_RANDOM_URNG_TEST), BOOST_RANDOM_URNG_TEST(), static_cast<result_type>(0));
  BOOST_RANDOM_URNG_TEST* type_ptr = 0;
  extra_tests(type_ptr);

}

#if defined(BOOST_MSVC) && _MSC_VER < 1300

// These explicit instantiations are necessary, otherwise MSVC does
// not find the <boost/operators.hpp> inline friends.
// We ease the typing with a suitable preprocessor macro.
#define INSTANT(x) \
template class boost::uniform_smallint<x>; \
template class boost::uniform_int<x>; \
template class boost::uniform_real<x>; \
template class boost::bernoulli_distribution<x>; \
template class boost::geometric_distribution<x>; \
template class boost::triangle_distribution<x>; \
template class boost::exponential_distribution<x>; \
template class boost::normal_distribution<x>; \
template class boost::uniform_on_sphere<x>; \
template class boost::lognormal_distribution<x>;

INSTANT(boost::minstd_rand0)
INSTANT(boost::minstd_rand)
INSTANT(boost::ecuyer1988)
INSTANT(boost::kreutzer1986)
INSTANT(boost::hellekalek1995)
INSTANT(boost::mt19937)
INSTANT(boost::mt11213b)

#undef INSTANT
#endif

int test_main(int, char*[])
{
  instantiate_all();
  return 0;
}


#pragma once

// system includes ------------------------------------------------------------
#include <Eigen/Sparse>
#include <boost/iterator/filter_iterator.hpp>
#include <boost/mpl/identity.hpp>
#include <functional>
#include <iterator>
#include <tuple>

// own includes ------------------------------------------------------------
#include "spectral/basis/spectral_basis.hpp"
#include "spectral/basis/spectral_elem_accessor.hpp"

namespace spectral {
// ----------------------------------------------------------------------
template <typename BASIS>
unsigned int
get_max_l(const BASIS &basis)
{
  typedef typename std::tuple_element<0, typename BASIS::elem_t::container_t>::type elem_t;
  // radial basis
  typename BASIS::elem_t::Acc::template get<elem_t> get_xir;
  unsigned int maxL = 0;
  for (auto it = basis.begin(); it != basis.end(); ++it) {
    unsigned int l = get_xir(*it).get_id().l;
    if (l > maxL) maxL = l;
  }

  return maxL;
}

// ----------------------------------------------------------------------
/**
 * @Brief Return the max polynomial degree for the polar basis
 *
 * TODO, works only for the polar basis
 *
 * @param basis
 *
 * @return
 */
template <typename BASIS>
unsigned int
get_max_k(const BASIS &basis)
{
  typedef typename std::tuple_element<1, typename BASIS::elem_t::container_t>::type elem_t;
  typename BASIS::elem_t::Acc::template get<elem_t> get_phi;
  unsigned int maxK = 0;
  for (auto it = basis.begin(); it != basis.end(); ++it) {
    unsigned int k = get_phi(*it).get_id().k;
    if (k > maxK) maxK = k;
  }
  return maxK;
}

/**
 *  @brief max. polynomial degree + 1
 *
 *  @param spectral basis
 *  @return return type
 */
template <typename BASIS>
unsigned int
get_K(const BASIS &basis)
{
  return get_max_k(basis) + 1;
}

}  // end namespace spectral_basis


#include "face_frontalizer.h"

#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Weverything"
#endif

#ifdef USE_DLIB
#include <dlib/matrix.h>
#endif

#ifdef __clang__
#pragma clang diagnostic pop
#endif

#include <iostream>

namespace prnet {

void FrontalizeFaceMesh(Mesh *front_mesh, const FaceData &face_data) {
#ifdef USE_DLIB
    const int N_VTX = 43867;  // Defined by PRNet template.

    assert(front_mesh->vertices.size() == N_VTX * 3);

    dlib::matrix<float, N_VTX, 4> vertices_homo;
    for (int i = 0; i < N_VTX; i++) {
        vertices_homo(i, 0) = front_mesh->vertices[3 * size_t(i) + 0];
        vertices_homo(i, 1) = front_mesh->vertices[3 * size_t(i) + 1];
        vertices_homo(i, 2) = front_mesh->vertices[3 * size_t(i) + 2];
        vertices_homo(i, 3) = 1.f;
    }

    dlib::matrix<float, N_VTX, 3> canonical_vertices;
    for (int i = 0; i < N_VTX; i++) {
        canonical_vertices(i, 0) = face_data.canonical_vertices[size_t(i)][0];
        canonical_vertices(i, 1) = face_data.canonical_vertices[size_t(i)][1];
        canonical_vertices(i, 2) = face_data.canonical_vertices[size_t(i)][2];
    }

    // c = P * v
    // c * vt * (v * vt)^-1 = P
    dlib::matrix<float, 4, 3> P = dlib::pinv(vertices_homo) * canonical_vertices;
    dlib::matrix<float, N_VTX, 3> front_vertices = vertices_homo * P;

    for (int i = 0; i < N_VTX; i++) {
        front_mesh->vertices[3 * size_t(i) + 0] = front_vertices(i, 0);
        front_mesh->vertices[3 * size_t(i) + 1] = front_vertices(i, 1);
        front_mesh->vertices[3 * size_t(i) + 2] = front_vertices(i, 2);
    }

#if 1
    // Centerize vertex position.
    {
        float bmin[3];
        float bmax[3];
        for (size_t i = 0; i < N_VTX; i++) {
            float x = front_mesh->vertices[3 * i + 0];
            float y = front_mesh->vertices[3 * i + 1];
            float z = front_mesh->vertices[3 * i + 2];
            if (i == 0) {
                bmin[0] = bmax[0] = x;
                bmin[1] = bmax[1] = y;
                bmin[2] = bmax[2] = z;
            } else {
                bmin[0] = std::min(bmin[0], x);
                bmin[1] = std::min(bmin[1], y);
                bmin[2] = std::min(bmin[2], z);

                bmax[0] = std::max(bmax[0], x);
                bmax[1] = std::max(bmax[1], y);
                bmax[2] = std::max(bmax[2], z);
            }
        }
        float bsize[3];
        bsize[0] = bmax[0] - bmin[0];
        bsize[1] = bmax[1] - bmin[1];
        bsize[2] = bmax[2] - bmin[2];
        for (size_t i = 0; i < front_mesh->vertices.size() / 3; i++) {
            front_mesh->vertices[3 * i + 0] -= (bmin[0] + 0.5f * bsize[0]);
            front_mesh->vertices[3 * i + 1] -= (bmin[1] + 0.5f * bsize[1]);
            front_mesh->vertices[3 * i + 2] -= (bmin[2] + 0.5f * bsize[2]);
        }
    }
#endif

#else
  (void)front_mesh;
  (void)face_data;

  std::cerr << "Face frontalization is not supported in non dlib buid at the momemnt." << std::endl;

#endif
}

} // namespace prnet


#ifndef REGISTERED_FACE_HPP
#define REGISTERED_FACE_HPP

#include <memory>
#include <vector>
#include <numeric>
#include <Eigen/Dense>

/**
 * @brief face registered to the recognition system
 *        this class consists of a set of face features ,face_id, tracker_id, name of the registered face
 */
class RegisteredFace {
public:
  using Ptr = std::shared_ptr<RegisteredFace>;

  /**
   * @brief constructor
   * @param face_id      id of the face
   * @param tracker_id   id of the tracker associated with this face (-1 means no tracker is associated)
   */
  RegisteredFace(int face_id, int tracker_id = -1)
    : face_id (face_id),
      tracker_id (tracker_id)
  {
    faces.reserve(32);
  }

  /**
   * @brief constructor
   * @param name    the name of the person
   */
  RegisteredFace(const std::string& name)
    : face_id(-1),
      tracker_id(-1),
      name(name)
  {
    faces.reserve(32);
  }

  virtual ~RegisteredFace() {}

  /**
   * @brief adds a face feature to the feature list
   * @param face  feature vector to be added
   */
  void addFace(const Eigen::VectorXf& face) {
    faces.push_back(face);
  }

  /**
   * @brief adds a set of face features to the feature list
   * @param faces  features to be added
   */
  void addFaces(const RegisteredFace::Ptr& faces) {
    for(const auto& face : faces->faces) {
      addFace(face);
    }
  }

  /**
   * @brief calculates the distance between #face and the closest face in the feature list
   * @param face  face feature vector
   * @return the distance between #face and the closest face
   */
  double calcDistance(const Eigen::VectorXf& face) const {
    double dist = std::numeric_limits<double>::max();
    for(const auto& f : faces) {
      if(f.rows() == face.rows() && f.cols() == face.cols()){
        dist = std::min<double>( dist, (f - face).squaredNorm() );
      }
      else{
        std::cout << "row/col comparison failed" << std::endl;
        return 0;
      }
    }
    return std::sqrt(dist);
  }

  /**
   * @brief calculates the distance between #face and the closest face in the feature list
   * @param face  face feature vector
   * @return the distance between #face and the closest face
   */
  double calcDistance(const Eigen::VectorXf& face, int n) const {
    std::vector<double> dists(faces.size());
    std::transform(faces.begin(), faces.end(), dists.begin(), [&](const Eigen::VectorXf& f) { return (f - face).norm(); });
    std::sort(dists.begin(), dists.end());

    n = std::min<int>(n, dists.size());
    double accum = std::accumulate(dists.begin(), dists.begin() + n, 0.0);
    return accum / n;
  }

  /**
   * @brief calculates the distances
   * @param features
   * @return
   */
  std::vector<double> calcDistances(const std::vector<std::shared_ptr<Eigen::VectorXf>>& features) const {
    std::vector<double> dists(features.size());
    std::transform(features.begin(), features.end(), dists.begin(),
      [&](const std::shared_ptr<Eigen::VectorXf>& f) {
        return calcDistance(*f);
      }
    );

    return dists;
  }

  /**
   * @brief calculates the distances
   * @param features
   * @return
   */
  std::vector<double> calcDistances(const std::vector<std::shared_ptr<Eigen::VectorXf>>& features, int n) const {
    std::vector<double> dists(features.size());
    std::transform(features.begin(), features.end(), dists.begin(),
      [&](const std::shared_ptr<Eigen::VectorXf>& f) {
        return calcDistance(*f, n);
      }
    );

    return dists;
  }

  /**
   * @brief calculates the average distance between features in #rhs and the n closest neighbors in the feature list
   * @param rhs  feature vectors
   * @param n    number of the neighbors
   * @return the distance
   */
  double calcDistance(const RegisteredFace& rhs, int n) const {
    std::vector<double> dists(rhs.size());
    std::transform(rhs.faces.begin(), rhs.faces.end(), dists.begin(), [&](const Eigen::VectorXf& v){ return calcDistance(v);});

    std::sort(dists.begin(), dists.end());

    n = std::min<int>(n, dists.size());
    double accum = std::accumulate(dists.begin(), dists.begin() + n, 0.0);
    return accum / n;
  }

  /**
   * @brief the number of face features in the list
   * @return
   */
  int size() const {
    return faces.size();
  }

  int getFaceId() const {
    return face_id;
  }

  void setTrackerId(int id) {
    tracker_id = id;
  }

  int getTrackerId() const {
    return tracker_id;
  }

  const std::string& getName() const {
    return name;
  }

  std::vector<Eigen::VectorXf>& getFaces() {
    return faces;
  }

  const std::vector<Eigen::VectorXf>& getFaces() const {
    return faces;
  }

private:
  int face_id;        // ID of the face
  int tracker_id;     // ID of the tracker associated with the face (-1 means no tracker is associated)
  std::string name;   // name of the person

  std::vector<Eigen::VectorXf> faces;   // face feature list
};

#endif // FACE_DATA_HPP