#include <NTL/FFT.h>

#include <NTL/new.h>

NTL_START_IMPL

FFTTablesType FFTTables;
// a truly GLOBAL variable, shared among all threads

long IsFFTPrime(long n, long& w)
{
   long  m, x, y, z;
   long j, k;

   if (n <= 1 || n >= NTL_SP_BOUND) return 0;

   if (n % 2 == 0) return 0;

   if (n % 3 == 0) return 0;

   if (n % 5 == 0) return 0;

   if (n % 7 == 0) return 0;

   m = n - 1;
   k = 0;
   while ((m & 1) == 0) {
      m = m >> 1;
      k++;
   }

   for (;;) {
      x = RandomBnd(n);

      if (x == 0) continue;
      z = PowerMod(x, m, n);
      if (z == 1) continue;

      x = z;
      j = 0;
      do {
         y = z;
         z = MulMod(y, y, n);
         j++;
      } while (j != k && z != 1);

      if (z != 1 || y !=  n-1) return 0;

      if (j == k)
         break;
   }

   /* x^{2^k} = 1 mod n, x^{2^{k-1}} = -1 mod n */

   long TrialBound;

   TrialBound = m >> k;
   if (TrialBound > 0) {
      if (!ProbPrime(n, 5)) return 0;

      /* we have to do trial division by special numbers */

      TrialBound = SqrRoot(TrialBound);

      long a, b;

      for (a = 1; a <= TrialBound; a++) {
         b = (a << k) + 1;
         if (n % b == 0) return 0;
      }
   }

   /* n is an FFT prime */

   for (j = NTL_FFTMaxRoot; j < k; j++)
      x = MulMod(x, x, n);

   w = x;
   return 1;
}

static
void NextFFTPrime(long& q, long& w, long index)
{
   static long m = NTL_FFTMaxRootBnd + 1;
   static long k = 0;
   // m and k are truly GLOBAL variables, shared among
   // all threads.  Access is protected by a critical section
   // guarding FFTTables

   static long last_index = -1;
   static long last_m = 0;
   static long last_k = 0;

   if (index == last_index) {
      // roll back m and k...part of a simple error recovery
      // strategy if an exception was thrown in the last
      // invocation of UseFFTPrime...probably of academic
      // interest only

      m = last_m;
      k = last_k;
   }
   else {
      last_index = index;
      last_m = m;
      last_k = k;
   }

   long t, cand;

   for (;;) {
      if (k == 0) {
         m--;
         if (m < 5) ResourceError("ran out of FFT primes");
         k = 1L << (NTL_SP_NBITS-m-2);
      }

      k--;

      cand = (1L << (NTL_SP_NBITS-1)) + (k << (m+1)) + (1L << m) + 1;

      if (!IsFFTPrime(cand, t)) continue;
      q = cand;
      w = t;
      return;
   }
}

long CalcMaxRoot(long p)
{
   p = p-1;
   long k = 0;
   while ((p & 1) == 0) {
      p = p >> 1;
      k++;
   }

   if (k > NTL_FFTMaxRoot)
      return NTL_FFTMaxRoot;
   else
      return k;
}

void InitFFTPrimeInfo(FFTPrimeInfo& info, long q, long w, bool bigtab)
{
   double qinv = 1/((double) q);

   long mr = CalcMaxRoot(q);

   info.q = q;
   info.qinv = qinv;
   info.zz_p_context = 0;

   info.RootTable.SetLength(mr+1);
   info.RootInvTable.SetLength(mr+1);
   info.TwoInvTable.SetLength(mr+1);
   info.TwoInvPreconTable.SetLength(mr+1);

   long *rt = &info.RootTable[0];
   long *rit = &info.RootInvTable[0];
   long *tit = &info.TwoInvTable[0];
   mulmod_precon_t *tipt = &info.TwoInvPreconTable[0];

   long j;
   long t;

   rt[mr] = w;
   for (j = mr-1; j >= 0; j--)
      rt[j] = MulMod(rt[j+1], rt[j+1], q);

   rit[mr] = InvMod(w, q);
   for (j = mr-1; j >= 0; j--)
      rit[j] = MulMod(rit[j+1], rit[j+1], q);

   t = InvMod(2, q);
   tit[0] = 1;
   for (j = 1; j <= mr; j++)
      tit[j] = MulMod(tit[j-1], t, q);

   for (j = 0; j <= mr; j++)
      tipt[j] = PrepMulModPrecon(tit[j], q, qinv);

   if (bigtab)
      info.bigtab.make();
}

#ifndef NTL_WIZARD_HACK
SmartPtr<zz_pInfoT> Build_zz_pInfo(FFTPrimeInfo *info);
#else
SmartPtr<zz_pInfoT> Build_zz_pInfo(FFTPrimeInfo *info) { return 0; }
#endif

void UseFFTPrime(long index)
{
   if (index < 0) LogicError("invalud FFT prime index");
   if (index >= NTL_MAX_FFTPRIMES) ResourceError("FFT prime index too large");

   do {  // NOTE: thread safe lazy init
      FFTTablesType::Builder bld(FFTTables, index+1);
      long amt = bld.amt();
      if (!amt) break;

      long first = index+1-amt;
      // initialize entries first..index

      long i;
      for (i = first; i <= index; i++) {
         UniquePtr<FFTPrimeInfo> info;
         info.make();

         long q, w;
         NextFFTPrime(q, w, i);

         bool bigtab = false;

#ifdef NTL_FFT_BIGTAB
         if (i < NTL_FFT_BIGTAB_LIMIT)
            bigtab = true;
#endif

         InitFFTPrimeInfo(*info, q, w, bigtab);
         info->zz_p_context = Build_zz_pInfo(info.get());
         bld.move(info);
      }

   } while (0);
}

/*
 * Our FFT is based on the routine in Cormen, Leiserson, Rivest, and Stein.
 * For very large inputs, it should be relatively cache friendly.
 * The inner loop has been unrolled and pipelined, to exploit any
 * low-level parallelism in the machine.
 *
 * This version now allows input to alias output.
 */

#define NTL_PIPELINE
//  Define to gets some software pipelining...actually seems
//  to help somewhat

#define NTL_LOOP_UNROLL
//  Define to unroll some loops.  Seems to help a little

// FIXME: maybe the above two should be tested by the wizard

static
long RevInc(long a, long k)
{
   long j, m;

   j = k;
   m = 1L << (k-1);

   while (j && (m & a)) {
      a ^= m;
      m >>= 1;
      j--;
   }
   if (j) a ^= m;
   return a;
}

NTL_THREAD_LOCAL static
Vec<long> brc_mem[NTL_FFTMaxRoot+1];
// FIXME: This could potentially be shared across threads, using
// a "lazy table".

#if 0

static
void BitReverseCopy(long * NTL_RESTRICT A, const long * NTL_RESTRICT a, long k)
{
   long n = 1L << k;
   long* NTL_RESTRICT rev;
   long i, j;

   rev = brc_mem[k].elts();
   if (!rev) {
      brc_mem[k].SetLength(n);
      rev = brc_mem[k].elts();
      for (i = 0, j = 0; i < n; i++, j = RevInc(j, k))
         rev[i] = j;
   }

   for (i = 0; i < n; i++)
      A[rev[i]] = a[i];
}

static
void BitReverseCopy(unsigned long * NTL_RESTRICT A, const long * NTL_RESTRICT a, long k)
{
   long n = 1L << k;
   long* NTL_RESTRICT rev;
   long i, j;

   rev = brc_mem[k].elts();
   if (!rev) {
      brc_mem[k].SetLength(n);
      rev = brc_mem[k].elts();
      for (i = 0, j = 0; i < n; i++, j = RevInc(j, k))
         rev[i] = j;
   }

   for (i = 0; i < n; i++)
      A[rev[i]] = a[i];
}

#else

// A more cache-friendly bit-reverse copy algorithm.
// The "COBRA" algorithm from Carter and Gatlin, "Towards an
// optimal bit-reversal permutation algorithm", FOCS 1998.

// NOTE: for basic poly mul, we could consider not doing any
// bit-reverse copy; however, a number of other operations
// depend on this, so it would not be easy to do.

#define NTL_BRC_THRESH (12)
#define NTL_BRC_Q (5)

// Must have NTL_BRC_THRESH > 2*NTL_BRC_Q
// Should also have (1L << (2*NTL_BRC_Q)) small enough
// so that we can fit that many long's into the cache

static
long *BRC_init(long k)
{
   long n = (1L << k);
   brc_mem[k].SetLength(n);
   long *rev = brc_mem[k].elts();
   long i, j;
   for (i = 0, j = 0; i < n; i++, j = RevInc(j, k))
      rev[i] = j;
   return rev;
}

static
void BasicBitReverseCopy(long * NTL_RESTRICT B,
                         const long * NTL_RESTRICT A, long k)
{
   long n = 1L << k;
   long* NTL_RESTRICT rev;
   long i, j;

   rev = brc_mem[k].elts();
   if (!rev) rev = BRC_init(k);

   for (i = 0; i < n; i++)
      B[rev[i]] = A[i];
}

static
void COBRA(long * NTL_RESTRICT B, const long * NTL_RESTRICT A, long k)
{
   NTL_THREAD_LOCAL static Vec<long> BRC_temp;

   long q = NTL_BRC_Q;
   long k1 = k - 2*q;
   long * NTL_RESTRICT rev_k1, * NTL_RESTRICT rev_q;
   long *NTL_RESTRICT T;
   long a, b, c, a1, b1, c1;
   long i, j;

   rev_k1 = brc_mem[k1].elts();
   if (!rev_k1) rev_k1 = BRC_init(k1);

   rev_q = brc_mem[q].elts();
   if (!rev_q) rev_q = BRC_init(q);

   T = BRC_temp.elts();
   if (!T) {
      BRC_temp.SetLength(1L << (2*q));
      T = BRC_temp.elts();
   }

   for (b = 0; b < (1L << k1); b++) {
      b1 = rev_k1[b];
      for (a = 0; a < (1L << q); a++) {
         a1 = rev_q[a];
         for (c = 0; c < (1L << q); c++)
            T[(a1 << q) + c] = A[(a << (k1+q)) + (b << q) + c];
      }

      for (c = 0; c < (1L << q); c++) {
         c1 = rev_q[c];
         for (a1 = 0; a1 < (1L << q); a1++)
            B[(c1 << (k1+q)) + (b1 << q) + a1] = T[(a1 << q) + c];
      }
   }
}

static
void BitReverseCopy(long * NTL_RESTRICT B, const long * NTL_RESTRICT A, long k)
{
   if (k <= NTL_BRC_THRESH)
      BasicBitReverseCopy(B, A, k);
   else
      COBRA(B, A, k);
}

