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


#include <NTL/new.h>

NTL_START_IMPL


static inline
void CheckFinite(double *p)
{
   if (!IsFinite(p)) Error("G_LLL_QP: numbers too big...use G_LLL_XD");
}


static inline
void CheckFinite(quad_float *p)
{
   if (!IsFinite(p)) Error("G_LLL_QP: numbers too big...use G_LLL_XD");
}



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

#define TR_BND (NTL_FDOUBLE_PRECISION/2.0)
// Just to be safe!!

static double max_abs(quad_float *v, long n)
{
   long i;
   double res, t;

   res = 0;

   for (i = 1; i <= n; i++) {
      t = fabs(v[i].hi);
      if (t > res) res = t;
   }

   return res;
}


static void RowTransformStart(quad_float *a, long *in_a, long& in_float, long n)
{
   long i;
   long inf = 1;

   for (i = 1; i <= n; i++) {
      in_a[i] = (a[i].hi < TR_BND && a[i].hi > -TR_BND);
      inf = inf & in_a[i];
   }

   in_float = inf;
}


static void RowTransformFinish(vec_ZZ& A, quad_float *a, long *in_a)
{
   long n = A.length();
   long i;

   for (i = 1; i <= n; i++) {
      if (in_a[i])  {
         conv(A(i), a[i].hi);
      }
      else {
         conv(a[i], A(i));
         CheckFinite(&a[i]);
      }
   }
}



   



static void RowTransform(vec_ZZ& A, vec_ZZ& B, const ZZ& MU1, 
                         quad_float *a, quad_float *b, long *in_a, 
                         double& max_a, double max_b, long& in_float)
// x = x - y*MU
{
   static ZZ T, MU;
   long k;
   double mu;


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

   conv(mu, MU1);
   CheckFinite(&mu);

   if (in_float) {
      double mu_abs = fabs(mu);
      if (mu_abs > 0 && max_b > 0 && (mu_abs >= TR_BND || max_b >= TR_BND)) {
         in_float = 0;
      }
      else {
         max_a += mu_abs*max_b;
         if (max_a >= TR_BND)
            in_float = 0;
      }
   }

   if (in_float) {
      if (mu == 1) {
         for (i = 1; i <= n; i++)
            a[i].hi -= b[i].hi;

         return;
      }

      if (mu == -1) {
         for (i = 1; i <= n; i++)
            a[i].hi += b[i].hi;

         return;
      }

      if (mu == 0) return;

      for (i = 1; i <= n; i++)
         a[i].hi -= mu*b[i].hi;


      return;
   }

   MU = MU1;

   if (MU == 1) {
      for (i = 1; i <= n; i++) {
         if (in_a[i] && a[i].hi < TR_BND && a[i].hi > -TR_BND &&
             b[i].hi < TR_BND && b[i].hi > -TR_BND) {

            a[i].hi -= b[i].hi;
         }
         else {
            if (in_a[i]) {
               conv(A(i), a[i].hi);
               in_a[i] = 0;
            }

            sub(A(i), A(i), B(i));
         }
      }

      return;
   }

   if (MU == -1) {
      for (i = 1; i <= n; i++) {
         if (in_a[i] && a[i].hi < TR_BND && a[i].hi > -TR_BND &&
             b[i].hi < TR_BND && b[i].hi > -TR_BND) {

            a[i].hi += b[i].hi;
         }
         else {
            if (in_a[i]) {
               conv(A(i), a[i].hi);
               in_a[i] = 0;
            }

            add(A(i), A(i), B(i));
         }
      }

      return;
   }

   if (MU == 0) return;

   double b_bnd = fabs(TR_BND/mu) - 1;
   if (b_bnd < 0) b_bnd = 0;


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


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

      if (k > 0) {
         for (i = 1; i <= n; i++) {
            if (in_a[i]) {
               conv(A(i), a[i].hi);
               in_a[i] = 0;
            }

            mul(T, B(i), mu1);
            LeftShift(T, T, k);
            sub(A(i), A(i), T);
         }
      }
      else {
         for (i = 1; i <= n; i++) {
            if (in_a[i] && a[i].hi < TR_BND && a[i].hi > -TR_BND &&
                b[i].hi < b_bnd && b[i].hi > -b_bnd) {
  
               a[i].hi -= b[i].hi*mu;
            }
            else {
               if (in_a[i]) {
                  conv(A(i), a[i].hi);
                  in_a[i] = 0;
               }
               mul(T, B(i), mu1);
               sub(A(i), A(i), T);
           }
         }
      }
   }
   else {
      for (i = 1; i <= n; i++) {
         if (in_a[i]) {
            conv(A(i), a[i].hi);
            in_a[i] = 0;
         }
         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_QP {
public:
   GivensCache_QP(long m, long n);
   ~GivensCache_QP();

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

   long sz;

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


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

   typedef quad_float *quad_floatptr;

   long i;
   buf = NTL_NEW_OP quad_floatptr[sz]; 
   if (!buf) Error("out of memory");
   for (i = 0; i < sz; i++)
      if (!(buf[i] = NTL_NEW_OP quad_float[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_QP::~GivensCache_QP()
{
   long i;

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

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

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

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

void GivensCache_QP::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_QP::swap()
{
   swap(bl[bp] - 1);
}

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

void GivensCache_QP::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(quad_float **B1, quad_float **mu, quad_float **aux, long k, long n,
                     GivensCache_QP& cache)
{
   long i, j;

   quad_float c, s, a, b, t;

   quad_float *p = mu[k];

   quad_float *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++) {
         quad_float *cptr = mu[i];
         quad_float *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++) {
      quad_float *cptr = mu[i];
      quad_float *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];