#include <NTL/mat_GF2.h>
#include <NTL/vec_long.h>

NTL_START_IMPL

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

   if (B.NumRows() != n || B.NumCols() != m)
      LogicError("matrix add: dimension mismatch");

   X.SetDims(n, m);

   long mw = (m + NTL_BITS_PER_LONG - 1)/NTL_BITS_PER_LONG;

   long i;
   for (i = 0; i < n; i++) {
      _ntl_ulong *xp = X[i].rep.elts();
      const _ntl_ulong *ap = A[i].rep.elts();
      const _ntl_ulong *bp = B[i].rep.elts();
      long j;
      for (j = 0; j < mw; j++)
         xp[j] = ap[j] ^ bp[j];
   }
}

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

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

   x.SetLength(n);

   long i;

   for (i = 0; i < n; i++) {
      x.put(i, A[i] * b);
   }
}

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

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

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

   x.SetLength(l);
   clear(x);

   const _ntl_ulong *ap = a.rep.elts();
   _ntl_ulong a_mask = 1;

   _ntl_ulong *xp = x.rep.elts();

   long lw = (l + NTL_BITS_PER_LONG - 1)/NTL_BITS_PER_LONG;

   long i;
   for (i = 0; i < n; i++) {
      if (*ap & a_mask) {
         const _ntl_ulong *bp = B[i].rep.elts();
         long j;
         for (j = 0; j < lw; j++)
            xp[j] ^= bp[j];
      }

      a_mask <<= 1;
      if (!a_mask) {
         a_mask = 1;
         ap++;
      }
   }
}

void mul(vec_GF2& x, const vec_GF2& a, const mat_GF2& B)
{
   if (&a == &x || B.alias(x)) {
      vec_GF2 tmp;
      mul_aux(tmp, a, B);
      x = tmp;
   }
   else
      mul_aux(x, a, B);
}

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

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

   X.SetDims(n, m);

   long i;

   for (i = 1; i <= n; i++) {
      mul_aux(X(i), A(i), B);
   }
}

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

void ident(mat_GF2& X, long n)
{
   X.SetDims(n, n);
   clear(X);
   long i;

   for (i = 0; i < n; i++)
      X.put(i, i, to_GF2(1));
}

void determinant(ref_GF2 d, const mat_GF2& M_in)
{
   long k, n;
   long i, j;
   long pos;

   n = M_in.NumRows();

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

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

   mat_GF2 M;

   M = M_in;

   long wn = (n + NTL_BITS_PER_LONG - 1)/NTL_BITS_PER_LONG;

   for (k = 0; k < n; k++) {
      long wk = k/NTL_BITS_PER_LONG;
      long bk = k - wk*NTL_BITS_PER_LONG;
      _ntl_ulong k_mask = 1UL << bk;

      pos = -1;
      for (i = k; i < n; i++) {
         if (M[i].rep.elts()[wk] & k_mask) {
            pos = i;
            break;
         }
      }

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

         _ntl_ulong *y = M[k].rep.elts();

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

            if (M[i].rep.elts()[wk] & k_mask) {
               _ntl_ulong *x = M[i].rep.elts();

               for (j = wk; j < wn; j++)
                  x[j] ^= y[j];
            }

         }
      }
      else {
         clear(d);
         return;
      }
   }

   set(d);
   return;
}

static
long IsUnitVector(const vec_GF2& a, long i)
{
   long wi = i/NTL_BITS_PER_LONG;
   long bi = i - wi*NTL_BITS_PER_LONG;

   const _ntl_ulong *p = a.rep.elts();
   long wdlen = a.rep.length();

   long j;

   for (j = 0; j < wi; j++)
      if (p[j] != 0) return 0;

   if (p[wi] != (1UL << bi))
      return 0;

   for (j = wi+1; j < wdlen; j++)
      if (p[j] != 0) return 0;

   return 1;
}

long IsIdent(const mat_GF2& A, long n)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   if (n == 0) return 1;

   long i;

   for (i = 0; i < n; i++)
      if (!IsUnitVector(A[i], i))
         return 0;

   return 1;
}

void AddToCol(mat_GF2& x, long j, const vec_GF2& a)
// add a to column j of x
// ALIAS RESTRICTION: a should not alias any row of x
{
   long n = x.NumRows();
   long m = x.NumCols();

   if (a.length() != n || j < 0 || j >= m)
      LogicError("AddToCol: bad args");

   long wj = j/NTL_BITS_PER_LONG;
   long bj = j - wj*NTL_BITS_PER_LONG;
   _ntl_ulong j_mask = 1UL << bj;

   const _ntl_ulong *ap = a.rep.elts();
   _ntl_ulong a_mask = 1;

   long i;
   for (i = 0; i < n; i++) {
      if (*ap & a_mask)
         x[i].rep.elts()[wj] ^= j_mask;

      a_mask <<= 1;
      if (!a_mask) {
         a_mask = 1;
         ap++;
      }
   }
}

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

   X.SetDims(m, n);
   clear(X);

   long i;
   for (i = 0; i < n; i++)
      AddToCol(X, i, A[i]);
}

void transpose(mat_GF2& X, const mat_GF2& A)
{
   if (&X == &A) {
      mat_GF2 tmp;
      transpose_aux(tmp, A);
      X = tmp;
   }
   else
      transpose_aux(X, A);
}

static
void solve_impl(ref_GF2 d, vec_GF2& X, const mat_GF2& A, const vec_GF2& b, bool trans)

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

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

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

   long i, j, k, pos;

   mat_GF2 M;
   M.SetDims(n, n+1);

   if (trans) {
      for (i = 0; i < n; i++) {
	 AddToCol(M, i, A[i]);
      }
   }
   else {
      for (i = 0; i < n; i++) {
         VectorCopy(M[i], A[i], n+1);
      }
   }

   AddToCol(M, n, b);

   long wn = ((n+1) + NTL_BITS_PER_LONG - 1)/NTL_BITS_PER_LONG;

   for (k = 0; k < n; k++) {
      long wk = k/NTL_BITS_PER_LONG;
      long bk = k - wk*NTL_BITS_PER_LONG;
      _ntl_ulong k_mask = 1UL << bk;

      pos = -1;
      for (i = k; i < n; i++) {
         if (M[i].rep.elts()[wk] & k_mask) {
            pos = i;
            break;
         }
      }

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

         _ntl_ulong *y = M[k].rep.elts();

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

            if (M[i].rep.elts()[wk] & k_mask) {
               _ntl_ulong *x = M[i].rep.elts();

               for (j = wk; j < wn; j++)
                  x[j] ^= y[j];
            }

         }
      }
      else {
         clear(d);
         return;
      }
   }

   vec_GF2 XX;
   XX.SetLength(n+1);
   XX.put(n, 1);

   for (i = n-1; i >= 0; i--) {
      XX.put(i, XX*M[i]);
   }

   XX.SetLength(n);
   X = XX;

   set(d);
   return;
}

void solve(ref_GF2 d, vec_GF2& x, const mat_GF2& A, const vec_GF2& b)
{
   solve_impl(d, x, A, b, true);
}

void solve(ref_GF2 d, const mat_GF2& A, vec_GF2& x,  const vec_GF2& b)
{
   solve_impl(d, x, A, b, false);
}

void inv(ref_GF2 d, mat_GF2& X, const mat_GF2& A)
{
   long n = A.NumRows();
   if (A.NumCols() != n)
      LogicError("solve: nonsquare matrix");

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

   long i, j, k, pos;

   mat_GF2 M;
   M.SetDims(n, 2*n);

   vec_GF2 aa;
   aa.SetLength(2*n);

   for (i = 0; i < n; i++) {
      aa = A[i];
      aa.SetLength(2*n);
      aa.put(n+i, 1);
      M[i] = aa;
   }

   long wn = ((2*n) + NTL_BITS_PER_LONG - 1)/NTL_BITS_PER_LONG;

   for (k = 0; k < n; k++) {
      long wk = k/NTL_BITS_PER_LONG;
      long bk = k - wk*NTL_BITS_PER_LONG;
      _ntl_ulong k_mask = 1UL << bk;

      pos = -1;
      for (i = k; i < n; i++) {
         if (M[i].rep.elts()[wk] & k_mask) {
            pos = i;
            break;
         }
      }

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

         _ntl_ulong *y = M[k].rep.elts();

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

            if (M[i].rep.elts()[wk] & k_mask) {
               _ntl_ulong *x = M[i].rep.elts();

               for (j = wk; j < wn; j++)
                  x[j] ^= y[j];
            }

         }
      }
      else {
         clear(d);
         return;
      }
   }

   vec_GF2 XX;
   XX.SetLength(2*n);

   X.SetDims(n, n);
   clear(X);

   for (j = 0; j < n; j++) {
      XX.SetLength(n+j+1);
      clear(XX);
      XX.put(n+j, to_GF2(1));

      for (i = n-1; i >= 0; i--) {
         XX.put(i, XX*M[i]);
      }

      XX.SetLength(n);
      AddToCol(X, j, XX);
   }

   set(d);
   return;
}

long gauss(mat_GF2& M, long w)
{
   long k, l;
   long i, j;
   long pos;

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

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

   long wm = (m + NTL_BITS_PER_LONG - 1)/NTL_BITS_PER_LONG;

   l = 0;
   for (k = 0; k < w && l < n; k++) {
      long wk = k/NTL_BITS_PER_LONG;
      long bk = k - wk*NTL_BITS_PER_LONG;
      _ntl_ulong k_mask = 1UL << bk;

      pos = -1;
      for (i = l; i < n; i++) {
         if (M[i].rep.elts()[wk] & k_mask) {
            pos = i;
            break;
         }
      }

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

         _ntl_ulong *y = M[l].rep.elts();

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

            if (M[i].rep.elts()[wk] & k_mask) {
               _ntl_ulong *x = M[i].rep.elts();

               for (j = wk; j < wm; j++)
                  x[j] ^= y[j];
            }
         }

         l++;
      }
   }

   return l;
}

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

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

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

   mat_GF2 M;
   long r;

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

   X.SetDims(m-r, m);
   clear(X);

   long i, j, k;

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

   j = -1;
   for (i = 0; i < r; i++) {
      do {
         j++;
      } while (M.get(i, j) == 0);

      D[j] = i;
   }

   for (k = 0; k < m-r; k++) {
      vec_GF2& v = X[k];
      long pos = 0;
      for (j = m-1; j >= 0; j--) {
         if (D[j] == -1) {
            if (pos == k) {
               v[j] = 1;
               // v.put(j, to_GF2(1));
            }
            pos++;
         }
         else {
            v[j] = v*M[D[j]];
            // v.put(j, v*M[D[j]]);
         }
      }
   }
}