static
void BasicBitReverseCopy(unsigned long * NTL_RESTRICT B,
                         const long * NTL_RESTRICT A, long k)
{
   long n = 1L << k;
   long* NTL_RESTRICT rev;
   long i, j;

   rev = brc_mem[k].elts();
   if (!rev) rev = BRC_init(k);

   for (i = 0; i < n; i++)
      B[rev[i]] = A[i];
}

static
void COBRA(unsigned long * NTL_RESTRICT B, const long * NTL_RESTRICT A, long k)
{
   NTL_THREAD_LOCAL static Vec<unsigned long> BRC_temp;

   long q = NTL_BRC_Q;
   long k1 = k - 2*q;
   long * NTL_RESTRICT rev_k1, * NTL_RESTRICT rev_q;
   unsigned long *NTL_RESTRICT T;
   long a, b, c, a1, b1, c1;
   long i, j;

   rev_k1 = brc_mem[k1].elts();
   if (!rev_k1) rev_k1 = BRC_init(k1);

   rev_q = brc_mem[q].elts();
   if (!rev_q) rev_q = BRC_init(q);

   T = BRC_temp.elts();
   if (!T) {
      BRC_temp.SetLength(1L << (2*q));
      T = BRC_temp.elts();
   }

   for (b = 0; b < (1L << k1); b++) {
      b1 = rev_k1[b];
      for (a = 0; a < (1L << q); a++) {
         a1 = rev_q[a];
         for (c = 0; c < (1L << q); c++)
            T[(a1 << q) + c] = A[(a << (k1+q)) + (b << q) + c];
      }

      for (c = 0; c < (1L << q); c++) {
         c1 = rev_q[c];
         for (a1 = 0; a1 < (1L << q); a1++)
            B[(c1 << (k1+q)) + (b1 << q) + a1] = T[(a1 << q) + c];
      }
   }
}

static
void BitReverseCopy(unsigned long * NTL_RESTRICT B, const long * NTL_RESTRICT A, long k)
{
   if (k <= NTL_BRC_THRESH)
      BasicBitReverseCopy(B, A, k);
   else
      COBRA(B, A, k);
}

#endif

void FFT(long* A, const long* a, long k, long q, const long* root)
// performs a 2^k-point convolution modulo q

{
   if (k <= 1) {
      if (k == 0) {
	 A[0] = a[0];
	 return;
      }
      if (k == 1) {
	 long a0 = AddMod(a[0], a[1], q);
	 long a1 = SubMod(a[0], a[1], q);
         A[0] = a0;
         A[1] = a1;
	 return;
      }
   }

   // assume k > 1

   NTL_THREAD_LOCAL static Vec<long> wtab_store;
   NTL_THREAD_LOCAL static Vec<mulmod_precon_t> wqinvtab_store;
   NTL_THREAD_LOCAL static Vec<long> AA_store;

   wtab_store.SetLength(1L << (k-2));
   wqinvtab_store.SetLength(1L << (k-2));
   AA_store.SetLength(1L << k);

   long * NTL_RESTRICT wtab = wtab_store.elts();
   mulmod_precon_t * NTL_RESTRICT wqinvtab = wqinvtab_store.elts();
   long *AA = AA_store.elts();

   double qinv = 1/((double) q);

   wtab[0] = 1;
   wqinvtab[0] = PrepMulModPrecon(1, q, qinv);

   BitReverseCopy(AA, a, k);

   long n = 1L << k;

   long s, m, m_half, m_fourth, i, j, t, u, t1, u1, tt, tt1;

   long w;
   mulmod_precon_t wqinv;

   // s = 1

   for (i = 0; i < n; i += 2) {
      t = AA[i + 1];
      u = AA[i];
      AA[i] = AddMod(u, t, q);
      AA[i+1] = SubMod(u, t, q);
   }

   for (s = 2; s < k; s++) {
      m = 1L << s;
      m_half = 1L << (s-1);
      m_fourth = 1L << (s-2);

      w = root[s];
      wqinv = PrepMulModPrecon(w, q, qinv);

      // prepare wtab...

      if (s == 2) {
         wtab[1] = MulModPrecon(wtab[0], w, q, wqinv);
         wqinvtab[1] = PrepMulModPrecon(wtab[1], q, qinv);
      }
      else {
         // some software pipelining

         i = m_half-1; j = m_fourth-1;
         wtab[i-1] = wtab[j];
         wqinvtab[i-1] = wqinvtab[j];
         wtab[i] = MulModPrecon(wtab[i-1], w, q, wqinv);

         i -= 2; j --;

         for (; i >= 0; i -= 2, j --) {
            long wp2 = wtab[i+2];
            long wm1 = wtab[j];
            wqinvtab[i+2] = PrepMulModPrecon(wp2, q, qinv);
            wtab[i-1] = wm1;
            wqinvtab[i-1] = wqinvtab[j];
            wtab[i] = MulModPrecon(wm1, w, q, wqinv);
         }

         wqinvtab[1] = PrepMulModPrecon(wtab[1], q, qinv);
      }

      for (i = 0; i < n; i+= m) {

         long * NTL_RESTRICT AA0 = &AA[i];
         long * NTL_RESTRICT AA1 = &AA[i + m_half];

// #ifdef NTL_LOOP_UNROLL
#if 1

         t = AA1[0];
         u = AA0[0];
         t1 = MulModPrecon(AA1[1], w, q, wqinv);
         u1 = AA0[1];

         for (j = 0; j < m_half-2; j += 2) {
            long a02 = AA0[j+2];
            long a03 = AA0[j+3];
            long a12 = AA1[j+2];
            long a13 = AA1[j+3];
            long w2 = wtab[j+2];
            long w3 = wtab[j+3];
            mulmod_precon_t wqi2 = wqinvtab[j+2];
            mulmod_precon_t wqi3 = wqinvtab[j+3];

            tt = MulModPrecon(a12, w2, q, wqi2);
            long b00 = AddMod(u, t, q);
            long b10 = SubMod(u, t, q);
            t = tt;
            u = a02;

            tt1 = MulModPrecon(a13, w3, q, wqi3);
            long b01 = AddMod(u1, t1, q);
            long b11 = SubMod(u1, t1, q);
            t1 = tt1;
            u1 = a03;

            AA0[j] = b00;
            AA1[j] = b10;
            AA0[j+1] = b01;
            AA1[j+1] = b11;
         }

         AA0[j] = AddMod(u, t, q);
         AA1[j] = SubMod(u, t, q);
         AA0[j + 1] = AddMod(u1, t1, q);
         AA1[j + 1] = SubMod(u1, t1, q);

#else

         t = AA1[0];
         u = AA0[0];

         for (j = 0; j < m_half-1; j++) {
            long a02 = AA0[j+1];
            long a12 = AA1[j+1];
            long w2 = wtab[j+1];
            mulmod_precon_t wqi2 = wqinvtab[j+1];

            tt = MulModPrecon(a12, w2, q, wqi2);
            long b00 = AddMod(u, t, q);
            long b10 = SubMod(u, t, q);
            t = tt;
            u = a02;

            AA0[j] = b00;
            AA1[j] = b10;
         }

         AA0[j] = AddMod(u, t, q);
         AA1[j] = SubMod(u, t, q);

#endif
      }
   }

   // s == k...special case

   m = 1L << s;
   m_half = 1L << (s-1);
   m_fourth = 1L << (s-2);

   w = root[s];
   wqinv = PrepMulModPrecon(w, q, qinv);

   // j = 0, 1

   t = AA[m_half];
   u = AA[0];
   t1 = MulModPrecon(AA[1+ m_half], w, q, wqinv);
   u1 = AA[1];

   A[0] = AddMod(u, t, q);
   A[m_half] = SubMod(u, t, q);
   A[1] = AddMod(u1, t1, q);
   A[1 + m_half] = SubMod(u1, t1, q);

   for (j = 2; j < m_half; j += 2) {
      t = MulModPrecon(AA[j + m_half], wtab[j >> 1], q, wqinvtab[j >> 1]);
      u = AA[j];
      t1 = MulModPrecon(AA[j + 1+ m_half], wtab[j >> 1], q,
                        wqinvtab[j >> 1]);
      t1 = MulModPrecon(t1, w, q, wqinv);
      u1 = AA[j + 1];

      A[j] = AddMod(u, t, q);
      A[j + m_half] = SubMod(u, t, q);
      A[j + 1] = AddMod(u1, t1, q);
      A[j + 1 + m_half] = SubMod(u1, t1, q);

   }
}

#if (!defined(NTL_FFT_LAZYMUL) || (!defined(NTL_SPMM_ULL) && !defined(NTL_SPMM_ASM)))

// FFT with precomputed tables

static
void PrecompFFTMultipliers(long k, long q, const long *root, const FFTMultipliers& tab)
{
   if (k < 1) LogicError("PrecompFFTMultipliers: bad input");

   do {  // NOTE: thread safe lazy init
      FFTMultipliers::Builder bld(tab, k+1);
      long amt = bld.amt();
      if (!amt) break;

      long first = k+1-amt;
      // initialize entries first..k

      double qinv = 1/((double) q);

      for (long s = first; s <= k; s++) {
         UniquePtr<FFTVectorPair> item;

         if (s == 0) {
            bld.move(item); // position 0 not used
            continue;
         }

         if (s == 1) {
            item.make();
            item->wtab_precomp.SetLength(1);
            item->wqinvtab_precomp.SetLength(1);
            item->wtab_precomp[0] = 1;
            item->wqinvtab_precomp[0] = PrepMulModPrecon(1, q, qinv);
            bld.move(item);
            continue;
         }

         item.make();
         item->wtab_precomp.SetLength(1L << (s-1));
         item->wqinvtab_precomp.SetLength(1L << (s-1));

         long m = 1L << s;
         long m_half = 1L << (s-1);
         long m_fourth = 1L << (s-2);

         const long *wtab_last = tab[s-1]->wtab_precomp.elts();
         const mulmod_precon_t *wqinvtab_last = tab[s-1]->wqinvtab_precomp.elts();

         long *wtab = item->wtab_precomp.elts();
         mulmod_precon_t *wqinvtab = item->wqinvtab_precomp.elts();

         for (long i = 0; i < m_fourth; i++) {
            wtab[i] = wtab_last[i];
            wqinvtab[i] = wqinvtab_last[i];
         }

         long w = root[s];
         mulmod_precon_t wqinv = PrepMulModPrecon(w, q, qinv);

         // prepare wtab...

         if (s == 2) {
            wtab[1] = MulModPrecon(wtab[0], w, q, wqinv);
            wqinvtab[1] = PrepMulModPrecon(wtab[1], q, qinv);
         }
         else {
            // some software pipelining
            long i, j;

            i = m_half-1; j = m_fourth-1;
            wtab[i-1] = wtab[j];
            wqinvtab[i-1] = wqinvtab[j];
            wtab[i] = MulModPrecon(wtab[i-1], w, q, wqinv);

            i -= 2; j --;

            for (; i >= 0; i -= 2, j --) {
               long wp2 = wtab[i+2];
               long wm1 = wtab[j];
               wqinvtab[i+2] = PrepMulModPrecon(wp2, q, qinv);
               wtab[i-1] = wm1;
               wqinvtab[i-1] = wqinvtab[j];
               wtab[i] = MulModPrecon(wm1, w, q, wqinv);
            }

            wqinvtab[1] = PrepMulModPrecon(wtab[1], q, qinv);
         }

         bld.move(item);
      }
   } while (0);
}

