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

#include <NTL/new.h>

NTL_START_IMPL

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

      for (i = 1; i <= n; i++) {
         mul(T, B(i), mu1);
         if (k > 0) LeftShift(T, T, k);
         sub(A(i), A(i), T);
      }
   }
   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);
      }
   }
}

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

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)
{
   long i, j;
   RR s, t, t1;
   ZZ T1;

   if (st < k) {
      for (i = 1; i < st; i++)
         mul(buf(i), mu(k,i), c(i));
   }

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

      sqr(t1, s);
      mul(t1, t1, bound);
      mul(t, b(k), b(j));

      if (t >= bound2 && t >= t1) {
         InnerProduct(T1, B(k), B(j));
         conv(s, T1);
      }

      clear(t1);
      for (i = 1; i <= j-1; i++) {
         mul(t, mu(j, i), buf(i));
         add(t1, t1, t);
      }

      sub(t, s, t1);
      buf(j) = t;
      div(mu(k, j), t, c(j));
   }

   clear(s);
   for (j = 1; j <= k-1; j++) {
      mul(t, mu(k, j), buf(j));
      add(s, s, t);
   }

   sub(c(k), b(k), s);
}

NTL_THREAD_LOCAL static RR red_fudge;
NTL_THREAD_LOCAL static long log_red = 0;

static void init_red_fudge()
{
   log_red = long(0.50*RR::precision());

   power2(red_fudge, -log_red);
}

static void inc_red_fudge()
{

   mul(red_fudge, red_fudge, 2);
   log_red--;

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

   if (log_red < 4)
      ResourceError("LLL_RR: can not continue...sorry");
}

NTL_THREAD_LOCAL static long verbose = 0;
NTL_THREAD_LOCAL static unsigned long NumSwaps = 0;
NTL_THREAD_LOCAL static double StartTime = 0;
NTL_THREAD_LOCAL static double LastTime = 0;