void mul(mat_GF2& X, const mat_GF2& A, GF2 b)
{
   X = A;
   if (b == 0)
      clear(X);
}

void diag(mat_GF2& X, long n, GF2 d)
{
   if (d == 1)
      ident(X, n);
   else {
      X.SetDims(n, n);
      clear(X);
   }
}

long IsDiag(const mat_GF2& A, long n, GF2 d)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   if (d == 1)
      return IsIdent(A, n);
   else
      return IsZero(A);
}

long IsZero(const mat_GF2& a)
{
   long n = a.NumRows();
   long i;

   for (i = 0; i < n; i++)
      if (!IsZero(a[i]))
         return 0;

   return 1;
}

void clear(mat_GF2& x)
{
   long n = x.NumRows();
   long i;
   for (i = 0; i < n; i++)
      clear(x[i]);
}

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

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

mat_GF2 operator-(const mat_GF2& a, const mat_GF2& b)
{
   mat_GF2 res;
   add(res, a, b);
   NTL_OPT_RETURN(mat_GF2, res);
}

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

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

void inv(mat_GF2& X, const mat_GF2& A)
{
   GF2 d;
   inv(d, X, A);
   if (d == 0) ArithmeticError("inv: non-invertible matrix");
}

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

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

   mat_GF2 T1, T2;
   long i, k;

   k = NumBits(e);
   T1 = A;

   for (i = k-2; i >= 0; i--) {
      sqr(T2, T1);
      if (bit(e, i))
         mul(T1, T2, A);
      else
         T1 = T2;
   }

   if (e < 0)
      inv(X, T1);
   else
      X = T1;
}

void random(mat_GF2& x, long n, long m)
{
   x.SetDims(n, m);
   for (long i = 0; i < n; i++) random(x[i], m);
}

NTL_END_IMPL


#ifndef __PICSAR_MULTIPHYSICS_UTILITIES__
#define __PICSAR_MULTIPHYSICS_UTILITIES__

//This .hpp file contains general purpose functions to perform useful
//operations

#include <vector>
#include <cmath>
#include <functional>
#include <algorithm>
#include <utility>

#ifndef PXRMP_CORE_ONLY
    #include <limits>
    //Uses the root finding algorithms provided by boost
    #include <boost/math/tools/roots.hpp>
#endif //PXRMP_CORE_ONLY

//Should be included by all the src files of the library
#include "qed_commons.h"

//############################################### Declaration

namespace picsar{
    namespace multi_physics{

        //Generates a linearly spaced vector
        template<typename _REAL>
        std::vector<_REAL> generate_lin_spaced_vec
        (_REAL min, _REAL max, size_t size);

        //Generates a logarithmically spaced vector
        template<typename _REAL>
        std::vector<_REAL> generate_log_spaced_vec
        (_REAL min, _REAL max, size_t size);

        //Generates log_lin_log spaced vector
        template<typename _REAL>
        std::vector<_REAL> generate_log_lin_log_spaced_vec
        (_REAL min, _REAL max, size_t size);

        //GPU-friendly replacement of "std::upper_bound"
        template<typename T>
        PXRMP_GPU
        PXRMP_FORCE_INLINE
        const T* picsar_upper_bound(const T* first, const T* last, const T& val);

        //A function providing values extracted from a poisson distribution,
        //given lambda and a number in the interval [0,1)
        template<typename T>
        PXRMP_GPU
        PXRMP_FORCE_INLINE
        size_t poisson_distrib(T lambda, T unf_zero_one_minus_epsi);

#ifndef PXRMP_CORE_ONLY
        //A wrapper around the function provided by Boost library
        template<typename _REAL>
        _REAL bracket_and_solve_root
        (const std::function<_REAL(_REAL)>& f, _REAL guess, bool rising);
#endif
    }
}

//############################################### Implementation

template<typename _REAL>
std::vector<_REAL>
picsar::multi_physics::generate_log_spaced_vec
(_REAL min, _REAL max, size_t size)
{
    //Return empty vector upon error
    if( min >= max ||
        min < static_cast<_REAL>(0) || max< static_cast<_REAL>(0) ||
        size < 2)
        return std::vector<_REAL>(0);

    std::vector<_REAL> vec(size);

    _REAL val = min;
    _REAL mul = pow(max/min, static_cast<_REAL>(1.0/(size-1)));

    for(size_t i = 0; i < size-1; ++i){
        vec[i] = val;
        val*=mul;
    }
    vec.back() = max; //Enforces this exactly
    return vec;
}

//Generates a linearly spaced vector
template<typename _REAL>
std::vector<_REAL>
picsar::multi_physics::generate_lin_spaced_vec
(_REAL min, _REAL max, size_t size)
{
    //Return empty vector upon error
    if( min >= max || size < 2)
        return std::vector<_REAL>(0);

    std::vector<_REAL> vec(size);

    for(size_t i = 0; i < size-1; ++i){
        vec[i] = static_cast<_REAL>(i*(max-min)/(size-1.0)) + min;
    }
    vec.back() = max; //Enforces this exactly
    return vec;
}

//Generates log_lin_log spaced vector
template<typename _REAL>
std::vector<_REAL>
picsar::multi_physics::generate_log_lin_log_spaced_vec
(_REAL min, _REAL max, size_t size)
{
    std::vector<_REAL> vec(size);
    size_t size_first = size/3;
    size_t size_second = size/3;
    size_t size_third = size/3;

    _REAL first_val = max*static_cast<_REAL>(1.0/10.0);
    _REAL second_val = max*static_cast<_REAL>(9.0/10.0);

    size_t n = 0;
    std::generate(vec.begin(), vec.begin()+size_first,
    [=] () mutable { return min*exp((n++)*log(first_val/min)/(size_first)); });

    std::generate(vec.begin()+size_first, vec.begin()+size_first+size_second,
    [=] () mutable { return first_val + (second_val-first_val)*(n++)/(size_second); });

    std::generate(vec.begin()+size_first+size_second, vec.end(),
    [=] () mutable { return max*exp((size_third-1-(n++))*log(second_val/max)/(size_third-1)); });

    vec.front() = min;
    vec.back() = max;

    return vec;
}

template<typename T>
PXRMP_GPU
PXRMP_FORCE_INLINE
const T*
picsar::multi_physics::picsar_upper_bound
(const T* first, const T* last, const T& val)
{
    const T* it;
    size_t count, step;
    count = last-first;
    while(count>0){
        it = first;
        step = count/2;
        it += step;
         if (!(val<*it)){
             first = ++it;
             count -= step + 1;
         }
         else{
             count = step;
         }
    }
    return first;
}

//A function providing values extracted from a poisson distribution,
//given lambda and a number in the interval [0,1)
template<typename T>
PXRMP_GPU
PXRMP_FORCE_INLINE
size_t picsar::multi_physics::poisson_distrib
(T lambda, T unf_zero_one_minus_epsi)
{
    size_t k = 0;
    T p = exp(-lambda);
    T s = p;
    T old_s;
    while (unf_zero_one_minus_epsi > s){
        old_s = s;
        p = p*lambda/(++k);
        s += p;
        //If this is true we have reached the limit of the floating
        //point number that we are using
        if(s <= old_s)
            break;
    }
    return k;
}

#ifndef PXRMP_CORE_ONLY
    //A wrapper around the function provided by Boost library
    template<typename _REAL>
    _REAL picsar::multi_physics::bracket_and_solve_root
    (const std::function<_REAL(_REAL)>& f, _REAL guess, bool rising)
    {
        size_t digits = std::numeric_limits<_REAL>::digits;
        size_t precision_digits = digits - 2;
        boost::math::tools::eps_tolerance<_REAL> tol(precision_digits);

        _REAL factor = static_cast<_REAL>(2.0);

        size_t max_iter = 32;

        std::pair<_REAL, _REAL> r =
            boost::math::tools::bracket_and_solve_root
            (f, guess, factor, rising, tol, max_iter);

        return r.first + (r.second - r.first)/static_cast<_REAL>(2.0);
    }
#endif //PXRMP_CORE_ONLY

#endif // __PICSAR_MULTIPHYSICS_UTILITIES__


#pragma once

#include "tools.hpp"
#include <Eigen/Dense>

namespace kde1d {

namespace interp {

//! A class for cubic spline interpolation in one dimension
//!
//! The class is used for implementing kernel estimators. It makes storing the
//! observations obsolete and allows for fast numerical integration.
class InterpolationGrid
{
  public:
    InterpolationGrid() {}

    InterpolationGrid(const Eigen::VectorXd& grid_points,
                        const Eigen::VectorXd& values,
                        int norm_times);

    void normalize(int times);

    Eigen::VectorXd interpolate(const Eigen::VectorXd& x) const;

    Eigen::VectorXd integrate(const Eigen::VectorXd& u,
                              bool normalize = false) const;

    Eigen::VectorXd get_values() const { return values_; }
    Eigen::VectorXd get_grid_points() const { return grid_points_; }
    double get_grid_max() const
    {
        return grid_points_[grid_points_.size() - 1];
    }
    double get_grid_min() const { return grid_points_[0]; }

  private:
    // Utility functions for spline Interpolation
    double cubic_poly(const double& x, const Eigen::VectorXd& a) const;
    double cubic_indef_integral(const double& x,
                                const Eigen::VectorXd& a) const;
    double cubic_integral(const double& lower,
                          const double& upper,
                          const Eigen::VectorXd& a) const;
    int find_cell(const double& x0) const;
    Eigen::VectorXd find_cell_coefs(const int& k) const;