void FFT(long* A, const long* a, long k, long q, const long* root, const FFTMultipliers& tab)
// performs a 2^k-point convolution modulo q

{
   if (k <= 1) {
      if (k == 0) {
	 A[0] = a[0];
	 return;
      }
      if (k == 1) {
	 long a0 = AddMod(a[0], a[1], q);
	 long a1 = SubMod(a[0], a[1], q);
         A[0] = a0;
         A[1] = a1;
	 return;
      }
   }

   // assume k > 1

   if (k >= tab.length()) PrecompFFTMultipliers(k, q, root, tab);

   NTL_THREAD_LOCAL static Vec<long> AA_store;
   AA_store.SetLength(1L << k);
   long *AA = AA_store.elts();

   BitReverseCopy(AA, a, k);

   long n = 1L << k;

   long s, m, m_half, m_fourth, i, j, t, u, t1, u1, tt, tt1;

   // s = 1

   for (i = 0; i < n; i += 2) {
      t = AA[i + 1];
      u = AA[i];
      AA[i] = AddMod(u, t, q);
      AA[i+1] = SubMod(u, t, q);
   }

   for (s = 2; s < k; s++) {
      m = 1L << s;
      m_half = 1L << (s-1);
      m_fourth = 1L << (s-2);

      const long* wtab = tab[s]->wtab_precomp.elts();
      const mulmod_precon_t *wqinvtab = tab[s]->wqinvtab_precomp.elts();

      for (i = 0; i < n; i+= m) {

         long *AA0 = &AA[i];
         long *AA1 = &AA[i + m_half];

#ifdef NTL_PIPELINE

// pipelining: seems to be faster

         t = AA1[0];
         u = AA0[0];
         t1 = MulModPrecon(AA1[1], wtab[1], q, wqinvtab[1]);
         u1 = AA0[1];

         for (j = 0; j < m_half-2; j += 2) {
            long a02 = AA0[j+2];
            long a03 = AA0[j+3];
            long a12 = AA1[j+2];
            long a13 = AA1[j+3];
            long w2 = wtab[j+2];
            long w3 = wtab[j+3];
            mulmod_precon_t wqi2 = wqinvtab[j+2];
            mulmod_precon_t wqi3 = wqinvtab[j+3];

            tt = MulModPrecon(a12, w2, q, wqi2);
            long b00 = AddMod(u, t, q);
            long b10 = SubMod(u, t, q);

            tt1 = MulModPrecon(a13, w3, q, wqi3);
            long b01 = AddMod(u1, t1, q);
            long b11 = SubMod(u1, t1, q);

            AA0[j] = b00;
            AA1[j] = b10;
            AA0[j+1] = b01;
            AA1[j+1] = b11;

            t = tt;
            u = a02;
            t1 = tt1;
            u1 = a03;
         }

         AA0[j] = AddMod(u, t, q);
         AA1[j] = SubMod(u, t, q);
         AA0[j + 1] = AddMod(u1, t1, q);
         AA1[j + 1] = SubMod(u1, t1, q);
      }
#else
         for (j = 0; j < m_half; j += 2) {
            const long a00 = AA0[j];
            const long a01 = AA0[j+1];
            const long a10 = AA1[j];
            const long a11 = AA1[j+1];

            const long w0 = wtab[j];
            const long w1 = wtab[j+1];
            const mulmod_precon_t wqi0 = wqinvtab[j];
            const mulmod_precon_t wqi1 = wqinvtab[j+1];

            const long tt = MulModPrecon(a10, w0, q, wqi0);
            const long uu = a00;
            const long b00 = AddMod(uu, tt, q);
            const long b10 = SubMod(uu, tt, q);

            const long tt1 = MulModPrecon(a11, w1, q, wqi1);
            const long uu1 = a01;
            const long b01 = AddMod(uu1, tt1, q);
            const long b11 = SubMod(uu1, tt1, q);

            AA0[j] = b00;
            AA0[j+1] = b01;
            AA1[j] = b10;
            AA1[j+1] = b11;
         }
      }
#endif
   }

   // s == k, special case
   {
      m = 1L << s;
      m_half = 1L << (s-1);
      m_fourth = 1L << (s-2);

      const long* wtab = tab[s]->wtab_precomp.elts();
      const mulmod_precon_t *wqinvtab = tab[s]->wqinvtab_precomp.elts();

      for (i = 0; i < n; i+= m) {

         long *AA0 = &AA[i];
         long *AA1 = &AA[i + m_half];
         long *A0 = &A[i];
         long *A1 = &A[i + m_half];

#ifdef NTL_PIPELINE

// pipelining: seems to be faster

         t = AA1[0];
         u = AA0[0];
         t1 = MulModPrecon(AA1[1], wtab[1], q, wqinvtab[1]);
         u1 = AA0[1];

         for (j = 0; j < m_half-2; j += 2) {
            long a02 = AA0[j+2];
            long a03 = AA0[j+3];
            long a12 = AA1[j+2];
            long a13 = AA1[j+3];
            long w2 = wtab[j+2];
            long w3 = wtab[j+3];
            mulmod_precon_t wqi2 = wqinvtab[j+2];
            mulmod_precon_t wqi3 = wqinvtab[j+3];

            tt = MulModPrecon(a12, w2, q, wqi2);
            long b00 = AddMod(u, t, q);
            long b10 = SubMod(u, t, q);

            tt1 = MulModPrecon(a13, w3, q, wqi3);
            long b01 = AddMod(u1, t1, q);
            long b11 = SubMod(u1, t1, q);

            A0[j] = b00;
            A1[j] = b10;
            A0[j+1] = b01;
            A1[j+1] = b11;

            t = tt;
            u = a02;
            t1 = tt1;
            u1 = a03;
         }

         A0[j] = AddMod(u, t, q);
         A1[j] = SubMod(u, t, q);
         A0[j + 1] = AddMod(u1, t1, q);
         A1[j + 1] = SubMod(u1, t1, q);
      }
#else
         for (j = 0; j < m_half; j += 2) {
            const long a00 = AA0[j];
            const long a01 = AA0[j+1];
            const long a10 = AA1[j];
            const long a11 = AA1[j+1];

            const long w0 = wtab[j];
            const long w1 = wtab[j+1];
            const mulmod_precon_t wqi0 = wqinvtab[j];
            const mulmod_precon_t wqi1 = wqinvtab[j+1];

            const long tt = MulModPrecon(a10, w0, q, wqi0);
            const long uu = a00;
            const long b00 = AddMod(uu, tt, q);
            const long b10 = SubMod(uu, tt, q);

            const long tt1 = MulModPrecon(a11, w1, q, wqi1);
            const long uu1 = a01;
            const long b01 = AddMod(uu1, tt1, q);
            const long b11 = SubMod(uu1, tt1, q);

            A0[j] = b00;
            A0[j+1] = b01;
            A1[j] = b10;
            A1[j+1] = b11;
         }
      }
#endif
   }

}

#else

// FFT with precomputed tables and David Harvey's lazy multiplication
// strategy:
//   David Harvey.
//   Faster arithmetic for number-theoretic transforms.
//   J. Symb. Comp. 60 (2014) 113–119.

static inline
unsigned long LazyPrepMulModPrecon(long b, long n, double ninv)
{
   unsigned long q, r;

   q = (long) ( (((double) b) * NTL_SP_FBOUND) * ninv );
   r = (((unsigned long) b) << NTL_SP_NBITS ) - q * ((unsigned long) n);

   if (r >> (NTL_BITS_PER_LONG-1)) {
      q--;
      r += n;
   }
   else if (((long) r) >= n) {
      q++;
      r -=n;
   }

   unsigned long res = q << (NTL_BITS_PER_LONG - NTL_SP_NBITS);
   long qq, rr;

   rr = MulDivRem(qq, (long) r, 4, n, 4*ninv);

   res = res + (qq << (NTL_BITS_PER_LONG - NTL_SP_NBITS-2));

   return res;
}

static inline
unsigned long LazyMulModPrecon(unsigned long a, unsigned long b,
                               unsigned long n, unsigned long bninv)
{
   unsigned long q = MulHiUL(a, bninv);
   unsigned long res = a*b - q*n;
   return res;
}

static inline
unsigned long LazyReduce(unsigned long a, unsigned long q)
{
  unsigned long res;
#if (NTL_ARITH_RIGHT_SHIFT && defined(NTL_AVOID_BRANCHING) && !defined(NTL_CLEAN_INT))
  res = a - q;
  res  += (((long) res) >> (NTL_BITS_PER_LONG-1)) & q;
#elif (defined(NTL_AVOID_BRANCHING))
  res = a - q;
  res  += (-(res >> (NTL_BITS_PER_LONG-1))) & q;
#else
  if (a >= q)
    res = a - q;
  else
    res = a;
#endif

  return res;
}

