#include <NTL/lzz_pX.h>

#include <NTL/new.h>

NTL_START_IMPL

long divide(zz_pX& q, const zz_pX& a, const zz_pX& b)
{
   if (IsZero(b)) {
      if (IsZero(a)) {
         clear(q);
         return 1;
      }
      else
         return 0;
   }

   zz_pX lq, r;
   DivRem(lq, r, a, b);
   if (!IsZero(r)) return 0;
   q = lq;
   return 1;
}

long divide(const zz_pX& a, const zz_pX& b)
{
   if (IsZero(b)) return IsZero(a);
   zz_pX lq, r;
   DivRem(lq, r, a, b);
   if (!IsZero(r)) return 0;
   return 1;
}

void zz_pXMatrix::operator=(const zz_pXMatrix& M)
{
   elts[0][0] = M.elts[0][0];
   elts[0][1] = M.elts[0][1];
   elts[1][0] = M.elts[1][0];
   elts[1][1] = M.elts[1][1];
}

void RightShift(zz_pX& x, const zz_pX& a, long n)
{
   if (IsZero(a)) {
      clear(x);
      return;
   }

   if (n < 0) {
      if (n < -NTL_MAX_LONG) Error("overflow in RightShift");
      LeftShift(x, a, -n);
      return;
   }

   long da = deg(a);
   long i;

   if (da < n) {
      clear(x);
      return;
   }

   if (&x != &a)
      x.rep.SetLength(da-n+1);

   for (i = 0; i <= da-n; i++)
      x.rep[i] = a.rep[i+n];

   if (&x == &a)
      x.rep.SetLength(da-n+1);

   x.normalize();
}

void LeftShift(zz_pX& x, const zz_pX& a, long n)
{
   if (IsZero(a)) {
      clear(x);
      return;
   }

   if (n < 0) {
      if (n < -NTL_MAX_LONG)
         clear(x);
      else
         RightShift(x, a, -n);
      return;
   }

   if (NTL_OVERFLOW(n, 1, 0))
      Error("overflow in LeftShift");

   long m = a.rep.length();

   x.rep.SetLength(m+n);

   long i;
   for (i = m-1; i >= 0; i--)
      x.rep[i+n] = a.rep[i];

   for (i = 0; i < n; i++)
      clear(x.rep[i]);
}

void ShiftAdd(zz_pX& U, const zz_pX& V, long n)
// assumes input does not alias output
{
   if (IsZero(V))
      return;

   long du = deg(U);
   long dv = deg(V);

   long d = max(du, n+dv);

   U.rep.SetLength(d+1);
   long i;

   for (i = du+1; i <= d; i++)
      clear(U.rep[i]);

   for (i = 0; i <= dv; i++)
      add(U.rep[i+n], U.rep[i+n], V.rep[i]);

   U.normalize();
}

void ShiftSub(zz_pX& U, const zz_pX& V, long n)
// assumes input does not alias output
{
   if (IsZero(V))
      return;

   long du = deg(U);
   long dv = deg(V);

   long d = max(du, n+dv);

   U.rep.SetLength(d+1);
   long i;

   for (i = du+1; i <= d; i++)
      clear(U.rep[i]);

   for (i = 0; i <= dv; i++)
      sub(U.rep[i+n], U.rep[i+n], V.rep[i]);

   U.normalize();
}

void mul(zz_pX& U, zz_pX& V, const zz_pXMatrix& M)
// (U, V)^T = M*(U, V)^T
{
   long d = deg(U) - deg(M(1,1));
   long k = NextPowerOfTwo(d - 1);

   // When the GCD algorithm is run on polynomials of degree n, n-1,
   // where n is a power of two, then d-1 is likely to be a power of two.
   // It would be more natural to set k = NextPowerOfTwo(d+1), but this
   // would be much less efficient in this case.

   long n = (1L << k);
   long xx;
   zz_p a0, a1, b0, b1, c0, d0, u0, u1, v0, v1, nu0, nu1, nv0;
   zz_p t1, t2;

   if (n == d-1)
      xx = 1;
   else if (n == d)
      xx = 2;
   else
      xx = 3;

   switch (xx) {
   case 1:
      GetCoeff(a0, M(0,0), 0);
      GetCoeff(a1, M(0,0), 1);
      GetCoeff(b0, M(0,1), 0);
      GetCoeff(b1, M(0,1), 1);
      GetCoeff(c0, M(1,0), 0);
      GetCoeff(d0, M(1,1), 0);

      GetCoeff(u0, U, 0);
      GetCoeff(u1, U, 1);
      GetCoeff(v0, V, 0);
      GetCoeff(v1, V, 1);

      mul(t1, (a0), (u0));
      mul(t2, (b0), (v0));
      add(t1, t1, t2);
      nu0 = t1;

      mul(t1, (a1), (u0));
      mul(t2, (a0), (u1));
      add(t1, t1, t2);
      mul(t2, (b1), (v0));
      add(t1, t1, t2);
      mul(t2, (b0), (v1));
      add(t1, t1, t2);
      nu1 = t1;

      mul(t1, (c0), (u0));
      mul(t2, (d0), (v0));
      add (t1, t1, t2);
      nv0 = t1;

      break;

   case 2:
      GetCoeff(a0, M(0,0), 0);
      GetCoeff(b0, M(0,1), 0);

      GetCoeff(u0, U, 0);
      GetCoeff(v0, V, 0);

      mul(t1, (a0), (u0));
      mul(t2, (b0), (v0));
      add(t1, t1, t2);
      nu0 = t1;

      break;

   case 3:
      break;

   }

   fftRep RU(INIT_SIZE, k), RV(INIT_SIZE, k), R1(INIT_SIZE, k),
          R2(INIT_SIZE, k);

   TofftRep(RU, U, k);
   TofftRep(RV, V, k);

   TofftRep(R1, M(0,0), k);
   mul(R1, R1, RU);
   TofftRep(R2, M(0,1), k);
   mul(R2, R2, RV);
   add(R1, R1, R2);
   FromfftRep(U, R1, 0, d);

   TofftRep(R1, M(1,0), k);
   mul(R1, R1, RU);
   TofftRep(R2, M(1,1), k);
   mul(R2, R2, RV);
   add(R1, R1, R2);
   FromfftRep(V, R1, 0, d-1);

   // now fix-up results

   switch (xx) {
   case 1:
      GetCoeff(u0, U, 0);
      sub(u0, u0, nu0);
      SetCoeff(U, d-1, u0);
      SetCoeff(U, 0, nu0);

      GetCoeff(u1, U, 1);
      sub(u1, u1, nu1);
      SetCoeff(U, d, u1);
      SetCoeff(U, 1, nu1);

      GetCoeff(v0, V, 0);
      sub(v0, v0, nv0);
      SetCoeff(V, d-1, v0);
      SetCoeff(V, 0, nv0);

      break;

   case 2:
      GetCoeff(u0, U, 0);
      sub(u0, u0, nu0);
      SetCoeff(U, d, u0);
      SetCoeff(U, 0, nu0);

      break;

   }
}

void mul(zz_pXMatrix& A, zz_pXMatrix& B, zz_pXMatrix& C)
// A = B*C, B and C are destroyed
{
   long db = deg(B(1,1));
   long dc = deg(C(1,1));
   long da = db + dc;

   long k = NextPowerOfTwo(da+1);

   fftRep B00, B01, B10, B11, C0, C1, T1, T2;

   TofftRep(B00, B(0,0), k); B(0,0).kill();
   TofftRep(B01, B(0,1), k); B(0,1).kill();
   TofftRep(B10, B(1,0), k); B(1,0).kill();
   TofftRep(B11, B(1,1), k); B(1,1).kill();

   TofftRep(C0, C(0,0), k);  C(0,0).kill();
   TofftRep(C1, C(1,0), k);  C(1,0).kill();

   mul(T1, B00, C0);
   mul(T2, B01, C1);
   add(T1, T1, T2);
   FromfftRep(A(0,0), T1, 0, da);

   mul(T1, B10, C0);
   mul(T2, B11, C1);
   add(T1, T1, T2);
   FromfftRep(A(1,0), T1, 0, da);

   TofftRep(C0, C(0,1), k);  C(0,1).kill();
   TofftRep(C1, C(1,1), k);  C(1,1).kill();

   mul(T1, B00, C0);
   mul(T2, B01, C1);
   add(T1, T1, T2);
   FromfftRep(A(0,1), T1, 0, da);

   mul(T1, B10, C0);
   mul(T2, B11, C1);
   add(T1, T1, T2);
   FromfftRep(A(1,1), T1, 0, da);
}

void IterHalfGCD(zz_pXMatrix& M_out, zz_pX& U, zz_pX& V, long d_red)
{
   M_out(0,0).SetMaxLength(d_red);
   M_out(0,1).SetMaxLength(d_red);
   M_out(1,0).SetMaxLength(d_red);
   M_out(1,1).SetMaxLength(d_red);

   set(M_out(0,0));   clear(M_out(0,1));
   clear(M_out(1,0)); set(M_out(1,1));

   long goal = deg(U) - d_red;

   if (deg(V) <= goal)
      return;

   zz_pX Q, t(INIT_SIZE, d_red);

   while (deg(V) > goal) {
      PlainDivRem(Q, U, U, V);
      swap(U, V);

      mul(t, Q, M_out(1,0));
      sub(t, M_out(0,0), t);
      M_out(0,0) = M_out(1,0);
      M_out(1,0) = t;

      mul(t, Q, M_out(1,1));
      sub(t, M_out(0,1), t);
      M_out(0,1) = M_out(1,1);
      M_out(1,1) = t;
   }
}

void HalfGCD(zz_pXMatrix& M_out, const zz_pX& U, const zz_pX& V, long d_red)
{
   if (IsZero(V) || deg(V) <= deg(U) - d_red) {
      set(M_out(0,0));   clear(M_out(0,1));
      clear(M_out(1,0)); set(M_out(1,1));

      return;
   }

   long n = deg(U) - 2*d_red + 2;
   if (n < 0) n = 0;

   zz_pX U1, V1;

   RightShift(U1, U, n);
   RightShift(V1, V, n);

   if (d_red <= NTL_zz_pX_HalfGCD_CROSSOVER) {
      IterHalfGCD(M_out, U1, V1, d_red);
      return;
   }

   long d1 = (d_red + 1)/2;
   if (d1 < 1) d1 = 1;
   if (d1 >= d_red) d1 = d_red - 1;

   zz_pXMatrix M1;

   HalfGCD(M1, U1, V1, d1);
   mul(U1, V1, M1);

   long d2 = deg(V1) - deg(U) + n + d_red;

   if (IsZero(V1) || d2 <= 0) {
      M_out = M1;
      return;
   }

   zz_pX Q;
   zz_pXMatrix M2;

   DivRem(Q, U1, U1, V1);
   swap(U1, V1);

   HalfGCD(M2, U1, V1, d2);

   zz_pX t(INIT_SIZE, deg(M1(1,1))+deg(Q)+1);

   mul(t, Q, M1(1,0));
   sub(t, M1(0,0), t);
   swap(M1(0,0), M1(1,0));
   swap(M1(1,0), t);

   t.kill();

   t.SetMaxLength(deg(M1(1,1))+deg(Q)+1);

   mul(t, Q, M1(1,1));
   sub(t, M1(0,1), t);
   swap(M1(0,1), M1(1,1));
   swap(M1(1,1), t);

   t.kill();

   mul(M_out, M2, M1);
}

void XHalfGCD(zz_pXMatrix& M_out, zz_pX& U, zz_pX& V, long d_red)
{
   if (IsZero(V) || deg(V) <= deg(U) - d_red) {
      set(M_out(0,0));   clear(M_out(0,1));
      clear(M_out(1,0)); set(M_out(1,1));

      return;
   }

   long du = deg(U);

   if (d_red <= NTL_zz_pX_HalfGCD_CROSSOVER) {
      IterHalfGCD(M_out, U, V, d_red);
      return;
   }

   long d1 = (d_red + 1)/2;
   if (d1 < 1) d1 = 1;
   if (d1 >= d_red) d1 = d_red - 1;

   zz_pXMatrix M1;

   HalfGCD(M1, U, V, d1);
   mul(U, V, M1);

   long d2 = deg(V) - du + d_red;

   if (IsZero(V) || d2 <= 0) {
      M_out = M1;
      return;
   }

   zz_pX Q;
   zz_pXMatrix M2;

   DivRem(Q, U, U, V);
   swap(U, V);

   XHalfGCD(M2, U, V, d2);

   zz_pX t(INIT_SIZE, deg(M1(1,1))+deg(Q)+1);

   mul(t, Q, M1(1,0));
   sub(t, M1(0,0), t);
   swap(M1(0,0), M1(1,0));
   swap(M1(1,0), t);

   t.kill();

   t.SetMaxLength(deg(M1(1,1))+deg(Q)+1);

   mul(t, Q, M1(1,1));
   sub(t, M1(0,1), t);
   swap(M1(0,1), M1(1,1));
   swap(M1(1,1), t);

   t.kill();

   mul(M_out, M2, M1);
}

void HalfGCD(zz_pX& U, zz_pX& V)
{
   long d_red = (deg(U)+1)/2;

   if (IsZero(V) || deg(V) <= deg(U) - d_red) {
      return;
   }

   long du = deg(U);

   long d1 = (d_red + 1)/2;
   if (d1 < 1) d1 = 1;
   if (d1 >= d_red) d1 = d_red - 1;

   zz_pXMatrix M1;

   HalfGCD(M1, U, V, d1);
   mul(U, V, M1);

   long d2 = deg(V) - du + d_red;

   if (IsZero(V) || d2 <= 0) {
      return;
   }

   M1(0,0).kill();
   M1(0,1).kill();
   M1(1,0).kill();
   M1(1,1).kill();

   zz_pX Q;

   DivRem(Q, U, U, V);
   swap(U, V);

   HalfGCD(M1, U, V, d2);

   mul(U, V, M1);
}