    Eigen::VectorXd grid_points_;
    Eigen::VectorXd values_;
};

//! Constructor
//!
//! @param grid_points an ascending sequence of grid points.
//! @param values a vector of values of same length as grid_points.
//! @param norm_times how many times the normalization routine should run.
inline InterpolationGrid::InterpolationGrid(
  const Eigen::VectorXd& grid_points,
  const Eigen::VectorXd& values,
  int norm_times)
{
    if (grid_points.size() != values.size())
        throw std::runtime_error(
          "grid_points and values must be of equal length");

    grid_points_ = grid_points;
    values_ = values;
    this->normalize(norm_times);
}

//! renormalizes the estimate to integrate to one
//!
//! @param times how many times the normalization routine should run.
inline void
InterpolationGrid::normalize(int times)
{
    double x_max = grid_points_(grid_points_.size() - 1);
    double int_max;
    for (int k = 0; k < times; ++k) {
        int_max = this->integrate(Eigen::VectorXd::Constant(1, x_max))(0);
        values_ /= int_max;
    }
}

//! Interpolation
//! @param x vector of evaluation points.
inline Eigen::VectorXd
InterpolationGrid::interpolate(const Eigen::VectorXd& x) const
{
    Eigen::VectorXd tmp_coefs(4);
    auto interpolate_one = [&](const double& xx) {
        int k = find_cell(xx);
        double xev =
          (xx - grid_points_(k)) / (grid_points_(k + 1) - grid_points_(k));

        // use Gaussian tail for extrapolation
        if (xev <= 0) {
            return values_(k) * std::exp(-0.5 * xev * xev);
        } else if (xev >= 1) {
            return values_(k + 1) * std::exp(-0.5 * xev * xev);
        }

        return cubic_poly(xev, find_cell_coefs(k));
    };

    return tools::unaryExpr_or_nan(x, interpolate_one);
}

//! Integration along the grid
//!
//! @param x a vector  of evaluation points
//! @param normalize whether to normalize the integral to a maximum value of 1.
inline Eigen::VectorXd
InterpolationGrid::integrate(const Eigen::VectorXd& x, bool normalize) const
{
    Eigen::VectorXd res(x.size());
    auto ord = tools::get_order(x);

    // temporaries for the loop
    Eigen::VectorXd tmp_coefs(4);
    double new_int, tmp_eps, cum_int = 0.0;
    int k = 0, m = grid_points_.size();
    tmp_coefs = find_cell_coefs(0);
    tmp_eps = (grid_points_(1) - grid_points_(0));

    for (long i = 0; i < x.size(); ++i) {
        double upr = x(ord(i));

        if (std::isnan(upr)) {
            res(ord(i)) = upr;
            continue;
        }
        if (upr <= grid_points_(0)) {
            res(ord(i)) = 0.0;
            continue;
        }

        // go up the grid and integrate
        while (k < m - 1) {
            // halt loop if integration limit is in kth cell
            if (upr < grid_points_(k + 1))
                break;
            // integrate over full cell
            tmp_coefs = find_cell_coefs(k);
            tmp_eps = (grid_points_(k + 1) - grid_points_(k));
            cum_int += cubic_integral(0.0, 1.0, tmp_coefs) * tmp_eps;
            k++;
        }

        // integrate over partial cell
        if (upr < grid_points_(m - 1)) { // only if still in interior
            tmp_coefs = find_cell_coefs(k);
            tmp_eps = (grid_points_(k + 1) - grid_points_(k));
            upr = (upr - grid_points_(k)) / tmp_eps;
            new_int = cubic_integral(0.0, upr, tmp_coefs) * tmp_eps;
        } else {
            new_int = 0.0;
        }

        res(ord(i)) = cum_int + new_int;
    }

    if (!normalize)
        return res;

    // integrate until end
    while (k < m - 1) {
        tmp_coefs = find_cell_coefs(k);
        tmp_eps = (grid_points_(k + 1) - grid_points_(k));
        cum_int += cubic_integral(0.0, 1.0, tmp_coefs) * tmp_eps;
        k++;
    }
    return res / cum_int;
}

// ---------------- Utility functions for spline interpolation ----------------

//! Evaluate a cubic polynomial
//!
//! @param x evaluation point.
//! @param a polynomial coefficients
inline double
InterpolationGrid::cubic_poly(const double& x, const Eigen::VectorXd& a) const
{
    double x2 = x * x;
    double x3 = x2 * x;
    return a(0) + a(1) * x + a(2) * x2 + a(3) * x3;
}

//! Indefinite integral of a cubic polynomial
//!
//! @param x evaluation point.
//! @param a polynomial coefficients.
inline double
InterpolationGrid::cubic_indef_integral(const double& x,
                                          const Eigen::VectorXd& a) const
{
    double x2 = x * x;
    double x3 = x2 * x;
    double x4 = x3 * x;
    return a(0) * x + a(1) / 2.0 * x2 + a(2) / 3.0 * x3 + a(3) / 4.0 * x4;
}

//! Definite integral of a cubic polynomial
//!
//! @param lower lower limit of the integral.
//! @param upper upper limit of the integral.
//! @param a polynomial coefficients.
inline double
InterpolationGrid::cubic_integral(const double& lower,
                                    const double& upper,
                                    const Eigen::VectorXd& a) const
{
    return cubic_indef_integral(upper, a) - cubic_indef_integral(lower, a);
}

inline int
InterpolationGrid::find_cell(const double& x0) const
{
    int low = 0, high = grid_points_.size() - 1;
    int mid;
    while (low < high - 1) {
        mid = low + (high - low) / 2;
        if (x0 < grid_points_(mid))
            high = mid;
        else
            low = mid;
    }

    return low;
}

//! Calculate coefficients for cubic intrpolation spline
//!
//! @param k the cell index.
inline Eigen::VectorXd
InterpolationGrid::find_cell_coefs(const int& k) const
{
    // indices for cell and neighboring grid points
    int k0 = std::max(k - 1, 0);
    int k2 = k + 1;
    int k3 = std::min(k + 2, static_cast<int>(grid_points_.size()) - 1);

    double dt0 = grid_points_(k) - grid_points_(k0);
    double dt1 = grid_points_(k2) - grid_points_(k);
    double dt2 = grid_points_(k3) - grid_points_(k2);

    // compute tangents when parameterized in (t1,t2)
    // for smooth extrapolation, derivative is set to zero at boundary
    double dx1 = 0.0, dx2 = 0.0;
    if (dt0 > 0) {
        dx1 = (values_(k) - values_(k0)) / dt0;
        dx1 -= (values_(k2) - values_(k0)) / (dt0 + dt1);
        dx1 += (values_(k2) - values_(k)) / dt1;
    }
    if (dt2 > 0) {
        dx2 = (values_(k2) - values_(k)) / dt1;
        dx2 -= (values_(k3) - values_(k)) / (dt1 + dt2);
        dx2 += (values_(k3) - values_(k2)) / dt2;
    }

    // rescale tangents for parametrization in (0,1)
    dx1 *= dt1;
    dx2 *= dt1;

    // ensure positivity (Schmidt and Hess, DOI:10.1007/bf01934097)
    dx1 = std::max(dx1, -3 * values_(k));
    dx2 = std::min(dx2, 3 * values_(k2));

    // compute coefficents
    Eigen::VectorXd a(4);
    a(0) = values_(k);
    a(1) = dx1;
    a(2) = -3 * (values_(k) - values_(k2)) - 2 * dx1 - dx2;
    a(3) = 2 * (values_(k) - values_(k2)) + dx1 + dx2;

    return a;
}

} // end kde1d::interp

} // end kde1d


#include <NTL/ZZ.h>
#include <NTL/BasicThreadPool.h>
#include "FHE.h"
#include "timing.h"
#include "EncryptedArray.h"
#include <NTL/lzz_pXFactoring.h>

#include <cassert>
#include <cstdio>
#include <iostream>
#include <vector>
#include <bitset>
#include <string>
using namespace std;
Ctxt operator + (Ctxt left, Ctxt right)
{
	Ctxt temp(left);
	temp+=right;
	return temp;
}
Ctxt operator * (Ctxt left, Ctxt right)
{
	Ctxt temp(left);
	temp*=right;
	return temp;
}
int biggerThan(int plainText1, int plainText2)
{
	FHEcontext * context;
	FHESecKey *secretKey;
	const FHEPubKey * publicKey;
	long p = 2;
	long r = 1;
	long L = 16;
	long c = 3;
	long w = 64;
	long d = 0;
	long k = 128;
	long s = 0;

	long m = FindM(k, L, c, p, d, s, 0);
	unsigned bits =2;

	context = new FHEcontext(m, p, r);
	buildModChain(*context, L, c);

	ZZX G = context->alMod.getFactorsOverZZ()[0];

	secretKey = new FHESecKey(*context);
	publicKey = secretKey;
	secretKey->GenSecKey(w);

	//plain text
	vector<long> plainTextBinaryVector1(4, 0);
	vector<long> plainTextBinaryVector2(4, 0);

	//ciphertext
	vector<Ctxt> ctxtVector1(4, Ctxt(*publicKey));
	vector<Ctxt> ctxtVector2(4, Ctxt(*publicKey));

	//转换成二进制串
	bitset<4> b1(plainText1);
	bitset<4> b2(plainText2);

	for(int i=0; i<b1.size(); i++)
	{
		plainTextBinaryVector1[i]=b1[i];
		plainTextBinaryVector2[i]=b2[i];
	}

	for(int i=0; i<b1.size(); i++)
	{
		publicKey->Encrypt(ctxtVector1[i], to_ZZX(plainTextBinaryVector1[i]));
		publicKey->Encrypt(ctxtVector2[i], to_ZZX(plainTextBinaryVector2[i]));
	}

	//boolean circuit

	//1s
	Ctxt ctxtOne(*publicKey);
	publicKey->Encrypt(ctxtOne, to_ZZX(1));

	ZZX temp;
	Ctxt comparisionResult(*publicKey);
	publicKey->Encrypt(comparisionResult, to_ZZX(0));

	comparisionResult = ctxtVector1[3]*(ctxtVector2[3]+ctxtOne);
	// comparisionResult = ctxtVector1[3]*=(ctxtVector2[3]+=ctxtOne);
	secretKey->Decrypt(temp, comparisionResult);
	//cout<<"1:  "<<temp<<endl;

	comparisionResult = comparisionResult+ (ctxtVector1[3]+ctxtVector2[3]+ctxtOne)
	*ctxtVector1[2]*(ctxtVector2[2]+ctxtOne);
	//这里用XOR可以代替OR，因为运算表达式决定了两个加数不可能同时为1
	secretKey->Decrypt(temp, comparisionResult);
	//cout<<"2:  "<<temp<<endl;

	comparisionResult = comparisionResult +
	(ctxtVector1[3]+ctxtVector2[3]+ctxtOne) *
	(ctxtVector1[2]+ctxtVector2[2]+ctxtOne) *
	ctxtVector1[1]*(ctxtVector2[1]+ctxtOne);
	secretKey->Decrypt(temp, comparisionResult);
	//cout<<"3:  "<<temp<<endl;

	comparisionResult = comparisionResult +
	(ctxtVector1[3]+ctxtVector2[3]+ctxtOne) *
	(ctxtVector1[2]+ctxtVector2[2]+ctxtOne) *
	(ctxtVector1[1]+ctxtVector2[1]+ctxtOne) *
	ctxtVector1[0]*(ctxtVector2[0]+ctxtOne);
	secretKey->Decrypt(temp, comparisionResult);
	//cout<<"4:  "<<temp<<endl;

	ZZX result;
	secretKey->Decrypt(result, comparisionResult);
	//cout<<"result : "<<result<<endl;
	long hh;
	conv(hh, result[0]);

	delete context;
	delete secretKey;

	return int(hh);
}
int main()
{
	for(int x=0; x<16; x++)
	{
		for(int y=0; y<16; y++)
		{
			cout<<"x  "<<x<<"y  "<<y<<endl;
			if(biggerThan(x, y) != (x>y))
			{
				cout<<"error!"<<endl;
				cout<<"biggerThan : "<<biggerThan(x,y)<<endl;
				cout<<"x>y: "<<(x>y)<<endl;
				return 0;
			}
		}
	}
	while(true)
	{
		int a;
		int b;
		cin>>a;
		cin>>b;
		cout<<biggerThan(a, b)<<endl;
	}
}