static
void LazyPrecompFFTMultipliers(long k, long q, const long *root, const FFTMultipliers& tab)
{
   if (k < 1) LogicError("LazyPrecompFFTMultipliers: bad input");

   do { // NOTE: thread safe lazy init
      FFTMultipliers::Builder bld(tab, k+1);
      long amt = bld.amt();
      if (!amt) break;

      long first = k+1-amt;
      // initialize entries first..k

      double qinv = 1/((double) q);

      for (long s = first; s <= k; s++) {
         UniquePtr<FFTVectorPair> item;

         if (s == 0) {
            bld.move(item); // position 0 not used
            continue;
         }

         if (s == 1) {
            item.make();
            item->wtab_precomp.SetLength(1);
            item->wqinvtab_precomp.SetLength(1);
            item->wtab_precomp[0] = 1;
            item->wqinvtab_precomp[0] = LazyPrepMulModPrecon(1, q, qinv);
            bld.move(item);
            continue;
         }

         item.make();
         item->wtab_precomp.SetLength(1L << (s-1));
         item->wqinvtab_precomp.SetLength(1L << (s-1));

         long m = 1L << s;
         long m_half = 1L << (s-1);
         long m_fourth = 1L << (s-2);

         const long *wtab_last = tab[s-1]->wtab_precomp.elts();
         const mulmod_precon_t *wqinvtab_last = tab[s-1]->wqinvtab_precomp.elts();

         long *wtab = item->wtab_precomp.elts();
         mulmod_precon_t *wqinvtab = item->wqinvtab_precomp.elts();

         for (long i = 0; i < m_fourth; i++) {
            wtab[i] = wtab_last[i];
            wqinvtab[i] = wqinvtab_last[i];
         }

         long w = root[s];
         mulmod_precon_t wqinv = LazyPrepMulModPrecon(w, q, qinv);

         // prepare wtab...

         if (s == 2) {
            wtab[1] = LazyReduce(LazyMulModPrecon(wtab[0], w, q, wqinv), q);
            wqinvtab[1] = LazyPrepMulModPrecon(wtab[1], q, qinv);
         }
         else {
            // some software pipelining
            long i, j;

            i = m_half-1; j = m_fourth-1;
            wtab[i-1] = wtab[j];
            wqinvtab[i-1] = wqinvtab[j];
            wtab[i] = LazyReduce(LazyMulModPrecon(wtab[i-1], w, q, wqinv), q);

            i -= 2; j --;

            for (; i >= 0; i -= 2, j --) {
               long wp2 = wtab[i+2];
               long wm1 = wtab[j];
               wqinvtab[i+2] = LazyPrepMulModPrecon(wp2, q, qinv);
               wtab[i-1] = wm1;
               wqinvtab[i-1] = wqinvtab[j];
               wtab[i] = LazyReduce(LazyMulModPrecon(wm1, w, q, wqinv), q);
            }

            wqinvtab[1] = LazyPrepMulModPrecon(wtab[1], q, qinv);
         }

         bld.move(item);
      }
   } while (0);
}

void FFT(long* A, const long* a, long k, long q, const long* root, const FFTMultipliers& tab)

// performs a 2^k-point convolution modulo q

{
   if (k <= 1) {
      if (k == 0) {
	 A[0] = a[0];
	 return;
      }
      if (k == 1) {
	 long a0 = AddMod(a[0], a[1], q);
	 long a1 = SubMod(a[0], a[1], q);
         A[0] = a0;
         A[1] = a1;
	 return;
      }
   }

   // assume k > 1

   if (k >= tab.length()) LazyPrecompFFTMultipliers(k, q, root, tab);

   NTL_THREAD_LOCAL static Vec<unsigned long> AA_store;
   AA_store.SetLength(1L << k);
   unsigned long *AA = AA_store.elts();

   BitReverseCopy(AA, a, k);

   long n = 1L << k;

   /* we work with redundant representations, in the range [0, 4q) */

   long s, m, m_half, m_fourth, i, j;
   unsigned long t, u, t1, u1;

   long two_q = 2 * q;

   // s = 1

   for (i = 0; i < n; i += 2) {
      t = AA[i + 1];
      u = AA[i];
      AA[i] = u + t;
      AA[i+1] = u - t + q;
   }

   // s = 2

   {
      const long * NTL_RESTRICT wtab = tab[2]->wtab_precomp.elts();
      const mulmod_precon_t * NTL_RESTRICT wqinvtab = tab[2]->wqinvtab_precomp.elts();

      for (i = 0; i < n; i += 4) {

         unsigned long * NTL_RESTRICT AA0 = &AA[i];
         unsigned long * NTL_RESTRICT AA1 = &AA[i + 2];

         {
            const long w1 = wtab[0];
            const mulmod_precon_t wqi1 = wqinvtab[0];
            const unsigned long a11 = AA1[0];
            const unsigned long a01 = AA0[0];

            const unsigned long tt1 = LazyMulModPrecon(a11, w1, q, wqi1);
            const unsigned long uu1 = LazyReduce(a01, two_q);
            const unsigned long b01 = uu1 + tt1;
            const unsigned long b11 = uu1 - tt1 + two_q;

            AA0[0] = b01;
            AA1[0] = b11;
         }
         {
            const long w1 = wtab[1];
            const mulmod_precon_t wqi1 = wqinvtab[1];
            const unsigned long a11 = AA1[1];
            const unsigned long a01 = AA0[1];

            const unsigned long tt1 = LazyMulModPrecon(a11, w1, q, wqi1);
            const unsigned long uu1 = LazyReduce(a01, two_q);
            const unsigned long b01 = uu1 + tt1;
            const unsigned long b11 = uu1 - tt1 + two_q;

            AA0[1] = b01;
            AA1[1] = b11;
         }
      }
   }

   //  s = 3..k

   for (s = 3; s <= k; s++) {
      m = 1L << s;
      m_half = 1L << (s-1);
      m_fourth = 1L << (s-2);

      const long* NTL_RESTRICT wtab = tab[s]->wtab_precomp.elts();
      const mulmod_precon_t * NTL_RESTRICT wqinvtab = tab[s]->wqinvtab_precomp.elts();

      for (i = 0; i < n; i += m) {

         unsigned long * NTL_RESTRICT AA0 = &AA[i];
         unsigned long * NTL_RESTRICT AA1 = &AA[i + m_half];

#ifdef NTL_LOOP_UNROLL

         for (j = 0; j < m_half; j += 4) {
            {
               const long w1 = wtab[j+0];
               const mulmod_precon_t wqi1 = wqinvtab[j+0];
               const unsigned long a11 = AA1[j+0];
               const unsigned long a01 = AA0[j+0];

               const unsigned long tt1 = LazyMulModPrecon(a11, w1, q, wqi1);
               const unsigned long uu1 = LazyReduce(a01, two_q);
               const unsigned long b01 = uu1 + tt1;
               const unsigned long b11 = uu1 - tt1 + two_q;

               AA0[j+0] = b01;
               AA1[j+0] = b11;
            }
            {
               const long w1 = wtab[j+1];
               const mulmod_precon_t wqi1 = wqinvtab[j+1];
               const unsigned long a11 = AA1[j+1];
               const unsigned long a01 = AA0[j+1];

               const unsigned long tt1 = LazyMulModPrecon(a11, w1, q, wqi1);
               const unsigned long uu1 = LazyReduce(a01, two_q);
               const unsigned long b01 = uu1 + tt1;
               const unsigned long b11 = uu1 - tt1 + two_q;

               AA0[j+1] = b01;
               AA1[j+1] = b11;
            }
            {
               const long w1 = wtab[j+2];
               const mulmod_precon_t wqi1 = wqinvtab[j+2];
               const unsigned long a11 = AA1[j+2];
               const unsigned long a01 = AA0[j+2];

               const unsigned long tt1 = LazyMulModPrecon(a11, w1, q, wqi1);
               const unsigned long uu1 = LazyReduce(a01, two_q);
               const unsigned long b01 = uu1 + tt1;
               const unsigned long b11 = uu1 - tt1 + two_q;

               AA0[j+2] = b01;
               AA1[j+2] = b11;
            }
            {
               const long w1 = wtab[j+3];
               const mulmod_precon_t wqi1 = wqinvtab[j+3];
               const unsigned long a11 = AA1[j+3];
               const unsigned long a01 = AA0[j+3];

               const unsigned long tt1 = LazyMulModPrecon(a11, w1, q, wqi1);
               const unsigned long uu1 = LazyReduce(a01, two_q);
               const unsigned long b01 = uu1 + tt1;
               const unsigned long b11 = uu1 - tt1 + two_q;

               AA0[j+3] = b01;
               AA1[j+3] = b11;
            }
         }
#else

         for (j = 0; j < m_half; j++) {
            const long w1 = wtab[j];
            const mulmod_precon_t wqi1 = wqinvtab[j];
            const unsigned long a11 = AA1[j];
            const unsigned long a01 = AA0[j];

            const unsigned long tt1 = LazyMulModPrecon(a11, w1, q, wqi1);
            const unsigned long uu1 = LazyReduce(a01, two_q);
            const unsigned long b01 = uu1 + tt1;
            const unsigned long b11 = uu1 - tt1 + two_q;

            AA0[j] = b01;
            AA1[j] = b11;
         }

#endif

      }
   }

   /* need to reduce redundant representations */

   for (i = 0; i < n; i++) {
      unsigned long tmp = LazyReduce(AA[i], two_q);
      A[i] = LazyReduce(tmp, q);
   }
}

#endif

NTL_END_IMPL


#pragma once

/*
 * This file declares and implements a class representing a cubic spline.
 * After instantiating a Spline points (x, y) can be added; call calculate()
 * to compute coefficients of the cubic polynomial for each interval.
 * Spline::operator()(double x) computes the corresponding value of y
 * according to the computed spline.
 *
 * This isn't an elegant or optimal implementation but it gets the job done
 * for my purposes. Eigen is a dependency to do an SVD factorization to
 * compute the spline coefficients (the matrix obtained in my use cases
 * is numerically low-rank).
 */

#include <cassert>
#include <vector>

#include <Eigen/Dense>

namespace {

using Eigen::MatrixXd;
using Eigen::VectorXd;

VectorXd find_spline_coeffs(const std::vector<double>& xs_,
                            const std::vector<double>& ys_)
{
    assert(xs_.size() == ys_.size());
    unsigned nintervals = xs_.size() - 1;
    MatrixXd A = MatrixXd::Zero(nintervals*4, nintervals*4);
    VectorXd B = VectorXd::Zero(nintervals*4);

    A(0, 0) = 3*xs_[0]*xs_[0];
    A(0, 1) = 2*xs_[0];
    A(0, 2) = 1.0;
    A(1, 0) = xs_[0] * xs_[0] * xs_[0];
    A(1, 1) = xs_[0] * xs_[0];
    A(1, 2) = xs_[0];
    A(1, 3) = 1.0;
    B(1) = ys_[0];

    unsigned curr_row = 2;
    for (unsigned node = 1; node < nintervals; ++node) {
        double xn2 = xs_[node] * xs_[node];
        double xn3 = xn2 * xs_[node];
        unsigned j = (node - 1) * 4;
        A(curr_row, j) = xn3;
        A(curr_row, j+1) = xn2;
        A(curr_row, j+2) = xs_[node];
        A(curr_row, j+3) = 1.0;
        B(curr_row) = ys_[node];
        curr_row += 1;
        A(curr_row, j) = 3*xn2;
        A(curr_row, j+1) = 2*xs_[node];
        A(curr_row, j+2) = 1.0;
        A(curr_row, j+4) = -3*xn2;
        A(curr_row, j+5) = -2*xs_[node];
        A(curr_row, j+6) = -1.0;
        curr_row += 1;
        A(curr_row, j) = 6*xs_[node];
        A(curr_row, j+1) = 2.0;
        A(curr_row, j+4) = -6*xs_[node];
        A(curr_row, j+5) = -2.0;
        curr_row += 1;
        A(curr_row, j+4) = xn3;
        A(curr_row, j+5) = xn2;
        A(curr_row, j+6) = xs_[node];
        A(curr_row, j+7) = 1.0;
        B(curr_row) = ys_[node];
        curr_row += 1;
    }

    unsigned j = (nintervals - 1) * 4;
    A(curr_row, j) = xs_[nintervals] * xs_[nintervals] * xs_[nintervals];
    A(curr_row, j+1) = xs_[nintervals] * xs_[nintervals];
    A(curr_row, j+2) = xs_[nintervals];
    A(curr_row, j+3) = 1.0;
    B(curr_row++) = ys_[nintervals];
    A(curr_row, j) = 3*xs_[nintervals]*xs_[nintervals];
    A(curr_row, j+1) = 2 * xs_[nintervals];
    A(curr_row, j+2) = 1.0;

    return A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);
}

}