void GCD(zz_pX& d, const zz_pX& u, const zz_pX& v)
{
   zz_pX u1, v1;

   u1 = u;
   v1 = v;

   if (deg(u1) == deg(v1)) {
      if (IsZero(u1)) {
         clear(d);
         return;
      }

      rem(v1, v1, u1);
   }
   else if (deg(u1) < deg(v1)) {
      swap(u1, v1);
   }

   // deg(u1) > deg(v1)

   while (deg(u1) > NTL_zz_pX_GCD_CROSSOVER && !IsZero(v1)) {
      HalfGCD(u1, v1);

      if (!IsZero(v1)) {
         rem(u1, u1, v1);
         swap(u1, v1);
      }
   }

   PlainGCD(d, u1, v1);
}

void XGCD(zz_pX& d, zz_pX& s, zz_pX& t, const zz_pX& a, const zz_pX& b)
{
   zz_p w;

   if (IsZero(a) && IsZero(b)) {
      clear(d);
      set(s);
      clear(t);
      return;
   }

   zz_pX U, V, Q;

   U = a;
   V = b;

   long flag = 0;

   if (deg(U) == deg(V)) {
      DivRem(Q, U, U, V);
      swap(U, V);
      flag = 1;
   }
   else if (deg(U) < deg(V)) {
      swap(U, V);
      flag = 2;
   }

   zz_pXMatrix M;

   XHalfGCD(M, U, V, deg(U)+1);

   d = U;

   if (flag == 0) {
      s = M(0,0);
      t = M(0,1);
   }
   else if (flag == 1) {
      s = M(0,1);
      mul(t, Q, M(0,1));
      sub(t, M(0,0), t);
   }
   else {  /* flag == 2 */
      s = M(0,1);
      t = M(0,0);
   }

   // normalize

   inv(w, LeadCoeff(d));
   mul(d, d, w);
   mul(s, s, w);
   mul(t, t, w);
}

void IterBuild(zz_p* a, long n)
{
   long i, k;
   zz_p b, t;

   if (n <= 0) return;

   negate(a[0], a[0]);

   for (k = 1; k <= n-1; k++) {
      negate(b, a[k]);
      add(a[k], b, a[k-1]);
      for (i = k-1; i >= 1; i--) {
         mul(t, a[i], b);
         add(a[i], t, a[i-1]);
      }
      mul(a[0], a[0], b);
   }
}

void mul(zz_p* x, const zz_p* a, const zz_p* b, long n)
{
   zz_p t, accum;

   long i, j, jmin, jmax;

   long d = 2*n-1;

   for (i = 0; i <= d; i++) {
      jmin = max(0, i-(n-1));
      jmax = min(n-1, i);
      clear(accum);
      for (j = jmin; j <= jmax; j++) {
         mul(t, (a[j]), (b[i-j]));
         add(accum, accum, t);
      }
      if (i >= n) {
         add(accum, accum, (a[i-n]));
         add(accum, accum, (b[i-n]));
      }

      x[i] = accum;
   }
}

void BuildFromRoots(zz_pX& x, const vec_zz_p& a)
{
   long n = a.length();

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

   long k0 = NextPowerOfTwo(NTL_zz_pX_MUL_CROSSOVER)-1;
   long crossover = 1L << k0;

   if (n <= NTL_zz_pX_MUL_CROSSOVER) {
      x.rep.SetMaxLength(n+1);
      x.rep = a;
      IterBuild(&x.rep[0], n);
      x.rep.SetLength(n+1);
      SetCoeff(x, n);
      return;
   }

   long k = NextPowerOfTwo(n);

   long m = 1L << k;
   long i, j;
   long l, width;

   zz_pX b(INIT_SIZE, m+1);

   b.rep = a;
   b.rep.SetLength(m+1);
   for (i = n; i < m; i++)
      clear(b.rep[i]);

   set(b.rep[m]);

   fftRep R1(INIT_SIZE, k), R2(INIT_SIZE, k);

   zz_p t1, one;
   set(one);

   vec_zz_p G(INIT_SIZE, crossover), H(INIT_SIZE, crossover);
   zz_p *g = G.elts();
   zz_p *h = H.elts();
   zz_p *tmp;

   for (i = 0; i < m; i+= crossover) {
      for (j = 0; j < crossover; j++)
         negate(g[j], b.rep[i+j]);

      if (k0 > 0) {
         for (j = 0; j < crossover; j+=2) {
            mul(t1, g[j], g[j+1]);
            add(g[j+1], g[j], g[j+1]);
            g[j] = t1;
         }
      }

      for (l = 1; l < k0; l++) {
         width = 1L << l;

         for (j = 0; j < crossover; j += 2*width)
            mul(&h[j], &g[j], &g[j+width], width);

         tmp = g; g = h; h = tmp;
      }

      for (j = 0; j < crossover; j++)
         b.rep[i+j] = g[j];
   }

   for (l = k0; l < k; l++) {
      width = 1L << l;
      for (i = 0; i < m; i += 2*width) {
         t1 = b.rep[i+width];
         set(b.rep[i+width]);
         TofftRep(R1, b, l+1, i, i+width);
         b.rep[i+width] = t1;
         t1 = b.rep[i+2*width];
         set(b.rep[i+2*width]);
         TofftRep(R2, b, l+1, i+width, i+2*width);
         b.rep[i+2*width] = t1;
         mul(R1, R1, R2);
         FromfftRep(&b.rep[i], R1, 0, 2*width-1);
         sub(b.rep[i], b.rep[i], one);
      }
   }

   x.rep.SetLength(n+1);
   long delta = m-n;
   for (i = 0; i <= n; i++)
     x.rep[i] = b.rep[i+delta];

   // no need to normalize
}

void eval(zz_p& b, const zz_pX& f, zz_p a)
// does a Horner evaluation
{
   zz_p acc;
   long i;

   clear(acc);
   for (i = deg(f); i >= 0; i--) {
      mul(acc, acc, a);
      add(acc, acc, f.rep[i]);
   }

   b = acc;
}

void eval(vec_zz_p& b, const zz_pX& f, const vec_zz_p& a)
// naive algorithm:  repeats Horner
{
   if (&b == &f.rep) {
      vec_zz_p bb;
      eval(bb, f, a);
      b = bb;
      return;
   }

   long m = a.length();
   b.SetLength(m);
   long i;
   for (i = 0; i < m; i++)
      eval(b[i], f, a[i]);
}

void interpolate(zz_pX& f, const vec_zz_p& a, const vec_zz_p& b)
{
   long m = a.length();
   if (b.length() != m) Error("interpolate: vector length mismatch");

   if (m == 0) {
      clear(f);
      return;
   }

   vec_zz_p prod;
   prod = a;

   zz_p t1, t2;

   long k, i;

   vec_zz_p res;
   res.SetLength(m);

   for (k = 0; k < m; k++) {

      const zz_p& aa = a[k];

      set(t1);
      for (i = k-1; i >= 0; i--) {
         mul(t1, t1, aa);
         add(t1, t1, prod[i]);
      }

      clear(t2);
      for (i = k-1; i >= 0; i--) {
         mul(t2, t2, aa);
         add(t2, t2, res[i]);
      }

      inv(t1, t1);
      sub(t2, b[k], t2);
      mul(t1, t1, t2);

      for (i = 0; i < k; i++) {
         mul(t2, prod[i], t1);
         add(res[i], res[i], t2);
      }

      res[k] = t1;

      if (k < m-1) {
         if (k == 0)
            negate(prod[0], prod[0]);
         else {
            negate(t1, a[k]);
            add(prod[k], t1, prod[k-1]);
            for (i = k-1; i >= 1; i--) {
               mul(t2, prod[i], t1);
               add(prod[i], t2, prod[i-1]);
            }
            mul(prod[0], prod[0], t1);
         }
      }
   }

   while (m > 0 && IsZero(res[m-1])) m--;
   res.SetLength(m);
   f.rep = res;
}

void InnerProduct(zz_pX& x, const vec_zz_p& v, long low, long high,
                   const vec_zz_pX& H, long n, vec_zz_p& t)
{
   zz_p s;
   long i, j;

   zz_p *tp = t.elts();

   for (j = 0; j < n; j++)
      clear(tp[j]);

   long p = zz_p::modulus();
   double pinv = zz_p::ModulusInverse();

   high = min(high, v.length()-1);
   for (i = low; i <= high; i++) {
      const vec_zz_p& h = H[i-low].rep;
      long m = h.length();
      zz_p w = (v[i]);

      long W = rep(w);
      mulmod_precon_t Wpinv = PrepMulModPrecon(W, p, pinv); // ((double) W)*pinv;
      const zz_p *hp = h.elts();

      for (j = 0; j < m; j++) {
         long S = MulModPrecon(rep(hp[j]), W, p, Wpinv);
         S = AddMod(S, rep(tp[j]), p);
         tp[j].LoopHole() = S;
      }
   }

   x.rep = t;
   x.normalize();
}

void CompMod(zz_pX& x, const zz_pX& g, const zz_pXArgument& A,
             const zz_pXModulus& F)
{
   if (deg(g) <= 0) {
      x = g;
      return;
   }

   zz_pX s, t;
   vec_zz_p scratch(INIT_SIZE, F.n);

   long m = A.H.length() - 1;
   long l = ((g.rep.length()+m-1)/m) - 1;

   zz_pXMultiplier M;
   build(M, A.H[m], F);

   InnerProduct(t, g.rep, l*m, l*m + m - 1, A.H, F.n, scratch);
   for (long i = l-1; i >= 0; i--) {
      InnerProduct(s, g.rep, i*m, i*m + m - 1, A.H, F.n, scratch);
      MulMod(t, t, M, F);
      add(t, t, s);
   }

   x = t;
}

void build(zz_pXArgument& A, const zz_pX& h, const zz_pXModulus& F, long m)
{
   if (m <= 0 || deg(h) >= F.n) Error("build: bad args");

   if (m > F.n) m = F.n;

   long i;

   if (zz_pXArgBound > 0) {
      double sz = 1;
      sz = sz*F.n;
      sz = sz+6;
      sz = sz*(sizeof (long));
      sz = sz/1024;
      m = min(m, long(zz_pXArgBound/sz));
      m = max(m, 1);
   }

   zz_pXMultiplier M;

   build(M, h, F);

   A.H.SetLength(m+1);

   set(A.H[0]);
   A.H[1] = h;
   for (i = 2; i <= m; i++)
      MulMod(A.H[i], A.H[i-1], M, F);
}

long zz_pXArgBound = 0;

void CompMod(zz_pX& x, const zz_pX& g, const zz_pX& h, const zz_pXModulus& F)
   // x = g(h) mod f
{
   long m = SqrRoot(g.rep.length());

   if (m == 0) {
      clear(x);
      return;
   }

   zz_pXArgument A;

   build(A, h, F, m);

   CompMod(x, g, A, F);
}

void Comp2Mod(zz_pX& x1, zz_pX& x2, const zz_pX& g1, const zz_pX& g2,
              const zz_pX& h, const zz_pXModulus& F)

{
   long m = SqrRoot(g1.rep.length() + g2.rep.length());

   if (m == 0) {
      clear(x1);
      clear(x2);
      return;
   }

   zz_pXArgument A;

   build(A, h, F, m);

   zz_pX xx1, xx2;

   CompMod(xx1, g1, A, F);
   CompMod(xx2, g2, A, F);

   x1 = xx1;
   x2 = xx2;
}

void Comp3Mod(zz_pX& x1, zz_pX& x2, zz_pX& x3,
              const zz_pX& g1, const zz_pX& g2, const zz_pX& g3,
              const zz_pX& h, const zz_pXModulus& F)

{
   long m = SqrRoot(g1.rep.length() + g2.rep.length() + g3.rep.length());

   if (m == 0) {
      clear(x1);
      clear(x2);
      clear(x3);
      return;
   }

   zz_pXArgument A;

   build(A, h, F, m);

   zz_pX xx1, xx2, xx3;

   CompMod(xx1, g1, A, F);
   CompMod(xx2, g2, A, F);
   CompMod(xx3, g3, A, F);

   x1 = xx1;
   x2 = xx2;
   x3 = xx3;
}

static void StripZeroes(vec_zz_p& x)
{
   long n = x.length();
   while (n > 0 && IsZero(x[n-1]))
      n--;
   x.SetLength(n);
}

void PlainUpdateMap(vec_zz_p& xx, const vec_zz_p& a,
                    const zz_pX& b, const zz_pX& f)
{
   long n = deg(f);
   long i, m;

   if (IsZero(b)) {
      xx.SetLength(0);
      return;
   }

   m = n-1 - deg(b);

   vec_zz_p x(INIT_SIZE, n);

   for (i = 0; i <= m; i++)
      InnerProduct(x[i], a, b.rep, i);

   if (deg(b) != 0) {
      zz_pX c(INIT_SIZE, n);
      LeftShift(c, b, m);

      for (i = m+1; i < n; i++) {
         MulByXMod(c, c, f);
         InnerProduct(x[i], a, c.rep);
      }
   }

   xx = x;
}