#include <iostream>
#include <fstream>
#include <Eigen>

using namespace std;
using namespace Eigen;

// function definitions for reading matrices from .txt file
void read_sparse_matrix(const std::string& filename, SparseMatrix<float, RowMajor>& matrix);
void read_matrix(std::string file, MatrixXf& matrix);

int main()
{
    // read matrices from benchmark test files
    SparseMatrix<float, RowMajor> P;
    read_sparse_matrix("../benchmark/P.txt", P);
    SparseMatrix<float, RowMajor> Gamma;
    read_sparse_matrix("../benchmark/SH.txt", Gamma);
    SparseMatrix<float, RowMajor> invCeta;
    read_sparse_matrix("../benchmark/invCeta.txt", invCeta);
    SparseMatrix<float, RowMajor> F;
    read_sparse_matrix("../benchmark/F.txt", F);
    MatrixXf invCphi(P.cols(),P.cols());
    read_matrix("../benchmark/invCphi.txt", invCphi);

    // read sensor measurements from benchmark test files
    VectorXf s(invCeta.cols());
    fstream input("../benchmark/s.txt");
    string line;
    int indx = 0;
    if (input.is_open()) {
        while (getline(input,line)) {
            s(indx)=stof(line);
            indx++;
        }
        input.close();
    }
    cout << "read input done!" << endl;

    // calculate reconstruction matrix R
    MatrixXf R(P.cols(), P.cols());
    cout << "start computation..." << endl;
    R = (Gamma*P).transpose()*invCeta*(Gamma*P)+invCphi;

    // reconstruct turbulent layers from sensor measurements
    cout << "reconstruct layers from sensor measurements..." << endl;
    VectorXf b = (Gamma*P).transpose()*invCeta*s;
    VectorXf phi = R.colPivHouseholderQr().solve(b);

    // apply mirror fitting
    cout << "compute actuator commands..." << endl;
    VectorXf a(F.cols());
    a = F*phi;
    cout << "computation done!" << endl;

    // save actuator commands to file
    ofstream output("../benchmark/out.txt");
    if (output.is_open()) {
        for(int i = 0; i < a.size(); i++) {
            output << a(i) << '\n';
        }
    }
    cout << "stored output file out.txt!" << endl;

    return 0;
}

void read_sparse_matrix(const std::string& filename, SparseMatrix<float, RowMajor>& matrix) {
    ifstream fin(filename);
    if(fin.is_open()) {
        int M = 0, N = 0, L = 0;
        fin >> M >> N >> L;
        vector<Eigen::Triplet<float>> triple;
        triple.reserve(L);
        int m, n;
        float data;
        while (fin >> n >> m >> data) {
            triple.push_back(Triplet<float>(m-1, n-1, data));// m - 1 and n - 1 to set index start from 0
        }
        fin.close();

        matrix.resize(M, N);
        matrix.reserve(L);
        matrix.setFromTriplets(triple.begin(), triple.end());
    }
    else {
        std::cout << "Can not open sparse matrix file: " << filename << std::endl;
    }
}

void read_matrix(std::string file, MatrixXf& matrix) {
    std::ifstream in(file);
    std::string line;
    int row=0, col=0;

    if (in.is_open()) {
        while (std::getline(in, line)) {
            char* ptr = (char*) line.c_str();
            int len = line.length();

            col = 0;
            char* start = ptr;
            for (int i = 0; i<len; i++) {
                if (ptr[i]=='\t') {
                    matrix(row, col++) = stof(start);
                    start = ptr+i+1;
                }
            }
            row++;
        }
        in.close();
    }
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_HYPERGEOMETRIC_LPMF_HPP
#define STAN_MATH_PRIM_SCAL_PROB_HYPERGEOMETRIC_LPMF_HPP

#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_bounded.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_greater.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <stan/math/prim/scal/fun/size_zero.hpp>
#include <stan/math/prim/scal/fun/binomial_coefficient_log.hpp>
#include <stan/math/prim/scal/meta/length.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>
#include <stan/math/prim/scal/meta/return_type.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <boost/math/distributions.hpp>

namespace stan {
namespace math {

// Hypergeometric(n|N, a, b)  [0 <= n <= a;  0 <= N-n <= b;  0 <= N <= a+b]
// n: #white balls drawn;  N: #balls drawn;
// a: #white balls;  b: #black balls
template <bool propto, typename T_n, typename T_N, typename T_a, typename T_b>
double hypergeometric_lpmf(const T_n& n, const T_N& N, const T_a& a,
                           const T_b& b) {
  static const char* function = "hypergeometric_lpmf";

  if (size_zero(n, N, a, b))
    return 0.0;

  scalar_seq_view<T_n> n_vec(n);
  scalar_seq_view<T_N> N_vec(N);
  scalar_seq_view<T_a> a_vec(a);
  scalar_seq_view<T_b> b_vec(b);
  size_t size = max_size(n, N, a, b);

  double logp(0.0);
  check_bounded(function, "Successes variable", n, 0, a);
  check_greater(function, "Draws parameter", N, n);
  for (size_t i = 0; i < size; i++) {
    check_bounded(function, "Draws parameter minus successes variable",
                  N_vec[i] - n_vec[i], 0, b_vec[i]);
    check_bounded(function, "Draws parameter", N_vec[i], 0,
                  a_vec[i] + b_vec[i]);
  }
  check_consistent_sizes(function, "Successes variable", n, "Draws parameter",
                         N, "Successes in population parameter", a,
                         "Failures in population parameter", b);

  if (!include_summand<propto>::value)
    return 0.0;

  for (size_t i = 0; i < size; i++)
    logp += math::binomial_coefficient_log(a_vec[i], n_vec[i])
            + math::binomial_coefficient_log(b_vec[i], N_vec[i] - n_vec[i])
            - math::binomial_coefficient_log(a_vec[i] + b_vec[i], N_vec[i]);
  return logp;
}

template <typename T_n, typename T_N, typename T_a, typename T_b>
inline double hypergeometric_lpmf(const T_n& n, const T_N& N, const T_a& a,
                                  const T_b& b) {
  return hypergeometric_lpmf<false>(n, N, a, b);
}

}  // namespace math
}  // namespace stan
#endif


#include <Eigen/Array>

int main(int argc, char *argv[])
{
  std::cout.precision(2);

  // demo static functions
  Eigen::Matrix3f m3 = Eigen::Matrix3f::Random();
  Eigen::Matrix4f m4 = Eigen::Matrix4f::Identity();

  std::cout << "*** Step 1 ***\nm3:\n" << m3 << "\nm4:\n" << m4 << std::endl;

  // demo non-static set... functions
  m4.setZero();
  m3.diagonal().setOnes();

  std::cout << "*** Step 2 ***\nm3:\n" << m3 << "\nm4:\n" << m4 << std::endl;

  // demo fixed-size block() expression as lvalue and as rvalue
  m4.block<3,3>(0,1) = m3;
  m3.row(2) = m4.block<1,3>(2,0);

  std::cout << "*** Step 3 ***\nm3:\n" << m3 << "\nm4:\n" << m4 << std::endl;

  // demo dynamic-size block()
  {
    int rows = 3, cols = 3;
    m4.block(0,1,3,3).setIdentity();
    std::cout << "*** Step 4 ***\nm4:\n" << m4 << std::endl;
  }

  // demo vector blocks
  m4.diagonal().block(1,2).setOnes();
  std::cout << "*** Step 5 ***\nm4.diagonal():\n" << m4.diagonal() << std::endl;
  std::cout << "m4.diagonal().start(3)\n" << m4.diagonal().start(3) << std::endl;

  // demo coeff-wise operations
  m4 = m4.cwise()*m4;
  m3 = m3.cwise().cos();
  std::cout << "*** Step 6 ***\nm3:\n" << m3 << "\nm4:\n" << m4 << std::endl;

  // sums of coefficients
  std::cout << "*** Step 7 ***\n m4.sum(): " << m4.sum() << std::endl;
  std::cout << "m4.col(2).sum(): " << m4.col(2).sum() << std::endl;
  std::cout << "m4.colwise().sum():\n" << m4.colwise().sum() << std::endl;
  std::cout << "m4.rowwise().sum():\n" << m4.rowwise().sum() << std::endl;

  // demo intelligent auto-evaluation
  m4 = m4 * m4; // auto-evaluates so no aliasing problem (performance penalty is low)
  Eigen::Matrix4f other = (m4 * m4).lazy(); // forces lazy evaluation
  m4 = m4 + m4; // here Eigen goes for lazy evaluation, as with most expressions
  m4 = -m4 + m4 + 5 * m4; // same here, Eigen chooses lazy evaluation for all that.
  m4 = m4 * (m4 + m4); // here Eigen chooses to first evaluate m4 + m4 into a temporary.
                       // indeed, here it is an optimization to cache this intermediate result.
  m3 = m3 * m4.block<3,3>(1,1); // here Eigen chooses NOT to evaluate block() into a temporary
    // because accessing coefficients of that block expression is not more costly than accessing
    // coefficients of a plain matrix.
  m4 = m4 * m4.transpose(); // same here, lazy evaluation of the transpose.
  m4 = m4 * m4.transpose().eval(); // forces immediate evaluation of the transpose

  std::cout << "*** Step 8 ***\nm3:\n" << m3 << "\nm4:\n" << m4 << std::endl;
}


/*
 * Ewald.hpp
 *
 *  Created on: Mar 2, 2017
 *      Author: wyan
 */

/*
 * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 * !!!   For unit cubic box only !!!
 * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 * */

#ifndef STOKES3D3D_EWALD_HPP_
#define STOKES3D3D_EWALD_HPP_

#include <cmath>

#include <Eigen/Dense>

constexpr int DIRECTLAYER = 2;
constexpr double PI314 = 3.1415926535897932384626433;

inline double ERFC(double x) { return std::erfc(x); }

inline double ERF(double x) { return std::erf(x); }

inline double boxperiodic(double x, double xlow, double xhigh) {
    double temp = (x - xlow) / (xhigh - xlow);
    return (x - xlow) - floor(temp) * (xhigh - xlow);
}

inline void Gkernel(const Eigen::Vector3d &target,
                    const Eigen::Vector3d &source, Eigen::Matrix3d &answer) {
    auto rst = target - source;
    double rnorm = rst.norm();
    double rnormReg = rnorm;
    if (rnorm < 1e-13) {
        answer = Eigen::Matrix3d::Zero();
        return;
    }
    auto part2 = rst * rst.transpose() / (rnormReg * rnormReg * rnormReg);
    auto part1 = Eigen::Matrix3d::Identity() / rnormReg;
    answer = part1 + part2;
}

// periodic in X direction for a unit cubic box [0,1)^3
inline void Gkernel1D(const Eigen::Vector3d &target, Eigen::Matrix3d &G) {
    const int Nsum = 10000;
    G.setZero();
    Eigen::Matrix3d Gtemp1;
    Eigen::Matrix3d Gtemp2;
    Gkernel(target, Eigen::Vector3d(0, 0, 0), G);
    for (int i = 1; i < Nsum + 1; i++) {
        Gkernel(target, Eigen::Vector3d(i, 0, 0), Gtemp1);
        Gkernel(target, Eigen::Vector3d(-i, 0, 0), Gtemp2);
        G += (Gtemp1 + Gtemp2);
    }
}

/*
 * def AEW(xi,rvec):
 r=np.sqrt(rvec.dot(rvec))
 A = 2*(xi*np.exp(-(xi**2)*(r**2))/(np.sqrt(np.pi)*r**2)+ss.erfc(xi*r)/(2*r**3))
 \
    *(r*r*np.identity(3)+np.outer(rvec,rvec)) -
 4*xi/np.sqrt(np.pi)*np.exp(-(xi**2)*(r**2))*np.identity(3) return A
 *
 * */
inline Eigen::Matrix3d AEW(const double xi, const Eigen::Vector3d &rvec) {
    const double r = rvec.norm();
    Eigen::Matrix3d A =
        2 *
            (xi * exp(-(xi * xi) * (r * r)) / (sqrt(PI314) * r * r) +
             erfc(xi * r) / (2 * r * r * r)) *
            (r * r * Eigen::Matrix3d::Identity() + (rvec * rvec.transpose())) -
        4 * xi / sqrt(PI314) * exp(-(xi * xi) * (r * r)) *
            Eigen::Matrix3d::Identity();
    return A;
}

/*
 *
 def BEW(xi,kvec):
 k=np.sqrt(kvec.dot(kvec))
 B =
 8*np.pi*(1+k*k/(4*(xi**2)))*((k**2)*np.identity(3)-np.outer(kvec,kvec))/(k**4)
 return B*np.exp(-k**2/(4*xi**2))
 *
 * */
inline Eigen::Matrix3d BEW(const double xi, const Eigen::Vector3d &kvec) {
    const double k = kvec.norm();
    Eigen::Matrix3d B =
        8 * PI314 * (1 + k * k / (4 * (xi * xi))) *
        ((k * k) * Eigen::Matrix3d::Identity() - (kvec * kvec.transpose())) /
        (k * k * k * k);
    B *= exp(-k * k / (4 * xi * xi));
    return B;
}

/*
 * def stokes3DEwald(rvec,force):
 xi = 2
 r=np.sqrt(rvec.dot(rvec))
 real = 0
 N=4
 for i in range(-N,N+1):
 for j in range(-N,N+1):
 for k in range(-N,N+1):
 real = real + AEW(xi,rvec+1.0*np.array([i,j,k])).dot(force)
 wave = 0
 N=4
 for i in range(-N,N+1):
 for j in range(-N,N+1):
 for k in range(-N,N+1):
 kvec=2*np.pi*np.array([i,j,k]) # L = 1
 if(i==0 and j==0 and k==0):
 continue
 else:
 wave = wave + BEW(xi,kvec).dot(force)*np.exp(-complex(0,1)*kvec.dot(rvec))

 return (np.real(wave)+real)

 * */
// periodic in XYZ direction for a unit cubic box [0,1)^3
inline void GkernelEwald3D(const Eigen::Vector3d &rvecIn, Eigen::Matrix3d &Gsum,
                           double box) {
    const double xi = 2;
    Eigen::Vector3d rvec = rvecIn;
    rvec[0] = rvec[0] - floor(rvec[0]);
    rvec[1] = rvec[1] - floor(rvec[1]);
    rvec[2] = rvec[2] - floor(rvec[2]);
    const double r = rvec.norm();
    Eigen::Matrix3d real = Eigen::Matrix3d::Zero();
    const int N = 10;
    if (r < 1e-11) {
        auto Gself = -4 * xi / sqrt(PI314) *
                     Eigen::Matrix3d::Identity(); // the self term
        for (int i = -N; i < N + 1; i++) {
            for (int j = -N; j < N + 1; j++) {
                for (int k = -N; k < N + 1; k++) {
                    if (i == 0 && j == 0 && k == 0) {
                        continue;
                    }
                    real =
                        real + AEW(xi, rvec + Eigen::Vector3d(i * box, j * box,
                                                              k * box));
                }
            }
        }
        real += Gself;
    } else {
        for (int i = -N; i < N + 1; i++) {
            for (int j = -N; j < N + 1; j++) {
                for (int k = -N; k < N + 1; k++) {
                    real =
                        real + AEW(xi, rvec + Eigen::Vector3d(i * box, j * box,
                                                              k * box));
                }
            }
        }
    }
    Eigen::Matrix3d wave = Eigen::Matrix3d::Zero();

    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            for (int k = -N; k < N + 1; k++) {
                Eigen::Vector3d kvec(2 * PI314 * i / box, 2 * PI314 * j / box,
                                     2 * PI314 * k / box);
                if (i == 0 and j == 0 and k == 0) {
                    continue;
                } else {
                    wave = wave + BEW(xi, kvec) * cos(kvec.dot(rvec));
                }
            }
        }
    }
    Gsum = real + wave * (1 / (box * box * box));
}