class Spline
{
private:
    struct Coefficients {
        double a; double b;
        double c; double d;
    };
    std::vector<double> xs_;
    std::vector<double> ys_;
    std::vector<Coefficients> coeffs_;

    template <typename It>
    It get_element_position(double x, It start, It end) const
    {
        if (end == start) {
            return start;
        } else if (end - start == 1) {
            return *start > x ? start : end;
        } else {
            It middle = start + (end - start) / 2;
            if (*middle > x) {
                return get_element_position(x, start, middle);
            } else {
                return get_element_position(x, middle, end);
            }
        }
    }

public:
    typedef std::pair<double, double> Point;

    void add_point(Point p)
    {
        auto it = get_element_position(p.first, xs_.begin(), xs_.end());
        auto offset = it - xs_.begin();
        xs_.insert(it, p.first);
        ys_.insert(ys_.begin() + offset, p.second);
    }

    void reset()
    {
        *this = Spline();
    }

    void calculate()
    {
        assert(xs_.size() == ys_.size());
        // X is a vector of a, b, c, d for each interval in the spline
        // in that order.
        auto X = find_spline_coeffs(xs_, ys_);
        coeffs_.resize(xs_.size() - 1);
        for (unsigned i = 0; i < coeffs_.size()*4; i += 4)
            coeffs_[i/4] = Coefficients{ X(i), X(i + 1), X(i + 2), X(i + 3) };
    }

    double operator()(double x) const
    {
        auto it = get_element_position(x, xs_.begin(), xs_.end());
        assert(!(it == xs_.begin() || it == xs_.end()));
        const auto& coeffs = coeffs_[it - xs_.begin() - 1];
        return coeffs.a * x * x * x + coeffs.b * x * x + coeffs.c * x +
            coeffs.d;
    }
};


#include <CGAL/trace.h>
#include <CGAL/Timer.h>
#include <iostream>
#include <string>
#include <fstream>
#include <CGAL/FPU_extension.h>

#include <Eigen/Eigen>
#include <Eigen/SVD>

double norm_1(Eigen::Matrix3d X)
{
  double sum = 0;
  for ( int i = 0; i < 3; i++ )
  {
    for ( int j = 0; j < 3; j++ )
    {
      sum += abs(X(i,j));
    }
  }
  return sum;
}

double norm_inf(Eigen::Matrix3d X)
{
  double max_abs = abs(X(0,0));
  for ( int i = 0; i < 3; i++ )
  {
    for ( int j = 0; j < 3; j++ )
    {
      double new_abs = abs(X(i,j));
      if ( new_abs > max_abs )
      {
        max_abs = new_abs;
      }
    }
  }
  return max_abs;
}

template<typename Mat>
void polar_eigen(const Mat& A, Mat& R, bool& SVD)
{
  typedef typename Mat::Scalar Scalar;
  typedef Eigen::Matrix<typename Mat::Scalar,3,1> Vec;

  const Scalar th = std::sqrt(Eigen::NumTraits<Scalar>::dummy_precision());

  Eigen::SelfAdjointEigenSolver<Mat> eig;
  feclearexcept(FE_UNDERFLOW);
  eig.computeDirect(A.transpose()*A);

  if(fetestexcept(FE_UNDERFLOW) || eig.eigenvalues()(0)/eig.eigenvalues()(2)/100.0<th)
  {
    // The computation of the eigenvalues might have diverged.
    // Fallback to an accurate SVD based decomposiiton method.
    Eigen::JacobiSVD<Mat> svd;
    svd.compute(A, Eigen::ComputeFullU | Eigen::ComputeFullV );
    const Mat& u = svd.matrixU(); const Mat& v = svd.matrixV(); const Vec& w = svd.singularValues();
    R = u*v.transpose();
    SVD = true;
    return;
  }

  Vec S = eig.eigenvalues().cwiseSqrt();
  R = A  * eig.eigenvectors() * S.asDiagonal().inverse()
    * eig.eigenvectors().transpose();
  SVD = false;
}

int main() {
  std::ifstream file;
  file.open("Polar_benchmark");
  if (!file)
  {
    CGAL_TRACE_STREAM << "Error loading file!\n";
    return 0;
  }

  Eigen::Matrix3d A, U, r;
  bool SVD = false;
  int num_svd = 0;
  int num_mtr = 0;

  CGAL_TRACE_STREAM << "start polar decomposition...\n";
  while (!file.eof())
  {
    for (int j = 0; j < 3; j++)
    {
      for (int k = 0; k < 3; k++)
      {
        file >> A(j, k);
      }
    }
    num_mtr++;
    double det = A.determinant();
    if (A.determinant() > 0)
    {
      polar_eigen<Eigen::Matrix3d> (A, U, SVD);
      if (SVD)
        num_svd++;
      U.transposeInPlace();
      double det_2 = U.determinant();
      if ( abs(det_2-1) > 1e-2 )
      {
        CGAL_TRACE_STREAM << "error matrix: det = " << det_2 << "\n";
      }
    }
  }
  file.close();

  CGAL_TRACE_STREAM << "done. " << num_svd << " SVD decompositions in " << num_mtr << " matrices\n";
  return 0;

  /*int ite = 200000;
  Eigen::JacobiSVD<Eigen::Matrix3d> svd;
  Eigen::Matrix3d A, U, H, r;

  int matrix_idx = rand()%200;
  for (int i = 0; i < matrix_idx; i++)
  {
    for (int j = 0; j < 3; j++)
    {
      for (int k = 0; k < 3; k++)
      {
        file >> A(j, k);
      }
    }
  }
  file.close();

  A(0,0) = 0.0014667022958104185; A(0,1) = 0.0024644551180079627; A(0,2) = 0.0040659871060825092;
  A(1,0) = 0.0028327558016991478; A(1,1) = 0.0054236820249146406; A(1,2) = 0.0079280090866983826;
  A(2,0) = 0.0039090073251031232; A(2,1) = 0.0066744523074963374; A(2,2) = 0.010848552426718550;
  A = A*10000;
  double det = A.determinant();

  CGAL::Timer task_timer;

  CGAL_TRACE_STREAM << "Start SVD decomposition...";
  task_timer.start();
  for (int i = 0; i < ite; i++)
  {
    if (A.determinant() > 0)
    {
      polar_eigen<Eigen::Matrix3d> (A, U);
      U.transposeInPlace();
      double det_2 = U.determinant();
      r = U*U.transpose();
    }
  }
  task_timer.stop();

  CGAL_TRACE_STREAM << "done: " << task_timer.time() << "s\n";

        return 0;*/
}


/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */

#include <cstdlib>
#include <cstdio>
#include <random>
#include <iostream>
#include <armadillo>
#include <ctime>
#include <chrono>
#include <boost/program_options.hpp>
namespace po = boost::program_options;

std::default_random_engine generator;

std::normal_distribution<double> distribution1(0,1);

std::chi_squared_distribution<double> distribution2(1.0);

std::student_t_distribution<double> distribution3(1.0);

std::uniform_real_distribution<double> distribution4(-1.0,1.0);

std::exponential_distribution<double> distribution5(1.0);

std::geometric_distribution<int> distribution6(0.5);

//double laplace_random_number = distribution5(generator) ;
//
//if(distribution4(generator) < 0.0 )
//
//   laplace_random_number = - laplace_random_number;

double get_random_number(int distribution)
{
    switch(distribution)
    {
        case 1: // Normal distribution
            return distribution1(generator);
        case 2: // chi-square
            return distribution2(generator);
        case 3: // student-t
            return distribution3(generator);
        case 4: // uniform
            return distribution4(generator);
        case 5: // exponential
            return distribution5(generator);
        case 6: // geometric
            return distribution6(generator);
        case 7:
            return distribution5(generator)*(distribution4(generator) < 0.0 ? -1 : 1);
        default:
            return 0.0;
            ;
    }
}

int get_distribution_code(const char* var_dist)
{
    if(strcmp(var_dist,"normal") == 0)
        return 1;
    else if(strcmp(var_dist,"chi-square") == 0)
        return 2;
    else if(strcmp(var_dist,"student-t") == 0)
        return 3;
    else if(strcmp(var_dist,"uniform") == 0)
        return 4;
    else if(strcmp(var_dist,"exponential") == 0)
        return 5;
    else if(strcmp(var_dist,"geometric") == 0)
        return 6;
    else if(strcmp(var_dist, "laplace")== 0)
        return 7;
    return 0;
}

int main(int argc, char** argv)