void UpdateMap(vec_zz_p& x, const vec_zz_p& aa,
               const zz_pXMultiplier& B, const zz_pXModulus& F)
{
   long n = F.n;

   vec_zz_p a;
   a = aa;
   StripZeroes(a);

   if (a.length() > n) Error("UpdateMap: bad args");
   long i;

   if (!B.UseFFT) {
      PlainUpdateMap(x, a, B.b, F.f);
      StripZeroes(x);
      return;
   }

   fftRep R1(INIT_SIZE, F.k), R2(INIT_SIZE, F.l);
   vec_zz_p V1(INIT_SIZE, n);

   RevTofftRep(R1, a, F.k, 0, a.length()-1, 0);
   mul(R2, R1, F.FRep);
   RevFromfftRep(V1, R2, 0, n-2);
   for (i = 0; i <= n-2; i++)  negate(V1[i], V1[i]);
   RevTofftRep(R2, V1, F.l, 0, n-2, n-1);
   mul(R2, R2, B.B1);
   mul(R1, R1, B.B2);

   AddExpand(R2, R1);
   RevFromfftRep(x, R2, 0, n-1);
   StripZeroes(x);
}

void ProjectPowers(vec_zz_p& x, const vec_zz_p& a, long k,
                   const zz_pXArgument& H, const zz_pXModulus& F)

{
   long n = F.n;

   if (a.length() > n || k < 0 || NTL_OVERFLOW(k, 1, 0))
      Error("ProjectPowers: bad args");

   long m = H.H.length()-1;
   long l = (k+m-1)/m - 1;

   zz_pXMultiplier M;
   build(M, H.H[m], F);

   vec_zz_p s(INIT_SIZE, n);
   s = a;
   StripZeroes(s);

   x.SetLength(k);

   for (long i = 0; i <= l; i++) {
      long m1 = min(m, k-i*m);
      zz_p* w = &x[i*m];
      for (long j = 0; j < m1; j++)
         InnerProduct(w[j], H.H[j].rep, s);
      if (i < l)
         UpdateMap(s, s, M, F);
   }
}

void ProjectPowers(vec_zz_p& x, const vec_zz_p& a, long k,
                   const zz_pX& h, const zz_pXModulus& F)

{
   if (a.length() > F.n || k < 0) Error("ProjectPowers: bad args");

   if (k == 0) {
      x.SetLength(0);
      return;
   }

   long m = SqrRoot(k);

   zz_pXArgument H;

   build(H, h, F, m);
   ProjectPowers(x, a, k, H, F);
}

void BerlekampMassey(zz_pX& h, const vec_zz_p& a, long m)
{
   zz_pX Lambda, Sigma, Temp;
   long L;
   zz_p Delta, Delta1, t1;
   long shamt;

   // cerr << "*** " << m << "\n";

   Lambda.SetMaxLength(m+1);
   Sigma.SetMaxLength(m+1);
   Temp.SetMaxLength(m+1);

   L = 0;
   set(Lambda);
   clear(Sigma);
   set(Delta);
   shamt = 0;

   long i, r, dl;

   for (r = 1; r <= 2*m; r++) {
      // cerr << r << "--";
      clear(Delta1);
      dl = deg(Lambda);
      for (i = 0; i <= dl; i++) {
         mul(t1, Lambda.rep[i], a[r-i-1]);
         add(Delta1, Delta1, t1);
      }

      if (IsZero(Delta1)) {
         shamt++;
         // cerr << "case 1: " << deg(Lambda) << " " << deg(Sigma) << " " << shamt << "\n";
      }
      else if (2*L < r) {
         div(t1, Delta1, Delta);
         mul(Temp, Sigma, t1);
         Sigma = Lambda;
         ShiftSub(Lambda, Temp, shamt+1);
         shamt = 0;
         L = r-L;
         Delta = Delta1;
         // cerr << "case 2: " << deg(Lambda) << " " << deg(Sigma) << " " << shamt << "\n";
      }
      else {
         shamt++;
         div(t1, Delta1, Delta);
         mul(Temp, Sigma, t1);
         ShiftSub(Lambda, Temp, shamt);
         // cerr << "case 3: " << deg(Lambda) << " " << deg(Sigma) << " " << shamt << "\n";
      }
   }

   // cerr << "finished: " << L << " " << deg(Lambda) << "\n";

   dl = deg(Lambda);
   h.rep.SetLength(L + 1);

   for (i = 0; i < L - dl; i++)
      clear(h.rep[i]);

   for (i = L - dl; i <= L; i++)
      h.rep[i] = Lambda.rep[L - i];
}

void GCDMinPolySeq(zz_pX& h, const vec_zz_p& x, long m)
{
   long i;
   zz_pX a, b;
   zz_pXMatrix M;
   zz_p t;

   a.rep.SetLength(2*m);
   for (i = 0; i < 2*m; i++) a.rep[i] = x[2*m-1-i];
   a.normalize();

   SetCoeff(b, 2*m);

   HalfGCD(M, b, a, m+1);

   /* make monic */

   inv(t, LeadCoeff(M(1,1)));
   mul(h, M(1,1), t);
}

void MinPolySeq(zz_pX& h, const vec_zz_p& a, long m)
{
   if (m < 0 || NTL_OVERFLOW(m, 1, 0)) Error("MinPoly: bad args");
   if (a.length() < 2*m) Error("MinPoly: sequence too short");

   if (m > NTL_zz_pX_BERMASS_CROSSOVER)
      GCDMinPolySeq(h, a, m);
   else
      BerlekampMassey(h, a, m);
}

void DoMinPolyMod(zz_pX& h, const zz_pX& g, const zz_pXModulus& F, long m,
               const vec_zz_p& R)
{
   vec_zz_p x;

   ProjectPowers(x, R, 2*m, g, F);
   MinPolySeq(h, x, m);
}

void ProbMinPolyMod(zz_pX& h, const zz_pX& g, const zz_pXModulus& F, long m)
{
   long n = F.n;
   if (m < 1 || m > n) Error("ProbMinPoly: bad args");

   long i;
   vec_zz_p R(INIT_SIZE, n);

   for (i = 0; i < n; i++) random(R[i]);
   DoMinPolyMod(h, g, F, m, R);
}

void MinPolyMod(zz_pX& hh, const zz_pX& g, const zz_pXModulus& F, long m)
{
   zz_pX h, h1;
   long n = F.n;
   if (m < 1 || m > n) Error("MinPoly: bad args");

   /* probabilistically compute min-poly */

   ProbMinPolyMod(h, g, F, m);
   if (deg(h) == m) { hh = h; return; }
   CompMod(h1, h, g, F);
   if (IsZero(h1)) { hh = h; return; }

   /* not completely successful...must iterate */

   long i;

   zz_pX h2, h3;
   zz_pXMultiplier H1;
   vec_zz_p R(INIT_SIZE, n);

   for (;;) {
      R.SetLength(n);
      for (i = 0; i < n; i++) random(R[i]);
      build(H1, h1, F);
      UpdateMap(R, R, H1, F);
      DoMinPolyMod(h2, g, F, m-deg(h), R);

      mul(h, h, h2);
      if (deg(h) == m) { hh = h; return; }
      CompMod(h3, h2, g, F);
      MulMod(h1, h3, H1, F);
      if (IsZero(h1)) { hh = h; return; }
   }
}

void IrredPolyMod(zz_pX& h, const zz_pX& g, const zz_pXModulus& F, long m)
{
   vec_zz_p R(INIT_SIZE, 1);
   if (m < 1 || m > F.n) Error("IrredPoly: bad args");

   set(R[0]);
   DoMinPolyMod(h, g, F, m, R);
}

void diff(zz_pX& x, const zz_pX& a)
{
   long n = deg(a);
   long i;

   if (n <= 0) {
      clear(x);
      return;
   }

   if (&x != &a)
      x.rep.SetLength(n);

   for (i = 0; i <= n-1; i++) {
      mul(x.rep[i], a.rep[i+1], i+1);
   }

   if (&x == &a)
      x.rep.SetLength(n);

   x.normalize();
}

void MakeMonic(zz_pX& x)
{
   if (IsZero(x))
      return;

   if (IsOne(LeadCoeff(x)))
      return;

   zz_p t;

   inv(t, LeadCoeff(x));
   mul(x, x, t);
}

void PlainMulTrunc(zz_pX& x, const zz_pX& a, const zz_pX& b, long n)
{
   zz_pX y;
   mul(y, a, b);
   trunc(x, y, n);
}

void FFTMulTrunc(zz_pX& x, const zz_pX& a, const zz_pX& b, long n)
{
   if (IsZero(a) || IsZero(b)) {
      clear(x);
      return;
   }

   long d = deg(a) + deg(b);
   if (n > d + 1)
      n = d + 1;

   long k = NextPowerOfTwo(d + 1);
   fftRep R1(INIT_SIZE, k), R2(INIT_SIZE, k);

   TofftRep(R1, a, k);
   TofftRep(R2, b, k);
   mul(R1, R1, R2);
   FromfftRep(x, R1, 0, n-1);
}

void MulTrunc(zz_pX& x, const zz_pX& a, const zz_pX& b, long n)
{
   if (n < 0) Error("MulTrunc: bad args");

   if (deg(a) <= NTL_zz_pX_MUL_CROSSOVER || deg(b) <= NTL_zz_pX_MUL_CROSSOVER)
      PlainMulTrunc(x, a, b, n);
   else
      FFTMulTrunc(x, a, b, n);
}

void PlainSqrTrunc(zz_pX& x, const zz_pX& a, long n)
{
   zz_pX y;
   sqr(y, a);
   trunc(x, y, n);
}

void FFTSqrTrunc(zz_pX& x, const zz_pX& a, long n)
{
   if (IsZero(a)) {
      clear(x);
      return;
   }

   long d = 2*deg(a);
   if (n > d + 1)
      n = d + 1;

   long k = NextPowerOfTwo(d + 1);
   fftRep R1(INIT_SIZE, k);

   TofftRep(R1, a, k);
   mul(R1, R1, R1);
   FromfftRep(x, R1, 0, n-1);
}

void SqrTrunc(zz_pX& x, const zz_pX& a, long n)
{
   if (n < 0) Error("SqrTrunc: bad args");

   if (deg(a) <= NTL_zz_pX_MUL_CROSSOVER)
      PlainSqrTrunc(x, a, n);
   else
      FFTSqrTrunc(x, a, n);
}

void FastTraceVec(vec_zz_p& S, const zz_pX& f)
{
   long n = deg(f);

   if (n <= 0)
      Error("FastTraceVec: bad args");

   if (n == 0) {
      S.SetLength(0);
      return;
   }

   if (n == 1) {
      S.SetLength(1);
      set(S[0]);
      return;
   }

   long i;
   zz_pX f1;

   f1.rep.SetLength(n-1);
   for (i = 0; i <= n-2; i++)
      f1.rep[i] = f.rep[n-i];
   f1.normalize();

   zz_pX f2;
   f2.rep.SetLength(n-1);
   for (i = 0; i <= n-2; i++)
      mul(f2.rep[i], f.rep[n-1-i], i+1);
   f2.normalize();

   zz_pX f3;
   InvTrunc(f3, f1, n-1);
   MulTrunc(f3, f3, f2, n-1);

   S.SetLength(n);

   S[0] = n;
   for (i = 1; i < n; i++)
      negate(S[i], coeff(f3, i-1));
}

void PlainTraceVec(vec_zz_p& S, const zz_pX& ff)
{
   if (deg(ff) <= 0)
      Error("TraceVec: bad args");

   zz_pX f;
   f = ff;

   MakeMonic(f);

   long n = deg(f);

   S.SetLength(n);

   if (n == 0)
      return;

   long k, i;
   zz_p acc, t;

   const zz_p *fp = f.rep.elts();;
   zz_p *sp = S.elts();

   sp[0] = n;

   for (k = 1; k < n; k++) {
      mul(acc, fp[n-k], k);

      for (i = 1; i < k; i++) {
         mul(t, fp[n-i], rep(sp[k-i]));
         add(acc, acc, t);
      }

      negate(sp[k], acc);
   }
}

void TraceVec(vec_zz_p& S, const zz_pX& f)
{
   if (deg(f) <= NTL_zz_pX_TRACE_CROSSOVER)
      PlainTraceVec(S, f);
   else
      FastTraceVec(S, f);
}

void ComputeTraceVec(const zz_pXModulus& F)
{
   vec_zz_p& S = *((vec_zz_p *) &F.tracevec);

   if (S.length() > 0)
      return;

   if (!F.UseFFT) {
      PlainTraceVec(S, F.f);
      return;
   }

   long i;
   long n = F.n;

   fftRep R;
   zz_pX P, g;

   g.rep.SetLength(n-1);
   for (i = 1; i < n; i++)
      mul(g.rep[n-i-1], F.f.rep[n-i], i);
   g.normalize();

   TofftRep(R, g, F.l);
   mul(R, R, F.HRep);
   FromfftRep(P, R, n-2, 2*n-4);

   S.SetLength(n);

   S[0] = n;
   for (i = 1; i < n; i++)
      negate(S[i], coeff(P, n-1-i));
}

void TraceMod(zz_p& x, const zz_pX& a, const zz_pXModulus& F)
{
   long n = F.n;

   if (deg(a) >= n)
      Error("trace: bad args");

   if (F.tracevec.length() == 0)
      ComputeTraceVec(F);

   InnerProduct(x, a.rep, F.tracevec);
}

void TraceMod(zz_p& x, const zz_pX& a, const zz_pX& f)
{
   if (deg(a) >= deg(f) || deg(f) <= 0)
      Error("trace: bad args");

   project(x, TraceVec(f), a);
}

void PlainResultant(zz_p& rres, const zz_pX& a, const zz_pX& b)
{
   zz_p res;

   if (IsZero(a) || IsZero(b))
      clear(res);
   else if (deg(a) == 0 && deg(b) == 0)
      set(res);
   else {
      long d0, d1, d2;
      zz_p lc;
      set(res);

      long n = max(deg(a),deg(b)) + 1;
      zz_pX u(INIT_SIZE, n), v(INIT_SIZE, n);

      u = a;
      v = b;

      for (;;) {
         d0 = deg(u);
         d1 = deg(v);
         lc = LeadCoeff(v);

         PlainRem(u, u, v);
         swap(u, v);

         d2 = deg(v);
         if (d2 >= 0) {
            power(lc, lc, d0-d2);
            mul(res, res, lc);
            if (d0 & d1 & 1) negate(res, res);
         }
         else {
            if (d1 == 0) {
               power(lc, lc, d0);
               mul(res, res, lc);
            }
            else
               clear(res);

            break;
         }
      }
   }

   rres = res;
}