/*
 *
 def stokes3DM2L(rvec,force):
 uEwald=stokes3DEwald(rvec,force)
 uNB=0
 N=3
 for i in range(-N,N+1):
 for j in range(-N,N+1):
 for k in range(-N,N+1):
 uNB=uNB+Gkernel(rvec-np.array([i,j,k])).dot(force)
 return uEwald-uNB
 * */
// Out of Layer 1
inline void GkernelEwald3DFF(const Eigen::Vector3d &rvec,
                             Eigen::Matrix3d &GsumO1) {
    Eigen::Matrix3d Gfree = Eigen::Matrix3d::Zero();
    GkernelEwald3D(rvec, GsumO1, 1.0);
    const int N = DIRECTLAYER;
    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            for (int k = -N; k < N + 1; k++) {
                Gkernel(rvec, Eigen::Vector3d(i, j, k), Gfree);
                GsumO1 -= Gfree;
            }
        }
    }
}

inline double lbda(double k, double xi, double z) {
    return exp(-k * k / (4 * xi * xi) - (xi * xi) * (z * z));
}

inline double thetaplus(double k, double xi, double z) {
    return exp(k * z) * ERFC(k / (2 * xi) + xi * z);
}

inline double thetaminus(double k, double xi, double z) {
    return exp(-k * z) * ERFC(k / (2 * xi) - xi * z);
}

inline double J00(double k, double xi, double z) {
    return sqrt(PI314) * lbda(k, xi, z) * xi;
}

inline double J10(double k, double xi, double z) {
    return PI314 * (thetaplus(k, xi, z) + thetaminus(k, xi, z)) / (4 * k);
}

inline double J20(double k, double xi, double z) {
    return sqrt(PI314) * lbda(k, xi, z) / (4 * k * k * xi) +
           PI314 *
               ((thetaplus(k, xi, z) + thetaminus(k, xi, z)) / (8 * k * k * k) +
                (thetaminus(k, xi, z) - thetaplus(k, xi, z)) * z / (8 * k * k) -
                (thetaplus(k, xi, z) + thetaminus(k, xi, z)) /
                    (16 * k * (xi * xi)));
}

inline double J12(double k, double xi, double z) {
    return PI314 * (-thetaplus(k, xi, z) - thetaminus(k, xi, z)) * k / 4 +
           sqrt(PI314) * lbda(k, xi, z) * xi;
}

inline double J22(double k, double xi, double z) {
    return PI314 * ((thetaplus(k, xi, z) + thetaminus(k, xi, z)) * k /
                        (16 * xi * xi) +
                    (thetaplus(k, xi, z) + thetaminus(k, xi, z)) / (8 * k) +
                    (thetaplus(k, xi, z) - thetaminus(k, xi, z)) * z / 8) -
           sqrt(PI314) * lbda(k, xi, z) / (4 * xi);
}

inline double K11(double k, double xi, double z) {
    return PI314 * ((thetaminus(k, xi, z) - thetaplus(k, xi, z))) / 4;
}

inline double K12(double k, double xi, double z) {
    return PI314 *
           ((thetaplus(k, xi, z) - thetaminus(k, xi, z)) / (16 * xi * xi) +
            (thetaminus(k, xi, z) + thetaplus(k, xi, z)) * z / (8 * k));
}

inline void QI(const Eigen::Vector3d &kvec, double xi, double z,
               Eigen::Matrix3d &QI) {
    // 3*3 tensor
    // kvec: np.array([k1,k2,0])
    double knorm = sqrt(kvec[0] * kvec[0] + kvec[1] * kvec[1]);
    QI = 2 * (J00(knorm, xi, z) / (4 * xi * xi) + J10(knorm, xi, z)) *
         Eigen::Matrix3d::Identity();
}