{
    //int64_t seed = time(NULL);
    //generator.seed(seed);
    generator.seed(std::chrono::system_clock::now().time_since_epoch().count());
    double signal_noise_ratio = 6.0;
    int num_vars = 2;
    int dependent_var_idx = 0;
    int distribution = 1; // 1 for normal distribution
    int noise_distribution = 1;
    std::string outputFile;
    int N = 1000;//sample size, number of rows
    std::string description = std::string("Generate a csv file.  Example usage: ") + argv[0] + " ";
//    po::options_description desc(description);
//    desc.add_options()
//            ("signal,s", po::value<double > (&signal_noise_ratio), "signal noise ratio")
//            ("output,o", po::value<std::string > (&outputFile), "The output file. Second positional argument.")
//            ("num_independents,i", po::value<int>(&num_vars)->default_value(1), "The number of independent variables")
//            ("distribution,d", po::value<int>(&distribution)->default_value(1)
//              , (std::string("The probability distributions\n\t 1 for normal distribution\n2 for chi-square")
//             // + "\n\t 3 for student-t distribution"
//             // + "\n\t 4 for uniform distribution"
//             // + "\n\t 5 for exponential distribution"
//              + "\n\t 6 for geometric distribution").c_str()
//            )
//            ("noise_distribution,z", po::value<int>(&noise_distribution)->default_value(1), "noise distribution")
//            ("sample size, n", po::value<int>(&N)->default_value(1000), "number of rows")
//            ("help,h", "Show this help message.")
//            ;

//    po::positional_options_description positionalOptions;
//    positionalOptions.add("input", 1);
//    positionalOptions.add("output", 1);

//    po::variables_map vm;
//    po::store(po::parse_command_line(argc, argv, desc), vm);
//    po::notify(vm);
//    if (vm.count("help") || argc == 1) {
//        std::cout << desc;
//        return 0;
//    }

    // temporary solution due to program options not working
    if(argc < 4)
    {
        printf("Usage: %s num_vars distribution noise_distribution noise_strength\n", argv[0]);
        exit(1);
    }
    num_vars = atoi(argv[1]);
    distribution = get_distribution_code ( argv[2] );
    noise_distribution = get_distribution_code( argv[3] );

    double noise_strength = 1.0;
    if(argc > 4)
        noise_strength = atof(argv[4]);

       const int cols =  num_vars + 1;

       double data[N][cols];

       //arma::vec error(N);

       double sumX=0.0;

       double sumX2=0.0;

       //dependent_var_idx = int((get_random_number(4)+1) * cols + 1) % cols;
       arma::vec beta(num_vars+1);
       //beta(dependent_var_idx) = 0.0;
       std::vector<int> indices(cols);
       for(int j=0;j<cols;j++)
           indices[j] = j;

       //random_shuffle(indices.begin(), indices.end());

       for(int i=0; i < N; i++)
       {
           data[i][ indices[0] ] = get_random_number(distribution);
       }
       for(int j=1; j < cols; j++)
       {
           const int curr = indices[j];
           const int prev = indices[j-1];
           beta[curr] = get_random_number(4) + 8.0;// range (6, 11) + signal_noise_ratio;
           if(abs(beta[curr]) < signal_noise_ratio)
               beta[curr] = abs(beta[curr])/beta[curr]*signal_noise_ratio;
           //double strength = int((get_random_number(4)) * 2000) % 50;

           for(int i=0; i<N; i++)
           {
                data[i][curr] = beta[curr] * data[i][prev] + noise_strength*get_random_number(noise_distribution);
           }
       }

       for(int j=1; j < cols; j++)
           printf("# %d -> %d, %12.4f\n", indices[j-1], indices[j], beta(indices[j]));
       printf("\n");
       for(int i=0; i<N; i++)
       {
           for(int j=0; j < cols; j++)
              printf("%12.4f%s ", data[i][j], j == cols-1 ? "" : ",");
           printf("\n");
       }

       return 0;
}


//
// Created by yuan on 27/07/2021.
//
#include <cstdlib>
#include <cstdio>
#include <iostream>
#include <chrono>
#include <thread>
#include <array>
#include <cmath>
#include <functional>
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <Eigen/QR>
#include <vector>

Eigen::MatrixXd pinv_Eigen_SVD(Eigen::MatrixXd &origin) {

  const float er = 0;
  // perform svd decomposition
  Eigen::JacobiSVD<Eigen::MatrixXd> svd_holder(origin, Eigen::ComputeThinU | Eigen::ComputeThinV);

  // Build SVD decomposition results
  Eigen::MatrixXd U = svd_holder.matrixU();
  Eigen::MatrixXd V = svd_holder.matrixV();
  Eigen::MatrixXd D = svd_holder.singularValues();

  // Build the S matrix
  Eigen::MatrixXd S(V.cols(), U.cols());
  S.setZero();

  for (unsigned int i = 0; i < D.size(); ++i) {

    if (D(i, 0) > er) {
      S(i, i) = 1 / D(i, 0);
    } else {
      S(i, i) = 0;
    }
  }

  // pinv_matrix = V * S * U^T
  return V * S * U.transpose();
}

int main()
{
  std::vector<int> values(10000);

  // Generate Random values
  auto f = []() -> int { return rand() % 10000; };

  // Fill up the vector
  generate(values.begin(), values.end(), f);

  // Get starting timepoint
  auto start = std::chrono::high_resolution_clock::now();

  // Call the function, here sort()
  Eigen::MatrixXd jacobian;

//  for ( int i(0); i<1; i++ ) {
    jacobian.setRandom(6,7);
//    Eigen::MatrixXd pinv_jacobian1 = pinv_Eigen_SVD(jacobian);
//    Eigen::MatrixXd pinv_jacobian2 = jacobian.completeOrthogonalDecomposition().pseudoInverse();
    Eigen::MatrixXd pinv_jacobian3 = jacobian.transpose() * (jacobian * jacobian.transpose()).inverse();
//  }

  // Get ending timepoint
  auto stop = std::chrono::high_resolution_clock::now();

//  std::cout << pinv_jacobian1 << std::endl << std::endl;
//  std::cout << pinv_jacobian2 << std::endl << std::endl;
//  std::cout << pinv_jacobian3 << std::endl << std::endl;

  // Get duration. Substart timepoints to
  // get durarion. To cast it to proper unit
  // use duration cast method
  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);

  std::cout << "Time taken by function: "
       << duration.count() << " microseconds" << std::endl;

  return 0;
}


#include "wave.h"
#include <boost/math/special_functions/bessel.hpp>

/**
 * Hankel_n^{(1)} function
 *
 * @param n index
 * @param x argument
 * @return value
 */
std::complex<double>
hankel1(const int n, const double x) {
    double besselJ = boost::math::cyl_bessel_j<int, double>(n, x);
    double besselY = boost::math::cyl_neumann<int, double>(n, x);
    return besselJ + J1*besselY;
}

/**
 * Incident wave
 *
 * @param kvec incident wave vector
 * @param point target point
 * @return amplitude
 */
std::complex<double>
incident(const double kvec[], const double point[]) {
    return std::exp(J1*(kvec[0]*point[0] + kvec[1]*point[1]));
}

/**
 * Normal gradient of the incident wave, assumes incident wave is exp(1j * kvec.x)
 *
 * @param nvec normal vector pointing inwards
 * @param kvec incident wave vector
 * @param point (source) point
 * @return amplitude
 */
std::complex<double>
gradIncident(const double nvec[], const double kvec[],
             const double point[]) {
    return J1*(nvec[0]*kvec[0] + nvec[1]*kvec[1])*incident(kvec, point);
}

/**
 * Scattered wave contribution from a single segment
 *
 * @param kvec incident wave vector
 * @param p0 starting point of the segment
 * @param p1 end point of the segment
 * @param point observer point
 * @return wave contribution
 */
std::complex<double>
computeScatteredWaveElement(const double kvec[], const double p0[],
                            const double p1[], const double point[]) {

    // xdot is anticlockwise
    double xdot[] = {p1[0] - p0[0], p1[1] - p0[1]};

    // mid point of the segment
    double pmid[] = {0.5*(p0[0] + p1[0]), 0.5*(p0[1] + p1[1])};

    // segment length
    double dsdt = std::sqrt(xdot[0]*xdot[0] + xdot[1]*xdot[1]);

    // normal vector, pointintg inwards and normalised
    double nvec[] = {-xdot[1]/dsdt, xdot[0]/dsdt, 0.};

    // from segment mid-point to observer
    double rvec[] = {point[0] - pmid[0], point[1] - pmid[1]};
    double r = std::sqrt(rvec[0]*rvec[0] + rvec[1]*rvec[1]);

    double kmod = std::sqrt(kvec[0]*kvec[0] + kvec[1]*kvec[1]);
    double kr = kmod * r;

    // Green functions and normal derivatives
    std::complex<double> g = (J1/4.) * hankel1(0, kr);
    double nDotR = nvec[0]*rvec[0] + nvec[1]*rvec[1];
    std::complex<double> dgdn = (-J1/4.) * hankel1(1, kr) * kmod * nDotR / r;

    // contribution from the gradient of the incident wave on the surface
    // of the obstacle. The normal derivative of the scattered wave is
    // - normal derivative of the incident wave.
    std::complex<double> scattered_wave = - dsdt * g * gradIncident(nvec, kvec, pmid);

    // shadow side: total wave is nearly zero
    //              => scattered wave amplitude = -incident wave ampl.
    //
    // illuminated side:
    //              => scattered wave amplitude = +incident wave ampl.
    //
    double nDotK = nvec[0]*kvec[0] + nvec[1]*kvec[1];
    double shadow = (nDotK > 0 ? 1.0 : -1.0);

    scattered_wave += shadow * dsdt * dgdn * incident(kvec, pmid);

    return scattered_wave;
}

extern "C" void
cincident (const double kvec[], const double point[], double* real_part, double* imag_part) {
    std::complex<double> res = incident(kvec, point);
    *real_part = res.real();
    *imag_part = res.imag();
}

extern "C" void computeScatteredWave(const double kvec[], int nc, const double xc[], const double yc[],
                                     const double point[], double* real_part, double* imag_part) {
    double res_real = 0;
    double res_imag = 0;
    // ADD OPENMP PRAGMA HERE
    for (int i = 0; i < nc - 1; ++i) {
        double p0[] = {xc[i], yc[i]};
        double p1[] = {xc[i + 1], yc[i + 1]};
        std::complex<double> res = computeScatteredWaveElement(kvec, p0, p1, point);
        res_real += res.real();
        res_imag += res.imag();
    }
    *real_part = res_real;
    *imag_part = res_imag;
}


// main.cpp
#include <stdio.h>
#include <iostream>
#include <Eigen/Dense>
#include <iostream>
#include "sophus/so3.hpp"
#include "sophus/common.hpp"