void ResIterHalfGCD(zz_pXMatrix& M_out, zz_pX& U, zz_pX& V, long d_red,
                    vec_zz_p& cvec, vec_long& dvec)
{
   M_out(0,0).SetMaxLength(d_red);
   M_out(0,1).SetMaxLength(d_red);
   M_out(1,0).SetMaxLength(d_red);
   M_out(1,1).SetMaxLength(d_red);

   set(M_out(0,0));   clear(M_out(0,1));
   clear(M_out(1,0)); set(M_out(1,1));

   long goal = deg(U) - d_red;

   if (deg(V) <= goal)
      return;

   zz_pX Q, t(INIT_SIZE, d_red);

   while (deg(V) > goal) {
      append(cvec, LeadCoeff(V));
      append(dvec, dvec[dvec.length()-1]-deg(U)+deg(V));
      PlainDivRem(Q, U, U, V);
      swap(U, V);

      mul(t, Q, M_out(1,0));
      sub(t, M_out(0,0), t);
      M_out(0,0) = M_out(1,0);
      M_out(1,0) = t;

      mul(t, Q, M_out(1,1));
      sub(t, M_out(0,1), t);
      M_out(0,1) = M_out(1,1);
      M_out(1,1) = t;
   }
}

void ResHalfGCD(zz_pXMatrix& M_out, const zz_pX& U, const zz_pX& V, long d_red,
                vec_zz_p& cvec, vec_long& dvec)
{
   if (IsZero(V) || deg(V) <= deg(U) - d_red) {
      set(M_out(0,0));   clear(M_out(0,1));
      clear(M_out(1,0)); set(M_out(1,1));

      return;
   }

   long n = deg(U) - 2*d_red + 2;
   if (n < 0) n = 0;

   zz_pX U1, V1;

   RightShift(U1, U, n);
   RightShift(V1, V, n);

   if (d_red <= NTL_zz_pX_HalfGCD_CROSSOVER) {
      ResIterHalfGCD(M_out, U1, V1, d_red, cvec, dvec);
      return;
   }

   long d1 = (d_red + 1)/2;
   if (d1 < 1) d1 = 1;
   if (d1 >= d_red) d1 = d_red - 1;

   zz_pXMatrix M1;

   ResHalfGCD(M1, U1, V1, d1, cvec, dvec);
   mul(U1, V1, M1);

   long d2 = deg(V1) - deg(U) + n + d_red;

   if (IsZero(V1) || d2 <= 0) {
      M_out = M1;
      return;
   }

   zz_pX Q;
   zz_pXMatrix M2;

   append(cvec, LeadCoeff(V1));
   append(dvec, dvec[dvec.length()-1]-deg(U1)+deg(V1));
   DivRem(Q, U1, U1, V1);
   swap(U1, V1);

   ResHalfGCD(M2, U1, V1, d2, cvec, dvec);

   zz_pX t(INIT_SIZE, deg(M1(1,1))+deg(Q)+1);

   mul(t, Q, M1(1,0));
   sub(t, M1(0,0), t);
   swap(M1(0,0), M1(1,0));
   swap(M1(1,0), t);

   t.kill();

   t.SetMaxLength(deg(M1(1,1))+deg(Q)+1);

   mul(t, Q, M1(1,1));
   sub(t, M1(0,1), t);
   swap(M1(0,1), M1(1,1));
   swap(M1(1,1), t);

   t.kill();

   mul(M_out, M2, M1);
}

void ResHalfGCD(zz_pX& U, zz_pX& V, vec_zz_p& cvec, vec_long& dvec)
{
   long d_red = (deg(U)+1)/2;

   if (IsZero(V) || deg(V) <= deg(U) - d_red) {
      return;
   }

   long du = deg(U);

   long d1 = (d_red + 1)/2;
   if (d1 < 1) d1 = 1;
   if (d1 >= d_red) d1 = d_red - 1;

   zz_pXMatrix M1;

   ResHalfGCD(M1, U, V, d1, cvec, dvec);
   mul(U, V, M1);

   long d2 = deg(V) - du + d_red;

   if (IsZero(V) || d2 <= 0) {
      return;
   }

   M1(0,0).kill();
   M1(0,1).kill();
   M1(1,0).kill();
   M1(1,1).kill();

   zz_pX Q;

   append(cvec, LeadCoeff(V));
   append(dvec, dvec[dvec.length()-1]-deg(U)+deg(V));
   DivRem(Q, U, U, V);
   swap(U, V);

   ResHalfGCD(M1, U, V, d2, cvec, dvec);

   mul(U, V, M1);
}

void resultant(zz_p& rres, const zz_pX& u, const zz_pX& v)
{
   if (deg(u) <= NTL_zz_pX_GCD_CROSSOVER || deg(v) <= NTL_zz_pX_GCD_CROSSOVER) {
      PlainResultant(rres, u, v);
      return;
   }

   zz_pX u1, v1;

   u1 = u;
   v1 = v;

   zz_p res, t;
   set(res);

   if (deg(u1) == deg(v1)) {
      rem(u1, u1, v1);
      swap(u1, v1);

      if (IsZero(v1)) {
         clear(rres);
         return;
      }

      power(t, LeadCoeff(u1), deg(u1) - deg(v1));
      mul(res, res, t);
      if (deg(u1) & 1)
         negate(res, res);
   }
   else if (deg(u1) < deg(v1)) {
      swap(u1, v1);
      if (deg(u1) & deg(v1) & 1)
         negate(res, res);
   }

   // deg(u1) > deg(v1) && v1 != 0

   vec_zz_p cvec;
   vec_long  dvec;

   cvec.SetMaxLength(deg(v1)+2);
   dvec.SetMaxLength(deg(v1)+2);

   append(cvec, LeadCoeff(u1));
   append(dvec, deg(u1));

   while (deg(u1) > NTL_zz_pX_GCD_CROSSOVER && !IsZero(v1)) {
      ResHalfGCD(u1, v1, cvec, dvec);

      if (!IsZero(v1)) {
         append(cvec, LeadCoeff(v1));
         append(dvec, deg(v1));
         rem(u1, u1, v1);
         swap(u1, v1);
      }
   }

   if (IsZero(v1) && deg(u1) > 0) {
      clear(rres);
      return;
   }

   long i, l;
   l = dvec.length();

   if (deg(u1) == 0) {
      // we went all the way...

      for (i = 0; i <= l-3; i++) {
         power(t, cvec[i+1], dvec[i]-dvec[i+2]);
         mul(res, res, t);
         if (dvec[i] & dvec[i+1] & 1)
            negate(res, res);
      }

      power(t, cvec[l-1], dvec[l-2]);
      mul(res, res, t);
   }
   else {
      for (i = 0; i <= l-3; i++) {
         power(t, cvec[i+1], dvec[i]-dvec[i+2]);
         mul(res, res, t);
         if (dvec[i] & dvec[i+1] & 1)
            negate(res, res);
      }

      power(t, cvec[l-1], dvec[l-2]-deg(v1));
      mul(res, res, t);
      if (dvec[l-2] & dvec[l-1] & 1)
         negate(res, res);

      PlainResultant(t, u1, v1);
      mul(res, res, t);
   }

   rres = res;
}

void NormMod(zz_p& x, const zz_pX& a, const zz_pX& f)
{
   if (deg(f) <= 0 || deg(a) >= deg(f))
      Error("norm: bad args");

   if (IsZero(a)) {
      clear(x);
      return;
   }

   zz_p t;
   resultant(t, f, a);
   if (!IsOne(LeadCoeff(f))) {
      zz_p t1;
      power(t1, LeadCoeff(f), deg(a));
      inv(t1, t1);
      mul(t, t, t1);
   }

   x = t;
}

NTL_END_IMPL


/*
	* Implements leapfrog solving with multiple solvers
	*
	*
*/

#ifdef _MAIN_

#else
#include <armadillo>

#include "../core/UAmoeba.hpp"

#ifdef _ALGEBRA_TOOLS_
#else
#include "../core/algebraTools.hpp"
#endif

#ifdef _AMOEBA_INIT_
#else
#include "../core/amoebaParam.cpp"
#endif

#include "../core/translate.cpp"

using namespace algebraTools;
using namespace arma;

#endif

//cx_mat X0, cx_mat X1, int matSize, long maxAmoebaIters, long nGridPoints, double precision, long maxMainIters,

//size, rank MPI directives
//
template <typename T>
void segSolver(const AmoebaParam<T> amoebaParam, int rank, int size)
{

	cx_mat idMat = eye<T>(amoebaParam.matSize, amoebaParam.matSize);
	arma_rng::set_seed_random();

	// UAmoeba(long maxIters, int nGridPoints, double precision, int matSize, int lieDimension, vector<cx_mat> *inputBasis)

	cout << "Creating amoeba object \n";

	UAmoeba* amoeba = new UAmoeba(amoebaParam.maxAmoebaIters, amoebaParam.nGridPoints, amoebaParam.precision, amoebaParam.matSize, amoebaParam.lieDimension, amoebaParam.basis);

	cout << "Allocating MPI buffers \n";

	ArmadilloMPI* armaMPI = new ArmadilloMPI( amoebaParam.matSize, amoebaParam.matSize);
  vector<T> world;
  int bufferSize = 2 * size;

  if(rank == 0)
  {
   	//get X0
		cx_mat X0 = amoebaParam.startBoundary;
		cx_mat X1 = amoebaParam.endBoundary;

		cout << "Start Boundary = " << X0 << "\n"
				 << "Target Matrix = " << X1 << "\n";

    cx_mat kMid;
    invCayley<T>(X1, idMat, kMid);

   	world.push_back(X0);

		for(int i = 1; i < (bufferSize); ++i)
  	{
			kMid *= -0.5 * (i/static_cast<double>(bufferSize));
   		cayley<T>(kMid, idMat, X0);
  		world.push_back(X0);
  	}

    world.push_back(X1);
    cout << "Number of leap-frog points = "<< world.size() << "\n";
	}

	/*
		* int tag = 4 * rank;
		* need to send/recv 4 unique things
		* send and recieves have tag and tag+1 internally
		*
	*/

	int tag = 4 * rank;
	int offSet = 0;

	double localEnergy;
	double totalEnergy = 0;

	cx_mat bL(amoebaParam.matSize, amoebaParam.matSize);
	cx_mat bU(amoebaParam.matSize, amoebaParam.matSize);
	cx_mat newBound(amoebaParam.matSize, amoebaParam.matSize);

	vector<vec> newGuess;
	newGuess.resize(amoebaParam.nGridPoints);

	for(int i=0; i<amoebaParam.nGridPoints; ++i)
	{
		newGuess[i] = randu<vec>(amoebaParam.lieDimension);
	}
	/*
		* to keep track of where all the solvers are
		* each solver has to advance across the world
		* taking two boundary points
		* and outputing a new boundary
	*/

	int indLow;
	int indUp;
	int indMid;

	bufferSize = bufferSize + 1;

	int iters = 0;

	while(iters < amoebaParam.maxMainIters )
	{
		if(rank==0)
		{
			for(int src = 1; src < size; ++src)
			{
				indLow = (2 * src + offSet)%(bufferSize) ;
				indUp = (indLow + 2)%(bufferSize);
				if(indUp < indLow)
				{
					indLow = 0;
					indUp = indLow + 2;
				}

				//cout << "sending boundaries to " << src << endl;
				//cout << "sending " << indLow << " and " << indUp << endl;

				if(size > 0)
				{
					armaMPI->matDestroySend(world[indLow], src, 4 * src );
					armaMPI->matDestroySend(world[indUp], src, (4 * src) + 2);
				}
			}

			indLow = offSet%bufferSize;
			indUp = (indLow + 2)%(bufferSize);
			if(indUp < indLow)
			{
				indLow = 0;
				indUp = indLow + 2;
			}
			indMid = indLow + 1;
			//cout << "solving on " << indLow << " and " << indUp << endl;

			amoeba->curveSeeder(newGuess, amoebaParam.nGridPoints, world[indUp]);
			amoeba->solver(newGuess, world[indLow], world[indUp]);
			amoeba->newBoundary(world[indMid]);
			localEnergy = amoeba->getEnergy();

			for(int src = 1; src < size; ++src)
			{
				indLow = (2 * src + offSet)%(bufferSize) ;
				indUp = (indLow + 2)%(bufferSize);
				if(indUp < indLow)
				{
					indLow = 0;
				}
				indMid = indLow + 1;

				world[indMid] = armaMPI->matConstructRecv(src, 4 * src);
				//cout << "recieved updated boundary from" << src << endl;
			}

			offSet +=1;
			offSet = (offSet)%(bufferSize);
			//cout << "offSet is now " << offSet << endl;
		}
		else
		{
			bL = armaMPI->matConstructRecv(0, tag);
			bU = armaMPI->matConstructRecv(0, tag + 2);
			amoeba->curveSeeder(newGuess, amoebaParam.nGridPoints, bU);

			//cout << "recieved boundary on thread " << rank << endl;

			amoeba->solver(newGuess, bL, bU);
			amoeba->newBoundary(newBound);
			armaMPI->matDestroySend(newBound, 0, tag);
			localEnergy = amoeba->getEnergy();
		}

		MPI_Reduce(&localEnergy, &totalEnergy, size, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);

		if(rank == 0)
		{
			ofstream energyFile;
			energyFile.open("eOut.txt", ios::app);
			energyFile << totalEnergy << endl;
			cout << "iters" << iters << endl;
			cout << "=======================" << endl;
			totalEnergy = 0 ;

		}

		++iters;
	}

	if(rank == 0)
	{
		for(int i = 0; i < (2 * size - 1); ++i)
		{
			cout << "boundary is now" << world[i] << world[i+2];
			amoeba->curveSeeder(newGuess, amoebaParam.nGridPoints, world[i + 2]);
			amoeba->solver(newGuess, world[i], world[i + 2]);
			amoeba->curvePrint();
			amoeba->newBoundary(world[i+1]);

		}
	}

	delete armaMPI;
}