static void LLLStatus(long max_k, double t, long m, const mat_ZZ& B)
{
   cerr << "---- LLL_RR 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
long ll_LLL_RR(mat_ZZ& B, mat_ZZ* U, const RR& delta, long deep,
           LLLCheckFct check, mat_RR& B1, mat_RR& mu,
           vec_RR& b, vec_RR& c, long m, long init_k, long &quit)
{
   long n = B.NumCols();

   long i, j, k, Fc1;
   ZZ MU;
   RR mu1, t1, t2, cc;
   ZZ T1;

   RR bound;

      // we tolerate a 15% loss of precision in computing
      // inner products in ComputeGS.

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

   RR bound2;

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

   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;

   vec_RR buf;
   buf.SetLength(m);

   long rst;
   long counter;

   long trigger_index;
   long small_trigger;
   long cnt;

   RR half;
   conv(half,  0.5);
   RR half_plus_fudge;
   add(half_plus_fudge, half, red_fudge);

   long max_k = 0;
   double tt;

   while (k <= m) {

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

      if (verbose) {
         tt = GetTime();

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

      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, bound2);
      st[k] = k;

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

      do {
         // size reduction

         counter++;
         if (counter > 10000) {
            cerr << "LLL_XD: warning--possible infinite loop\n";
            counter = 0;
         }

         Fc1 = 0;

         for (j = rst-1; j >= 1; j--) {
            abs(t1, mu(k,j));
            if (t1 > half_plus_fudge) {

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

                     cnt++;

                     if (cnt > 10) {
                        inc_red_fudge();
                        add(half_plus_fudge, half, red_fudge);
                        cnt = 0;
                     }
                  }

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

               Fc1 = 1;

               mu1 = mu(k,j);
               if (sign(mu1) >= 0) {
                  sub(mu1, mu1, half);
                  ceil(mu1, mu1);
               }
               else {
                  add(mu1, mu1, half);
                  floor(mu1, mu1);
               }

               if (mu1 == 1) {
                  for (i = 1; i <= j-1; i++)
                     sub(mu(k,i), mu(k,i), mu(j,i));
               }
               else if (mu1 == -1) {
                  for (i = 1; i <= j-1; i++)
                     add(mu(k,i), mu(k,i), mu(j,i));
               }
               else {
                  for (i = 1; i <= j-1; i++) {
                     mul(t2, mu1, mu(j,i));
                     sub(mu(k,i), mu(k,i), t2);
                  }
               }

               conv(MU, mu1);

               sub(mu(k,j), mu(k,j), mu1);

               RowTransform(B(k), B(j), MU);
               if (U) RowTransform((*U)(k), (*U)(j), MU);
            }
         }

         if (Fc1) {
            for (i = 1; i <= n; i++)
               conv(B1(k, i), B(k, i));

            InnerProduct(b(k), B1(k), B1(k));
            ComputeGS(B, B1, mu, b, c, k, bound, 1, buf, bound2);
         }
      } while (Fc1);

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

      if (IsZero(b(k))) {
         for (i = k; i < m; i++) {
            // swap i, i+1
            swap(B(i), B(i+1));
            swap(B1(i), B1(i+1));
            swap(b(i), b(i+1));
            if (U) swap((*U)(i), (*U)(i+1));
         }

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

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

      if (quit) break;

      if (deep > 0) {
         // deep insertions

         cc = b(k);
         long l = 1;
         while (l <= k-1) {
            mul(t1, delta, c(l));
            if (t1 > cc) break;
            sqr(t1, mu(k,l));
            mul(t1, t1, c(l));
            sub(cc, cc, t1);
            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));
               swap(B1(i), B1(i-1));
               swap(mu(i), mu(i-1));
               swap(b(i), b(i-1));
               if (U) swap((*U)(i), (*U)(i-1));
            }

            k = l;
            continue;
         }
      } // end deep insertions

      // test LLL reduction condition

      if (k <= 1) {
         k++;
      }
      else {
         sqr(t1, mu(k,k-1));
         mul(t1, t1, c(k-1));
         add(t1, t1, c(k));
         mul(t2, delta, c(k-1));
         if (t2 > t1) {
            // swap rows k, k-1
            swap(B(k), B(k-1));
            swap(B1(k), B1(k-1));
            swap(mu(k), mu(k-1));
            swap(b(k), b(k-1));
            if (U) swap((*U)(k), (*U)(k-1));

            k--;
            NumSwaps++;
         }
         else {
            k++;
         }
      }
   }

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

   return m;
}

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

   long i, j;
   long new_m, dep, quit;
   RR s;
   ZZ MU;
   RR mu1;

   RR t1;
   ZZ T1;

   init_red_fudge();

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

   mat_RR B1;  // approximates B
   B1.SetDims(m, n);

   mat_RR mu;
   mu.SetDims(m, m);

   vec_RR c;  // squared lengths of Gramm-Schmidt basis vectors
   c.SetLength(m);

   vec_RR b; // squared lengths of basis vectors
   b.SetLength(m);

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

   for (i = 1; i <= m; i++) {
      InnerProduct(b(i), B1(i), B1(i));
   }

   new_m = ll_LLL_RR(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_RR(mat_ZZ& B, double delta, long deep,
            LLLCheckFct check, long verb)
{
   verbose = verb;
   NumSwaps = 0;
   if (verbose) {
      StartTime = GetTime();
      LastTime = StartTime;
   }

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

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

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

NTL_THREAD_LOCAL static vec_RR BKZConstant;

static
void ComputeBKZConstant(long beta, long p)
{
   RR c_PI;
   ComputePi(c_PI);

   RR LogPI = log(c_PI);

   BKZConstant.SetLength(beta-1);

   vec_RR Log;
   Log.SetLength(beta);

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

   for (j = 1; j <= beta; j++)
      Log(j) = log(to_RR(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 += Log(j);

         x = exp(x/k);

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

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

         x = exp(2*x/i);
      }

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

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

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

}

NTL_THREAD_LOCAL static vec_RR BKZThresh;

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

   long i;
   RR x;
   RR t1;

   x = 0;

   for (i = 1; i <= beta-1; i++) {
      log(t1, c[i-1]);
      add(x, x, t1);
      div(t1, x, i);
      exp(t1, t1);
      mul(BKZThresh(i), t1, BKZConstant(i));
   }
}

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_RR 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_RR(mat_ZZ& BB, mat_ZZ* UU, const RR& delta,
         long beta, long prune, LLLCheckFct check)
{
   long m = BB.NumRows();
   long n = BB.NumCols();
   long m_orig = m;

   long i, j;
   ZZ MU;

   RR t1, t2;
   ZZ T1;

   init_red_fudge();

   mat_ZZ B;
   B = BB;

   B.SetDims(m+1, n);

   mat_RR B1;
   B1.SetDims(m+1, n);

   mat_RR mu;
   mu.SetDims(m+1, m);

   vec_RR c;
   c.SetLength(m+1);

   vec_RR b;
   b.SetLength(m+1);

   RR cbar;

   vec_RR ctilda;
   ctilda.SetLength(m+1);

   vec_RR vvec;
   vvec.SetLength(m+1);

   vec_RR yvec;
   yvec.SetLength(m+1);

   vec_RR uvec;
   uvec.SetLength(m+1);

   vec_RR utildavec;
   utildavec.SetLength(m+1);

   vec_long Deltavec;
   Deltavec.SetLength(m+1);

   vec_long deltavec;
   deltavec.SetLength(m+1);

   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;

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

   for (i = 1; i <= m; i++) {
      InnerProduct(b(i), B1(i), B1(i));
   }

   // cerr << "\n";
   // cerr << "first LLL\n";

   m = ll_LLL_RR(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;

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

   long clean = 1;

   if (!quit && m > 1) {
      // cerr << "continuing\n";

      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);
         conv(utildavec(jj), 1);
         conv(uvec(jj), 1);

         conv(yvec(jj), 0);
         conv(vvec(jj), 0);
         Deltavec(jj) = 0;

         s = t = jj;
         deltavec(jj) = 1;

         for (i = jj+1; i <= kk+1; i++) {
            conv(ctilda(i), 0);
            conv(uvec(i), 0);
            conv(utildavec(i), 0);
            conv(yvec(i), 0);
            Deltavec(i) = 0;
            conv(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);
                  }
               }
            }

            add(t1, yvec(t), utildavec(t));
            sqr(t1, t1);
            mul(t1, t1, c(t));
            add(ctilda(t), ctilda(t+1), t1);

            if (prune > 0 && t > jj)
               sub(t1, cbar, BKZThresh(t-jj));
            else
               t1 = cbar;

            if (ctilda(t) <t1) {
               if (t > jj) {
                  t--;
                  clear(t1);
                  for (i = t+1; i <= s; i++) {
                     mul(t2, utildavec(i), mu(i,t));
                     add(t1, t1, t2);
                  }

                  yvec(t) = t1;
                  negate(t1, t1);
                  if (sign(t1) >= 0) {
                     sub(t1, t1, 0.5);
                     ceil(t1, t1);
                  }
                  else {
                     add(t1, t1, 0.5);
                     floor(t1, t1);
                  }

                  utildavec(t) = t1;
                  vvec(t) = t1;
                  Deltavec(t) = 0;

                  negate(t1, t1);

                  if (t1 < 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);
               add(utildavec(t), vvec(t), Deltavec(t));
            }
         }

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

         NumIterations++;

         h = min(kk+1, m);

         mul(t1, red_fudge, -8);
         add(t1, t1, delta);
         mul(t1, t1, c(jj));

         if (t1 > 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_RR: internal error");

            if (s > 0) {
               // special case
               // cerr << "special case\n";

               NumTrivial++;

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

               new_m = ll_LLL_RR(B, U, delta, 0, check,
                                B1, mu, b, c, h, jj, quit);
               if (new_m != h) LogicError("BKZ_RR: 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));
                  swap(B1(i-1), B1(i));
                  swap(b(i-1), b(i));
                  if (U) swap((*U)(i-1), (*U)(i));
               }

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

               InnerProduct(b(jj), B1(jj), B1(jj));

               if (b(jj) == 0) LogicError("BKZ_RR: internal error");

               // remove linear dependencies

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

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

               // remove zero vector

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

               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_RR(B, U, delta, 0, check,
                                   B1, mu, b, c, h, h, quit);

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

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

            NumNoOps++;

            if (!clean) {
               new_m =
                  ll_LLL_RR(B, U, delta, 0, check, B1, mu, b, c, h, h, quit);
               if (new_m != h) LogicError("BKZ_RR: 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_RR(mat_ZZ& BB, mat_ZZ& UU, double delta,
         long beta, long prune, LLLCheckFct check, long verb)
{
   verbose = verb;
   NumSwaps = 0;
   if (verbose) {
      StartTime = GetTime();
      LastTime = StartTime;
   }

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

   RR Delta;
   conv(Delta, delta);

   return BKZ_RR(BB, &UU, Delta, beta, prune, check);
}

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

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

   RR Delta;
   conv(Delta, delta);

   return BKZ_RR(BB, 0, Delta, beta, prune, check);
}

void NearVector(vec_ZZ& ww, const mat_ZZ& BB, const vec_ZZ& a)
{
   long n = BB.NumCols();

   if (n != BB.NumRows())
      LogicError("NearVector: matrix must be square");

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

   long i, j;
   mat_ZZ B;

   B.SetDims(n+1, n);
   for (i = 1; i <= n; i++)
      B(i) = BB(i);

   B(n+1) = a;

   mat_RR B1, mu;
   vec_RR b, c;

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

   vec_RR buf;
   buf.SetLength(n+1);

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

   for (i = 1; i <= n+1; 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 <= n+1; i++)
      ComputeGS(B, B1, mu, b, c, i, bound, 1, buf, bound2);

   init_red_fudge();

   RR half;
   conv(half,  0.5);
   RR half_plus_fudge;
   add(half_plus_fudge, half, red_fudge);

   RR t1, t2, mu1;
   ZZ MU;

   long trigger_index = n+1;
   long small_trigger = 0;
   long cnt = 0;

   long Fc1;

   vec_ZZ w;
   w.SetLength(n);
   clear(w);

   do {
      Fc1 = 0;

      for (j = n; j >= 1; j--) {
         abs(t1, mu(n+1,j));
         if (t1 > half_plus_fudge) {

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

                  cnt++;

                  if (cnt > 10) {
                     inc_red_fudge();
                     add(half_plus_fudge, half, red_fudge);
                     cnt = 0;
                  }
               }

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

            Fc1 = 1;

            mu1 = mu(n+1,j);
            if (sign(mu1) >= 0) {
               sub(mu1, mu1, half);
               ceil(mu1, mu1);
            }
            else {
               add(mu1, mu1, half);
               floor(mu1, mu1);
            }

            if (mu1 == 1) {
               for (i = 1; i <= j-1; i++)
                  sub(mu(n+1,i), mu(n+1,i), mu(j,i));
            }
            else if (mu1 == -1) {
               for (i = 1; i <= j-1; i++)
                  add(mu(n+1,i), mu(n+1,i), mu(j,i));
            }
            else {
               for (i = 1; i <= j-1; i++) {
                  mul(t2, mu1, mu(j,i));
                  sub(mu(n+1,i), mu(n+1,i), t2);
               }
            }

            conv(MU, mu1);

            sub(mu(n+1,j), mu(n+1,j), mu1);

            RowTransform(B(n+1), B(j), MU);
            RowTransform2(w, B(j), MU);
         }
      }

      if (Fc1) {
         for (i = 1; i <= n; i++)
            conv(B1(n+1, i), B(n+1, i));

         InnerProduct(b(n+1), B1(n+1), B1(n+1));
         ComputeGS(B, B1, mu, b, c, n+1, bound, 1, buf, bound2);
      }
   } while (Fc1);

   ww = w;
}

NTL_END_IMPL


// ============================================================================
//
// Copyright (c) 2001-2006 Max-Planck-Institut Saarbruecken (Germany).
// All rights reserved.
//
// This file is part of EXACUS (http://www.mpi-inf.mpg.de/projects/EXACUS/).
// You can redistribute it and/or modify it under the terms of the GNU
// General Public License as published by the Free Software Foundation,
// either version 3 of the License, or (at your option) any later version.
//
// Licensees holding a valid commercial license may use this file in
// accordance with the commercial license agreement provided with the software.
//
// This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
//
// ----------------------------------------------------------------------------
//
// Library       : NiX
// File          : NiX/poly2ntl.C
// NiX_release   : $Name:  $
// Revision      : $Revision: 1.1 $
// Revision_date : $Date: 2009-06-30 13:14:58 $
//
// Author(s)     : ... ?
//
// ============================================================================

/*! \file NiX/poly2ntl.C
 *
 *  Conversion of polynomials to NTL format & back
 */

#include <CGAL/config.h>

#define NDEBUG 1
// #define CGAL_POLYNOMIAL_USE_NTL_MUL

// #define CGAL_ACK_BENCHMARK_RES

#ifdef CGAL_POLYNOMIAL_USE_NTL_MUL

//#include <CGAL/leda_integer.h>
#include <CGAL/CORE/BigInt.h>
#include <NTL/ZZX.h>

#include <CGAL/Arithmetic_kernel.h>
#include <CGAL/Polynomial.h>
#include <CGAL/Timer.h>

#define POLY_NTL_MIN_DEGREE 10 // minimal degree of polynomials to multiply
                                // using NTL

#endif // CGAL_POLYNOMIAL_USE_NTL_MUL

namespace CGAL {

#ifdef CGAL_POLYNOMIAL_USE_NTL_MUL

// namespace internal {
// int *primes = CGAL::CGALi::primes;
// }

struct NTL_bigint_rep {
    long alloc;
    long size;
    mp_limb_t data;
};

#endif

#ifdef CGAL_POLYNOMIAL_USE_NTL_MUL

namespace {

// typedef leda_integer CCC_int;
typedef CORE::BigInt Integer;

typedef Polynomial< Integer > Poly_1;

void poly2ntl(const Poly_1& p, NTL::ZZX& q) {

//     if(p.is_zero()) { // TODO should be handled earlier
//         //special handling
//         //a is zero if a.rep.length() == 0;
//         q = NTL::ZZX();
//     }
    q.rep.SetLength(p.degree() + 1);

    int i;
    Poly_1::const_iterator pit;
    for(i = 0, pit = p.begin(); pit != p.end(); pit++, i++) {

        NTL::ZZ& zz = q.rep[i];
        mpz_srcptr tmp = pit->get_mp();
        int sz = tmp->_mp_size;
        if(sz == 0)
            continue;
        if(sz < 0)
            sz = -sz;

        zz.SetSize(sz);
        NTL_bigint_rep *rep = (NTL_bigint_rep *)zz.rep;
        rep->size = tmp->_mp_size;
        // copy limbs directly
        memcpy(&rep->data, tmp->_mp_d, sz*sizeof(mp_limb_t));
         //std::cerr << "pit = " << *pit << "; and " <<
            //zz << "\n\n";
    }
}

} // anonymous namespace

#endif

#ifdef CGAL_ACK_BENCHMARK_RES
#warning timing resultants
extern Timer res_tm;
#endif

#ifdef CGAL_POLYNOMIAL_USE_NTL_MUL

#warning using NTL

template <>
Poly_1& Poly_1::operator *= (const Poly_1& p2) {

    Poly_1 p1 = *this;

    if(p1.is_zero() || p2.is_zero()) {
//         std::cout << "mul NTL: zero poly\n";
        return (*this) = Poly_1(Integer(0));
    }
//  TODO: use this if poly size is small..
    if(p1.degree() <= POLY_NTL_MIN_DEGREE &&
        p2.degree() <= POLY_NTL_MIN_DEGREE) {

        internal::Creation_tag TAG;
        Poly_1 p(TAG, p1.degree() + p2.degree() + 1);
        for (int i=0; i <= p1.degree(); ++i)
          for (int j=0; j <= p2.degree(); ++j)
            p.coeff(i+j) += (p1[i]*p2[j]);
        p.reduce();
//         std::cout << "mul usual: " << p << "\n\n";;
        return (*this) = p ;
    }
    NTL::ZZX q, q2;
    poly2ntl(p1, q);
    poly2ntl(p2, q2);

    q *= q2;

    int d = NTL::deg(q);
//     if(d == -1) {
//         std::cout << "Fatal: zero poly\n";
//         throw -1;
//     }

    this->copy_on_write(); // ??
    this->coeffs().resize(d + 1);

    // TODO: use reduce ??
    mpz_t tmp;
     mpz_init(tmp);
    for(int i = 0; i <= d; i++) {

        const NTL::ZZ& zz = q.rep[i];
        if(NTL::IsZero(zz)) {
            coeff(i) = Integer(0);
            continue;
        }

        NTL_bigint_rep *rep = (NTL_bigint_rep *)zz.rep;
        int sz = rep->size;
        if(sz < 0)
            sz = -sz;

        mpz_realloc2(tmp, sz * GMP_NUMB_BITS);
        tmp->_mp_size = rep->size;
        memcpy(tmp->_mp_d, &rep->data, sz*sizeof(mp_limb_t));

//         coeff(i).makeCopy();
//         mpz_ptr mpd = coeff(i).get_mp();
//         mpd->_mp_size = rep->size;
//         mpz_realloc2(mpd, sz * GMP_NUMB_BITS);
//         memcpy(mpd->_mp_d, &rep->data, sz*sizeof(mp_limb_t));
        coeff(i) = Integer(tmp);

//          mpz_init_set(coeff(i).get_mp(), tmp);
    }
    mpz_clear(tmp);

//         CGALi::Creation_tag TAG;
//         Poly_1 p(TAG, p1.degree() + p2.degree() + 1);
//         for (int i=0; i <= p1.degree(); ++i)
//           for (int j=0; j <= p2.degree(); ++j)
//             p.coeff(i+j) += (p1[i]*p2[j]);
//         //p.reduce();
// //         std::cout << "mul usual: " << p << "\n\n";;
//
//     if(*this != p) {
//
//         std::cout << "------------ p1: " << p1 << "---------- p2: " <<
//                 p2 << "\n";
//         std::cout << "FATAL: " << *this << "----------- and " << p << "\n";
//
//     }

//     Poly_1 pp(vec.begin(), vec.end());
    //p.reduce();
//       std::cout << "mul NTL: " << *this << "\n";
    return (*this);// = pp;
}

template <>
Integer prs_resultant_ufd< Integer >(Poly_1 A, Poly_1 B) {

#ifdef CGAL_ACK_BENCHMARK_RES
// std::cout << "start res " << "\n";
res_tm.start();
#endif

    // implemented using the subresultant algorithm for resultant computation
    // see [Cohen, 1993], algorithm 3.3.7

    typedef Integer NT;

    if (A.is_zero() || B.is_zero()) return NT(0);

    NTL::ZZX q1, q2;
    poly2ntl(A, q1);
    poly2ntl(B, q2);

    NTL::ZZ zz;
    NTL::resultant(zz, q1, q2);

    if(NTL::IsZero(zz))
        return Integer(0);

    Integer res;
    NTL_bigint_rep *rep = (NTL_bigint_rep *)zz.rep;
    int sz = rep->size;
    if(sz < 0)
        sz = -sz;

    mpz_ptr tmp = res.get_mp();
    mpz_realloc2(tmp, sz * GMP_NUMB_BITS);
    tmp->_mp_size = rep->size;
    memcpy(tmp->_mp_d, &rep->data, sz*sizeof(mp_limb_t));

//     std::cout << "stop res " << "\n";
#ifdef CGAL_ACK_BENCHMARK_RES
// std::cout << "stop res " << "\n";
res_tm.stop();
#endif

    return res;
}
#else // CGAL_POLYNOMIAL_USE_NTL_MUL

#if 0
template <>
Integer prs_resultant_ufd< Integer >(Poly_1 A, Poly_1 B) {

#ifdef CGAL_ACK_BENCHMARK_RES
res_tm.start();
#endif

    // implemented using the subresultant algorithm for resultant computation
    // see [Cohen, 1993], algorithm 3.3.7

    typedef Integer NT;

    if (A.is_zero() || B.is_zero()) return NT(0);

    int signflip;
    if (A.degree() <123 B.degree()) {
        Polynomial<NT> T = A; A = B; B = T;
        signflip = (A.degree() & B.degree() & 1);
    } else {
        signflip = 0;
    }

    NT a = A.content(), b = B.content();
    NT g(1), h(1), t = CGAL::ipower(a, B.degree()) * CGAL::ipower(b, A.degree());
    Polynomial<NT> Q, R; NT d;
    int delta;

    A /= a; B /= b;
    do {
        signflip ^= (A.degree() & B.degree() & 1);
        Polynomial<NT>::pseudo_division(A, B, Q, R, d);
        delta = A.degree() - B.degree();
        typedef CGAL::Algebraic_structure_traits<NT>::Is_exact
          Is_exact;

        A = B;
        B = R / (g * CGAL::ipower(h, delta));
        g = A.lcoeff();
        // h = h^(1-delta) * g^delta
        CGALi::hgdelta_update(h, g, delta);
    } while (B.degree() > 0);
    // h = h^(1-deg(A)) * lcoeff(B)^deg(A)
    delta = A.degree();
    g = B.lcoeff();
    CGALi::hgdelta_update(h, g, delta);
    h = signflip ? -(t*h) : t*h;
    Algebraic_structure_traits<NT>::Simplify simplify;
    simplify(h);

   return h;
}
#endif

#endif // CGAL_POLYNOMIAL_USE_NTL_MUL

} // namespace CGAL


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing ifloor capabilities

    Computes the integer conversion of the floor of its parameter.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    as_integer_t<T> r = ifloor(x);
    @endcode

    is equivalent to:

    @code
    as_integer_t<T> r = saturated_(toint)(floor(x));
    @endcode

    @par Note:
    This operation is properly saturated

  **/
  as_integer_T<Value> ifloor(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/ifloor.hpp>
#include <boost/simd/function/simd/ifloor.hpp>

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_TWO_NORM_INCLUDE
#define MTL_TWO_NORM_INCLUDE

#include <iostream>
#include <cmath>

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/concept/magnitude.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/vector/lazy_reduction.hpp>
#include <boost/numeric/mtl/vector/reduction.hpp>
#include <boost/numeric/mtl/vector/reduction_functors.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl {

    namespace vec {

	template <unsigned long Unroll, typename Value>
	typename RealMagnitude<typename Collection<Value>::value_type>::type
	inline two_norm(const Value& value)
	{
	    using std::sqrt;
	    vampir_trace<2039> tracer;
	    typedef typename RealMagnitude<typename Collection<Value>::value_type>::type result_type;
	    return sqrt(reduction<Unroll, two_norm_functor, result_type>::apply(value));
	}

	/*! Two-norm for vectors: two_norm(x) \f$\rightarrow |x|_2\f$.
	    \retval The magnitude type of the respective value type, see Magnitude.
	    The norms are defined as \f$|v|_2=\sqrt{\sum_i |v_i|^2}\f$.

	    Vector norms are unrolled 8-fold by default.
	    An n-fold unrolling can be generated with two_norm<n>(x).
	    The maximum for n is 8 (it might be increased later).
	**/
	template <typename Value>
	typename RealMagnitude<typename Collection<Value>::value_type>::type
	inline two_norm(const Value& value)
	{
	    return two_norm<4>(value);
	}

	template <typename Vector>
	lazy_reduction<Vector, two_norm_functor> inline lazy_two_norm(const Vector& v)
	{  return lazy_reduction<Vector, two_norm_functor>(v); 	}

    } // namespace vector

    // two_norm for matrices not implemented (would need enable_if like one_norm)

    using vec::two_norm;
    using vec::lazy_two_norm;

} // namespace mtl

#endif // MTL_TWO_NORM_INCLUDE


////////////////////////////////////////////////////////////////
// Orkid Media Engine
// Copyright 1996-2020, Michael T. Mayers.
// Distributed under the Boost Software License - Version 1.0 - August 17, 2003
// see http://www.boost.org/LICENSE_1_0.txt
////////////////////////////////////////////////////////////////

#include <ork/lev2/config.h>
#if defined(ENABLE_IGL)

#include <ork/kernel/orklut.hpp>
#include <ork/math/plane.h>
#include <ork/lev2/gfx/meshutil/submesh.h>
#include <ork/lev2/gfx/meshutil/igl.h>
#include <iostream>

#include <Eigen/Core>
#include <igl/gaussian_curvature.h>
#include <igl/principal_curvature.h>
#include <igl/cotmatrix.h>
#include <igl/massmatrix.h>
#include <igl/invert_diag.h>

namespace ork::meshutil {
iglprinciplecurvature_ptr_t IglMesh::computePrincipleCurvature() const {
  auto rval = std::make_shared<IglPrincipleCurvature>();
  // Alternative discrete mean curvature
  Eigen::MatrixXd HN;
  Eigen::SparseMatrix<double> L, M, Minv;
  igl::cotmatrix(_verts, _faces, L);
  igl::massmatrix(_verts, _faces, igl::MASSMATRIX_TYPE_VORONOI, M);
  igl::invert_diag(M, Minv);
  // Laplace-Beltrami of position
  HN = -Minv * (L * _verts);
  // Extract magnitude as mean curvature
  rval->H = HN.rowwise().norm();

  // Compute curvature directions via quadric fitting
  igl::principal_curvature(_verts, _faces, rval->PD1, rval->PD2, rval->PV1, rval->PV2);
  // mean curvature
  rval->H = 0.5 * (rval->PV1 + rval->PV2);
  return rval;
}

Eigen::VectorXd IglMesh::computeGaussianCurvature() const {
  Eigen::VectorXd rval;
  igl::gaussian_curvature(_verts, _faces, rval);
  // Compute mass matrix
  Eigen::SparseMatrix<double> M, Minv;
  igl::massmatrix(_verts, _faces, igl::MASSMATRIX_TYPE_DEFAULT, M);
  igl::invert_diag(M, Minv);
  // Divide by area to get integral average
  rval = (Minv * rval).eval();
  return rval;
}

} // namespace ork::meshutil

#endif


#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include <OpenMesh/Core/IO/MeshIO.hh>
#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
#include <string>
#include <iostream>
#include <vector>
//#include "include/pybind11/pybind11.h"
//#include "include/pybind11/numpy.h"
//#include "include/pybind11/stl.h"
#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>
#include <fstream>

struct TriTraits : public OpenMesh::DefaultTraits
{
  /// Use double precision points
  typedef OpenMesh::Vec3d Point;
  /// Use double precision Normals
  typedef OpenMesh::Vec3d Normal;
  /// Use double precision TexCood2D
  typedef OpenMesh::Vec2d TexCoord2D;
};

/// Simple Name for Mesh
typedef OpenMesh::TriMesh_ArrayKernelT<TriTraits>  TriMesh;

namespace py=pybind11;

// compute matrix exp
Eigen::Matrix3d exp(Eigen::Matrix3d angle_cross_axis){
        Eigen::Matrix3d test=angle_cross_axis+angle_cross_axis.transpose();
        if(test.norm()>1e-6)
        {
            std::cout<<"rotation_log_exp::exp3::input matrix isn't skew-matrix!!!"<<std::endl;
            return Eigen::Matrix3d::Identity();
        }

        Eigen::Vector3d angle_axis;
        angle_axis(0)=angle_cross_axis(2,1);
        angle_axis(1)=angle_cross_axis(0,2);
        angle_axis(2)=angle_cross_axis(1,0);
        double angle=sqrt(angle_axis(0)*angle_axis(0)+angle_axis(1)*angle_axis(1)+angle_axis(2)*angle_axis(2));
        if(angle<1e-6)
            return Eigen::Matrix3d::Identity();
        Eigen::Matrix3d cross_axis=angle_cross_axis/angle;

        test=cross_axis+cross_axis.transpose();
        if(test.norm()>1e-6)
        {
            std::cout<<"rotation_log_exp::exp2::input matrix isn't skew-matrix!!!"<<std::endl;
            return Eigen::Matrix3d::Identity();
        }

        return Eigen::Matrix3d::Identity()+sin(angle)*cross_axis+(1.0-cos(angle))*cross_axis*cross_axis;
}

py::array_t<double> get_mesh(std::string file, py::array_t<double> feature_input){
    TriMesh ref_mesh_;
    if(!OpenMesh::IO::read_mesh(ref_mesh_, file)){
        std::cerr<<"Read Mesh error"<<std::endl;
    }
    int nver=ref_mesh_.n_vertices();
    OpenMesh::EPropHandleT<double> LB_weights;
    ref_mesh_.add_property(LB_weights);

    // compute LB weight
    TriMesh::EdgeIter e_it, e_end(ref_mesh_.edges_end());
    TriMesh::HalfedgeHandle    h0, h1, h2;
    TriMesh::VertexHandle      v0, v1;
    TriMesh::Point             p0, p1, p2, d0, d1;
    TriMesh::Scalar w;
    for (e_it=ref_mesh_.edges_begin(); e_it!=e_end; e_it++)
    {
        w  = 0.0;
        if(ref_mesh_.is_boundary(*e_it))
        {
            h0 = ref_mesh_.halfedge_handle(e_it.handle(),0);
            if(ref_mesh_.is_boundary(h0))
                h0 = ref_mesh_.opposite_halfedge_handle(h0);

            v0 = ref_mesh_.to_vertex_handle(h0);
            v1 = ref_mesh_.from_vertex_handle(h0);
            p0 = ref_mesh_.point(v0);
            p1 = ref_mesh_.point(v1);
            h1 = ref_mesh_.next_halfedge_handle(h0);
            p2 = ref_mesh_.point(ref_mesh_.to_vertex_handle(h1));
            d0 = (p0-p2).normalize();
            d1 = (p1-p2).normalize();
            w += 2.0 / tan(acos(std::min(0.99, std::max(-0.99, (d0|d1)))));
            if(std::isnan(w))
                std::cout<<"Some weight NAN"<<std::endl;
            ref_mesh_.property(LB_weights,e_it) = w;
            continue;
        }
        h0 = ref_mesh_.halfedge_handle(e_it.handle(),0);
        v0 = ref_mesh_.to_vertex_handle(h0);
        p0 = ref_mesh_.point(v0);

        h1 = ref_mesh_.opposite_halfedge_handle(h0);
        v1 = ref_mesh_.to_vertex_handle(h1);
        p1 = ref_mesh_.point(v1);

        h2 = ref_mesh_.next_halfedge_handle(h0);
        p2 = ref_mesh_.point(ref_mesh_.to_vertex_handle(h2));
        d0 = (p0 - p2).normalize();
        d1 = (p1 - p2).normalize();
        w += 1.0/ tan(acos(std::max(-1.0, std::min(1.0, dot(d1,d0) ))));

        h2 = ref_mesh_.next_halfedge_handle(h1);
        p2 = ref_mesh_.point(ref_mesh_.to_vertex_handle(h2));
        d0 = (p0 - p2).normalize();
        d1 = (p1 - p2).normalize();
        w += 1.0 / tan(acos(std::max(-1.0, std::min(1.0, dot(d1,d0)))));

        if(std::isnan(w))
            std::cout<<"Some weight is NAN"<<std::endl;
        ref_mesh_.property(LB_weights,e_it) = w;
    }

    // prepare Sparse A_ matrix
    Eigen::SparseMatrix<double> A_;
    A_.resize(3*nver,3*nver);
    std::vector<Eigen::Triplet<double> > tripletlist;
    TriMesh::VertexIter v_it = ref_mesh_.vertices_begin();
    for(;v_it!=ref_mesh_.vertices_end();v_it++)
    {
        // fix one point
        TriMesh::VertexEdgeIter ve_iter = ref_mesh_.ve_iter(*v_it);
        int center_id = (*v_it).idx();
        double center_val[3]={0.0,0.0,0.0};
        for(;ve_iter.is_valid();ve_iter++)
        {
            double w = ref_mesh_.property(LB_weights,*ve_iter);
            TriMesh::VertexHandle to_v = ref_mesh_.to_vertex_handle(ref_mesh_.halfedge_handle(*ve_iter,0));
            if(to_v.idx() == center_id)
                to_v = ref_mesh_.from_vertex_handle(ref_mesh_.halfedge_handle(*ve_iter,0));
            for(int i =0;i<3;i++)
            {
                center_val[i]+=w;
                tripletlist.push_back(Eigen::Triplet<double>(3*center_id+i,3*to_v.idx()+i,-w));
//                outfile << 3*center_id + i << " " << 3*to_v.idx()+i << " "<< -w << '\n';
            }
        }
        for(int i =0;i<3;i++)
        {
            tripletlist.push_back(Eigen::Triplet<double>(3*center_id+i,3*center_id+i,center_val[i]));
        }
    }
    A_.setFromTriplets(tripletlist.begin(),tripletlist.end());
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > A_solver_;
    A_solver_.compute(A_);
    // prepare T_matrix
    py::buffer_info feature_array = feature_input.request();
    std::vector<Eigen::Matrix3d> T_array;
    for(int i =0; i<ref_mesh_.n_vertices(); i++)
    {
        Eigen::Matrix3d logR;
        logR<<0,((double*)feature_array.ptr)[9*i+6],((double*)feature_array.ptr)[9*i+7],-((double*)feature_array.ptr)[9*i+6],0,((double*)feature_array.ptr)[9*i+8],-((double*)feature_array.ptr)[9*i+7],-((double*)feature_array.ptr)[9*i+8],0;
        Eigen::Matrix3d S;
        S<<((double*)feature_array.ptr)[9*i+0],((double*)feature_array.ptr)[9*i+1],((double*)feature_array.ptr)[9*i+2],((double*)feature_array.ptr)[9*i+1],((double*)feature_array.ptr)[9*i+3],((double*)feature_array.ptr)[9*i+4],((double*)feature_array.ptr)[9*i+2],((double*)feature_array.ptr)[9*i+4],((double*)feature_array.ptr)[9*i+5];
        T_array.push_back(exp(logR)*S);
    }

    // compute b_
    Eigen::VectorXd b_;
    b_.resize(3*nver);
    b_.setZero();
    v_it = ref_mesh_.vertices_begin();
    for(;v_it!=ref_mesh_.vertices_end();v_it++)
    {
        TriMesh::VertexEdgeIter ve_iter = ref_mesh_.ve_iter(*v_it);
        Eigen::Vector3d temp(0.0,0.0,0.0);
        for(;ve_iter.is_valid();ve_iter++)
        {
            TriMesh::VertexHandle v0 = ref_mesh_.to_vertex_handle(ref_mesh_.halfedge_handle(*ve_iter,0));
            if(v0.idx() == (*v_it).idx())
                v0 = ref_mesh_.from_vertex_handle(ref_mesh_.halfedge_handle(*ve_iter,0));
            OpenMesh::Vec3d Pj,Pk;
            Pj = ref_mesh_.point(*v_it);
            Pk = ref_mesh_.point(v0);
            Eigen::Vector3d e1jk(Pj[0]-Pk[0],Pj[1]-Pk[1],Pj[2]-Pk[2]);
            double cjk = ref_mesh_.property(LB_weights,*ve_iter);

            temp+= cjk*( T_array[v0.idx()] + T_array[(*v_it).idx()] )*e1jk;
        }
        temp*=0.5;
        b_.block<3,1>(3*(*v_it).idx(),0) = temp;
    }

    // solve
    Eigen::VectorXd out;
    out.resize(3*nver);
    out = A_solver_.solve(b_);

    // std::vector<double> P_;
        // std::cout<<"dahsdas"<<std::endl;

    // std::memcpy(P_.data(), out.data(), out.size()*sizeof(double));
    // Eigen::VectorXd::Map(&P_[0], out.size()) = out;
    std::vector<double> P_(out.data(), out.data()+out.size());
    auto result = py::array_t<double>(P_.size());
    auto result_buffer = result.request();
    double *result_ptr = (double *)result_buffer.ptr;

    std::memcpy(result_ptr, P_.data(), P_.size()*sizeof(double));

    // std::cout<<result[0]<<std::endl;
    return result;

    // for(int i =0; i< P_.rows(); i++)
    // std::cout<<P_[i]<<std::endl;
    // std::vector<size_t> strides = {sizeof(double)};
    // std::vector<size_t> shape = {feature_array.shape[0]/3};
    // size_t ndim = 1;
    // return py::array(py::buffer_info(P_.data(), sizeof(double), py::format_descriptor<double>::value, ndim, feature_array.shape, strides));
    // return py::array(nver, P_.data());
}
 PYBIND11_MODULE(get_mesh, m){
    m.doc() = "get mesh";
    m.def("get_mesh", &get_mesh, "get_mesh");
 }


#include <iostream>
#include <mtl/strided1D.h>
#include <mtl/light1D.h>
#include <mtl/mtl.h>

int
main()
{
  using namespace mtl;

  double dx[20], dy[20];
  for (int i = 0; i < 20; ++i) {
    if (i % 2 == 0) {
      dx[i] = i;
      dy[i] = 2*i;
    } else {
      dx[i] = 0;
      dy[i] = 0;
    }
  }
  light1D<double> x(dx, 20);
  strided1D< light1D<double> > sx(x, -2);

  light1D<double> y(dy, 20);
  strided1D< light1D<double> > sy(y, -2);

  if (dot(sx, sy) == dot(x, y))
    std::cout << "success" << std::endl;
  else
    std::cout << "failure" << std::endl;

  return 0;
}


/*
 * gpcxx/app/benchmark_problems/keijzer.hpp
 * Date: 2015-06-24
 * Author: Karsten Ahnert (karsten.ahnert@gmx.de)
 * Copyright: Karsten Ahnert
 *
 * 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 GPCXX_APP_BENCHMARK_PROBLEMS_KEIJZER_HPP_INCLUDED
#define GPCXX_APP_BENCHMARK_PROBLEMS_KEIJZER_HPP_INCLUDED

#include <gpcxx/app/generate_evenly_spaced_test_data.hpp>
#include <gpcxx/app/generate_uniform_distributed_test_data.hpp>
#include <gpcxx/util/assert.hpp>

#include <boost/math/constants/constants.hpp>
#include <cmath>

namespace gpcxx {

double keijzer_func4( double x ) { return 0.3 * x * sin( 2.0 * boost::math::double_constants::pi * x ); }

double keijzer_func5( double x ) {
    double sx = sin( x );
    double cx = cos( x );
    return x * x * x * exp( - x ) * cx * sx * ( sx * sx * cx - 1.0 );
}

double keijzer_func6( double x , double y , double z ) { return 30.0 * x * z / ( x - 10.0 ) / y / y; }

double keijzer_func7( double x ) {
    GPCXX_ASSERT( x >= 1.0 );
    double n = static_cast< double >( static_cast< int >( x ) );
    return n * ( n - 1 ) / 2.0;
}

double keijzer_func8( double x ) { return std::log( x ); }

double keijzer_func9( double x ) { return std::sqrt( x ); }

double keijzer_func10( double x ) { return std::asinh( x ); }

double keijzer_func11( double x , double y ) { return std::pow( x , y ); }

double keijzer_func12( double x , double y ) { return x * y + sin( ( x -1.0 ) * ( y-1.0 ) ); }

double keijzer_func13( double x , double y ) { return x * x * x * x - x * x * x + 0.5 * y * y - y; }

double keijzer_func14( double x , double y ) { return 6.0 * sin( x ) * cos( x ); }

double keijzer_func15( double x , double y ) { return 8.0 / ( 2.0 + x * x + y * y ); }

double keijzer_func16( double x , double y ) { return x * x * x / 3.0 + y * y * y / 2.0 - y - x; }

auto generate_keijzer1( void ) {
    return gpcxx::generate_evenly_spaced_test_data< 1 >( -1.0 , 1.0 , 0.1 , keijzer_func4 );
}

auto generate_keijzer2( void ) {
    return gpcxx::generate_evenly_spaced_test_data< 1 >( -2.0 , 2.0 , 0.1 , keijzer_func4 );
}

auto generate_keijzer3( void ) {
    return gpcxx::generate_evenly_spaced_test_data< 1 >( -3.0 , 3.0 , 0.1 , keijzer_func4 );
}

auto generate_keijzer4( void ) {
    return gpcxx::generate_evenly_spaced_test_data< 1 >( 0.0 , 10.0 , 0.05  , keijzer_func5 );
}

template< typename Rng >
auto generate_keijzer5( Rng& rng ) {
    return gpcxx::generate_uniform_distributed_test_data< 3 >( rng , 1000 ,
        std::array< std::pair< double , double > , 3 >{{ std::make_pair( -1.0 , 1.0 ) , std::make_pair( 1.0 , 2.0 ) , std::make_pair( -1.0 , 1.0 )  }} ,
        keijzer_func6 );
}

auto generate_keijzer6( void ) {
    return gpcxx::generate_evenly_spaced_test_data< 1 >( 1.0 , 50.0 , 1.0 , keijzer_func7 );
}

auto generate_keijzer7( void ) {
    return gpcxx::generate_evenly_spaced_test_data< 1 >( 1.0 , 100.0 , 1.0 , keijzer_func8 );
}

auto generate_keijzer8( void ) {
    return gpcxx::generate_evenly_spaced_test_data< 1 >( 0.0 , 100.0 , 1.0 , keijzer_func9 );
}

auto generate_keijzer9( void ) {
    return gpcxx::generate_evenly_spaced_test_data< 1 >( 0.0 , 100.0 , 1.0 , keijzer_func10 );
}

template< typename Rng >
auto generate_keijzer10( Rng& rng ) {
    return gpcxx::generate_uniform_distributed_test_data< 2 >( rng , 100 , 0.0 , 1.0 , keijzer_func11 );
}

template< typename Rng >
auto generate_keijzer11( Rng& rng ) {
    return gpcxx::generate_uniform_distributed_test_data< 2 >( rng , 20 , -3.0 , 3.0 , keijzer_func12 );
}

template< typename Rng >
auto generate_keijzer12( Rng& rng ) {
    return gpcxx::generate_uniform_distributed_test_data< 2 >( rng , 20 , -3.0 , 3.0 , keijzer_func13 );
}

template< typename Rng >
auto generate_keijzer13( Rng& rng ) {
    return gpcxx::generate_uniform_distributed_test_data< 2 >( rng , 20 , -3.0 , 3.0 , keijzer_func14 );
}

template< typename Rng >
auto generate_keijzer14( Rng& rng ) {
    return gpcxx::generate_uniform_distributed_test_data< 2 >( rng , 20 , -3.0 , 3.0 , keijzer_func15 );
}

template< typename Rng >
auto generate_keijzer15( Rng& rng ) {
    return gpcxx::generate_uniform_distributed_test_data< 2 >( rng , 20 , -3.0 , 3.0 , keijzer_func16 );
}

} // namespace gpcxx

#endif // GPCXX_APP_BENCHMARK_PROBLEMS_KEIJZER_HPP_INCLUDED


#pragma once

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

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

#include "dpmeans.hpp"

using namespace Eigen;
using std::cout;
using std::endl;

template<class T>
class DPvMFMeans : public DPMeans<T>
{
public:
  DPvMFMeans(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx, uint32_t K0,
      double lambda, boost::mt19937* pRndGen);
  virtual ~DPvMFMeans();

//  virtual void updateLabels();
//  virtual void updateCenters();

  virtual T dist(const Matrix<T,Dynamic,1>& a, const Matrix<T,Dynamic,1>& b);
  virtual bool closer(T a, T b);
  virtual uint32_t indOfClosestCluster(int32_t i);
  virtual Matrix<T,Dynamic,1> computeCenter(uint32_t k);

//protected:
//  double lambda_;
};
// --------------------------- impl -------------------------------------------

template<class T>
DPvMFMeans<T>::DPvMFMeans(
    const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx, uint32_t K0, double lambda,
    boost::mt19937* pRndGen)
  : DPMeans<T>(spx,K0, lambda, pRndGen)
{
  assert(-2.0 < this->lambda_ && this->lambda_ < 0.0);
}

template<class T>
DPvMFMeans<T>::~DPvMFMeans()
{}

template<class T>
T DPvMFMeans<T>::dist(const Matrix<T,Dynamic,1>& a, const Matrix<T,Dynamic,1>& b)
{
//  return acos(min(1.0,max(-1.0,(a.transpose()*b)(0)))); // angular similarity
  return a.transpose()*b; // cosine similarity
};

template<class T>
bool DPvMFMeans<T>::closer(T a, T b)
{
//  return a<b; // if dist a is greater than dist b a is closer than b (angular dist)
  return a>b; // if dist a is greater than dist b a is closer than b (cosine dist)
};

template<class T>
Matrix<T,Dynamic,1> DPvMFMeans<T>::computeCenter(uint32_t k)
{
  this->Ns_(k) = 0.0;
  Matrix<T,Dynamic,1> mean_k(this->D_);
  mean_k.setZero(this->D_);
  for(uint32_t i=0; i<this->N_; ++i)
    if(this->z_(i) == k)
    {
      mean_k += this->spx_->col(i);
      this->Ns_(k) ++;
    }
  if(this->Ns_(k) > 0)
    return mean_k/mean_k.norm();
  else
    return mean_k;
}

template<class T>
uint32_t DPvMFMeans<T>::indOfClosestCluster(int32_t i)
{
  // use cosine similarity because it is faster since acos is not computed
  int z_i = this->K_;
  T sim_closest = this->lambda_ +1.;// new formulation -2<lambda<0
  for(uint32_t k=0; k<this->K_; ++k)
  {
    T sim_k = this->ps_.col(k).transpose()* this->spx_->col(i);
    if( sim_k > sim_closest) // because of cosine distance
    {
      sim_closest = sim_k;
      z_i = k;
    }
  }
  return z_i;
};

//template<class T>
//void DPvMFMeans<T>::updateLabels()
//{
////#pragma omp parallel for
//// TODO not sure how to parallelize
//  for(uint32_t i=0; i<this->N_; ++i)
//  {
//
////    Matrix<T,Dynamic,1> sim(this->K_+1);
////    sim(this->K_) = lambda_;
////    for(uint32_t k=0; k<this->K_; ++k)
////      sim(k) = this->ps_.col(k).transpose()*this->spx_->col(i);
////    int z_i,dummy;
//////    cout<<sim.transpose()<<endl;
////    sim.maxCoeff(&z_i,&dummy);
//
//    int z_i = this->K_;
//    T sim_max = lambda_;
//    for (uint32_t k=0; k<this->K_; ++k)
//    {
//      T sim_k = this->ps_.col(k).transpose()*this->spx_->col(i);
//      if(sim_k > sim_max)
//      {
//        sim_max = sim_k;
//        z_i = k;
//      }
//    }
//
//    if(z_i == this->K_)
//    {
//      MatrixXd psNew(this->D_,this->K_+1);
//      psNew.leftCols(this->K_) = this->ps_;
//      psNew.col(this->K_) = this->spx_->col(i);
//      this->ps_ = psNew;
//      this->K_ ++;
//    }
//    this->z_(i) = z_i;
//  }
//}
//
//template<class T>
//void DPvMFMeans<T>::updateCenters()
//{
//  vector<bool> toDelete(this->K_,false);
//#pragma omp parallel for
//  for(uint32_t k=0; k<this->K_; ++k)
//  {
//    T N_k=0;
//    Matrix<T,Dynamic,1> mean_k(this->D_);
//    mean_k.setZero(this->D_);
//    for(uint32_t i=0; i<this->N_; ++i)
//      if(this->z_(i) == k)
//      {
//        mean_k += this->spx_->col(i);
//        N_k ++;
//      }
//    if (N_k > 0)
//      this->ps_.col(k) = mean_k/mean_k.norm();
//    else
//      toDelete[k] = true;
//  }
//
//  uint32_t kNew = this->K_;
//  for(int32_t k=this->K_-1; k>-1; --k)
//    if(toDelete[k])
//    {
//      cout<<"cluster k "<<k<<" empty"<<endl;
//#pragma omp parallel for
//      for(uint32_t i=0; i<this->N_; ++i)
//      {
//        if(this->z_(i) >= k) this->z_(i)--;
//      }
//      kNew --;
//    }
//
//  MatrixXd psNew(this->D_,kNew);
//  int32_t offset = 0;
//  for(uint32_t k=0; k<this->K_; ++k)
//    if(toDelete[k])
//    {
//      offset ++;
//    }else{
//      psNew.col(k-offset) = this->ps_.col(k);
//    }
//  this->ps_ = psNew;
//  this->K_ = kNew;
//
////  cout<<"centers="<<endl<<this->ps_<<endl;
//}
//


/**
 * \file dcs/math/stats/distribution/mmpp.hpp
 *
 * \brief Markov-modulated Poisson Process (MMPP).
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_STATS_DISTRIBUTION_MMPP_HPP
#define DCS_MATH_STATS_DISTRIBUTION_MMPP_HPP

#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/operation/diag.hpp>
#include <cstddef>
#include <dcs/math/policies/policy.hpp>
#include <dcs/math/stats/distribution/base_distribution.hpp>
#include <dcs/math/stats/distribution/map.hpp>
#include <dcs/math/stats/function/rand.hpp>
#include <iostream>
#include <vector>

namespace dcs { namespace math { namespace stats {

template <
	typename RealT = double,
	typename PolicyT = ::dcs::math::policies::policy<>
>
class mmpp_distribution//: public base_distribution<RealT>
{
	public: typedef RealT value_type;
	public: typedef value_type support_type;
	public: typedef ::boost::numeric::ublas::matrix<support_type> matrix_type;
	public: typedef ::boost::numeric::ublas::vector<support_type> vector_type;
	public: typedef ::std::size_t size_type;
	private: typedef map_distribution<RealT,PolicyT> map_distribution_type;

	public: template <typename VectorExprT, typename MatrixExprT>
		mmpp_distribution(::boost::numeric::ublas::vector_expression<VectorExprT> const& lambda,
						 ::boost::numeric::ublas::matrix_expression<MatrixExprT> const& Q)
	: //base_type(),
	  map_(Q-::boost::numeric::ublasx::diag(lambda), ::boost::numeric::ublasx::diag(lambda))
	{
	}

	// Compiler-generated copy-constructor, copy-assignment, and destructor
	// are fine.

	public: vector_type lambda() const
	{
		return ::boost::numeric::ublasx::diag(map_.D1());
	}

	public: matrix_type Q() const
	{
		return map_.D0()+map_.D1();
	}

/*
	private: value_type do_rand(::dcs::math::random::any_generator<value_type>& rng) const
	{
		return rand_sample(rng);
	}

	private: value_type do_rand(::dcs::math::random::base_generator<value_type>& rng) const
	{
		return rand_sample(rng);
	}
*/

	public: template <typename URNG>
		value_type rand(URNG& rng) const
	{
		return map_.rand(rng);
	}

	/**
	 * The following algorithm is an adaptation of the one found in the MMPP-QN
	 * toolbox (by G. Casale et al, http://www.cs.wm.edu/MMPPQN/).
	 */
	public: template <typename URNG>
		::std::vector<value_type> rand(URNG& rng, size_type n) const
	{
		return map_.rand(rng, n);
	}

	private: map_distribution_type map_;
};

template <
	typename CharT,
	typename CharTraitsT,
	typename RealT,
	typename PolicyT
>
::std::basic_ostream<CharT,CharTraitsT>& operator<<(::std::basic_ostream<CharT,CharTraitsT>& os, mmpp_distribution<RealT,PolicyT> const& dist)
{
	return os << "MMPP("
			  << "lambda=" <<  dist.lambda()
			  << ",Q=" <<  dist.Q()
			  << ")";
}

}}} // Namespace dcs:math::stats