using Eigen::MatrixXd;
int main()
{
  MatrixXd m(2,2);
  m(0,0) = 3;
  m(1,0) = 2.5;
  m(0,1) = -1;
  m(1,1) = m(1,0) + m(0,1);
  m(1,1) = 0;
  std::cout << m << std::endl;

  // The following demonstrates the group multiplication of rotation matrices

  // Create rotation matrices from rotations around the x and y and z axes:
  const double kPi = Sophus::Constants<double>::pi();
  Sophus::SO3d R1 = Sophus::SO3d::rotX(kPi / 4);
  Sophus::SO3d R2 = Sophus::SO3d::rotY(kPi / 6);
  Sophus::SO3d R3 = Sophus::SO3d::rotZ(-kPi / 3);

  std::cout << "The rotation matrices are" << std::endl;
  std::cout << "R1:\n" << R1.matrix() << std::endl;
  std::cout << "R2:\n" << R2.matrix() << std::endl;
  std::cout << "R3:\n" << R3.matrix() << std::endl;
  std::cout << "Their product R1*R2*R3:\n"
            << (R1 * R2 * R3).matrix() << std::endl;
  std::cout << std::endl;

  // Rotation matrices can act on vectors
  Eigen::Vector3d x;
  x << 0.0, 0.0, 1.0;
  std::cout << "Rotation matrices can act on vectors" << std::endl;
  std::cout << "x\n" << x << std::endl;
  std::cout << "R2*x\n" << R2 * x << std::endl;
  std::cout << "R1*(R2*x)\n" << R1 * (R2 * x) << std::endl;
  std::cout << "(R1*R2)*x\n" << (R1 * R2) * x << std::endl;
  std::cout << std::endl;

  // SO(3) are internally represented as unit quaternions.
  std::cout << "R1 in matrix form:\n" << R1.matrix() << std::endl;
  std::cout << "R1 in unit quaternion form:\n"
            << R1.unit_quaternion().coeffs() << std::endl;
  // Note that the order of coefficiences of Eigen's quaternion class is
  // (imag0, imag1, imag2, real)
  std::cout << std::endl;
  // The following demonstrates the group multiplication of rotation matrices

  // Create rotation matrices from rotations around the x and y and z axes:
  // const double kPi = Sophus::Constants<double>::pi();
  // Sophus::SO3d R1 = Sophus::SO3d::rotX(kPi / 4);
  // Sophus::SO3d R2 = Sophus::SO3d::rotY(kPi / 6);
  // Sophus::SO3d R3 = Sophus::SO3d::rotZ(-kPi / 3);

  // std::cout << "The rotation matrices are" << std::endl;
  // std::cout << "R1:\n" << R1.matrix() << std::endl;
  // std::cout << "R2:\n" << R2.matrix() << std::endl;
  // std::cout << "R3:\n" << R3.matrix() << std::endl;
  // std::cout << "Their product R1*R2*R3:\n"
  //           << (R1 * R2 * R3).matrix() << std::endl;
  // std::cout << std::endl;

  // // Rotation matrices can act on vectors
  // Eigen::Vector3d x;
  // x << 0.0, 0.0, 1.0;
  // std::cout << "Rotation matrices can act on vectors" << std::endl;
  // std::cout << "x\n" << x << std::endl;
  // std::cout << "R2*x\n" << R2 * x << std::endl;
  // std::cout << "R1*(R2*x)\n" << R1 * (R2 * x) << std::endl;
  // std::cout << "(R1*R2)*x\n" << (R1 * R2) * x << std::endl;
  // std::cout << std::endl;

  // // SO(3) are internally represented as unit quaternions.
  // std::cout << "R1 in matrix form:\n" << R1.matrix() << std::endl;
  // std::cout << "R1 in unit quaternion form:\n"
  //           << R1.unit_quaternion().coeffs() << std::endl;
  // // Note that the order of coefficiences of Eigen's quaternion class is
  // (imag0, imag1, imag2, real)
  std::cout << std::endl;

}


/*
 * Copyright 2017 Mahdi Khanalizadeh
 *
 * 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 HEADER_EXT_MATRIX2_HPP_INCLUDED
#define HEADER_EXT_MATRIX2_HPP_INCLUDED

#include "vector2.hpp"

#include <cassert>

#include <algorithm>

#include <boost/operators.hpp>

namespace ext
{

	template <typename T>
	class Matrix2 :
		boost::equality_comparable<Matrix2<T>,
		boost::ring_operators<Matrix2<T>,
		boost::multipliable<Matrix2<T>, T
		>>>
	{
	public:
		using Type = T;

		Type x[4];
	};

	using Matrix2f = Matrix2<float>;
	using Matrix2d = Matrix2<double>;
	using Matrix2ld = Matrix2<long double>;

	template <typename T>
	bool operator==(Matrix2<T> const& m1, Matrix2<T> const& m2)
	{
		return std::equal(m1.x, m1.x + (sizeof m1.x / sizeof m1.x[0]), m2.x);
	}

	template <typename T>
	Matrix2<T>& operator+=(Matrix2<T>& m1, Matrix2<T> const& m2)
	{
		std::transform(m1.x, m1.x + (sizeof m1.x / sizeof m1.x[0]), m2.x, m1.x, [](T one, T two){ return one + two; });
		return m1;
	}

	template <typename T>
	Matrix2<T>& operator-=(Matrix2<T>& m1, Matrix2<T> const& m2)
	{
		std::transform(m1.x, m1.x + (sizeof m1.x / sizeof m1.x[0]), m2.x, m1.x, [](T one, T two){ return one - two; });
		return m1;
	}

	template <typename T>
	Matrix2<T>& operator*=(Matrix2<T>& m1, Matrix2<T> const& m2)
	{
		auto const m = m1;
		m1.x[0] = m.x[0] * m2.x[0] + m.x[1] * m2.x[2];
		m1.x[1] = m.x[0] * m2.x[1] + m.x[1] * m2.x[3];
		m1.x[2] = m.x[2] * m2.x[0] + m.x[3] * m2.x[2];
		m1.x[3] = m.x[2] * m2.x[1] + m.x[3] * m2.x[3];
		return m1;
	}

	template <typename T>
	Matrix2<T>& operator*=(Matrix2<T>& m, T const a)
	{
		std::transform(m.x, m.x + (sizeof m.x / sizeof m.x[0]), m.x, [a](T x){ return x * a; });
		return m;
	}

	template <typename T>
	Vector2<T> operator * (Matrix2<T> const& m, Vector2<T> const& v)
	{
		return {m.x[0] * v.x + m.x[1] * v.y, m.x[2] * v.x + m.x[3] * v.y};
	}

	template <typename T>
	Vector2<T> operator * (Vector2<T> const& v, Matrix2<T> const& m)
	{
		return {v.x * m.x[0] + v.y * m.x[2], v.x * m.x[1] + v.y * m.x[3]};
	}

	template <typename T>
	Matrix2<T> transpose(Matrix2<T> const& m)
	{
		auto t = m;
		t.x[1] = m.x[2];
		t.x[2] = m.x[1];
		return t;
	}

	template <typename T>
	T trace(Matrix2<T> const& m)
	{
		return m.x[0] + m.x[3];
	}

	template <typename T>
	T determinant(Matrix2<T> const& m)
	{
		return m.x[0] * m.x[3] - m.x[1] * m.x[2];
	}

	template <typename T>
	Matrix2<T> inverse(Matrix2<T> const& m)
	{
		assert(determinant(m) != 0);
		Matrix2<T> n;
		n.x[0] = m.x[3];
		n.x[1] = -m.x[1];
		n.x[2] = -m.x[2];
		n.x[3] = m.x[0];
		n *= 1/determinant(m);
		return n;
	}

} // namespace ext

#endif // !HEADER_EXT_MATRIX2_HPP_INCLUDED


#ifndef _LINEAR_ALGEBRA_HPP_
#define _LINEAR_ALGEBRA_HPP_

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include <Eigen/StdVector>
#include <dlib/optimization.h>
#include <mimkl/definitions.hpp>
#include <mimkl/utilities.hpp>
#include <numeric>
#include <spdlog/fmt/ostr.h>
#include <spdlog/spdlog.h>

using mimkl::definitions::Index;

namespace mimkl
{
namespace linear_algebra
{

//! \f$ K' = K / (tr(K) / n)\f$
/*!
devide matrix by a factor so that trace will equal the number of rows \f$ tr(K')
= n \f$
@param K square matrix to be trace_normalized by reference
@return trace_factor \f$ tr(K) / n\f$
*/
template <typename Derived>
typename Derived::Scalar trace_normalize(Derived &K)
{
    typename Derived::Scalar trace_factor = K.trace() / K.rows();
    K = K / trace_factor;
    return trace_factor;
}

//! \f$ k^{*}(x,y) = \fraq{k(x,y)}{\sqrt{k(x,x) k(y,y)}}\f$
template <typename Derived>
Derived normalize_kernel(const Derived &K)
{
    COLUMN(typename Derived::Scalar) diag = 1 / K.diagonal().array().sqrt();
    return K.array() * (diag * diag.transpose()).array(); // outer product of
                                                          // diagonal elements
}

//! \f$ k^{*}(x,y) = \fraq{k(x,y)}{\sqrt{k(x,x) k(y,y)}}\f$, where k(x,x) stems
//! from the training data and k(y,y) from the test data
template <typename LhsDerived, typename RhsDerived, typename Derived>
Derived normalize_kernel_prediction(const Derived &K,
                                    const LhsDerived &lhs,
                                    const RhsDerived &rhs)
{
    COLUMN(typename Derived::Scalar)
    diag_lhs = 1 / lhs.diagonal().array().sqrt();
    COLUMN(typename Derived::Scalar)
    diag_rhs = 1 / rhs.diagonal().array().sqrt();
    return K.array() *
           (diag_lhs * diag_rhs.transpose()).array(); // outer product of
                                                      // diagonal elements
}

//! centralizing the data in feature space \f$ \mathbf{K}' = ( \mathbf{I} -
//! \mathbf{1_N}) \mathbf{K} ( \mathbf{I} - \mathbf{1_N}) =  \mathbf{K} -
//! \mathbf{1_N} \mathbf{K} - \mathbf{K} \mathbf{1_N} + \mathbf{1_N} \mathbf{K}
//! \mathbf{1_N}\f$
/*!
where \f$\mathbf{1_N}\f$ denotes a N-by-N matrix for which each element takes
value 1/N
*/
template <typename Derived>
Derived centralize_kernel(const Derived &K)
{ // TODO how to do for test data?
    Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, 1> rowwise_mean =
    K.rowwise().mean();
    return ((K.colwise() - rowwise_mean).rowwise() - rowwise_mean.transpose())
           .array() +
           rowwise_mean.mean();
}

template <typename Scalar,
          typename StorageOrder> // TODO why does const & not compile?
Eigen::Map<const COLUMN(Scalar)> dlib_to_eigen(
const dlib::matrix<Scalar, 0, 1, dlib::default_memory_manager, StorageOrder> &vector)
{ // more general case
    // with template
    // specializations not
    // needed as of yet.
    spdlog::get("console")->debug("sum over passed vector:\n{}",
                                  dlib::sum(vector));
    typedef COLUMN(Scalar) Column;
    typedef Eigen::Map<const Column> MapColumn;
    // Map Dlib to Eigen
    const Scalar *dlib_ptr = vector.begin(); //&vector.data(0,0);
    if (dlib::is_row_major(vector) ^ (Column::Flags & Eigen::RowMajorBit))
    { // if XOR warn, for instance Y: Y.isRowMajor
        auto logger = spdlog::get("console");
        logger->critical("make sure LibMat is in col-major or that mapping "
                         "happens "
                         "in row-major:\n"
                         "dlib::is_row_major(vector): {}\n"
                         "Column::Options: {}\n"
                         "(Column::Flags&Eigen::RowMajorBit): {}",
                         dlib::is_row_major(vector), Column::Options,
                         (Column::Flags & Eigen::RowMajorBit));
    };
    return MapColumn(dlib_ptr, vector.nr(), 1);
}