//
//  tFEM.hpp
//
//  Created by r. on 09/05/14
//

#ifndef round1_tFEM_hpp
#define round1_tFEM_hpp

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>

#include "tmesh.hpp"
#include "stopwatch.hpp"
//#include "EVD.hpp"
#include "AWT.hpp"

//#include <algorithm>

namespace spacetime
{
	namespace ublas = boost::numeric::ublas;

	class tFEM
	{
	public:
		typedef ublas::compressed_matrix<double> sparse_matrix;
		typedef ublas::matrix<double> dense_matrix;
		typedef ublas::vector<double> vector;
	public:
		// Should be hidden
		sparse_matrix MtE, AtE;
		sparse_matrix MtF;
		sparse_matrix CtFE, MtFE;
		sparse_matrix EtE;
		dense_matrix VtE;
		vector gamma;
//	public:
//		const sparse_matrix& getMtE() const { return MtE; }
//		const sparse_matrix& getAtE() const { return AtE; }
//		const sparse_matrix& getMtF() const { return MtF; }
//		const sparse_matrix& getCtFE() const { return CtFE; }
//		const sparse_matrix& getMtFE() const { return MtFE; }
//		const sparse_matrix& getEtE() const { return EtE; }
//		const dense_matrix& getVtE() const { return VtE; }
//		const vector& getgamma() const { return gamma; }
	private:
		tmesh TE, TF;
		unsigned int nref;
	public:
		// Should be hidden
		unsigned int dimE;
		unsigned int dimF;
	public:
		const tmesh& refTE() const { return TE; }
		const tmesh& refTF() const { return TF; }
//		unsigned int dimE() const { return (unsigned int)(TE.size()); }
//		unsigned int dimF() const { return (unsigned int)(TF.size() - 1); }
	public:
		void perform_evd()
		{
			stopwatch::Time time(stopwatch::watches, "tFEM::perform_evd()");

			{
				//gsl::EVD evd(AtE, MtE);
				//spacetime::AWT evd(AtE, MtE);
				//VtE = evd.V;
				//gamma = evd.d;
				//assert(gamma.size() == TE.size());

				VtE.resize(0, 0);
				gamma.resize(0);
			}

			for (vector::iterator i = gamma.begin(); i != gamma.end(); ++i)
				(*i) = sqrt(std::max(0.0, *i));
		}
	public:
		void assemble_matrices()
		{
			stopwatch::Time time(stopwatch::watches, "tFEM::assemble_matrices()");

			// nref >= 1 not implemented
			assert(nref == 0);

			typedef ublas::mapped_matrix<double> mapped_matrix;
			mapped_matrix MtE(dimE, dimE, 3*dimE); MtE.clear();
			mapped_matrix AtE(dimE, dimE, 3*dimE); AtE.clear();
			{
				std::vector<tmesh::interval> Is = TE.getIs();
				for (auto n = 0; n != Is.size(); ++n)
				{
					unsigned int m = n + 1;

					double h = Is[n].len;
					MtE(n, n) += (2./6.) * h;
					MtE(m, m) += (2./6.) * h;
					MtE(m, n) += (1./6.) * h;
					MtE(n, m) += (1./6.) * h;

					double g = 1 / h;
					AtE(n, n) += +g;
					AtE(m, m) += +g;
					AtE(m, n) += -g;
					AtE(n, m) += -g;
				}
			}

			mapped_matrix MtF(dimF, dimF, dimF); MtF.clear();
			{
				std::vector<tmesh::interval> Is = TF.getIs();
				for (auto i = 0; i != Is.size(); ++i)
				{
					MtF(i, i) = Is[i].len;
				}
			}

			mapped_matrix CtFE(dimF, dimE, 2*dimF); CtFE.clear();
			mapped_matrix MtFE(dimF, dimE, 2*dimF); MtFE.clear();
			{
				std::vector<tmesh::interval> Is = TE.getIs();
				for (auto n = 0; n != Is.size(); ++n)
				{
					MtFE(n, n+0) += (1./2.) * Is[n].len;
					MtFE(n, n+1) += (1./2.) * Is[n].len;

					CtFE(n, n+0) += -1;
					CtFE(n, n+1) += +1;
				}
			}

			mapped_matrix EtE(1, dimE, 1); EtE.clear();
			{
				EtE(0, 0) = 1;
			}

			this->MtE = MtE;
			this->AtE = AtE;
			this->MtF = MtF;
			this->CtFE = CtFE;
			this->MtFE = MtFE;
			this->EtE = EtE;
		}
	public:
		tFEM(const tmesh& te, unsigned nref = 0, bool perform_evd_now = true)
		:
		TE(te), TF(te), nref(nref), dimE(0), dimF(0)
		{
			for (unsigned int n = 0; n != nref; ++n)
				TF.refine();

			dimE = (unsigned int)(TE.size());
			dimF = (unsigned int)(TF.size() - 1);

			assemble_matrices();

			if (perform_evd_now) perform_evd();
		}
	};

} // namespace spacetime

#endif


// [[Rcpp::plugins(cpp11)]]
// [[Rcpp::plugins(openmp)]]
// [[Rcpp::depends(RcppProgress)]]
// [[Rcpp::depends(BH)]]

#include <Rcpp.h>
#include <progress.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include "IntrogressionSimulations.h"
#include "Rcpp_output.h"
#include "random.h"

#ifdef _OPENMP
    #include <omp.h>
#endif

Rcpp::List Rcpp_WriteOutput(const Parameters &GlobalPars, SimData &SimulationData){
    using namespace boost::accumulators;

    Rcpp::DataFrame parsdata =  Rcpp::DataFrame::create(
        Rcpp::_["NINIT0"] = GlobalPars.NINIT[0],
        Rcpp::_["NINIT1"] = GlobalPars.NINIT[1],
        Rcpp::_["NGEN"] = GlobalPars.NGEN,
        Rcpp::_["NREP"] = GlobalPars.NREP,
        Rcpp::_["NLOCI"] = GlobalPars.NLOCI,
        Rcpp::_["RECOMBINATIONRATE"] = GlobalPars.RECOMBINATIONRATE,
        Rcpp::_["GROWTHRATE"] = GlobalPars.BIRTHRATE,
        Rcpp::_["CARRYINGCAPACITY"] = GlobalPars.K,
        Rcpp::_["INDEX_MAJOR"] = GlobalPars.index[0]
    );
    const int NREP = SimulationData.DataSet.size();

    // Write outputfiles
    Rcpp::NumericVector generation(GlobalPars.NGEN);
    Rcpp::NumericVector popsizevecm(GlobalPars.NGEN);
    Rcpp::NumericVector major0vecm(GlobalPars.NGEN);
    Rcpp::NumericVector major1vecm(GlobalPars.NGEN);
    Rcpp::NumericVector introgressed0vecm(GlobalPars.NGEN);
    Rcpp::NumericVector introgressed1vecm(GlobalPars.NGEN);
    Rcpp::NumericVector popsizevecv(GlobalPars.NGEN);
    Rcpp::NumericVector major0vecv(GlobalPars.NGEN);
    Rcpp::NumericVector major1vecv(GlobalPars.NGEN);
    Rcpp::NumericVector introgressed0vecv(GlobalPars.NGEN);
    Rcpp::NumericVector introgressed1vecv(GlobalPars.NGEN);

    for(int i = 0; i < GlobalPars.NGEN; ++i){
        accumulator_set<int, stats<tag::mean, tag::variance > > popsize;
        accumulator_set<double, stats<tag::mean, tag::variance > > major0;
        accumulator_set<double, stats<tag::mean, tag::variance > > major1;
        accumulator_set<double, stats<tag::mean, tag::variance > > introgressed0;
        accumulator_set<double, stats<tag::mean, tag::variance > > introgressed1;

        for(int j = 0; j < NREP; ++j){
            popsize(SimulationData.DataSet[j]->popsize[i]);
            major0((double)SimulationData.DataSet[j]->major0[i]);
            major1((double)SimulationData.DataSet[j]->major1[i]);
            introgressed0(SimulationData.DataSet[j]->introgressed0[i]);
            introgressed1(SimulationData.DataSet[j]->introgressed1[i]);
        }
        generation[i] = i;
        popsizevecm[i] = mean(popsize);
        major0vecm[i] = mean(major0);
        major1vecm[i] = mean(major1);
        introgressed0vecm[i] = mean(introgressed0);
        introgressed1vecm[i] = mean(introgressed1);
        popsizevecv[i] = variance(popsize);
        major0vecv[i] = variance(major0);
        major1vecv[i] = variance(major1);
        introgressed0vecv[i] = variance(introgressed0);
        introgressed1vecv[i] = variance(introgressed1);
    };

    Rcpp::DataFrame data =  Rcpp::DataFrame::create(
        Rcpp::_["Generation"] = generation,
        Rcpp::_["Popsize_avg"] = (popsizevecm),
        Rcpp::_["Major0_avg"] = (major0vecm),
        Rcpp::_["Major1_avg"] = (major1vecm),
        Rcpp::_["Introgressed0_avg"] = (introgressed0vecm),
        Rcpp::_["Introgressed1_avg"] = (introgressed1vecm),
        Rcpp::_["Popsize_var"] = (popsizevecv),
        Rcpp::_["Major0_var"] = (major0vecv),
        Rcpp::_["Major1_var"] = (major1vecv),
        Rcpp::_["Introgressed0_var"] = (introgressed0vecv),
        Rcpp::_["Introgressed1_var"] = (introgressed1vecv)
    );

    std::vector<Rcpp::NumericVector> allelefrequencymean(GlobalPars.NLOCI);
    std::vector<Rcpp::NumericVector> allelefrequencyvar(GlobalPars.NLOCI);

    for(int i = 0; i < GlobalPars.NLOCI; ++i){
        allelefrequencymean[i] = Rcpp::NumericVector(GlobalPars.NGEN);
        allelefrequencyvar[i] = Rcpp::NumericVector(GlobalPars.NGEN);
    }

    for(int i = 0; i < GlobalPars.NGEN; ++i){
        for(int l = 0; l < GlobalPars.NLOCI; ++l)
        {
            accumulator_set<double, stats<tag::mean, tag::variance > > locus;
            for(int r = 0; r < NREP; ++r)
            {
                locus((double)SimulationData.DataSet[r]->allele0[i][l] / ((double)SimulationData.DataSet[r]->popsize[i]));
            }
            allelefrequencymean[l][i] = mean(locus);
            allelefrequencyvar[l][i] = variance(locus);
        }
    }

    Rcpp::DataFrame alleledatamean, alleledatavar;
    std::string avglocus = "avglocus", varlocus = "varlocus";
    alleledatamean.push_back(generation,"Generation");
    alleledatavar.push_back(generation,"Generation");
    for(int i = 0; i < GlobalPars.NLOCI; ++i){
        alleledatamean.push_back((allelefrequencymean[i]), avglocus+std::to_string(i));
        alleledatavar.push_back((allelefrequencyvar[i]), varlocus+std::to_string(i));
    }

    double fixation = (double)NREP/(double(NREP+SimulationData.nofixcounter.load()));

    // Cleanup
    for(int i = 0; i < NREP; ++i){
        delete SimulationData.DataSet[i];
    }
    SimulationData.DataSet.clear();

    return Rcpp::List::create(
        Rcpp::_["pars"] = (parsdata),
        Rcpp::_["data"] = (data),
        Rcpp::_["allelefavg"] = (alleledatamean),
        Rcpp::_["allelefvar"] = (alleledatavar),
        Rcpp::_["fixation"] = fixation
    );

}

// [[Rcpp::export]]
Rcpp::List RcppIntrogressionSimulation(Rcpp::List parslist, int setthreads = 0, bool progressbar = false){

    // Prepare for simulation
    rnd::set_seed();
    const Parameters GlobalPars(parslist);
    SimData SimulationData;

    // Run nrep successful simulations
    #ifdef _OPENMP
        const static int maxthreads = omp_get_max_threads();
        if(setthreads>0) omp_set_num_threads(setthreads);
        else omp_set_num_threads(maxthreads);
        REprintf("Parallel activated : Number of threads=%i\n",omp_get_max_threads());
    #endif
    //if(progressbar == true) Progress p(GlobalPars.NREP, true);

    #pragma omp parallel for schedule(static)
    for (int task = 0; task < GlobalPars.NREP; ++task){
        while(RunSimulation(GlobalPars, SimulationData)==false);
        //if(progressbar == true) p.increment();
    }

    return Rcpp_WriteOutput(GlobalPars, SimulationData);
}


#include <iostream>
#include <Eigen/Core>
#include <bench/BenchTimer.h>
using namespace Eigen;

#ifndef SIZE
#define SIZE 50
#endif

#ifndef REPEAT
#define REPEAT 10000
#endif

typedef float Scalar;

__attribute__ ((noinline)) void benchVec(Scalar* a, Scalar* b, Scalar* c, int size);
__attribute__ ((noinline)) void benchVec(MatrixXf& a, MatrixXf& b, MatrixXf& c);
__attribute__ ((noinline)) void benchVec(VectorXf& a, VectorXf& b, VectorXf& c);