inline void Qkk(const Eigen::Vector3d &kvec, double xi, double z,
                Eigen::Matrix3d &Qreal, Eigen::Matrix3d &Qimg) {
    double k1 = kvec[0];
    double k2 = kvec[1];
    double knorm = sqrt(k1 * k1 + k2 * k2);
    auto j10 = J10(knorm, xi, z);
    auto j20 = J20(knorm, xi, z);
    auto j12 = J12(knorm, xi, z);
    auto j22 = J22(knorm, xi, z);

    auto k11 = K11(knorm, xi, z);
    auto k12 = K12(knorm, xi, z);
    Qreal.setZero();
    Qreal(0, 0) = k1 * k1;
    Qreal(1, 1) = k2 * k2;
    Qreal(0, 1) = k1 * k2;
    Qreal(1, 0) = k1 * k2;

    Qreal *= (j10 / (4 * (xi * xi)) + j20);
    Qreal(2, 2) = (j12 / (4 * xi * xi) + j22);
    Qreal *= -2;

    Qimg.setZero();
    Qimg(0, 2) = k1;
    Qimg(1, 2) = k2;
    Qimg(2, 0) = k1;
    Qimg(2, 1) = k2;
    // Qimg=np.array([[0,0,k1],[0,0,k2],[k1,k2,0]])*( k11/(4*xi**2) + k12 )
    Qimg *= (k11 / (4 * xi * xi) + k12);
    Qimg *= -2;
}

// inline Eigen::Matrix3d uFk0(double xi, double zmn) {
//	Eigen::Matrix3d wavek0;
//	wavek0 = -(4.0 / 1) * (PI314 * (zmn) * ERF(zmn * xi) + sqrt(PI314) / (2
//* xi) * exp(-zmn * zmn * xi * xi)); 	return wavek0;
//
//}
// periodic in XY direction for a unit cubic box [0,1)^3
inline void GkernelEwald2D(const Eigen::Vector3d &rvecIn,
                           Eigen::Matrix3d &Gsum) {
    const double xi = 2;
    Eigen::Vector3d rvec = rvecIn;
    rvec[0] = rvec[0] - floor(rvec[0]);
    rvec[1] = rvec[1] - floor(rvec[1]); // reset to a periodic cell

    const double r = rvec.norm();
    Eigen::Matrix3d real = Eigen::Matrix3d::Zero();
    const int N = 5;
    if (r < 1e-14) {
        auto Gself = -4 * xi / sqrt(PI314) *
                     Eigen::Matrix3d::Identity(); // the self term
        for (int i = -N; i < N + 1; i++) {
            for (int j = -N; j < N + 1; j++) {
                if (i == 0 && j == 0) {
                    continue;
                }
                real = real + AEW(xi, rvec + Eigen::Vector3d(i, j, 0));
            }
        }
        real += Gself;
    } else {
        for (int i = -N; i < N + 1; i++) {
            for (int j = -N; j < N + 1; j++) {
                real = real + AEW(xi, rvec + Eigen::Vector3d(i, j, 0));
            }
        }
    }

    // k
    Eigen::Matrix3d wave = Eigen::Matrix3d::Zero();

    double zmn = rvec[2];
    Eigen::Vector3d rhomn = rvec;
    rhomn[2] = 0;
    Eigen::Matrix3d Qreal;
    Eigen::Matrix3d Qimg;
    Eigen::Matrix3d QImat;
    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            Eigen::Vector3d kvec(2 * PI314 * i, 2 * PI314 * j, 0);
            if (i == 0 and j == 0) {
                continue;
            }
            Qkk(kvec, xi, zmn, Qreal, Qimg);
            QI(kvec, xi, zmn, QImat);
            wave = wave + (QImat + Qreal) * cos(kvec.dot(rhomn)) -
                   (Qimg)*sin(kvec.dot(rhomn));
        }
    }
    wave *= 4;

    // k=0
    Eigen::Matrix3d waveK0;
    waveK0.setZero();
    /*
     *   I2fn=force
     I2fn[2]=0
     wavek0=-(4/1)*(np.pi*(zmn)*ss.erf(zmn*xi)+np.sqrt(np.pi)/(2*xi)*np.exp(-zmn**2*xi**2))*I2fn
     *
     * */
    waveK0 = -(4 / 1.0) *
             (PI314 * (zmn)*ERF(zmn * xi) +
              sqrt(PI314) / (2 * xi) * exp(-zmn * zmn * xi * xi)) *
             Eigen::Matrix3d::Identity();
    waveK0(2, 2) = 0;

    Gsum = real + wave + waveK0;
}

#endif /* STOKES3D3D_EWALD_HPP_ */


#include "mantella_bits/samplesAnalysis.hpp"

// C++ standard library
#include <algorithm>
#include <cassert>
#include <cmath>
#include <memory>
#include <stdexcept>
#include <utility>

// Armadillo
#include <armadillo>

// Mantella
#include "mantella_bits/assert.hpp"
#include "mantella_bits/combinatorics.hpp"
#include "mantella_bits/config.hpp"
#include "mantella_bits/optimisationProblem.hpp"
#include "mantella_bits/probability.hpp"

namespace mant {
  double fitnessDistanceCorrelation(
      const std::unordered_map<arma::vec, double, Hash, IsEqual>& samples) {
    assert(samples.size() > 1 && "fitnessDistanceCorrelation: The number of samples must be greater than 1.");
    assert(isDimensionallyConsistent(samples) && "fitnessDistanceCorrelation: The samples must be dimensionally consistent");

    // Converts the set of samples into a matrix of parameters (each row is a dimension and each column a parameter) and a row vector of objective values, such that we can use Armadillo C++ to manipulate them.
    arma::mat parameters(samples.cbegin()->first.n_elem, samples.size());
    arma::rowvec objectiveValues(parameters.n_cols);

    arma::uword n = 0;
    for (const auto& sample : samples) {
      parameters.col(n) = sample.first;
      objectiveValues(n) = sample.second;

      ++n;
    }

    // Determines one parameter having a minimal objective value within the set of samples.
    // The best parameter is then subtracted from all other parameters, so that we can later calculated the distance towards the best one.
    arma::uword bestParameterIndex = objectiveValues.index_min();
    parameters.each_col() -= parameters.col(bestParameterIndex);

    // Excludes the best/reference parameter from the correlation, as it will always perfectly correlate with itself and therefore bias the correlation coefficient.
    parameters.shed_col(bestParameterIndex);
    objectiveValues.shed_col(bestParameterIndex);

    // Uses `arma::sum(arma::square(...))` to emulate a column-wise vector norm operator that Armadillo C++ does not have (and this is not likely to change, as it interferes with matrix norms).
    return arma::as_scalar(arma::cor(arma::sqrt(arma::sum(arma::square(parameters))), objectiveValues));
  }

  double lipschitzContinuity(
      const std::unordered_map<arma::vec, double, Hash, IsEqual>& samples) {
    assert(samples.size() > 1 && "lipschitzContinuity: The number of samples must be greater than 1.");
    assert(isDimensionallyConsistent(samples) && "lipschitzContinuity: The samples must be dimensionally consistent");

    double lipschitzContinuity = 0.0;
    for (auto firstSample = samples.cbegin(); firstSample != samples.cend();) {
      for (auto secondSample = ++firstSample; secondSample != samples.cend(); ++secondSample) {
        lipschitzContinuity = std::max(lipschitzContinuity, std::abs(firstSample->second - secondSample->second) / arma::norm(firstSample->first - secondSample->first));
      }
    }

    return lipschitzContinuity;
  }