//! squared euclidean distance of rows in X to itself: M_ij =  x_i^T*x_i +
//! x_j^T*x_j -
//! 2*x_i^T*x_j
template <typename LhsDerived, typename MDerived>
void squared_euclidean_distances(const Eigen::MatrixBase<LhsDerived> &lhs,
                                 const Eigen::MatrixBase<MDerived> &distance_matrix)
{
    MDerived lin_kernel = lhs * lhs.transpose();
    const_cast<Eigen::MatrixBase<MDerived> &>(distance_matrix) =
    ((-2 * lin_kernel.real()).colwise() + lin_kernel.diagonal()).rowwise() +
    lin_kernel.diagonal().adjoint();
}

template <typename Scalar>
MATRIX(Scalar)
rowwise_soft_max(MATRIX(Scalar) M)
{
    for (Index i = 0; i < M.rows(); i++)
    {
        Scalar max = M.row(i).maxCoeff(); // numerical stability
        Scalar sum = 0;
        for (Index j = 0; j < M.cols(); j++)
        {
            sum += std::exp(M(i, j) - max);
        }
        Scalar normalizer = std::log(sum); // numerical stability
        for (Index j = 0; j < M.cols(); j++)
        {
            M(i, j) = std::exp(M(i, j) - max - normalizer);
        }
    }
    return M;
}

// TODO documentation
template <typename Scalar>
MATRIX(Scalar)
sum_matrices(const std::vector<MATRIX(Scalar)> &Ks)
{
    // fold over Ks
    return std::accumulate(Ks.begin(), Ks.end(),
                           MATRIX(Scalar)::Zero((*Ks.begin()).rows(),
                                                (*Ks.begin()).cols())
                           .eval()); //, [](a,b){operator+(a,b)}
}

template <typename Scalar>
MATRIX(Scalar)
sum_trace_normalized_matrices(const std::vector<MATRIX(Scalar)> &Ks,
                              std::vector<Scalar> &trace_factors)
{
    // fold over Ks, trace_normalize K before summing and store its trace
    MATRIX(Scalar) acc; // not writing 0s
    MATRIX(Scalar) K;   // temporary holding the trace_normalized kernel
    if (Ks.begin() != Ks.end())
    {
        Index i = 0;
        acc = Ks[i]; // non const copy
        trace_factors[i] = trace_normalize(acc);
        ++i;
        for (; i < Ks.size(); ++i)
        {
            K = Ks[i];
            trace_factors[i] = trace_normalize(K);
            acc += K;
        }
    }
}

// not using std::accumulate, see commented version below
template <typename Scalar, typename Function>
MATRIX(Scalar)
aggregate_kernels(const MATRIX(Scalar) & lhs,
                  const MATRIX(Scalar) & rhs,
                  const std::vector<Function> &Ks)
{
    MATRIX(Scalar) acc; // not writing 0s
    if (Ks.begin() != Ks.end())
    {
        typename std::vector<Function>::const_iterator iter = Ks.begin();
        acc = (*iter)(lhs, rhs);
        ++iter;
        for (; iter != Ks.end(); ++iter)
        {
            acc += (*iter)(lhs, rhs);
        }
    } // TODO else { throw | warn | ....}
    return acc;
}

template <typename Scalar, typename Function>
MATRIX(Scalar)
aggregate_trace_normalized_kernels(const MATRIX(Scalar) & lhs,
                                   const MATRIX(Scalar) & rhs,
                                   const std::vector<Function> &Ks,
                                   std::vector<Scalar> &trace_factors,
                                   bool update_trace_factors)
{
    if (Ks.size() != trace_factors.size())
        throw std::length_error(
        " number of functions does not match number of trace_factors");
    MATRIX(Scalar) acc; // not writing 0s
    if (update_trace_factors)
    {
        MATRIX(Scalar) K; // temporary holding the trace_normalized kernel
        if (Ks.begin() != Ks.end())
        {
            Index i = 0;
            K = Ks[i](lhs, rhs); // non const copy
            trace_factors[i] = trace_normalize(K);
            acc = K;
            ++i;
            for (; i < Ks.size(); ++i)
            {
                K = Ks[i](lhs, rhs);
                trace_factors[i] = trace_normalize(K);
                acc += K;
            }
        }
    }
    else
    {
        if (Ks.begin() != Ks.end())
        {
            Index i = 0;
            acc = Ks[i](lhs, rhs) / trace_factors[i];
            ++i;
            for (; i < Ks.size(); ++i)
            {
                acc += Ks[i](lhs, rhs) / trace_factors[i];
            }
        }
    }
    return acc;
}

// TODO documentation
template <typename Scalar>
MATRIX(Scalar)
sum_weighted_matrices(const std::vector<MATRIX(Scalar)> &Ks,
                      const COLUMN(Scalar) & eta)
{
    return std::inner_product(
    Ks.begin(), Ks.end(),
    eta.data(), // Eigen has no .begin() and .end()
    MATRIX(Scalar)::Zero((*Ks.begin()).rows(), (*Ks.begin()).cols()).eval());
}

template <typename Scalar, typename Function>
MATRIX(Scalar)
aggregate_weighted_kernels(const MATRIX(Scalar) & lhs,
                           const MATRIX(Scalar) & rhs,
                           const std::vector<Function> &fs,
                           const COLUMN(Scalar) & eta)
{
    MATRIX(Scalar) acc; // not writing 0s
    if (fs.begin() != fs.end())
    {
        Index i = 0;
        acc = fs[i](lhs, rhs) * eta(i);
        ++i;
        for (; i < fs.size(); ++i)
        {
            acc += fs[i](lhs, rhs) * eta(i);
        }
    } // TODO else { throw | warn | ....}
    return acc;
}

template <typename Scalar, typename Function>
MATRIX(Scalar)
aggregate_weighted_trace_normalized_kernels(const MATRIX(Scalar) & lhs,
                                            const MATRIX(Scalar) & rhs,
                                            const std::vector<Function> &Ks,
                                            std::vector<Scalar> &trace_factors,
                                            bool update_trace_factors, // update_trace_factors
                                                                       // or
                                            // read trace_factors
                                            const COLUMN(Scalar) & eta)
{
    if (Ks.size() != trace_factors.size())
        throw std::length_error(
        " number of functions does not match number of trace_factors");
    MATRIX(Scalar) acc; // not writing 0s
    if (update_trace_factors)
    {
        MATRIX(Scalar) K; // temporary holding the trace_normalized kernel
        if (Ks.begin() != Ks.end())
        {
            Index i = 0;
            K = Ks[i](lhs, rhs); // non const copy
            trace_factors[i] = trace_normalize(K);
            acc = K * eta(i);
            ++i;
            for (; i < Ks.size(); ++i)
            {
                K = Ks[i](lhs, rhs);
                trace_factors[i] = trace_normalize(K);
                acc += K * eta(i);
            }
        }
    }
    else
    {
        if (Ks.begin() != Ks.end())
        {
            Index i = 0;
            acc = Ks[i](lhs, rhs) / trace_factors[i] * eta(i);
            ++i;
            for (; i < Ks.size(); ++i)
            {
                acc += Ks[i](lhs, rhs) / trace_factors[i] * eta(i);
            }
        }
    } // TODO else { throw | warn | ....}
    return acc;
}

//! fill a sparse diagonal matrix with a given value
/*! only if elements don't exist already, else use coeffRef() instead of
insert()
\param I a sparse matrix of type Eigen::SparseMatrixBase<Derived>.
\param value a value to fill the diagonal of type double.
\sa test/sparse_diagonal
*/
template <typename Derived>
void fill_sparse_diagonal(Eigen::SparseMatrixBase<Derived> const &I,
                          const double value)
{
    // size of diagonal
    const Index &m = I.rows();
    const Index &n = I.cols();
    Index d = (m < n ? m : n);
    Eigen::SparseMatrixBase<Derived> &I_ =
    const_cast<Eigen::SparseMatrixBase<Derived> &>(I);
    for (Index i = 0; i < d; ++i)
    {
        I_.derived().insert(i, i) = value;
    }
}

// helpers for indexing an Eigen::Matrix
// adapted from
// http://eigen.tuxfamily.org/dox-devel/TopicCustomizing_NullaryExpr.html#title1
template <class ArgumentType, class RowIndexType, class ColumnIndexType>
class IndexingFunctor
{
    private:
    const ArgumentType &argument_;
    const RowIndexType &row_indices_;
    const ColumnIndexType &column_indices_;

    public:
    typedef Eigen::Matrix<
    typename ArgumentType::Scalar,
    RowIndexType::SizeAtCompileTime,
    ColumnIndexType::SizeAtCompileTime,
    ArgumentType::Flags & Eigen::RowMajorBit ? Eigen::RowMajor : Eigen::ColMajor,
    RowIndexType::MaxSizeAtCompileTime,
    ColumnIndexType::MaxSizeAtCompileTime>
    MatrixType;
    IndexingFunctor(const ArgumentType &argument,
                    const RowIndexType &row_indices,
                    const ColumnIndexType &column_indices)
    : argument_(argument),
      row_indices_(row_indices),
      column_indices_(column_indices)
    {
    }
    const typename ArgumentType::Scalar &
    operator()(Eigen::Index row, Eigen::Index column) const
    {
        return argument_(row_indices_[row], column_indices_[column]);
    }
};

template <class ArgumentType, class RowIndexType, class ColumnIndexType>
Eigen::CwiseNullaryOp<
IndexingFunctor<ArgumentType, RowIndexType, ColumnIndexType>,
typename IndexingFunctor<ArgumentType, RowIndexType, ColumnIndexType>::MatrixType>
indexing(const Eigen::MatrixBase<ArgumentType> &argument,
         const RowIndexType &row_indices,
         const ColumnIndexType &column_indices)
{
    typedef IndexingFunctor<ArgumentType, RowIndexType, ColumnIndexType> Indexer;
    typedef typename Indexer::MatrixType MatrixType;
    return MatrixType::NullaryExpr(row_indices.size(), column_indices.size(),
                                   Indexer(argument.derived(), row_indices,
                                           column_indices));
};
} // namespace linear_algebra
} // namespace mimkl

#endif /*_LINEAR_ALGEBRA_HPP_*/