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

#include <NTL/new.h>

NTL_START_IMPL




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

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

   MU = MU1;

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

      return;
   }

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

      return;
   }

   if (MU == 0) return;

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


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

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

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

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

   MU = MU1;

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

      return;
   }

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

      return;
   }

   if (MU == 0) return;

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

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

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


class GivensCache_XD {
public:
   GivensCache_XD(long m, long n);
   ~GivensCache_XD();

   void flush();
   void selective_flush(long l);
   void swap(long l);
   void swap();
   void touch();
   void incr();

   long sz;

   xdouble **buf;
   long *bl;
   long *bv;
   long bp;
};


GivensCache_XD::GivensCache_XD(long m, long n)
{
   sz = min(m, n)/10;
   if (sz < 2) 
      sz = 2;
   else if (sz > 20)
      sz = 20;

   typedef xdouble *xdoubleptr;

   long i;
   buf = NTL_NEW_OP xdoubleptr[sz]; 
   if (!buf) Error("out of memory");
   for (i = 0; i < sz; i++)
      if (!(buf[i] = NTL_NEW_OP xdouble[n+1])) Error("out of memory");

   bl = NTL_NEW_OP long[sz];
   if (!bl) Error("out of memory");
   for (i = 0; i < sz; i++) bl[0] = 0;

   bv = NTL_NEW_OP long[sz];
   if (!bv) Error("out of memory");
   for (i = 0; i < sz; i++) bv[0] = 0;

   bp = 0;
}

GivensCache_XD::~GivensCache_XD()
{
   long i;

   for (i = 0; i < sz; i++) delete [] buf[i];
   delete [] buf;
   delete [] bl;
   delete [] bv;
}

void GivensCache_XD::flush()
{
   long i;
   for (i = 0; i < sz; i++) bl[i] = 0;
}

void GivensCache_XD::selective_flush(long l)
{
   long i;

   for (i = 0; i < sz; i++)
      if (bl[i] && bv[i] >= l)
         bl[i] = 0;
}

void GivensCache_XD::swap(long l)
{
   long k = bl[bp];
   long i;

   i = 0;
   while (i < sz && bl[i] != l)
      i++;

   if (i < sz) {
      bl[bp] = l;
      bl[i] = k;
   }
   else
      bl[bp] = l;

   selective_flush(l);
}

void GivensCache_XD::swap()
{
   swap(bl[bp] - 1);
}

void GivensCache_XD::touch()
{
   long k = bl[bp];
   bl[bp] = 0;
   selective_flush(k);
}

void GivensCache_XD::incr()
{
   long k = bl[bp];
   long k1 = k+1;
   long i;

   i = 0;
   while (i < sz && bl[i] != k1)
      i++;

   if (i < sz) {
      bp = i;
      return;
   }

   i = 0; 
   while (i < sz && bl[i] != 0)
      i++;

   if (i < sz) {
      bp = i;
      return;
   }

   long max_val = 0;
   long max_index = 0;
   for (i = 0; i < sz; i++) {
      long t = labs(bl[i]-k1);
      if (t > max_val) {
         max_val = t;
         max_index = i;
      }
   }

   bp = max_index;
   bl[max_index] = 0;
}


static
void GivensComputeGS(xdouble **B1, xdouble **mu, xdouble **aux, long k, long n,
                     GivensCache_XD& cache)
{
   long i, j;

   xdouble c, s, a, b, t;

   xdouble *p = mu[k];

   xdouble *pp = cache.buf[cache.bp];

   if (!cache.bl[cache.bp]) {
      for (j = 1; j <= n; j++)
         pp[j] = B1[k][j];

      long backoff;
      backoff = k/4;
      if (backoff < 2)
         backoff = 2;
      else if (backoff > cache.sz + 2)
         backoff = cache.sz + 2; 

      long ub = k-(backoff-1);

      for (i = 1; i < ub; i++) {
         xdouble *cptr = mu[i];
         xdouble *sptr = aux[i];
   
         for (j = n; j > i; j--) {
            c = cptr[j];
            s = sptr[j];
   
            a = c*pp[j-1] - s*pp[j];
            b = s*pp[j-1] + c*pp[j];
   
            pp[j-1] = a;
            pp[j] = b;
         }
   
         pp[i] = pp[i]/mu[i][i]; 
      }

      cache.bl[cache.bp] = k;
      cache.bv[cache.bp] = k-backoff;
   }

   for (j = 1; j <= n; j++)
      p[j] = pp[j];

   for (i = max(cache.bv[cache.bp]+1, 1); i < k; i++) {
      xdouble *cptr = mu[i];
      xdouble *sptr = aux[i];
  
      for (j = n; j > i; j--) {
         c = cptr[j];
         s = sptr[j];
  
         a = c*p[j-1] - s*p[j];
         b = s*p[j-1] + c*p[j];
  
         p[j-1] = a;
         p[j] = b;
      }
  
      p[i] = p[i]/mu[i][i];
   }

   for (j = n; j > k; j--) {
      a = p[j-1];
      b = p[j];

      if (b == 0) {
         c = 1;
         s = 0;
      }
      else if (fabs(b) > fabs(a)) {
         t = -a/b;
         s = 1/sqrt(1 + t*t);
         c = s*t;
      }
      else {
         t = -b/a;
         c = 1/sqrt(1 + t*t);
         s = c*t;
      }
   
      p[j-1] = c*a - s*b;
      p[j] = c;
      aux[k][j] = s;
   }

   if (k > n+1) Error("G_LLL_XD: internal error");
   if (k > n) p[k] = 0;
}