int main(int argc, char* argv[])
{
    int size = SIZE * 8;
    int size2 = size * size;
    Scalar* a = internal::aligned_new<Scalar>(size2);
    Scalar* b = internal::aligned_new<Scalar>(size2+4)+1;
    Scalar* c = internal::aligned_new<Scalar>(size2);

    for (int i=0; i<size; ++i)
    {
        a[i] = b[i] = c[i] = 0;
    }

    BenchTimer timer;

    timer.reset();
    for (int k=0; k<10; ++k)
    {
        timer.start();
        benchVec(a, b, c, size2);
        timer.stop();
    }
    std::cout << timer.value() << "s  " << (double(size2*REPEAT)/timer.value())/(1024.*1024.*1024.) << " GFlops\n";
    return 0;
    for (int innersize = size; innersize>2 ; --innersize)
    {
        if (size2%innersize==0)
        {
            int outersize = size2/innersize;
            MatrixXf ma = Map<MatrixXf>(a, innersize, outersize );
            MatrixXf mb = Map<MatrixXf>(b, innersize, outersize );
            MatrixXf mc = Map<MatrixXf>(c, innersize, outersize );
            timer.reset();
            for (int k=0; k<3; ++k)
            {
                timer.start();
                benchVec(ma, mb, mc);
                timer.stop();
            }
            std::cout << innersize << " x " << outersize << "  " << timer.value() << "s   " << (double(size2*REPEAT)/timer.value())/(1024.*1024.*1024.) << " GFlops\n";
        }
    }

    VectorXf va = Map<VectorXf>(a, size2);
    VectorXf vb = Map<VectorXf>(b, size2);
    VectorXf vc = Map<VectorXf>(c, size2);
    timer.reset();
    for (int k=0; k<3; ++k)
    {
        timer.start();
        benchVec(va, vb, vc);
        timer.stop();
    }
    std::cout << timer.value() << "s   " << (double(size2*REPEAT)/timer.value())/(1024.*1024.*1024.) << " GFlops\n";

    return 0;
}

void benchVec(MatrixXf& a, MatrixXf& b, MatrixXf& c)
{
    for (int k=0; k<REPEAT; ++k)
        a = a + b;
}

void benchVec(VectorXf& a, VectorXf& b, VectorXf& c)
{
    for (int k=0; k<REPEAT; ++k)
        a = a + b;
}

void benchVec(Scalar* a, Scalar* b, Scalar* c, int size)
{
    typedef internal::packet_traits<Scalar>::type PacketScalar;
    const int PacketSize = internal::packet_traits<Scalar>::size;
    PacketScalar a0, a1, a2, a3, b0, b1, b2, b3;
    for (int k=0; k<REPEAT; ++k)
        for (int i=0; i<size; i+=PacketSize*8)
        {
//             a0 = internal::pload(&a[i]);
//             b0 = internal::pload(&b[i]);
//             a1 = internal::pload(&a[i+1*PacketSize]);
//             b1 = internal::pload(&b[i+1*PacketSize]);
//             a2 = internal::pload(&a[i+2*PacketSize]);
//             b2 = internal::pload(&b[i+2*PacketSize]);
//             a3 = internal::pload(&a[i+3*PacketSize]);
//             b3 = internal::pload(&b[i+3*PacketSize]);
//             internal::pstore(&a[i], internal::padd(a0, b0));
//             a0 = internal::pload(&a[i+4*PacketSize]);
//             b0 = internal::pload(&b[i+4*PacketSize]);
//
//             internal::pstore(&a[i+1*PacketSize], internal::padd(a1, b1));
//             a1 = internal::pload(&a[i+5*PacketSize]);
//             b1 = internal::pload(&b[i+5*PacketSize]);
//
//             internal::pstore(&a[i+2*PacketSize], internal::padd(a2, b2));
//             a2 = internal::pload(&a[i+6*PacketSize]);
//             b2 = internal::pload(&b[i+6*PacketSize]);
//
//             internal::pstore(&a[i+3*PacketSize], internal::padd(a3, b3));
//             a3 = internal::pload(&a[i+7*PacketSize]);
//             b3 = internal::pload(&b[i+7*PacketSize]);
//
//             internal::pstore(&a[i+4*PacketSize], internal::padd(a0, b0));
//             internal::pstore(&a[i+5*PacketSize], internal::padd(a1, b1));
//             internal::pstore(&a[i+6*PacketSize], internal::padd(a2, b2));
//             internal::pstore(&a[i+7*PacketSize], internal::padd(a3, b3));

            internal::pstore(&a[i+2*PacketSize], internal::padd(internal::ploadu(&a[i+2*PacketSize]), internal::ploadu(&b[i+2*PacketSize])));
            internal::pstore(&a[i+3*PacketSize], internal::padd(internal::ploadu(&a[i+3*PacketSize]), internal::ploadu(&b[i+3*PacketSize])));
            internal::pstore(&a[i+4*PacketSize], internal::padd(internal::ploadu(&a[i+4*PacketSize]), internal::ploadu(&b[i+4*PacketSize])));
            internal::pstore(&a[i+5*PacketSize], internal::padd(internal::ploadu(&a[i+5*PacketSize]), internal::ploadu(&b[i+5*PacketSize])));
            internal::pstore(&a[i+6*PacketSize], internal::padd(internal::ploadu(&a[i+6*PacketSize]), internal::ploadu(&b[i+6*PacketSize])));
            internal::pstore(&a[i+7*PacketSize], internal::padd(internal::ploadu(&a[i+7*PacketSize]), internal::ploadu(&b[i+7*PacketSize])));
        }
}


/*
 * StokesM2L.cpp
 *
 *  Created on: Oct 12, 2016
 *      Author: wyan
 */

#include "SVD_pvfmm.hpp"

#include <Eigen/Dense>

#include <iomanip>
#include <iostream>

#define DIRECTLAYER 2

namespace StokesPVel2D3D {

using EVec3 = Eigen::Vector3d;
using EVec4 = Eigen::Vector4d;
using EMat3 = Eigen::Matrix3d;
using EMat4 = Eigen::Matrix4d;
constexpr double eps = 1e-10;

/**
 * \brief Returns the coordinates of points on the surface of a cube.
 * \param[in] p Number of points on an edge of the cube is (n+1)
 * \param[in] c Coordinates to the centre of the cube (3D array).
 * \param[in] alpha Scaling factor for the size of the cube.
 * \param[in] depth Depth of the cube in the octree.
 * \return Vector with coordinates of points on the surface of the cube in the
 * format [x0 y0 z0 x1 y1 z1 .... ].
 */
template <class Real_t>
std::vector<Real_t> surface(int p, Real_t *c, Real_t alpha, int depth) {
    size_t n_ = (6 * (p - 1) * (p - 1) + 2); // Total number of points.

    std::vector<Real_t> coord(n_ * 3);
    coord[0] = coord[1] = coord[2] = -1.0;
    size_t cnt = 1;
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = -1.0;
            coord[cnt * 3 + 1] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = (2.0 * j - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * i - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = -1.0;
            coord[cnt * 3 + 2] = (2.0 * (j + 1) - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = (2.0 * j - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = -1.0;
            cnt++;
        }
    for (size_t i = 0; i < (n_ / 2) * 3; i++)
        coord[cnt * 3 + i] = -coord[i];

    Real_t r = 0.5 * pow(0.5, depth);
    Real_t b = alpha * r;
    for (size_t i = 0; i < n_; i++) {
        coord[i * 3 + 0] = (coord[i * 3 + 0] + 1.0) * b + c[0];
        coord[i * 3 + 1] = (coord[i * 3 + 1] + 1.0) * b + c[1];
        coord[i * 3 + 2] = (coord[i * 3 + 2] + 1.0) * b + c[2];
    }
    return coord;
}

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(M_PI) * r * r) + erfc(xi * r) / (2 * r * r * r)) *
                            (r * r * Eigen::Matrix3d::Identity() + (rvec * rvec.transpose())) -
                        4 * xi / sqrt(M_PI) * exp(-(xi * xi) * (r * r)) * Eigen::Matrix3d::Identity();
    return A;
}

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) * std::erfc(k / (2 * xi) + xi * z); }

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

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

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