#endif // DCS_MATH_STATS_DISTRIBUTION_MMPP_HPP


/*=========================================================================
 *
 *  Copyright 2011-2013 The University of North Carolina at Chapel Hill
 *  All rights reserved.
 *
 *  Licensed under the MADAI Software License. You may obtain a copy of
 *  this license at
 *
 *         https://madai-public.cs.unc.edu/visualization/software-license/
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/

#include <iostream>
#include <fstream>
#include <cmath>
#include <vector>

#include <Eigen/Dense>
#include "GaussianProcessEmulatorTestGenerator.h"
#include "GaussianProcessEmulator.h"
#include "GaussianProcessEmulatorDirectoryFormatIO.h"
#include "Paths.h"

const char DEFAULT_MODEL_OUTPUT_DIRECTORY[] = "model_output";
const char DEFAULT_EXPERIMENTAL_RESULTS_FILE[] = "experimental_results.dat";

inline double LogisticFunction(double x) {
  return 1.0 / (1.0 + std::exp(-x));
}
inline double sinc(double x) {
  if (x==0) return 1.0;
  x *= 3.141592653589793;
  return std::sin(x) / x;
}
void model(const std::vector< double > & params, std::vector< double > & out) {
  const double & x = params[0];
  const double & y = params[1];
  out.at(0) = sinc(std::sqrt((x * x) + (y * y) + (0.5 * x * y)));
  out.at(1) = LogisticFunction(x - 0.25 * y);
}

int main( int, char *[] ) {
  static const int N = 100;

  madai::Parameter param0( "param_0", -1, 1 );
  madai::Parameter param1( "param_1", -1, 1 );
  std::vector< madai::Parameter > parameters;
  parameters.push_back( param0 );
  parameters.push_back( param1 );

  GaussianProcessEmulatorTestGenerator generator( &model,2,2,N, parameters);

  std::string TempDirectory = "../Testing/Temporary/GaussianProcessEmulatorTest";
  if ( !generator.WriteDirectoryStructure(TempDirectory) ) {
    std::cerr << "Error writing directory structure\n";
    return EXIT_FAILURE;
  }

  std::string MOD = TempDirectory + madai::Paths::SEPARATOR +
    DEFAULT_MODEL_OUTPUT_DIRECTORY;
  std::string ERF = TempDirectory + madai::Paths::SEPARATOR +
    DEFAULT_EXPERIMENTAL_RESULTS_FILE;

  madai::GaussianProcessEmulator gpe;
  madai::GaussianProcessEmulatorDirectoryFormatIO directoryReader;
  if ( !directoryReader.LoadTrainingData( &gpe, MOD, TempDirectory, ERF ) ) {
    std::cerr << "Error loading from created directory structure\n";
    return EXIT_FAILURE;
  }

  if ( !gpe.PrincipalComponentDecompose() ) {
    std::cerr << "Error decomposing model data.\n";
    return EXIT_FAILURE;
  }

  std::string PCAFileName = TempDirectory + madai::Paths::SEPARATOR
      + madai::Paths::PCA_DECOMPOSITION_FILE;
  std::ofstream PCAFile( PCAFileName.c_str() );
  if ( !PCAFile ) {
    std::cerr << "Could not open file '" << PCAFileName << "'\n";
    return EXIT_FAILURE;
  }

  madai::GaussianProcessEmulatorDirectoryFormatIO directoryFormatIO;
  directoryFormatIO.WritePCA( &gpe, PCAFile );
  PCAFile.close();

  double fractionResolvingPower = 0.999;
  madai::GaussianProcessEmulator::CovarianceFunctionType covarianceFunction
      = madai::GaussianProcessEmulator::SQUARE_EXPONENTIAL_FUNCTION;
  int regressionOrder = 1;
  double defaultNugget = 1e-3;
  double amplitude = 1.0;
  double scale = 1e-2;

  if (! gpe.RetainPrincipalComponents( fractionResolvingPower ) )
    return EXIT_FAILURE;

  if (! gpe.BasicTraining(
          covarianceFunction,
          regressionOrder,
          defaultNugget,
          amplitude,
          scale))
    return EXIT_FAILURE;

  std::string EmulatorStateFileName = TempDirectory + madai::Paths::SEPARATOR
      + madai::Paths::EMULATOR_STATE_FILE;
  std::ofstream EmulatorStateFile( EmulatorStateFileName.c_str() );
  if ( !EmulatorStateFile ) {
    std::cerr << "Could not open file '" << EmulatorStateFileName << "'\n";
    return EXIT_FAILURE;
  }

  directoryFormatIO.Write( &gpe, EmulatorStateFile );
  EmulatorStateFile.close();

  if (! gpe.MakeCache()) {
    std::cerr << "Error while makeing cache.\n";
    return false;
  }

  std::cout.precision(17);
  double error = 0.0;

  std::vector< double > x(2,0.0);
  std::vector< double > y(2,0.0);
  for (int i = 0; i < N; ++i) {
    x[0] = generator.m_X(i,0);
    x[1] = generator.m_X(i,1);
    if (! gpe.GetEmulatorOutputs(x, y))
      return EXIT_FAILURE;
    error += std::abs(y[0] - generator.m_Y(i,0));
    error += std::abs(y[1] - generator.m_Y(i,1));
  }
  std::cout << "Sum of errors at training points: " << error << '\n';

  error = 0.0;
  std::vector< double > y2(2,0.0);
  double range_over_N = 2.0 / static_cast< double >(N);
  double half_range_over_N = 0.5 * range_over_N;
  for (int i = 0; i < N; ++i) {
    x[0] = range_over_N * i + half_range_over_N;
    for (int j = 0; j < N; ++j) {
      x[1] = range_over_N * j + half_range_over_N;
      gpe.GetEmulatorOutputs(x, y);
      model(x, y2);
      error = std::max(error, std::abs(y[0] - y2[0]));
      error = std::max(error, std::abs(y[1] - y2[1]));
    }
  }
  std::cout << "Maximum error over all space: " << error << '\n';

  std::string ThetaFileName = TempDirectory + madai::Paths::SEPARATOR + "thetas.dat";
  std::ofstream ThetaFile( ThetaFileName.c_str() );
  if(! directoryFormatIO.PrintThetas(&gpe,ThetaFile)) {
    std::cerr << "Error printing Thetas.\n";
    return EXIT_FAILURE;
  }
  ThetaFile.close();

  return EXIT_SUCCESS;
}