  std::vector<arma::uvec> additiveSeparability(
      OptimisationProblem& optimisationProblem,
      const arma::uword numberOfEvaluations,
      const double minimalConfidence) {
    assert(numberOfEvaluations > 0 && "additiveSeparability: The number of evaluations must be greater than 0.");
    assert(0.0 <= minimalConfidence && minimalConfidence <= 1.0 && "additiveSeparability: The minimal confidence must be within the interval (0, 1].");

    if (!isRepresentableAsFloatingPoint(numberOfEvaluations)) {
      throw std::range_error("additiveSeparability: The number of elements must be representable as a floating point.");
    }

    if (minimalConfidence <= 0) {
      std::vector<arma::uvec> partition;
      partition.reserve(optimisationProblem.numberOfDimensions_);

      for (arma::uword n = 0; n < optimisationProblem.numberOfDimensions_; ++n) {
        partition.push_back({n});
      }

      return partition;
    }

    /* The first of two steps to analyse the additive separability of a function is to estimate all two-set separations that fulfil the deviation and confidence requirements.
     * A function *f* is additive separable into two other function *g*, *h* if the following holds:
     *
     * f(x, y) - g(x) - h(y) = 0, for all x, y
     *
     * As it is practical impossible to simply guess two separations *g*, *h* of *f*, when we only got a caller to `.getObjectiveValue(...)` and no analytic form, we use a direct consequence from the equation above instead, that must also hold true for additive separable functions and uses only *f*:
     *
     * f(a, c) + f(b, d) - f(a, d) - f(b, c) = 0, for all a, b, c, d
     *
     */

    std::vector<std::pair<arma::uvec, arma::uvec>> partitionCandidates = twoSetsPartitions(optimisationProblem.numberOfDimensions_);

    arma::rowvec confidences(partitionCandidates.size(), arma::fill::zeros);
    for (arma::uword n = 0; n < partitionCandidates.size(); ++n) {
      const std::pair<arma::uvec, arma::uvec>& partitionCandidate = partitionCandidates.at(n);

      for (arma::uword k = 0; k < numberOfEvaluations; ++k) {
        const arma::vec& firstFirstParamter = uniformRandomNumbers(optimisationProblem.numberOfDimensions_);
        const arma::vec& secondSecondParameter = uniformRandomNumbers(optimisationProblem.numberOfDimensions_);

        arma::vec firstSecondParameter = firstFirstParamter;
        firstSecondParameter.elem(partitionCandidate.first) = secondSecondParameter.elem(partitionCandidate.first);
        arma::vec secondFirstParameter = secondSecondParameter;
        secondFirstParameter.elem(partitionCandidate.first) = firstFirstParamter.elem(partitionCandidate.first);

        // **Note:** The summation of not-a-number values results in a not-a-number value and comparing it with another value returns false, so everything will work out just fine.
        if (std::abs(optimisationProblem.getObjectiveValueOfNormalisedParameter(firstFirstParamter) + optimisationProblem.getObjectiveValueOfNormalisedParameter(secondSecondParameter) - optimisationProblem.getObjectiveValueOfNormalisedParameter(firstSecondParameter) - optimisationProblem.getObjectiveValueOfNormalisedParameter(secondFirstParameter)) < ::mant::machinePrecision) {
          ++confidences(n);
          if (confidences(n) / static_cast<double>(numberOfEvaluations) >= minimalConfidence) {
            // Proceeds with the next partition candidate, as we already reached the confidence threshold.
            break;
          }
        }
      }
    }

    const arma::uvec& acceptablePartitionCandidatesIndicies = arma::find(confidences / static_cast<double>(numberOfEvaluations) >= minimalConfidence);
    std::vector<std::pair<arma::uvec, arma::uvec>> acceptablePartitionCandidates;
    acceptablePartitionCandidates.reserve(acceptablePartitionCandidatesIndicies.n_elem);

    for (const auto acceptablePartitionCandidateIndex : acceptablePartitionCandidatesIndicies) {
      acceptablePartitionCandidates.push_back(partitionCandidates.at(acceptablePartitionCandidateIndex));
    }

    /* The last of the two steps is to calculate the partition with the maximal number of parts, from all acceptable two-set partitions.
     * If we now weaken our observation and assume that each accepted two-set partition holds true for **all** inputs, the partition with the maximal number of parts can be calculated by combining all intersections between one part and an other.
     *
     * For example, assume that we got 3 acceptable two-set partitions:
     *
     * - {{1, 2, 3, 4, 5}, {6}}
     * - {{1}, {2, 3, 4, 5, 6}}
     * - {{1, 2, 3}, {4, 5, 6}}
     *
     * We would then calculate the intersection between the first two partitions:
     *
     * {{1, 2, 3, 4, 5}, {6}} intersect {{1}, {2, 3, 4, 5, 6}} =
     *   {{1, 2, 3, 4, 5} intersect {1}}             union
     *   {{1, 2, 3, 4, 5} intersect {2, 3, 4, 5, 6}} union
     *   {{6} intersect {1}}                         union \
     *   {{6} intersect {2, 3, 4, 5, 6}}                   / skipped and directly replaced by {{6}}
     *   = {{1}, {2, 3, 4, 5}, {6}}
     *
     * And intersect the resulting partitions with the remaining one:
     *
     * {{1}, {2, 3, 4, 5}, {6}} intersect {{1, 2, 3}, {4, 5, 6}} =
     *   {{1} intersect {1, 2, 3}}          union \
     *   {{1} intersect {4, 5, 6}}          union / skipped and directly replaced by {{1}}
     *   {{2, 3, 4, 5} intersect {1, 2, 3}} union
     *   {{2, 3, 4, 5} intersect {4, 5, 6}} union
     *   {{6} intersect {1, 2, 3}}          union \
     *   {{6} intersect {4, 5, 6}}                / skipped and directly replaced by {{6}}
     *   = {{1}, {2, 3}, {4, 5}, {6}}
     *
     * And get {{1}, {2, 3}, {4, 5}, {6}} as partition with the maximal number of parts.
     */

    if (acceptablePartitionCandidates.size() > 1) {
      std::vector<arma::uvec> partition = {acceptablePartitionCandidates.at(0).first, acceptablePartitionCandidates.at(0).second};
      acceptablePartitionCandidates.erase(acceptablePartitionCandidates.cbegin());

      for (const auto& acceptablePartitionCandidate : acceptablePartitionCandidates) {
        std::vector<arma::uvec> nextPartition;
        for (const auto& part : partition) {
          if (part.n_elem == 1) {
            nextPartition.push_back(part);
          } else {
            // **Note:** `std::set_intersection` requires that all parts are ordered at this point.
            std::vector<arma::uword> intersection;
            std::set_intersection(part.begin(), part.end(), acceptablePartitionCandidate.first.begin(), acceptablePartitionCandidate.first.end(), intersection.begin());
            nextPartition.push_back(arma::uvec(intersection));
            intersection.clear();
            std::set_intersection(part.begin(), part.end(), acceptablePartitionCandidate.second.begin(), acceptablePartitionCandidate.second.end(), intersection.begin());
            nextPartition.push_back(arma::uvec(intersection));
          }
        }
        partition = nextPartition;

        // We are already finished, as there is no finer partition as having one part for each dimensions.
        if (partition.size() == optimisationProblem.numberOfDimensions_) {
          break;
        }
      }

      return partition;
    } else if (acceptablePartitionCandidates.size() == 1) {
      return {acceptablePartitionCandidates.at(0).first, acceptablePartitionCandidates.at(0).second};
    } else {
      return {arma::regspace<arma::uvec>(0, optimisationProblem.numberOfDimensions_ - 1)};
    }
  }

  std::vector<arma::uvec> additiveSeparability(
      OptimisationProblem& optimisationProblem,
      const arma::uword numberOfEvaluations) {
    return additiveSeparability(optimisationProblem, numberOfEvaluations, 1.0);
  }
}


#include <algorithm>
#include <functional>
#include <limits>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <glog/logging.h>

#include <product-quantization/learn-product-quantization.h>

namespace product_quantization {
void ComputePCARotation(
    const Eigen::MatrixXf& data_points, Eigen::MatrixXf* rotation_matrix,
    std::vector<float>* variances) {
  CHECK_NOTNULL(rotation_matrix);
  CHECK_NOTNULL(variances);
  CHECK_GE(data_points.cols(), data_points.rows());

  int num_dimensions = data_points.rows();
  int num_data_points = data_points.cols();
  rotation_matrix->resize(num_dimensions, num_dimensions);
  variances->resize(num_dimensions);

  Eigen::VectorXf mean = data_points.rowwise().mean();

  Eigen::MatrixXf covariance_matrix;
  covariance_matrix.setZero(num_dimensions, num_dimensions);
  for (int i = 0; i < num_data_points; ++i) {
    Eigen::VectorXf p = data_points.col(i) - mean;
    covariance_matrix += p * p.transpose();
  }
  covariance_matrix /= static_cast<float>(num_data_points - 1);

  Eigen::EigenSolver<Eigen::MatrixXf> eigen_solver(covariance_matrix);
  Eigen::VectorXf eigenvalues = eigen_solver.eigenvalues().real();
  *rotation_matrix = eigen_solver.eigenvectors().real().transpose();

  for (int i = 0; i < num_dimensions; ++i) {
    CHECK_GE(eigenvalues(i), 0.0);
    (*variances)[i] = eigenvalues(i);
  }
}

void EigenvalueAllocation(
    const Eigen::MatrixXf& rotation_matrix, const std::vector<float>& variances,
    int num_components, Eigen::MatrixXf* permutated_rotation_matrix) {
  CHECK_EQ(rotation_matrix.cols(), rotation_matrix.rows());
  CHECK_EQ(static_cast<unsigned int>(rotation_matrix.cols()), variances.size());
  CHECK_EQ(rotation_matrix.cols() % num_components, 0);
  CHECK_NOTNULL(permutated_rotation_matrix);

  int num_dimensions = rotation_matrix.cols();
  permutated_rotation_matrix->setIdentity(num_dimensions, num_dimensions);
  // Sorts the rows in decreasing order of variance.
  std::vector<std::pair<float, int> > variance_index_pairs(num_dimensions);
  for (int i = 0; i < num_dimensions; ++i) {
    variance_index_pairs[i].first = variances[i];
    variance_index_pairs[i].second = i;
  }
  std::sort(
      variance_index_pairs.begin(), variance_index_pairs.end(),
      std::greater<std::pair<float, int> >());

  // Performs EigenvalueAllocation by balancing the variances in a greedy
  // fashion: Given the sorted variances, the algorithm iteratively selects the
  // component with the minimum product of variances for which we have not
  // yet selected enough rows.
  std::vector<float> variance_product_per_component(num_components, -1.0f);
  std::vector<int> num_selected_dimensions_per_component(num_components, 0);
  int max_num_dimensions_per_component = num_dimensions / num_components;
  for (int i = 0; i < num_dimensions; ++i) {
    // Finds the component with the minimum product of variances.
    int selected_component = -1;
    float min_product_variance = std::numeric_limits<float>::max();
    for (int j = 0; j < num_components; ++j) {
      if (num_selected_dimensions_per_component[j] ==
          max_num_dimensions_per_component) {
        continue;
      }

      if (variance_product_per_component[j] < min_product_variance) {
        min_product_variance = variance_product_per_component[j];
        selected_component = j;
      }
    }
    CHECK_GE(selected_component, 0);
    if (min_product_variance == -1.0f) {
      variance_product_per_component[selected_component] =
          variance_index_pairs[i].first;
    } else {
      variance_product_per_component[selected_component] *=
          variance_index_pairs[i].first;
    }
    permutated_rotation_matrix->row(
        selected_component * max_num_dimensions_per_component +
        num_selected_dimensions_per_component[selected_component]) =
        rotation_matrix.row(variance_index_pairs[i].second);
    ++num_selected_dimensions_per_component[selected_component];
  }
}
}  // namespace product_quantization


/*
 * =====================================================================================
 *
 *       Filename:  ip3_atp_model.hpp
 *
 *    Description:
 *
 *        Version:  1.0
 *        Created:  Sunday 20 July 2014 10:44:10  IST
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Anup Pillai (), anupgpillai@gmail.com
 *   Organization:  IISER Pune
 *
 * =====================================================================================
 */

#ifndef ASTRON_IP3_ATP_MODEL_HPP_INCLUDED
#define ASTRON_IP3_ATP_MODEL_HPP_INCLUDED
#include <vector>
#include <iostream>
#include <cmath>
#include <boost/math/constants/constants.hpp>
#include "astron_utility_functions.hpp"
//const double pi = boost::math::constants::pi<double>();

//DECLATATIONS FOR IP3_SYSTEM
class IP3
{
   private:
//------------------parameters from reduced Li-Rinzel form for IP3-calicum signalling
      double a2 = 0.2E-03;     //    0.2         1/(micro M * sec)
      double d1 = 0.13;   //     0.13        micro M
      double d2 = 1.049;  //     1.049       micro M
      double d3 = 0.9434;   //   0.9434      micro M
      double d5 = 0.08234; //    0.08234     micro M

      double c0 = 2.0;  //       2.0         micro M
      double c1 = 0.185;   //                Dimensionless
      double v1 = 6.0E-03; //        6.0         1/sec
      double v2 = 0.11E-03;//        0.11        1/sec
      double v3 = 0.9E-03;     //    0.9         1/ (micro M * sec)
      double k3 = 0.1;   //      0.1         micro M

//------------ parameters from Stamastakis-Mantzaris model for IP3 via ATP

      double v4 = 4.0E-03;  //4.0 micro M/sec
      double k4 = 0.3;  // 0.3 micro M
      double a5 = 0.0; // dim.less (calcium feedback for IP3 production)
      double k5 = 1.1; // 1.1 micro M
      double v6 = 4E-03; // 0.19 /sec

      double c7 = 1.0; // Dim.less cy_vol/extr-cell_vol
      double v7 = 5.0E-03; // 5.0 micro M/sec (maximum ATP production rate)
      double v8 = 6.0E-03;  // 6.0 micro M/sec (maximum degeneration rate)
      double k8 = 5.0; // 5.0 micro M
      double F0 = 0.05; // dimention less (ATP feedback)
      double Cmax = 1.5; // 1.5 micro M (ATP feedback)
   public:

      double CaER = 0.0;      // micro M (Calcium concentration in the ER)
      //double ip3_conc = 0.0;      // micro M (Given IP3 concentration in the cytosol)
      //double ca_conc = 0.0;

      double ip3_tau = 0.0;
      double ip3_rate = 0.0;
      double ip3_thres = 0.0;
      double ip3_gen = 0.0;
      double ip3_equ = 0.0;

//------------------ CONSTRUCTORS

   IP3(): CaER(0.0)//, ca_conc(0.0), ip3_conc(0.0) /* IP3 class implicit constructor */
   {
   };

   /* ASTRO class explicit constructor */
   //IP3( double CaER_, double ca_conc_, double ip3_conc_ ): CaER(CaER_), ca_conc(ca_conc_), ip3_conc(ip3_conc_)
   IP3( double CaER_ ): CaER(CaER_)
   {
   };
//------------------ Function declarations
   void set_CaER(double ca_conc);

   double m_inf(double ip3_conc, double ca_conc);
   double h_inf(double ip3_conc, double ca_conc);
   double h_tau(double ip3_conc, double ca_conc);

   double F_ca(double ca_conc);

   template <class State, class Deriv >
   void operator() ( const State &x, Deriv &dxdt , const double  t );
};
//--------------------m_inf Function
double IP3::m_inf(double ip3_conc, double ca_conc)
{
   double value =  ( ip3_conc / ( ip3_conc + d1 ) ) * ( ca_conc / (ca_conc + d5) ) ;
   return value;
};
//--------------------h_tau Function
double IP3::h_tau(double ip3_conc, double ca_conc)
{
   double Q2 = d2 * ((ip3_conc + d1)/(ip3_conc + d3));
   double value  = 1 / (a2 * (Q2 + ca_conc) );
   return value;
}

//--------------------h_inf Function
double IP3::h_inf(double ip3_conc, double ca_conc)
{
   double Q2 = d2 * ((ip3_conc + d1)/(ip3_conc + d3));
   double value = Q2/(Q2+ca_conc);
   return value;
};
//--------------------Set CaER Function
void IP3::set_CaER(double ca_conc)
{
   this->CaER = (c0 - ca_conc) / c1;
   //std::cout << "Set_CaER = " << (c0 - ca_conc)/c1 << "\t" << CaER << "\n";
};
//----------Calicum induced ATP release function
double IP3::F_ca(double ca_conc)
{
   double val = ( (F0/(F0-1)) - (2 * (ca_conc/Cmax)) ) / ( (1/(F0-1)) - pow((ca_conc/Cmax),2) );
   return val;
}
//------- ASTRO class ODE Function
template <class State, class Deriv >
void IP3::operator() ( const State &x_, Deriv &dxdt_ , const double t )
{
   typename boost::range_iterator< const State >::type x = boost::begin( x_ );
   typename boost::range_iterator< Deriv >::type dxdt = boost::begin( dxdt_ );

   //std::cout << CaER << "\n";

   set_CaER(x[1]); // Update ER calcium level

   dxdt[0] = ( h_inf(x[2],x[1]) - x[0] ) / h_tau(x[2],x[1]) ; // dh/dt

   double JCh = ( c1 * v1 * pow(m_inf(x[2],x[1]),3) * pow(x[0],3) * (x[1] - CaER) ); //   J_channel
   double JPump = ( v3 * pow(x[1],2) ) / ( pow(k3,2) + pow(x[1],2) ) ;   //    J_Pump
   double JLeak = c1 * v2 * ( x[1] - CaER );   //    J_Leak

   dxdt[1] = - (JCh + JPump + JLeak) ;// dCa/dt
   double ip3_degrade = (x[2] - ip3_equ)/ip3_tau;
   double ip3_mGluR = ip3_rate * heaviside(ip3_gen,ip3_thres);
   //double ip3_atp = (v4 * x[3])/(k4+x[3]);
   double ip3_atp = 0.0;//(v4 * x[3])/(k4+x[3]) * ( (x[1] + ((1-a5)*k5)) / (x[1] + k5) ); //- v6*x[2];
   dxdt[2] =  -ip3_degrade + ip3_mGluR + ip3_atp ;//dip3/dt
   //dxdt[3] = ( c7 * v7 * F_ca(x[1]) ) - ( v8 * (x[3] / (k8 + x[3])) ) ; //dATP/dt

};

#endif // ASTRON_IP3_ATP_MODEL_HPP_INCLUDED


//==================================================================================================
/*!
  @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_ARCH_COMMON_SIMD_FUNCTION_HYPOT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_HYPOT_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/detail/constant/maxexponentm1.hpp>
#include <boost/simd/detail/constant/minexponent.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/exponent.hpp>
#include <boost/simd/function/ldexp.hpp>
#include <boost/simd/function/max.hpp>
#include <boost/simd/function/min.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/sqrt.hpp>
#include <boost/simd/function/unary_minus.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/function/logical_or.hpp>
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/function/is_nan.hpp>
#include <boost/simd/constant/inf.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF(hypot_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        using iA0 = bd::as_integer_t<A0>;
        A0 r =  bs::abs(a0);
        A0 i =  bs::abs(a1);
        iA0 e =  exponent(bs::max(i, r));
        e = bs::min(bs::max(e,Minexponent<A0>()),Maxexponentm1<A0>());
        A0 res =  ldexp(sqrt(sqr(ldexp(r, -e))+sqr(ldexp(i, -e))), e);
        #ifndef BOOST_SIMD_NO_INVALIDS
        auto test = logical_or(logical_and(is_nan(a0), is_inf(a1)),
                              logical_and(is_nan(a1), is_inf(a0)));
        return if_else(test, Inf<A0>(), res);
        #else
        return res;
        #endif
      }
   };

  BOOST_DISPATCH_OVERLOAD_IF ( hypot_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::fast_tag
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::floating_<A0>, X>
                          )
  {

    BOOST_FORCEINLINE A0 operator() (const fast_tag &,  A0 const& a0, A0 const& a1
                                    ) const BOOST_NOEXCEPT
    {
      return boost::simd::sqrt(bs::fma(a0, a0, sqr(a1)));
    }
  };
} } }

#endif


#include <NTL/vec_lzz_p.h>

NTL_START_IMPL

void conv(vec_zz_p& x, const vec_ZZ& a)
{
   long i, n;

   n = a.length();
   x.SetLength(n);

   zz_p* xp = x.elts();
   const ZZ* ap = a.elts();

   for (i = 0; i < n; i++)
      conv(xp[i], ap[i]);
}

void conv(vec_ZZ& x, const vec_zz_p& a)
{
   long n = a.length();
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      x[i] = rep(a[i]);
}

void InnerProduct(zz_p& x, const vec_zz_p& a, const vec_zz_p& b)
{
   long n = min(a.length(), b.length());
   long i;
   zz_p accum, t;

   clear(accum);
   for (i = 0; i < n; i++) {
      mul(t, a[i], b[i]);
      add(accum, accum, t);
   }

   x = accum;
}

void InnerProduct(zz_p& x, const vec_zz_p& a, const vec_zz_p& b,
                  long offset)
{
   if (offset < 0) Error("InnerProduct: negative offset");
   if (NTL_OVERFLOW(offset, 1, 0)) Error("InnerProduct: offset too big");

   long n = min(a.length(), b.length()+offset);
   long i;
   zz_p accum, t;

   clear(accum);
   for (i = offset; i < n; i++) {
      mul(t, a[i], b[i-offset]);
      add(accum, accum, t);
   }

   x = accum;
}

long CRT(vec_ZZ& gg, ZZ& a, const vec_zz_p& G)
{
   long n = gg.length();
   if (G.length() != n) Error("CRT: vector length mismatch");

   long p = zz_p::modulus();

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

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

   long p1;
   p1 = p >> 1;

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

   long p_odd = (p & 1);

   long modified = 0;

   long h;

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

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

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

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

      gg[i] = g;
   }

   a = new_a;

   return modified;
}

void mul(vec_zz_p& x, const vec_zz_p& a, zz_p b)
{
   long n = a.length();
   x.SetLength(n);

   long i;

   if (n <= 1) {

      for (i = 0; i < n; i++)
	 mul(x[i], a[i], b);

   }
   else {

      long p = zz_p::modulus();
      double pinv = zz_p::ModulusInverse();
      long bb = rep(b);
      mulmod_precon_t bpinv = PrepMulModPrecon(bb, p, pinv);

      const zz_p *ap = a.elts();
      zz_p *xp = x.elts();

      for (i = 0; i < n; i++)
         xp[i].LoopHole() = MulModPrecon(rep(ap[i]), bb, p, bpinv);

   }
}

void mul(vec_zz_p& x, const vec_zz_p& a, long b_in)
{
   zz_p b;
   b = b_in;
   mul(x, a, b);
}

void add(vec_zz_p& x, const vec_zz_p& a, const vec_zz_p& b)
{
   long n = a.length();
   if (b.length() != n) Error("vector add: dimension mismatch");

   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      add(x[i], a[i], b[i]);
}

void sub(vec_zz_p& x, const vec_zz_p& a, const vec_zz_p& b)
{
   long n = a.length();
   if (b.length() != n) Error("vector sub: dimension mismatch");
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      sub(x[i], a[i], b[i]);
}

void clear(vec_zz_p& x)
{
   long n = x.length();
   long i;
   for (i = 0; i < n; i++)
      clear(x[i]);
}

void negate(vec_zz_p& x, const vec_zz_p& a)
{
   long n = a.length();
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      negate(x[i], a[i]);
}

long IsZero(const vec_zz_p& a)
{
   long n = a.length();
   long i;

   for (i = 0; i < n; i++)
      if (!IsZero(a[i]))
         return 0;

   return 1;
}

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

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

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

zz_p operator*(const vec_zz_p& a, const vec_zz_p& b)
{
   zz_p res;
   InnerProduct(res, a, b);
   return res;
}

void VectorCopy(vec_zz_p& x, const vec_zz_p& a, long n)
{
   if (n < 0) Error("VectorCopy: negative length");
   if (NTL_OVERFLOW(n, 1, 0)) Error("overflow in VectorCopy");

   long m = min(n, a.length());

   x.SetLength(n);

   long i;

   for (i = 0; i < m; i++)
      x[i] = a[i];

   for (i = m; i < n; i++)
      clear(x[i]);
}

NTL_END_IMPL