static xdouble red_fudge = to_xdouble(0);
static long log_red = 0;

static void init_red_fudge()
{
   long i;

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

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

static void inc_red_fudge()
{

   red_fudge = red_fudge * 2;
   log_red--;

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

   if (log_red < 4)
      Error("G_LLL_XD: can not continue...sorry");
}



static long verbose = 0;

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



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

   ZZ t1;
   long i;
   double prodlen = 0;

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

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

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

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

      f.close();

      cerr << "\n";
   }

   LastTime = t;
   
}


static
long ll_G_LLL_XD(mat_ZZ& B, mat_ZZ* U, xdouble delta, long deep, 
           LLLCheckFct check, xdouble **B1, xdouble **mu, 
           xdouble **aux,
           long m, long init_k, long &quit, GivensCache_XD& cache)
{
   long n = B.NumCols();

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

   xdouble t1;
   ZZ T1;
   xdouble *tp;


   xdouble half = to_xdouble(0.5);
   xdouble half_plus_fudge = 0.5 + red_fudge;

   quit = 0;
   k = init_k;

   long counter;

   long trigger_index;
   long small_trigger;
   long cnt;

   long max_k = 0;

   double tt;

   cache.flush();

   while (k <= m) {

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

      if (verbose) {
         tt = GetTime();

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


      GivensComputeGS(B1, mu, aux, k, n, cache);

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

      do {
         // size reduction

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


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

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

                     cnt++;

                     if (cnt > 10) {
                        inc_red_fudge();
                        half_plus_fudge = 0.5 + red_fudge;
                        cnt = 0;
                     }
                  }

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


               Fc1 = 1;
   
               mu1 = mu[k][j];
               if (mu1 >= 0)
                  mu1 = ceil(mu1-half);
               else
                  mu1 = floor(mu1+half);
   
   
               xdouble *mu_k = mu[k];
               xdouble *mu_j = mu[j];
  
               if (mu1 == 1) {
                  for (i = 1; i <= j-1; i++)
                     mu_k[i] -= mu_j[i];
               }
               else if (mu1 == -1) {
                  for (i = 1; i <= j-1; i++)
                     mu_k[i] += mu_j[i];
               }
               else {
                  for (i = 1; i <= j-1; i++)
                     MulSub(mu_k[i], mu_k[i], mu1, mu_j[i]);
               }
  
               mu_k[j] -= mu1;

               conv(MU, mu1);

               // cout << j << " " << MU << "\n";
   
               RowTransform(B(k), B(j), MU);
               if (U) RowTransform((*U)(k), (*U)(j), MU);
            }
         }

         if (Fc1) {
            for (i = 1; i <= n; i++)
               conv(B1[k][i], B(k, i));
            cache.touch();
            GivensComputeGS(B1, mu, aux, k, n, cache);
         }
      } while (Fc1);

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

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

         cache.flush();

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

      if (quit) break;

      if (deep > 0) {
         // deep insertions
   
         Error("sorry...deep insertions not implemented");
      } // end deep insertions

      // test G_LLL reduction condition

      if (k > 1 && 
         (delta - mu[k][k-1]*mu[k][k-1])*(mu[k-1][k-1])*(mu[k-1][k-1]) >
         (mu[k][k])*(mu[k][k])) {

         // swap rows k, k-1
         swap(B(k), B(k-1));
         tp = B1[k]; B1[k] = B1[k-1]; B1[k-1] = tp;
         if (U) swap((*U)(k), (*U)(k-1));

         cache.swap();

         k--;
         NumSwaps++;

         // cout << "- " << k << "\n";
      }
      else {
         cache.incr();
         k++;
         // cout << "+ " << k << "\n";
      }
   }

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


   return m;
}




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

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

   xdouble t1;
   ZZ T1;

   init_red_fudge();

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

   xdouble **B1;  // approximates B

   typedef xdouble *xdoubleptr;

   B1 = NTL_NEW_OP xdoubleptr[m+1];
   if (!B1) Error("G_LLL_XD: out of memory");

   for (i = 1; i <= m; i++) {
      B1[i] = NTL_NEW_OP xdouble[n+1];
      if (!B1[i]) Error("G_LLL_XD: out of memory");
   }

   xdouble **mu;
   mu = NTL_NEW_OP xdoubleptr[m+1];
   if (!mu) Error("G_LLL_XD: out of memory");

   for (i = 1; i <= m; i++) {
      mu[i] = NTL_NEW_OP xdouble[n+2];
      if (!mu[i]) Error("G_LLL_XD: out of memory");
   }

   xdouble **aux;
   aux = NTL_NEW_OP xdoubleptr[m+1];
   if (!aux) Error("G_LLL_XD: out of memory");

   for (i = 1; i <= m; i++) {
      aux[i] = NTL_NEW_OP xdouble[n+1];
      if (!aux[i]) Error("G_LLL_XD: out of memory");
   }

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

   GivensCache_XD cache(m, n);

   new_m =
      ll_G_LLL_XD(B, U, delta, deep, check, B1, mu, aux, m, 1, quit, cache);

   dep = m - new_m;
   m = new_m;

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

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