inline double J20(double k, double xi, double z) {
    return sqrt(M_PI) * lbda(k, xi, z) / (4 * k * k * xi) +
           M_PI * ((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 M_PI * (-thetaplus(k, xi, z) - thetaminus(k, xi, z)) * k / 4 + sqrt(M_PI) * lbda(k, xi, z) * xi;
}

inline double J22(double k, double xi, double z) {
    return M_PI * ((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(M_PI) * lbda(k, xi, z) / (4 * xi);
}

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

inline double K12(double k, double xi, double z) {
    return M_PI * ((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;
}

// without 1/8pi prefactor
inline void GkernelEwald(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 < eps) {
        auto Gself = -4 * xi / sqrt(M_PI) * 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 * M_PI * i, 2 * M_PI * 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) * (M_PI * (zmn)*std::erf(zmn * xi) + sqrt(M_PI) / (2 * xi) * exp(-zmn * zmn * xi * xi)) *
             Eigen::Matrix3d::Identity();
    waveK0(2, 2) = 0;

    Gsum = real + wave + waveK0;
}

inline double freal(double xi, double r) { return std::erfc(xi * r) / r; }

inline double frealp(double xi, double r) {
    return -(2. * exp(-r * r * (xi * xi)) * xi) / (sqrt(M_PI) * r) - std::erfc(r * xi) / (r * r);
}

inline double gxkz(double xi, double k, double z) {
    return exp(k * z) * std::erfc(xi * z + k / (2 * xi)) + exp(-k * z) * std::erfc(-xi * z + k / (2 * xi));
}

inline double gxkzp(double xi, double k, double z) {
    double pisqrt = sqrt(M_PI);
    return k * exp(k * z) * std::erfc(xi * z + k / (2 * xi)) - k * exp(-k * z) * std::erfc(-xi * z + k / (2 * xi)) +
           (2 * xi / pisqrt) *
               (exp(-pow((-xi * z + k / (2 * xi)), 2) - k * z) - exp(-pow((xi * z + k / (2 * xi)), 2) + k * z));
}

inline void realSum(double xi, const EVec3 &target, const EVec3 &source, EVec3 &v) {

    EVec3 rvec = target - source;
    double rnorm = rvec.norm();
    if (rnorm < eps) {
        v.setZero();
    } else {
        v = (frealp(xi, rnorm) / rnorm) * rvec;
    }
}

inline void Lkernel(const EVec3 &target, const EVec3 &source, EVec3 &answer) {
    EVec3 rst = target - source;
    double rnorm = rst.norm();
    if (rnorm < eps) {
        answer.setZero();
        return;
    }
    double rnorm3 = rnorm * rnorm * rnorm;
    answer = rst / rnorm3;
}

// grad of Laplace potential, without 1/4pi prefactor, periodic of -r_k/r^3
inline void LkernelEwald(const EVec3 &target_, const EVec3 &source_, EVec3 &answer) {
    EVec3 target = target_;
    EVec3 source = source_;
    target[0] = target[0] - floor(target[0]); // periodic BC
    target[1] = target[1] - floor(target[1]);
    source[0] = source[0] - floor(source[0]);
    source[1] = source[1] - floor(source[1]);

    double xi = 2;
    //  real sum
    int rLim = 4;
    EVec3 Kreal = EVec3::Zero();
    for (int i = -rLim; i < rLim + 1; i++) {
        for (int j = -rLim; j < rLim + 1; j++) {
            EVec3 v = EVec3::Zero();
            realSum(xi, target, source - EVec3(i, j, 0), v);
            Kreal += v;
        }
    }

    //  wave sum
    using EVec2 = Eigen::Vector2d;
    int wLim = 4;
    EVec3 rmn = target - source;
    // double xi2 = xi * xi;
    // double rmnnorm = rmn.norm();
    EVec2 rxy(rmn[0], rmn[1]);
    double rz = rmn[2];
    EVec3 Kwave(0, 0, -2 * M_PI * std::erf(xi * rz));
    for (int i = -wLim; i < wLim + 1; i++) {
        for (int j = -wLim; j < wLim + 1; j++) {
            if (i == 0 && j == 0)
                continue;
            EVec2 kvec = EVec2(i, j) * (2 * M_PI);

            double k2 = kvec.dot(kvec);
            double knorm = sqrt(k2);
            double xyfac = -M_PI * sin(kvec.dot(rxy)) * gxkz(xi, knorm, rz) / knorm;
            Kwave[0] += xyfac * kvec[0];
            Kwave[1] += xyfac * kvec[1];
            Kwave[2] += M_PI * cos(kvec.dot(rxy)) * gxkzp(xi, knorm, rz) / knorm;
        }
    }

    answer = Kreal + Kwave;
}

// fx,fy,fz,trD -> p, vx,vy,vz
inline void Wkernel(const EVec3 &target, const EVec3 &source, EMat4 &answer) {
    auto rst = target - source;
    double rnorm = rst.norm();
    if (rnorm < eps) {
        answer.setZero();
        return;
    }
    double rnorm3 = rnorm * rnorm * rnorm;

    answer.block<3, 3>(1, 0) = EMat3::Identity() / rnorm;
    answer.block<3, 3>(1, 0) += rst * rst.transpose() / rnorm3;
    answer(0, 0) = rst[0] / rnorm3;
    answer(0, 1) = rst[1] / rnorm3;
    answer(0, 2) = rst[2] / rnorm3;
    answer(0, 3) = 0;
    answer(1, 3) = -rst[0] / rnorm3;
    answer(2, 3) = -rst[1] / rnorm3;
    answer(3, 3) = -rst[2] / rnorm3;
    answer.row(0) *= (1 / (4 * M_PI));
    answer.block<3, 4>(1, 0) *= (1 / (8 * M_PI));
}

inline void WkernelEwald(const EVec3 &target, const EVec3 &source, EMat4 &answer) {
    EMat3 G = EMat3::Zero();
    GkernelEwald(target - source, G);
    EVec3 L = EVec3::Zero();
    LkernelEwald(target, source, L);
    answer.block<3, 3>(1, 0) = G;
    answer(0, 0) = -L[0];
    answer(0, 1) = -L[1];
    answer(0, 2) = -L[2];
    answer(0, 3) = 0;
    answer(1, 3) = L[0];
    answer(2, 3) = L[1];
    answer(3, 3) = L[2];
    answer.row(0) *= (1 / (4 * M_PI));
    answer.block<3, 4>(1, 0) *= (1 / (8 * M_PI));
}

inline void WkernelFF(const EVec3 &target, const EVec3 &source, EMat4 &answer) {
    EMat4 WEwald = EMat4::Zero();
    WkernelEwald(target, source, WEwald);
    // for (int i = -2 * DIRECTLAYER; i < 2 * DIRECTLAYER + 1; i++) {
    //     for (int j = -2 * DIRECTLAYER; j < 2 * DIRECTLAYER + 1; j++) {
    //         EMat4 W = EMat4::Zero();
    //         Wkernel(target, source + EVec3(i, j, 0), W);
    //         WEwald += W;
    //     }
    // }

    for (int i = -DIRECTLAYER; i < DIRECTLAYER + 1; i++) {
        for (int j = -DIRECTLAYER; j < DIRECTLAYER + 1; j++) {
            EMat4 W = EMat4::Zero();
            Wkernel(target, source + EVec3(i, j, 0), W);
            WEwald -= W;
        }
    }
    answer = WEwald;
}

// calculate the M2L matrix of images from 2 to 1000
int main(int argc, char **argv) {
    Eigen::initParallel();
    Eigen::setNbThreads(1);

    // {
    //     EMat4 G1 = EMat4::Zero(), G2 = EMat4::Zero();
    //     std::vector<EVec3, Eigen::aligned_allocator<EVec3>> forcePoint(3);
    //     std::vector<EVec4, Eigen::aligned_allocator<EVec4>> forceValue(3);
    //     forcePoint[0] = EVec3(0.5, 0.55, 0.2);
    //     forcePoint[1] = EVec3(0.5, 0.5, 0.5);
    //     forcePoint[2] = EVec3(0.7, 0.7, 0.7);
    //     forceValue[0] = EVec4(0.1, 0.2, 0.3, 0.4);
    //     forceValue[1] = EVec4(-0.1, -0.1, -0.3, -0.4);
    //     forceValue[2] = EVec4(0, -0.1, 0, 0);

    //     EVec3 spoint(0.2, 0.3, 0.4);
    //     EVec4 vE, vD;
    //     vE.setZero();
    //     vD.setZero();
    //     for (int i = 0; i < forceValue.size(); i++) {
    //         EMat4 temp = EMat4::Zero();
    //         WkernelEwald(spoint, forcePoint[i], temp);
    //         vE += temp * forceValue[i];
    //     }
    //     const int N = 200;
    //     for (int i = -N; i <= N; i++) {
    //         for (int j = -N; j <= N; j++) {
    //             for (int k = 0; k < forceValue.size(); k++) {
    //                 EMat4 temp = EMat4::Zero();
    //                 Wkernel(spoint, forcePoint[k] + EVec3(i, j, 0), temp);
    //                 vD += temp * forceValue[k];
    //             }
    //         }
    //     }
    //     std::cout << vE.transpose() << std::endl;
    //     std::cout << (vE - vD).transpose() << std::endl;
    //     // std::exit(0);
    // }

    const int pEquiv = atoi(argv[1]); // (8-1)^2*6 + 2 points
    const int pCheck = atoi(argv[1]);
    const double scaleEquiv = 1.05;
    const double scaleCheck = 2.95;
    const double pCenterEquiv[3] = {-(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2};
    const double pCenterCheck[3] = {-(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2};
    auto pointMEquiv = surface(pEquiv, (double *)&(pCenterEquiv[0]), scaleEquiv,
                               0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointMCheck = surface(pCheck, (double *)&(pCenterCheck[0]), scaleCheck,
                               0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    auto pointLEquiv = surface(pEquiv, (double *)&(pCenterCheck[0]), scaleCheck,
                               0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointLCheck = surface(pCheck, (double *)&(pCenterEquiv[0]), scaleEquiv,
                               0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    // calculate the operator M2L with least square
    const int equivN = pointMEquiv.size() / 3;
    const int checkN = pointLCheck.size() / 3;
    Eigen::MatrixXd A(4 * checkN, 4 * equivN);
#pragma omp parallel for
    for (int k = 0; k < checkN; k++) {
        Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1], pointLCheck[3 * k + 2]);
        for (int l = 0; l < equivN; l++) {
            const Eigen::Vector3d Lpoint(pointLEquiv[3 * l], pointLEquiv[3 * l + 1], pointLEquiv[3 * l + 2]);
            EMat4 W = EMat4::Zero();
            Wkernel(Cpoint, Lpoint, W);
            A.block<4, 4>(4 * k, 4 * l) = W;
        }
    }
    Eigen::MatrixXd ApinvU(A.cols(), A.rows());
    Eigen::MatrixXd ApinvVT(A.cols(), A.rows());
    pinv(A, ApinvU, ApinvVT);

    Eigen::MatrixXd M2L(4 * equivN, 4 * equivN);

#pragma omp parallel for
    for (int i = 0; i < equivN; i++) {
        const Eigen::Vector3d Mpoint(pointMEquiv[3 * i], pointMEquiv[3 * i + 1], pointMEquiv[3 * i + 2]);
        Eigen::MatrixXd f(4 * checkN, 4);
        for (int k = 0; k < checkN; k++) {
            EMat4 temp = EMat4::Zero();
            Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1], pointLCheck[3 * k + 2]);
            WkernelFF(Cpoint, Mpoint, temp);
            f.block<4, 4>(4 * k, 0) = temp;
        }
        M2L.block(0, 4 * i, 4 * equivN, 4) = (ApinvU.transpose() * (ApinvVT.transpose() * f));
    }

    // dump M2L
    for (int i = 0; i < 4 * equivN; i++) {
        for (int j = 0; j < 4 * equivN; j++) {
            std::cout << i << " " << j << " " << std::scientific << std::setprecision(18) << M2L(i, j) << std::endl;
        }
    }

    // Test
    // Sum of force and trD must be zero
    std::vector<EVec3, Eigen::aligned_allocator<EVec3>> forcePoint(3);
    std::vector<EVec4, Eigen::aligned_allocator<EVec4>> forceValue(3);
    forcePoint[0] = EVec3(0.5, 0.55, 0.2);
    forcePoint[1] = EVec3(0.5, 0.5, 0.5);
    forcePoint[2] = EVec3(0.7, 0.7, 0.7);
    forceValue[0] = EVec4(0.1, 0.2, 0.3, 0.4);
    forceValue[1] = EVec4(-0.1, -0.1, -0.3, -0.4);
    forceValue[2] = EVec4(0, -0.1, 0, 0);

    // solve M
    A.resize(4 * checkN, 4 * equivN);
    ApinvU.resize(A.cols(), A.rows());
    ApinvVT.resize(A.cols(), A.rows());
    Eigen::VectorXd f(4 * checkN);
#pragma omp parallel for
    for (int k = 0; k < checkN; k++) {
        Eigen::Vector3d Cpoint(pointMCheck[3 * k], pointMCheck[3 * k + 1], pointMCheck[3 * k + 2]);
        EVec4 temp = EVec4::Zero();
        for (int p = 0; p < forceValue.size(); p++) {
            EMat4 W = EMat4::Zero();
            Wkernel(Cpoint, forcePoint[p], W);
            temp += W * (forceValue[p]);
        }
        f.block<4, 1>(4 * k, 0) = temp;
        for (int l = 0; l < equivN; l++) {
            Eigen::Vector3d Mpoint(pointMEquiv[3 * l], pointMEquiv[3 * l + 1], pointMEquiv[3 * l + 2]);
            EMat4 W = EMat4::Zero();
            Wkernel(Cpoint, Mpoint, W);
            A.block<4, 4>(4 * k, 4 * l) = W;
        }
    }
    pinv(A, ApinvU, ApinvVT);
    Eigen::VectorXd Msource = (ApinvU.transpose() * (ApinvVT.transpose() * f));

    std::cout << "Msource: " << Msource << std::endl;
    std::cout << "Msource Sum: " << Msource.sum() << std::endl;
    Eigen::VectorXd M2Lsource = M2L * (Msource);

    {
        EVec3 samplePoint(0.5, 0.2, 0.8);
        // Compute: WFF from L, WFF from WkernelFF
        EVec4 WFFL = EVec4::Zero();
        EVec4 WFFK = EVec4::Zero();

        for (int k = 0; k < equivN; k++) {
            EVec3 Lpoint(pointLEquiv[3 * k], pointLEquiv[3 * k + 1], pointLEquiv[3 * k + 2]);
            EMat4 W = EMat4::Zero();
            Wkernel(samplePoint, Lpoint, W);
            WFFL += W * M2Lsource.block<4, 1>(4 * k, 0);
        }

        for (int k = 0; k < forceValue.size(); k++) {
            EMat4 W;
            WkernelFF(samplePoint, forcePoint[k], W);
            WFFK += W * forceValue[k];
        }
        std::cout << "WFF from Lequiv: " << WFFL << std::endl;
        std::cout << "WFF from Kernel: " << WFFK << std::endl;
        std::cout << "FF Error: " << WFFL - WFFK << std::endl;
    }

    return 0;
}

} // namespace StokesPVel2D3D

#undef DIRECTLAYER


//==================================================================================================
/*!
  @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_SCALAR_FUNCTION_ERFC_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ERFC_HPP_INCLUDED

#include <boost/simd/function/std.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/arch/common/detail/generic/erf_kernel.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/sqrt.hpp>
#include <boost/simd/function/rec.hpp>
#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/function/is_nan.hpp>
#endif
#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/constant/inf.hpp>
#endif
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( erfc_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::single_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 a0) const BOOST_NOEXCEPT
    {
      A0 x =  bs::abs(a0);
      A0 r1 = Zero<A0>();
      A0 z =  x/inc(x);
      if (x < Ratio<A0, 2, 3>())
      {
        r1 = detail::erf_kernel<A0>::erfc3(z);
      }
      #ifndef BOOST_SIMD_NO_INFINITIES
      else if (BOOST_UNLIKELY(x == Inf<A0>()))
      {
        r1 = Zero<A0>();
      }
      #endif
      else
      {
       z-= 0.4f;
       r1 = exp(-sqr(x))*detail::erf_kernel<A0>::erfc2(z);
      }
      return (a0 < 0.0f) ? 2.0f-r1 : r1;
    }
  };
  BOOST_DISPATCH_OVERLOAD ( erfc_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::double_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 x) const BOOST_NOEXCEPT
    {
      #ifndef BOOST_SIMD_NO_INVALIDS
      if(is_nan(x)) return x;
      #endif
      A0 y =  bs::abs(x);
      if (y <= Ratio<A0, 15, 32>()) // 0.46875
      {
        A0 res = detail::erf_kernel1<A0>::erf1(x, y);
        res =  oneminus(res);
        return res;
      }
      else if (y <= Ratio<A0, 4>())
      {
        A0 res = detail::erf_kernel1<A0>::erf2(x, y);
        res =    detail::erf_kernel1<A0>::finalize2(res, y);
        if (is_ltz(x)) res = Two<A0>()-res;
        return res;
      }
      else if  (y <= 26.543)
      {
        A0 res = detail::erf_kernel1<A0>::erf3(x, y);
        res =    detail::erf_kernel1<A0>::finalize2(res, y);
        if (is_ltz(x)) res = Two<A0>()-res;
        return res;
      }
      else return Zero<A0>();
    }
  };
  BOOST_DISPATCH_OVERLOAD ( erfc_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &, A0 a0) const BOOST_NOEXCEPT
    {
      return std::erfc(a0);
    }
  };
} } }

#endif


///////////////////////////////////////////////////////////////////////////////
// distribution::model::models::posterior::log_unnormalized_pdf.hpp          //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_MODEL_MODELS_POSTERIOR_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_MODEL_MODELS_POSTERIOR_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#include <numeric>
#include <boost/range.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/function.hpp>
#include <boost/statistics/detail/distribution_common/meta/value.hpp>
#include <boost/statistics/detail/distribution_common/functor/log_unnormalized_pdf.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace model{

    template<typename Pr,typename L,typename P>
    typename distribution::meta::value<
        distribution::model::posterior<Pr,L>
    >::type
    log_unnormalized_pdf(
        const distribution::model::posterior<Pr,L>& post,
        const P& p
    )
    {
        typedef typename distribution::meta::value<
            distribution::model::posterior<Pr,L>
        >::type val_;
        val_ val = log_unnormalized_pdf(post.prior(),p);
        val += log_unnormalized_pdf(static_cast<const L&>(post),p);
        return val;

    }

}// model
}// distribution
}// detail
}// statistics
}// boost

#endif


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

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

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

#include <Eigen/Core>

#include <igl/boundary_loop.h>
#include <igl/lscm.h>
#include <igl/MappingEnergyType.h>
#include <igl/map_vertices_to_circle.h>
#include <igl/harmonic.h>
#include <igl/flipped_triangles.h>
#include <igl/topological_hole_fill.h>
#include <igl/scaf.h>

namespace ork::meshutil {
//////////////////////////////////////////////////////////////////////////////
Eigen::MatrixXd IglMesh::parameterizeHarmonic() const {
  Eigen::MatrixXd harmonic_uvs;
  // Find the open boundary
  Eigen::VectorXi bnd;
  igl::boundary_loop(_faces, bnd);

  // Map the boundary to a circle, preserving edge proportions
  Eigen::MatrixXd bnd_uv;
  igl::map_vertices_to_circle(_verts, bnd, bnd_uv);

  // Harmonic parametrization for the internal vertices
  igl::harmonic(_verts, _faces, bnd, bnd_uv, 1, harmonic_uvs);
  return harmonic_uvs;
}
//////////////////////////////////////////////////////////////////////////////
Eigen::MatrixXd IglMesh::parameterizeLCSM() {
  Eigen::MatrixXd lcsm_uvs;
  // Fix two points on the boundary
  Eigen::VectorXi bnd, b(2, 1);
  igl::boundary_loop(_faces, bnd);
  b(0) = bnd(0);
  b(1) = bnd(bnd.size() / 2);
  Eigen::MatrixXd bc(2, 2);
  bc << 0, 0, 1, 0;
  // LSCM parametrization
  igl::lscm(_verts, _faces, b, bc, lcsm_uvs);
  return lcsm_uvs;
}
//////////////////////////////////////////////////////////////////////////////
iglmesh_ptr_t IglMesh::parameterizedSCAF(int numiters, double scale, double bias) const {

  Eigen::MatrixXd V = _verts;
  Eigen::MatrixXi F = _faces;
  igl::SCAFData scaf_data;

  Eigen::MatrixXd bnd_uv, uv_init;

  Eigen::VectorXd M;
  igl::doublearea(V, F, M);
  std::vector<std::vector<int>> all_bnds;
  igl::boundary_loop(F, all_bnds);

  printf("numbnds<%zu>\n", all_bnds.size());

  // Heuristic primary boundary choice: longest
  auto primary_bnd = std::max_element(
      all_bnds.begin(), all_bnds.end(), [](const std::vector<int>& a, const std::vector<int>& b) { return a.size() < b.size(); });

  OrkAssert(primary_bnd != all_bnds.end()); // see https://github.com/libigl/libigl/issues/873

  Eigen::VectorXi bnd = Eigen::Map<Eigen::VectorXi>(primary_bnd->data(), primary_bnd->size());

  igl::map_vertices_to_circle(V, bnd, bnd_uv);
  bnd_uv *= sqrt(M.sum() / (2 * igl::PI));
  if (all_bnds.size() == 1) {
    if (bnd.rows() == V.rows()) // case: all vertex on boundary
    {
      uv_init.resize(V.rows(), 2);
      for (int i = 0; i < bnd.rows(); i++)
        uv_init.row(bnd(i)) = bnd_uv.row(i);
    } else {
      igl::harmonic(V, F, bnd, bnd_uv, 1, uv_init);
      if (igl::flipped_triangles(uv_init, F).size() != 0)
        igl::harmonic(F, bnd, bnd_uv, 1, uv_init); // fallback uniform laplacian
    }
  } else {
    // if there is a hole, fill it and erase additional vertices.
    all_bnds.erase(primary_bnd);
    Eigen::MatrixXi F_filled;
    igl::topological_hole_fill(F, bnd, all_bnds, F_filled);
    igl::harmonic(F_filled, bnd, bnd_uv, 1, uv_init);
    uv_init.conservativeResize(V.rows(), 2);
  }

  Eigen::VectorXi b;
  Eigen::MatrixXd bc;
  igl::scaf_precompute(V, F, uv_init, scaf_data, igl::MappingEnergyType::SYMMETRIC_DIRICHLET, b, bc, 0);

  //_verts = V;
  //_faces = F;

  igl::scaf_solve(scaf_data, numiters);

  auto rval                           = std::make_shared<IglMesh>(V, F);
  double uv_scale                     = 0.2 * 0.5 * scale;
  double uv_bias                      = 0.5 + bias;
  Eigen::MatrixXd scaledandbiased_uvs = uv_scale * scaf_data.w_uv.topRows(V.rows());
  size_t num_uvs                      = scaledandbiased_uvs.rows();
  for (size_t i = 0; i < num_uvs; i++) {
    scaledandbiased_uvs(i, 0) = scaledandbiased_uvs(i, 0) + uv_bias; // U
    scaledandbiased_uvs(i, 1) = scaledandbiased_uvs(i, 1) + uv_bias; // V
  }
  rval->_uvs = scaledandbiased_uvs;
  return rval;
}
//////////////////////////////////////////////////////////////////////////////
} // namespace ork::meshutil

#endif


/*
 * Chaos
 *
 * Copyright 2015 Operating Systems Laboratory EPFL
 *
 * 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 _ALS_GRAPHCHI_
#define _ALS_GRAPHCHI_

#include "../../utils/options_utils.h"
#include "../../utils/desc_utils.h"
#include "../../utils/boost_log_wrapper.h"
#include "../../core/x-lib.hpp"
#include <cmath>
#include <boost/random.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/bindings/lapack/gesv.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_vector2.hpp>

#define LAMBDA 0.065
#define RANK   5

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;

//typedef ublas::bounded_array<double, RANK> storage_t;
//typedef ublas::vector<double, storage_t> vector_t;
//typedef ublas::matrix<double, ublas::row_major, storage_t> matrix_t;

namespace algorithm {
  namespace sg_simple {
    class als_graphchi_per_processor_data : public per_processor_data {
    public:
        static double sse;
        double sse_local;
        bool local_continue;

        als_graphchi_per_processor_data()
            : sse_local(0.0), local_continue(false) { }

        bool reduce(per_processor_data **per_cpu_array,
                    unsigned long processors) {
          bool global_continue = false;
          for (unsigned long i = 0; i < processors; i++) {
            als_graphchi_per_processor_data *data =
                static_cast<als_graphchi_per_processor_data *>(per_cpu_array[i]);
            sse += data->sse_local;
            data->sse_local = 0;
            global_continue = global_continue || data->local_continue;
            data->local_continue = false;
          }
          return !global_continue;
        }
    } __attribute__((__aligned__(64)));

    template<typename F>
    class als_graphchi_factorization {

    private:

        struct vertex {
            bool rightside;
            vertex_t degree;
            vertex_t count;
            double feature_vec[2][RANK];
            double temp_mat[RANK][RANK];
        } __attribute__((__packed__));

        struct update {
            vertex_t target;
            double feature_vec[RANK];
            double rating;
        } __attribute__((__packed__));

        static unsigned long niters;

        // Helpers
        static void copy_vector(double src_vec[RANK], double dst_vec[RANK]) {
          for (int i = 0; i < RANK; i++)
            dst_vec[i] = src_vec[i];
        }

        static void zero_vector(double vec[RANK]) {
          for (int i = 0; i < RANK; i++)
            vec[i] = 0;
        }

        static void zero_matrix(double mat[RANK][RANK]) {
          for (int i = 0; i < RANK; i++)
            for (int j = 0; j < RANK; j++)
              mat[i][j] = 0;
        }

        static void init_vertex(struct vertex &v) {
          v.rightside = false;
          v.degree = 0;
          v.count = 0;

          boost::mt19937 generator;
          boost::uniform_int<> distribution(0, 1000);
          for (int i = 0; i < RANK; i++) {
            v.feature_vec[0][i] = 0.001 * distribution(generator);
            v.feature_vec[1][i] = 0.001 * distribution(generator);
          }
        }

        // Solve the system Ax=b using LAPACK library and boost bindings for it
        // This has some copying that should somehow be removed
        static void solve(double mat[RANK][RANK], double vec[RANK]) {
          ublas::matrix<double, ublas::column_major> A(RANK, RANK);
          ublas::vector<double> b(RANK);

          for (int i = 0; i < RANK; i++) {
            b(i) = vec[i];
            for (int j = 0; j < RANK; j++) {
              A(i, j) = mat[i][j];
            }
          }

          lapack::gesv(A, b);

          for (int i = 0; i < RANK; i++)
            vec[i] = b(i);
        }

    public:
        static unsigned long vertex_state_bytes() {
          return sizeof(struct vertex);
        }

        static unsigned long split_size_bytes() {
          return sizeof(struct update);
        }

        static unsigned long split_key(unsigned char *buffer, unsigned long jump) {
          struct update *u = (struct update *) buffer;
          vertex_t key = u->target;
          key = key >> jump;
          return key;
        }

        static bool init(unsigned char *vertex_state,
                         unsigned long vertex_index,
                         unsigned long bsp_phase,
                         per_processor_data *cpu_state) {
          struct vertex *vertices = (struct vertex *) vertex_state;
          init_vertex(*vertices);
          return true;
        }

        static void apply_one_update(unsigned char *vertex_state,
                                     unsigned char *update_stream,
                                     per_processor_data *per_cpu_data,
                                     bool loopback,
                                     unsigned long bsp_phase) {
          struct update *u = (struct update *) update_stream;
          struct vertex *vertices = (struct vertex *) vertex_state;
          struct vertex *v = &vertices[x_lib::configuration::map_offset(u->target)];

          if (loopback)
            bsp_phase++;

          // In even phases gather to vector 0, in odd to vector 1
          // This is required because scatter and loopback part of the gather happen simultaneously
          int which = (bsp_phase % 2 == 0) ? 0 : 1;

          if (bsp_phase <= niters) {
            // Track how many updates (edges) have been processed
            v->count++;

            // Initialize data structures for this iteration - zero out feature vector
            // and temp matrix since we'll be adding to them.
            if (v->count == 1) {
              zero_vector(v->feature_vec[which]);
              zero_matrix(v->temp_mat);
            }

            // To compute the new feature vector of vertex v, we need to solve the system: A*feature_vec=b
            // A is a matrix: A = O * O^T + D
            // O is a submatrix of the other side of the graph where column vectors are feature vectors of
            // those vertices that are connected to this vertex
            // O^T is a transpose of O
            // D is a diagonal matrix: D = lambda * degree * I, where I is an identity matrix
            // b is a vector: b = O * r
            // r is a ratings vector formed from the ratings of outgoing edges

            // Calculating O*O^T and b
            for (int i = 0; i < RANK; i++) {
              v->feature_vec[which][i] += u->feature_vec[i] * u->rating;
              for (int j = 0; j < RANK; j++) {
                v->temp_mat[i][j] += u->feature_vec[i] * u->feature_vec[j];
              }
            }

            // Additional procesing after all updates have been gathered
            if (v->count == v->degree) {
              // Adding D to A
              for (int i = 0; i < RANK; i++) {
                v->temp_mat[i][i] += LAMBDA * v->degree;
              }

              // Solve to get the new feature vector for the vertex
              solve(v->temp_mat, v->feature_vec[which]);

              // Reset count for the next iteration
              v->count = 0;
            }
          }

            // After all iterations are finished, we use one more phase to compute the sum of square errors.
            // This is done only on one side (right) so we don't add the error twice.
          else {
            if (v->rightside) {
              double sqerror = u->rating;
              for (int i = 0; i < RANK; i++)
                sqerror -= v->feature_vec[1 - which][i] * u->feature_vec[i];
              sqerror *= sqerror;

              static_cast<als_graphchi_per_processor_data *>(per_cpu_data)->sse_local += sqerror;
            }
          }
        }

        static bool generate_update(unsigned char *vertex_state,
                                    unsigned char *edge_format,
                                    unsigned char *update_stream,
                                    per_processor_data *per_cpu_data,
                                    unsigned long bsp_phase) {
          vertex_t src, dst;
          weight_t rating;
          F::read_edge(edge_format, src, dst, rating);

          struct vertex *vertices = (struct vertex *) vertex_state;
          struct vertex *v = &vertices[x_lib::configuration::map_offset(src)];

          // Iteration 0 is used to count the vertex degree and determine its side.
          // The graph is bipartite, and it is assumed that lower ids form the left side.
          if (bsp_phase == 0) {
            v->rightside = (src > dst) ? true : false;
            v->degree++;
            return false;
          }

          // In even phases scatter to vector 0, in odd to vector 1
          // This is required because scatter and loopback part of the gather happen simultaneously
          int which = (bsp_phase % 2 == 0) ? 0 : 1;

          // In the last iteration it is enough to send only the updates from the left side
          if (bsp_phase < niters || (bsp_phase == niters && !v->rightside)) {
            struct update *u = (struct update *) update_stream;
            u->target = dst;
            u->rating = (double) rating;
            copy_vector(v->feature_vec[which], u->feature_vec);

            // Continue processing in the next superstep
            static_cast<als_graphchi_per_processor_data *>(per_cpu_data)->local_continue = true;
            return true;
          }
          else
            return false;
        }

        static void preprocessing() {
          // The extra +1 is for the iteration 0 which is used just to compute the vertex degrees
          niters = 1 + vm["als::niters"].as < unsigned
          long > ();
        }

        static void postprocessing() {
          BOOST_LOG_TRIVIAL(info) << "ALGORITHM::ALS::SSE " << als_graphchi_per_processor_data::sse;
          unsigned long nedges = pt.get < unsigned
          long > ("graph.edges");
          double rmse = std::sqrt(als_graphchi_per_processor_data::sse / (1. * nedges));
          BOOST_LOG_TRIVIAL(info) << "ALGORITHM::ALS::RMSE " << rmse;
        }

        static per_processor_data *
        create_per_processor_data(unsigned long processor_id) {
          return new als_graphchi_per_processor_data();
        }

        static unsigned long min_super_phases() {
          return 1;
        }

        static bool need_init(unsigned long bsp_phase) {
          return (bsp_phase == 0);
        }

    };

    // These should be in a cpp file, but it's ok since we only include
    // this header once in driver.cpp
    template<typename F>
    unsigned long als_graphchi_factorization<F>::niters;

    double als_graphchi_per_processor_data::sse = 0;

  }
}
#endif