#include <NTL/FFT.h>
#include <NTL/new.h>

/********************************************************************

This is an implementation of a "small prime" FFT, which lies at the heart of
the ZZ_pX arithmetic, as well as some other applications.

The basic algorithm is loosely based on the routine in the Cormen, Leiserson,
Rivest, and Stein book on algorithms.

CACHE PERFORMANCE

Some attention has been paid to cache performance, but there is still more that
could be done.

The bit-reverse-copy (BRC) algorithm is a simple table-driven algorithm up to
a certain theshold, and then switches to the COBRA algorithm from  Carter and
Gatlin, "Towards an optimal bit-reversal permutation algorithm", FOCS 1998.
I've found that COBRA helps, but not much: just 5-10%.  I've also found that
getting rid of BRC altogether leads to another 5-10% improvement.  These
numbers are based on experiments with 2^{17}- and 2^{19}-point FFTs, looping
over 50 different primes on a Core 2 Duo machine.

One could get rid of bit-reverse-copy altogether.  The current FFT routines all
implement what is called Decimation-In-Time (DIT), which means that inputs are
bit reversed.  One can also implement the FFT using Decimation-In-Frequency
(DIF), which means that the outputs are bit reversed.  One can get rid of the
bit reversals for doing convolutions by simply doing the forward FFT using
DIF-FFT and and the reverse FFT using DIT-FFT.  This would allow one to simply
eliminate all of the bit-reversal steps, which would lead to some nontrivial
savings.  However, there are a few places in NTL where I rely on the ordering
of elements within an FFTRep to be their "natural ordering".  The reduce and
AddExpand routines in ZZ_pX come to mind (which actually may become simpler),
along with like RevToFFTRep and RevFromFFTRep (which may be trickier).  Anyway,
because BRC doesn't seem to be a big problem right now, it doesn't seem worth
worrying about this.

Within the FFT algorithm itself, I have not tried anything like Bailey's 4-step
algorithm.  Maybe this should be tested.  However, I somehow doubt that
anything more than modest gains will be achieved, since most modern processors
now employ a kind of memory prefetching technique, to keep the cache filled
with memory locations that are likely to be used next.  Moreover, the FFT
algorithm used here accesses memory for the most part in small, sequential
strides, which meshes well with hardware prefetching.  The paper "Algorithms to
Take Advantage of Hardware Prefetching" [Pan, Cherng, Dick, Ladner, Workshop on
Algorithm Engineering and Experiments, 2007] contains some interesting
experiments and useful background information.  Anyway, there is still room for
more experimentation.

SINGLE-PRECISION MODULAR ARITHMETIC

The implementation of arithmetic modulo n, where n is a "word sized" integer is
critical to the performance of the FFT.  Such word-sized modular arithmetic is
used throughout many other parts of NTL, and is a part of the external,
documented interface.

As NTL was initially built on top of Arjen Lenstra's LIP software, I stole a
lot of ideas from LIP.  One very nice ideas was LIP's way of handling
single-precision modular arithmetic.  Back in those days (the early 1990's), I
was targeting 32-machines, mainly SPARC stations.  LIP's stratgey was to
restrict n to 30 bits, and to compute a*b % n, where 0 <= a, b < n, the
follwong was computed:

   long q = long(double(a) * double(b) / double(n));
   long r = a*b - q*n;
   if (r >= n)
      r -= n;
   else if (r < 0)
      r += n;

With quite reasonable assumptions about floating point (certainly, anything
even remotely close to IEEE 64-bit doubles), the computation of q always gives
the true quotient floor(a*b / n), plus or minus 1.  The computation of r is
done modulo the 2^{word size}, and the following if/then/else adjusts r as
necessary.  To be more portable, some of these computations should really be
done using unsigned arithmetic, but that is not so important here.  Also, the
adjustment steps can be replaced by simple non-branching instrictions sequences
involving SHIFT, AND, and ADD/SUB instructions.  On many modern machines, this
is usually faster and NTL uses this non-branching strategy.

Other simple optimizations can be done, such as precomputing 1/double(n) when n
remains fixed for many computations, as is often the case.

Note also that this strategy works perfectly well even when a or b are larger
than n, but the quotient itself is bounded by 2^30.

This strategy worked well for many years.  I had considered employing
"Montgomery multiplication", but did not do so for a couple of reasons:
  1) it would require non-portable code, because Montgomery multiplication
     requires the computation of two-word products,
  2) I did not like the idea of working with "alternative representations"
     for integers mod n, as this would make the interfaces more awkward.

At some point in the early 2000's, this strategy was starting to slow things
down, as floating point arithmetic, especially the integer/floating point
conversions, was starting to slow down relative to integer arithmetic.  This
was especially true on x86 machines, which by this time was starting to become
the most important target.  As it happens, later in the 2000's, as the x86
platforms started to use SSE instructions in lieu of the old x87 FPU
instructions, this speed differential again became less of a problem.
Nevertheless, I introduced some new techniques that speed things up across a
variety of platforms.  I introduced this new technique in NTL 5.4 back in 2005.
I never claimed it was particularly new, and I never really documented many
details about it, but since then, it has come to be known as "Shoup
multiplcation" in a few papers, so I'll accept that. :-)  The paper "Faster
arithmetic for number-theoretic transforms" [David Harvey, J. Symb. Comp. 60
(2014) 113–119] seems to be the first place where it is discussed in detail,
and Harvey's paper also contains some improvements which I discuss below.

The basic idea is that in many computations, not only n, but one of the
arguments, say b, remains fixed for many computatations of a*b % n, and so we
can afford to do a little precomputation, based on b and n, to speed things up.
This approach does require the ability to compute double-word products
(actually, just the high word of the product), but it still presents the same
basic interface as before (i.e., no awkward, alternative representations);
moreover, on platforms where we can't get double-word products, the
implementation falls back to the old floating point strategy, and client code
need not be aware of this.

The basic idea is this: suppose 0 <= n < 2^w, and 0 <= a < 2^w, and 0 <= b < n.
We precompute bninv = floor(2^w*b/n).  Then if we compute q =
floor(a*bninv/2^w), it can be argued that q is either floor(a*b/n), or is 1 too
small.  The computation of bninv can be done using the floating point
techniques described above.  The computation of q can be done by computing the
high word of a double-word product (it helps if bninv is left-shifted an
appropriate amount first).  Once we have q, we can compute a*b - q*n as before,
and adjust (but now only one adjustment is needed).  So after the
precomputation.  the whole operation takes 3 multiplies (one doube-word and two
single-word), and a small handful of simple instructions (adds, shifts, etc).
Moreover, two of the three multiplies can start in parallel, on platforms where
this is possible.

David Harvey noticed that because on modern machines, multiplies are really not
that slow compared to additions, the cost of all of the adjustments (in the
MulMod, as well as in the AddMod and SubMod's in the basic FFT butterfly steps)
starts to dominate the cost of the FFT. Indeed, with a straightforward
implementation of the above ideas, there are three multiplies and three
adjustment steps in each butterfly step.  David's idea was to work with
redundant representations mod n, in the range [0..4*n), and thus reduce the
number of adjustments per butterfly from three to one.  I've implemented this
idea here, and it does indeed make a significant difference, which is even more
pronounced when all of the FFT multipliers b and corresponding bninv values are
precomputed.  My initial implementation of David's ideas (v6.0 in 2013) only
implemented his approach with these precomputated tables: it seemed that
without these tables, it was not a significant improvement.  However, I later
figured out how to reduce the cost of computing all the necessary data "on the
fly", in a way that seems only slightly (10-15%) slower overall.  I introduced
this in v9.1 in 2015, and set things up so that now the pre-computed tables are
still used, but not exclusively, in such a way as to reduce the memory used by
these tables for very large polynomials (either very high degree or lots of FFT
primes).   The idea here is simple, but I haven't seen it discussed elsewhere,
so I'll document the basic idea here.

Suppose we have the preconditioners for a and b, and want a*b % n along with
the preconditioner for a*b % n.

For a, let us suppose that we have both q1 and r1, where:
   2^w*a = n*q1 + r1
We can obtain both q1 and r1 using floating point techniques.

Step 1. Compute a*b % n, using the integer-only MulMod, using
either the preconditioner for either a or b.

Step 2. Compute q2 and r2 such that
   r1*b = n*q2 + r2
We can obtain these using the integer-only MulMod, preconditioned on b.
Actually, we only need q2, not r2.

Step 3. Compute
   q3 = q1*b + q2 mod 2^w
which we can compute with just a single-word multiply and an addition.

One can easily show that the value q3 computed above is indeed the
preconditioner for a*b % n.

Note that, in theory, if the computation in Step 2 is done using the
preconditioner for a (i.e., q1), then the multiplication q1*b in Step 3 should
not really be necessary (assuming that computing both high and low words of a
doube-wprd product is no more expensive than just computing the low word).
However, none of the compilers I've used have been able to perform that
optimization.

64-BIT MACHINES

FIXME: this discussion is out of date.
Current versions of NTL use (by default) 60-bit moduli based
on all-integer arithemtic.

Prior to v9.0 of NTL, on 64 bits, the modulus n was restricted to 50 bits, in
order to allow the use of double-precision techniques, as double's have 53 bits
of precisions.  However, since the x86-64 is such an importnat target, and the
one can still access the old x87 FPU, which provided 64-bit precision, the
bound on n on such platforms is now 60 bits.  Actually, 62 bits could be
supported, but other things (namely, the TBL_REM implementation in
lip.cpp) start to slow down if 62 bits are used, so 60 seems like a good
compromose.  Currently,  60-bit moduli are available only when using gcc on
x86-64 machines, and when compiling NTL with GMP.

Now, the FPU-based multiplies are in fact a bit slower than the SSE-based
multiplies. However, with the preconditioned all-integer MulMod's now used
extensively on almost all critical paths within NTL, this does not really
matter, and in fact, many things get faster with the wider moduli, so overall,
it is a net performance gain.

FUTURE TRENDS

In the future, I might also experiment with other MulMod techniques, such as
those described in "Improved Division by Invariant Integers" [Moeller,
Granlund, IEEE Transactions on Computers, June 2010].  This might allow for,
say, 60-bit moduli on 64-bit machines that don't have extended double
precision.   It is not clear how the performance of this would compare with the
floating-point methods; however, it probably doesn't matter too much, as the
preconditioned MulMod's are the most important ones.

It might also be useful to go back and reconsider Montgomery multiplication, at
least for "internal" use, like the FFT.  However, I doubt that this will help
significantly.

As mentioned above, it could be useful to experiment with more cache-friendly
variants of the FFT, like Bailey's 4-step method.  I could also experiment with
using the DIF/DIT.  This affects some code outside of FFT as well (in ZZ_pX and
zz_pX, like reduce and AddeExpand), but should not affect any documented
interfaces.

Another direction to consider is exploiting concurrency.  Besides using
multiple cores to parallelize things at a higher level, it would be nice to
exploit newer SIMD instructions.  Unfortunately, as of now (early 2015), these
don't seem to have the functionality I need.  A 64-bit x 64-bit -> low order
64-bit instruction is supposed to be available soon in the new AVX-512
instruction set.  That would be a good start, but I would really like to get
the high-order 64-bits too. Maybe that will come someday.  In the mean time, it
might be fun to experiment with using the AVX-512 instructions that will be
available, which will at least allow at least a floating-point-based
implementation, or an all-integer implementation with emulated MulHi.  I have
no idea how performance will compare.

********************************************************************/

//  #define NTL_BRC_TEST
//  Flag to test the cost of "bit reverse copy"

#define NTL_FFT_BIGTAB_LIMIT (200)
#ifndef NTL_BRC_TEST
#define NTL_FFT_BIGTAB_MAXROOT (17)
#else
#define NTL_FFT_BIGTAB_MAXROOT (25)
#endif
// big tables are only used for the first NTL_FFT_BIGTAB_LIMIT primes,
// and then only for k-values at most NTL_FFT_BIGTAB_MAXROOT

// NOTE: in newer versions of NTL (v9.1 and later), the BIGTAB
// code is only about 5-15% faster than the non-BIGTAB code, so
// this is not a great time/space trade-off.

// NOTE: NTL_FFT_BIGTAB_MAXROOT is set independently of the parameter
// NTL_FFTMaxRoot defined in FFT.h (and which is typically 25).
// The space for the LazyTable FFTMultipliers could be reduced a bit
// by using min(NTL_FFT_BIGTAB_MAXROOT, NTL_FFTMaxRoot) + 1 for the
// size of these tables.

NTL_START_IMPL

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

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

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

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

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

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

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

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

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

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

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

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

      if (j == k)
         break;
   }

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

   long TrialBound;

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

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

      TrialBound = SqrRoot(TrialBound);

      long a, b;

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

   /* n is an FFT prime */

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

   w = x;

   return 1;
}

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

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

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

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

   long t, cand;

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

      k--;

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

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

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

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

void InitFFTPrimeInfo(FFTPrimeInfo& info, long q, long w, bool bigtab)
{
   mulmod_t qinv = PrepMulMod(q);

   long mr = CalcMaxRoot(q);

   info.q = q;
   info.qinv = qinv;
   info.qrecip = 1/double(q);
   info.zz_p_context = 0;

   info.RootTable[0].SetLength(mr+1);
   info.RootTable[1].SetLength(mr+1);
   info.TwoInvTable.SetLength(mr+1);
   info.TwoInvPreconTable.SetLength(mr+1);

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

   long j;
   long t;

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

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

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

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

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

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

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

   if (index+1 >= NTL_NSP_BOUND) ResourceError("FFT prime index too large");
   // largely acacedemic, but it is a convenient assumption

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

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

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

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

         bool bigtab = false;

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

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

   } while (0);
}

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

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

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

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

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

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

// FIXME: This could potentially be shared across threads, using
// a "lazy table".
static inline
Vec<long> *get_brc_mem()
{
   NTL_TLS_LOCAL_INIT(Vec< Vec<long> >, brc_mem_vec, (INIT_SIZE, NTL_FFTMaxRoot+1));
   return brc_mem_vec.elts();
}

#if 0

static
void BitReverseCopy(long * NTL_RESTRICT A, const long * NTL_RESTRICT a, long k)
{
   Vec<long> *brc_mem = get_brc_mem();

   long n = 1L << k;
   long* NTL_RESTRICT rev;
   long i, j;

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

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

static
void BitReverseCopy(unsigned long * NTL_RESTRICT A, const long * NTL_RESTRICT a, long k)
{
   Vec<long> *brc_mem = get_brc_mem();

   long n = 1L << k;
   long* NTL_RESTRICT rev;
   long i, j;

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

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

#else

#define NTL_BRC_THRESH (11)
#define NTL_BRC_Q (5)

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

static
long *BRC_init(long k)
{
   Vec<long> *brc_mem = get_brc_mem();

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

static
void BasicBitReverseCopy(long * NTL_RESTRICT B,
                         const long * NTL_RESTRICT A, long k)
{
   Vec<long> *brc_mem = get_brc_mem();

   long n = 1L << k;
   long* NTL_RESTRICT rev;
   long i, j;

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

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

static
void COBRA(long * NTL_RESTRICT B, const long * NTL_RESTRICT A, long k)
{
   Vec<long> *brc_mem = get_brc_mem();

   NTL_TLS_LOCAL(Vec<long>, BRC_temp);

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

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

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

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

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

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

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

static
void BasicBitReverseCopy(unsigned long * NTL_RESTRICT B,
                         const long * NTL_RESTRICT A, long k)
{
   Vec<long> *brc_mem = get_brc_mem();

   long n = 1L << k;
   long* NTL_RESTRICT rev;
   long i, j;

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

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

static
void COBRA(unsigned long * NTL_RESTRICT B, const long * NTL_RESTRICT A, long k)
{
   Vec<long> *brc_mem = get_brc_mem();

   NTL_TLS_LOCAL(Vec<unsigned long>, BRC_temp);

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

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

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

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

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

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

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

#endif

#ifdef NTL_FFT_LAZYMUL
// we only honor the FFT_LAZYMUL flag if either the SPMM_ULL_VIABLE or LONGLONG_SP_MULMOD
// flags are set

#if (!defined(NTL_SPMM_ULL_VIABLE) && !defined(NTL_LONGLONG_SP_MULMOD))
#undef NTL_FFT_LAZYMUL

// raise an error if running the wizard
#if (defined(NTL_WIZARD_HACK))
#error "cannot honor NTL_FFT_LAZYMUL"
#endif

#endif

#endif

#ifndef NTL_FFT_BIGTAB

#define NTL_FFT_ROUTINE_TAB     FFT_aux
#define NTL_FFT_ROUTINE_NOTAB   FFT

#else

#define NTL_FFT_ROUTINE_TAB     FFT
#define NTL_FFT_ROUTINE_NOTAB   FFT_aux

#endif

#ifndef NTL_FFT_LAZYMUL

// A basic FFT, no tables, no lazy strategy

void NTL_FFT_ROUTINE_NOTAB(long* A, const long* a, long k, const FFTPrimeInfo& info, long dir)
// performs a 2^k-point convolution modulo q

{
   long q = info.q;
   const long *root = info.RootTable[dir].elts();
   mulmod_t qinv = info.qinv;

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

   // assume k > 1

   NTL_TLS_LOCAL(Vec<long>, wtab_store);
   NTL_TLS_LOCAL(Vec<mulmod_precon_t>, wqinvtab_store);
   NTL_TLS_LOCAL(Vec<long>, AA_store);

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

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

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

   BitReverseCopy(AA, a, k);

   long n = 1L << k;

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

   long w;
   mulmod_precon_t wqinv;

   // s = 1

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

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

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

      // prepare wtab...

#if 1
      // plain version...

      for (i = m_half-1, j = m_fourth-1; i >= 0; i -= 2, j--) {
         long w_j = wtab[j];
         mulmod_precon_t wqi_j = wqinvtab[j];
         long w_i = MulModPrecon(w_j, w, q, wqinv);
         mulmod_precon_t wqi_i = PrepMulModPrecon(w_i, q, qinv);

         wtab[i-1] = w_j;
         wqinvtab[i-1] = wqi_j;
         wtab[i] = w_i;
         wqinvtab[i] = wqi_i;
      }
#else
      // software pipeline version...doesn't seem to make a big difference

      if (s == 2) {
         wtab[1] = MulModPrecon(wtab[0], w, q, wqinv);
         wqinvtab[1] = PrepMulModPrecon(wtab[1], q, qinv);
      }
      else {
         i = m_half-1; j = m_fourth-1;
         wtab[i-1] = wtab[j];
         wqinvtab[i-1] = wqinvtab[j];
         wtab[i] = MulModPrecon(wtab[i-1], w, q, wqinv);

         i -= 2; j --;

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

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

#endif

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

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

#if 1
         // loop unrolling and pipelining

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

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

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

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

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

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

#else
         // no loop unrolling, but still some pipelining

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

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

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

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

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

#endif
      }
   }

   // s == k...special case

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

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

   // j = 0, 1

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

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

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

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

   }
}

#else

// FFT with  lazy multiplication

#if (defined(NTL_LONGLONG_SP_MULMOD))

#if (NTL_BITS_PER_LONG >= NTL_SP_NBITS+4)

static inline unsigned long
sp_NormalizedLazyPrepMulModPreconWithRem(unsigned long& rres, long b, long n, unsigned long ninv)
{
   unsigned long H = cast_unsigned(b);
   unsigned long Q = ll_mul_hi(H << 4, ninv);
   unsigned long L = cast_unsigned(b) << (NTL_SP_NBITS+2);
   long r = L - Q*cast_unsigned(n);  // r in [0..2*n)

   r = sp_CorrectExcessQuo(Q, r, n);
   rres = r;
   return Q; // NOTE: not shifted
}

static inline unsigned long
sp_NormalizedLazyPrepMulModPrecon(long b, long n, unsigned long ninv)
{
   unsigned long H = cast_unsigned(b);
   unsigned long Q = ll_mul_hi(H << 4, ninv);
   unsigned long L = cast_unsigned(b) << (NTL_SP_NBITS+2);
   long r = L - Q*cast_unsigned(n);  // r in [0..2*n)

   Q += 1L + sp_SignMask(r-n);
   return Q; // NOTE: not shifted
}

#else

// NTL_BITS_PER_LONG == NTL_SP_NBITS+2
static inline unsigned long
sp_NormalizedLazyPrepMulModPreconWithRem(unsigned long& rres, long b, long n, unsigned long ninv)
{
   unsigned long H = cast_unsigned(b) << 2;
   unsigned long Q = ll_mul_hi(H, (ninv << 1)) + H;
   unsigned long rr = -Q*cast_unsigned(n);  // r in [0..3*n)

   long r = sp_CorrectExcessQuo(Q, rr, n);
   r = sp_CorrectExcessQuo(Q, r, n);
   rres = r;
   return Q;  // NOTE: not shifted
}

static inline unsigned long
sp_NormalizedLazyPrepMulModPrecon(long b, long n, unsigned long ninv)
{
   unsigned long H = cast_unsigned(b) << 2;
   unsigned long Q = ll_mul_hi(H, (ninv << 1)) + H;
   unsigned long rr = -Q*cast_unsigned(n);  // r in [0..3*n)
   Q += 2L + sp_SignMask(rr-n) + sp_SignMask(rr-2*n);
   return Q; // NOTE: not shifted
}

#endif

static inline unsigned long
LazyPrepMulModPrecon(long b, long n, sp_inverse ninv)
{
   return sp_NormalizedLazyPrepMulModPrecon(b << ninv.shamt, n << ninv.shamt, ninv.inv) << (NTL_BITS_PER_LONG-NTL_SP_NBITS-2);
}

static inline unsigned long
LazyPrepMulModPreconWithRem(unsigned long& rres, long b, long n, sp_inverse ninv)
{
   unsigned long qq, rr;
   qq = sp_NormalizedLazyPrepMulModPreconWithRem(rr, b << ninv.shamt, n << ninv.shamt, ninv.inv);
   rres = rr >> ninv.shamt;
   return qq << (NTL_BITS_PER_LONG-NTL_SP_NBITS-2);
}

#elif (NTL_BITS_PER_LONG - NTL_SP_NBITS >= 4 && NTL_WIDE_DOUBLE_PRECISION - NTL_SP_NBITS >= 4)

// slightly faster functions, which should kick in on x86-64, where
//    NTL_BITS_PER_LONG == 64
//    NTL_SP_NBITS == 60 (another reason for holding this back to 60 bits)
//    NTL_WIDE_DOUBLE_PRECISION == 64

// DIRT: if the relative error in floating point calcuations (muls and reciprocals)
//   is <= epsilon, the relative error in the calculations is <= 3*epsilon +
//   O(epsilon^2), and we require that this relative error is at most
//   2^{-(NTL_SP_NBITS+2)}, so it should be pretty safe as long as
//   epsilon is at most, or not much geater than, 2^{-NTL_WIDE_DOUBLE_PRECISION}.

static inline
unsigned long LazyPrepMulModPrecon(long b, long n, wide_double ninv)
{
   long q = (long) ( (((wide_double) b) * wide_double(4*NTL_SP_BOUND)) * ninv );

   unsigned long rr = (cast_unsigned(b) << (NTL_SP_NBITS+2))
                       - cast_unsigned(q)*cast_unsigned(n);

   q += sp_SignMask(rr) + sp_SignMask(rr-n) + 1L;

   return cast_unsigned(q) << (NTL_BITS_PER_LONG - NTL_SP_NBITS - 2);
}

static inline
unsigned long LazyPrepMulModPreconWithRem(unsigned long& rres, long b, long n, wide_double ninv)
{
   long q = (long) ( (((wide_double) b) * wide_double(4*NTL_SP_BOUND)) * ninv );

   unsigned long rr = (cast_unsigned(b) << (NTL_SP_NBITS+2))
                       - cast_unsigned(q)*cast_unsigned(n);

   long r = sp_CorrectDeficitQuo(q, rr, n);
   r = sp_CorrectExcessQuo(q, r, n);

   unsigned long qres = cast_unsigned(q) << (NTL_BITS_PER_LONG - NTL_SP_NBITS - 2);
   rres = r;
   return qres;
}

#else

static inline
unsigned long LazyPrepMulModPrecon(long b, long n, wide_double ninv)
{
   long q = (long) ( (((wide_double) b) * wide_double(NTL_SP_BOUND)) * ninv );

   unsigned long rr = (cast_unsigned(b) << (NTL_SP_NBITS))
                       - cast_unsigned(q)*cast_unsigned(n);

   long r = sp_CorrectDeficitQuo(q, rr, n);
   r = sp_CorrectExcessQuo(q, r, n);

   unsigned long qq = q;

   qq = 2*qq;
   r = 2*r;
   r = sp_CorrectExcessQuo(qq, r, n);

   qq = 2*qq;
   r = 2*r;
   qq += sp_SignMask(r-n) + 1L;

   return qq << (NTL_BITS_PER_LONG - NTL_SP_NBITS - 2);
}

static inline
unsigned long LazyPrepMulModPreconWithRem(unsigned long& rres, long b, long n, wide_double ninv)
{
   long q = (long) ( (((wide_double) b) * wide_double(NTL_SP_BOUND)) * ninv );

   unsigned long rr = (cast_unsigned(b) << (NTL_SP_NBITS))
                       - cast_unsigned(q)*cast_unsigned(n);

   long r = sp_CorrectDeficitQuo(q, rr, n);
   r = sp_CorrectExcessQuo(q, r, n);

   unsigned long qq = q;

   qq = 2*qq;
   r = 2*r;
   r = sp_CorrectExcessQuo(qq, r, n);

   qq = 2*qq;
   r = 2*r;
   r = sp_CorrectExcessQuo(qq, r, n);

   rres = r;
   return qq << (NTL_BITS_PER_LONG - NTL_SP_NBITS - 2);
}

#endif

static inline
unsigned long LazyMulModPreconQuo(unsigned long a, unsigned long b,
                                  unsigned long n, unsigned long bninv)
{
   unsigned long q = ll_mul_hi(a, bninv);
   unsigned long r = a*b - q*n;
   q += sp_SignMask(r-n) + 1L;
   return q << (NTL_BITS_PER_LONG - NTL_SP_NBITS - 2);
}

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

static inline
unsigned long LazyReduce1(unsigned long a, long q)
{
  return sp_CorrectExcess(long(a), q);
}

static inline
unsigned long LazyReduce2(unsigned long a, long q)
{
  return sp_CorrectExcess(a, 2*q);
}

// FFT: Lazy, no tables

void NTL_FFT_ROUTINE_NOTAB(long* A, const long* a, long k, const FFTPrimeInfo& info, long dir)

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

{
   long q = info.q;
   const long *root = info.RootTable[dir].elts();
   mulmod_t qinv = info.qinv;

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

   // assume k >= 2

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

   NTL_TLS_LOCAL(Vec<long>, wtab_store);
   wtab_store.SetLength(max(2, 1L << (k-2)));
   // allocate space for at least 2 elements, to deal with a corner case when k == 2
   long * NTL_RESTRICT wtab = wtab_store.elts();

   NTL_TLS_LOCAL(Vec<mulmod_precon_t>, wqinvtab_store);
   wqinvtab_store.SetLength(max(2, 1L << (k-2)));
   // allocate space for at least 2 elements, to deal with a corner case when k == 2
   mulmod_precon_t * NTL_RESTRICT wqinvtab = wqinvtab_store.elts();

   BitReverseCopy(AA, a, k);

   long n = 1L << k;

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

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

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

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

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

      wtab[1] = w;
      wqinvtab[1] = wqinv;

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

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

         {
            const unsigned long a11 = AA1[0];
            const unsigned long a01 = AA0[0];

            const unsigned long tt1 = a11;
            const unsigned long uu1 = a01;
            const unsigned long b01 = uu1 + tt1;
            const unsigned long b11 = uu1 - tt1 + 2*q;

            AA0[0] = b01;
            AA1[0] = b11;
         }
         {
            const unsigned long a11 = AA1[1];
            const unsigned long a01 = AA0[1];

            const unsigned long tt1 = LazyMulModPrecon(a11, w, q, wqinv);
            const unsigned long uu1 = a01;
            const unsigned long b01 = uu1 + tt1;
            const unsigned long b11 = uu1 - tt1 + 2*q;

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

   //  s = 3..k-1

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

      long w = root[s];

#if 0
      // This computes all the multipliers in a straightforward fashion.
      // It's a bit slower that the strategy used below, even if
      // NTL_LONGLONG_SP_MULMOD is set

      mulmod_precon_t wqinv = LazyPrepMulModPrecon(w, q, qinv);

      for (i = m_half-1, j = m_fourth-1; i >= 0; i -= 2, j--) {
         long w_j = wtab[j];
         mulmod_precon_t wqi_j = wqinvtab[j];

         long w_i = LazyReduce1(LazyMulModPrecon(w_j, w, q, wqinv), q);
         mulmod_precon_t wqi_i = LazyPrepMulModPrecon(w_i, q, qinv);

         wtab[i-1] = w_j;
         wqinvtab[i-1] = wqi_j;
         wtab[i] = w_i;
         wqinvtab[i] = wqi_i;
      }
#else
      unsigned long wqinv_rem;
      mulmod_precon_t wqinv = LazyPrepMulModPreconWithRem(wqinv_rem, w, q, qinv);

      for (i = m_half-1, j = m_fourth-1; i >= 0; i -= 2, j--) {
         long w_j = wtab[j];
         mulmod_precon_t wqi_j = wqinvtab[j];

         // The next two lines are equivalent, but the first involves
         // a computation of hi(w_j*wqinv), which pairs with the
         // computation of lo(w_j*wqinv) below...but I don't think
         // the compiler sees this...oh well...

         long w_i = LazyReduce1(LazyMulModPrecon(w_j, w, q, wqinv), q);
         // long w_i = LazyReduce1(LazyMulModPrecon(w, w_j, q, wqi_j), q);

         mulmod_precon_t wqi_i = LazyMulModPreconQuo(wqinv_rem, w_j, q, wqi_j)
                                   + cast_unsigned(w_j)*wqinv;

         wtab[i-1] = w_j;
         wqinvtab[i-1] = wqi_j;
         wtab[i] = w_i;
         wqinvtab[i] = wqi_i;
      }

#endif

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

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

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

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

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

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

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

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

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

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

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

   // special case: s == k to avoid extraneous computation of constants

   if (k > 2) {
      s = k;

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

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

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

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

         long half_j;

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

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

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

               const unsigned long tt1 = LazyMulModPrecon(LazyMulModPrecon(a11, w1, q, wqi1),
                                                          w, q, wqinv);
               const unsigned long uu1 = LazyReduce2(a01, q);
               const unsigned long b01 = uu1 + tt1;
               const unsigned long b11 = uu1 - tt1 + 2*q;

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

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

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

               const unsigned long tt1 = LazyMulModPrecon(LazyMulModPrecon(a11, w1, q, wqi1),
                                                          w, q, wqinv);
               const unsigned long uu1 = LazyReduce2(a01, q);
               const unsigned long b01 = uu1 + tt1;
               const unsigned long b11 = uu1 - tt1 + 2*q;

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

   /* need to reduce redundant representations */

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

#endif

#ifndef NTL_FFT_LAZYMUL

// FFT with precomputed tables, no lazy mul

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

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

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

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

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

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

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

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

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

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

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

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

         // prepare wtab...

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

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

            i -= 2; j --;

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

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

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

// FFT: no lazy, table

void NTL_FFT_ROUTINE_TAB(long* A, const long* a, long k, const FFTPrimeInfo& info, long dir)
// performs a 2^k-point convolution modulo q

{
   if (!info.bigtab || k > NTL_FFT_BIGTAB_MAXROOT) {
      NTL_FFT_ROUTINE_NOTAB(A, a, k, info, dir);
      return;
   }

   long q = info.q;
   const long *root = info.RootTable[dir].elts();
   mulmod_t qinv = info.qinv;
   const FFTMultipliers& tab = info.bigtab->MulTab[dir];

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

   // assume k > 1

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

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

   BitReverseCopy(AA, a, k);

   long n = 1L << k;

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

   // s = 1

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

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

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

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

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

#ifdef NTL_PIPELINE

// pipelining: seems to be faster

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

#ifdef NTL_PIPELINE

// pipelining: seems to be faster

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

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

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

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

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

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

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

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

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

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

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

}

#else

// FFT with precomputed tables, lazy mul

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

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

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

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

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

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

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

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

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

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

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

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

         // prepare wtab...

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

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

            i -= 2; j --;

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

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

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

#ifdef NTL_BRC_TEST
bool BRC_test_flag = false;
#endif

// FFT: lazy, tables

void NTL_FFT_ROUTINE_TAB(long* A, const long* a, long k, const FFTPrimeInfo& info, long dir)

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

{
   if (!info.bigtab || k > NTL_FFT_BIGTAB_MAXROOT) {
      NTL_FFT_ROUTINE_NOTAB(A, a, k, info, dir);
      return;
   }

   long q = info.q;
   const long *root = info.RootTable[dir].elts();
   mulmod_t qinv = info.qinv;
   const FFTMultipliers& tab = info.bigtab->MulTab[dir];

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

   // assume k > 1

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

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

   long n = 1L << k;

#ifndef NTL_BRC_TEST
   BitReverseCopy(AA, a, k);
#else
   if (BRC_test_flag)
      for (long i = 0; i < n; i++) AA[i] = a[i];
   else
      BitReverseCopy(AA, a, k);
#endif

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

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

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

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

      const long w1 = wtab[1];
      const mulmod_precon_t wqi1 = wqinvtab[1];

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

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

         {
            const unsigned long a11 = AA1[0];
            const unsigned long a01 = AA0[0];

            const unsigned long tt1 = a11;
            const unsigned long uu1 = a01;
            const unsigned long b01 = uu1 + tt1;
            const unsigned long b11 = uu1 - tt1 + 2*q;

            AA0[0] = b01;
            AA1[0] = b11;
         }
         {
            const unsigned long a11 = AA1[1];
            const unsigned long a01 = AA0[1];

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

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

   //  s = 3..k

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

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

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

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

#if 1

         // a little loop unrolling: this gives the best code

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

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

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

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

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

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

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

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

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

#else

         // a plain loop: not as good as the unrolled version

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

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

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

#endif

      }
   }

   /* need to reduce redundant representations */

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

#endif

NTL_END_IMPL


#include "dSFMTsearch.hpp"
#include "Annihilate.h"
#include <errno.h>
#include <stdlib.h>
#include <getopt.h>
#include <errno.h>
#include <NTL/GF2X.h>
#include <NTL/GF2XFactoring.h>
#include <MTToolBox/period.hpp>
#include <MTToolBox/AlgorithmReducibleRecursionSearch.hpp>
#include "printBinary.h"

using namespace MTToolBox;
using namespace std;

void get_fixpoint(dSFMT& dsfmt);

static bool option(dSFMT_param& param, int argc, char * argv[]) {
    int c;
    bool error = false;
//    char *pgm = argv[0];
    for (;;) {
        c = getopt(argc, argv, "hd");
        if (error) {
            break;
        }
        if (c == -1) {
            break;
        }
        switch (c) {
        case 'd':
            //debug = true;
            break;
        case 'h':
        default:
            error = true;
            break;
        }
    }
    char * para = argv[1];
    param.mexp = strtoul(para, &para, 10);
    para++;
    param.pos1 = strtoul(para, &para, 10);
    para++;
    param.sl1 = strtoul(para, &para, 10);
    para++;
    param.msk1 = strtoull(para, &para, 16);
    para++;
    param.msk2 = strtoull(para, &para, 16);
    para++;
    param.fix1 = strtoull(para, &para, 16);
    para++;
    param.fix2 = strtoull(para, &para, 16);
    para++;
    param.parity1 = strtoull(para, &para, 16);
    para++;
    param.parity2 = strtoull(para, &para, 16);
    return true;
}

int main(int argc, char *argv[]) {
    dSFMT_param param;
    if (!option(param, argc, argv)) {
        return -1;
    }
    dSFMT dsfmt(param);
    int mexp = dsfmt.getMexp();
    int maxdegree = dsfmt.bitSize();
    cout << "succ = 1" << endl;
    cout << "mexp = " << dec << mexp;
    cout << ", maxdegree = " << dec << maxdegree << endl;
    w128_t w;
    w.u64[0] = 1;
    dsfmt.seed(w);
    cout << "pos1 = " << dec << param.pos1 << endl;
    cout << "sl1 = " << dec << param.sl1 << endl;
    cout << "msk1 = " << hex << setw(16) << setfill('0') << param.msk1 << endl;
    cout << "msk2 = " << hex << setw(16) << setfill('0') << param.msk2 << endl;
    cout << "fix1 = " << hex << setw(16) << setfill('0') << param.fix1 << endl;
    cout << "fix2 = " << hex << setw(16) << setfill('0') << param.fix2 << endl;
    cout << "pcv1 = " << hex << setw(16) << setfill('0') << param.parity1
         << endl;
    cout << "pcv2 = " << hex << setw(16) << setfill('0') << param.parity2
         << endl;
    get_fixpoint(dsfmt);
    return 0;
}

void get_fixpoint(dSFMT& dsfmt)
{
    GF2X poly;
    GF2X lcmpoly;

    minpoly<w128_t>(poly, dsfmt);
    GF2X irreducible = poly;
    if (!hasFactorOfDegree(irreducible, dsfmt.getMexp())) {
        cout << "error does not have factor of degree " << dec << dsfmt.getMexp()
             << endl;
        cout << "degree = " << dec << deg(irreducible) << endl;
        throw new logic_error("error does not have factor of degree ");
    }
    lcmpoly = poly;
    getLCMPoly(lcmpoly, dsfmt);
    printBinary(stdout, irreducible);
    printBinary(stdout, lcmpoly);
}


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

#define M_TO_KM 1000
#define H_TO_S 3600
#include <vector>
#include <string>
#include "LatLon.h"
#include <math.h>
#include "StreetsDatabaseAPI.h"
#include <string>
#include <iostream>
#include <iterator>
#include <queue>
#include <unordered_set>
#include <algorithm>
#include <sstream>
#include <iomanip>
#include <boost/algorithm/string.hpp>
#include <X11/Xlib.h>
#include "graphics.h"
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/io/wkt/wkt.hpp>
#include "global_functions.h"
#include "m2.h"
#include "m1.h"
#include "m3.h"
#include <sstream>
#include <chrono>
#include <ctime>

unsigned closest_POI;

// Returns the time required to travel along the path specified, in seconds.
// The path is given as a vector of street segment ids, and this function
// can assume the vector either forms a legal path or has size == 0.
// The travel time is the sum of the length/speed-limit of each street
// segment, plus the given turn_penalty (in seconds) per turn implied by the path.
// A turn occurs when two consecutive street segments have different street IDs.

double compute_path_travel_time(const std::vector<unsigned>& path,
        const double turn_penalty) {

    if (path.begin() == path.end())
        return 0;
    double pathTime = 0;

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

        double tempLength = find_street_segment_length(path[i]);
        double tempLimit = getStreetSegmentInfo(path[i]).speedLimit;

        if (i == 0) {
            pathTime = pathTime + (tempLength * 1 / M_TO_KM * 1 / tempLimit) * H_TO_S;
        } else {
            pathTime = pathTime + (tempLength * 1 / M_TO_KM * 1 / tempLimit) * H_TO_S;
            if (getStreetSegmentInfo(path[i - 1]).streetID != getStreetSegmentInfo(path[i]).streetID) {
                pathTime = pathTime + turn_penalty;
            }
        }

    }
    return pathTime;
}

// Returns a path (route) between the start intersection and the end
// intersection, if one exists. This routine should return the shortest path
// between the given intersections when the time penalty to turn (change
// street IDs) is given by turn_penalty (in seconds).
// If no path exists, this routine returns an empty (size == 0) vector.
// If more than one path exists, the path with the shortest travel time is
// returned. The path is returned as a vector of street segment ids; traversing
// these street segments, in the returned order, would take one from the start
// to the end intersection.

std::vector<unsigned> find_path_between_intersections(const unsigned intersect_id_start,
        const unsigned intersect_id_end,
        const double turn_penalty) {

    auto start_time = std::chrono::high_resolution_clock::now();

    std::vector<unsigned> path;
    if (intersect_id_start == intersect_id_end) return path;
    typedef double DVal; //short for Dijkstra Value which is the temporary value assigned to vertices when the algorithm is working
    typedef std::pair<DVal, IntersectionIndex> Node;

    std::vector<DVal> dijkstraValue;
    std::vector<std::pair<StreetSegmentIndex, IntersectionIndex>> parents;
    std::priority_queue<Node, std::deque<Node>, std::greater < Node>> heap;
    dijkstraValue.clear();
    dijkstraValue.resize(getNumberOfIntersections(), std::numeric_limits<double>::infinity());
    parents.clear();
    parents.resize(getNumberOfIntersections());
    std::unordered_set<unsigned> target_list;

    target_list.insert(intersect_id_end);

    heap.push(std::make_pair(0, intersect_id_start));
    dijkstraValue[intersect_id_start] = 0;
    IntersectionIndex currentNode;
    while (!heap.empty()) {
        currentNode = heap.top().second;
        heap.pop();

        std::unordered_set<unsigned>::iterator element = target_list.find(currentNode);
        if (element != target_list.end()) {
            break;
        }

        //get all adjacent nodes to current node
        std::deque <Node_Edge_Object> node_list = adjacency_list[currentNode];
        for (unsigned i = 0; i < node_list.size(); i++) {
            double cost = dijkstraValue[currentNode] + node_list[i].weight;

            //cost = cost + (find_distance_between_two_points(getIntersectionPosition(currentNode), getIntersectionPosition(intersect_id_end)));
            //insert stuff about turn penalties here
            StreetSegmentInfo prevStreetSeg = getStreetSegmentInfo(parents[currentNode].first);
            StreetSegmentInfo newStreetSeg = getStreetSegmentInfo(node_list[i].SegId);
            if (prevStreetSeg.streetID != newStreetSeg.streetID && currentNode != intersect_id_start) {
                cost = cost + turn_penalty;
            }

            if (dijkstraValue[node_list[i].id] > cost) {
                dijkstraValue[node_list[i].id] = cost;
                heap.push(std::make_pair(dijkstraValue[node_list[i].id], node_list[i].id));
                parents[node_list[i].id] = std::make_pair(node_list[i].SegId, currentNode);
            }
        }
    }
    if (currentNode != intersect_id_end) {
        return path;
    }
    if (dijkstraValue[currentNode] != std::numeric_limits<double>::infinity()) {
        IntersectionIndex index = currentNode;
        std::deque<Node_Edge_Object> node_list = adjacency_list[index];
        while (index != intersect_id_start) {
            path.push_back(parents[index].first);
            index = parents[index].second;
        }

    }
    std::reverse(path.begin(), path.end());
    auto time2 = std::chrono::high_resolution_clock::now();
    std::cout << std::chrono::duration_cast<std::chrono::milliseconds>(time2 - start_time).count() << "\n";
    return path;
}

std::vector<unsigned> find_path_between_intersections(const unsigned intersect_id_start,
        std::vector<unsigned> targets,
        const double turn_penalty) {
    std::vector<unsigned> path;
    typedef double DVal; //short for Dijkstra Value which is the temporary value assigned to vertices when the algorithm is working
    typedef std::pair<DVal, IntersectionIndex> Node;

    std::vector<DVal> dijkstraValue;
    std::vector<std::pair<StreetSegmentIndex, IntersectionIndex>> parents;
    std::priority_queue<Node, std::deque<Node>, std::greater < Node>> heap;
    dijkstraValue.clear();
    dijkstraValue.resize(getNumberOfIntersections(), std::numeric_limits<double>::infinity());
    parents.clear();
    parents.resize(getNumberOfIntersections());
    std::unordered_set<unsigned> target_list(targets.begin(), targets.end());

    heap.push(std::make_pair(0, intersect_id_start));
    dijkstraValue[intersect_id_start] = 0;
    IntersectionIndex currentNode;
    while (!heap.empty()) {

        currentNode = heap.top().second;
        heap.pop();

        std::unordered_set<unsigned>::iterator element = target_list.find(currentNode);
        if (element != target_list.end()) {
            break;
        }

        //get all adjacent nodes to current node
        std::deque <Node_Edge_Object> node_list = adjacency_list[currentNode];
        for (unsigned i = 0; i < node_list.size(); i++) {
            double cost = dijkstraValue[currentNode] + node_list[i].weight;

            //cost = cost + (find_distance_between_two_points(getIntersectionPosition(currentNode), getIntersectionPosition(intersect_id_end)));
            //insert stuff about turn penalties here
            StreetSegmentInfo prevStreetSeg = getStreetSegmentInfo(parents[currentNode].first);
            StreetSegmentInfo newStreetSeg = getStreetSegmentInfo(node_list[i].SegId);
            if (prevStreetSeg.streetID != newStreetSeg.streetID && currentNode != intersect_id_start) {
                cost = cost + turn_penalty;
            }

            if (dijkstraValue[node_list[i].id] > cost) {
                dijkstraValue[node_list[i].id] = cost;
                heap.push(std::make_pair(dijkstraValue[node_list[i].id], node_list[i].id));
                parents[node_list[i].id] = std::make_pair(node_list[i].SegId, currentNode);
            }
        }
    }
    if (target_list.find(currentNode) == target_list.end()) {

        return path;
    }
    if (dijkstraValue[currentNode] != std::numeric_limits<double>::infinity()) {
        IntersectionIndex index = currentNode;
        std::deque<Node_Edge_Object> node_list = adjacency_list[index];
        while (index != intersect_id_start) {
            path.push_back(parents[index].first);
            index = parents[index].second;
        }
    }
    std::reverse(path.begin(), path.end());

    return path;
}

// Returns the shortest travel time path (vector of street segments) from
// the start intersection to a point of interest with the specified name.
// The path will begin at the specified intersection, and end on the
// intersection that is closest (in Euclidean distance) to the point of
// interest.
// If no such path exists, returns an empty (size == 0) vector.

std::vector<unsigned> find_path_to_point_of_interest(const unsigned intersect_id_start,
        const std::string point_of_interest_name,
        const double turn_penalty) {

    std::vector<POIIndex> pathfinding_targets_1 = POI_list[point_of_interest_name];
    std::vector<IntersectionIndex> pathfinding_targets;
    for (unsigned i = 0; i < pathfinding_targets_1.size(); i++) {
        unsigned id = find_closest_intersection(getPointOfInterestPosition(pathfinding_targets_1[i])); //index of POI
        pathfinding_targets.push_back(id);

    }

    std::vector<unsigned> path = find_path_between_intersections(intersect_id_start, pathfinding_targets, turn_penalty);

    return path;
}

double determinant(std::pair<LatLon, LatLon> begin, std::pair<LatLon, LatLon> end) {
    double x1 = lon_to_x(begin.first);
    double x2 = lon_to_x(begin.second);
    double x3 = lon_to_x(end.first);
    double x4 = lon_to_x(end.second);
    double y1 = lat_to_y(begin.first);
    double y2 = lat_to_y(begin.second);
    double y3 = lat_to_y(end.first);
    double y4 = lat_to_y(end.second);

    return ((x2 - x1)*(y4 - y3) - (y2 - y1)*(x4 - x3));
}

std::vector<std::string> all_turns(std::vector<unsigned> path) {
    std::vector<std::string> path_directions;
    if (path.empty())
        return path_directions;

    int direction_turn_count = 0;
    std::vector<double> straight_distances;
    double straight_distance = 0;
    for (unsigned i = 0; i < path.size() - 1; i++) {
        StreetSegmentInfo s1 = getStreetSegmentInfo(path[i]);
        straight_distance = straight_distance + find_street_segment_length(path[i]);
        StreetSegmentInfo s2 = getStreetSegmentInfo(path[i + 1]);
        if (s1.streetID != s2.streetID) {
            direction_turn_count++;
            straight_distances.push_back(straight_distance);
            straight_distance = 0;
        }
    }
    straight_distance = straight_distance + find_street_segment_length(path[path.size() - 1]);
    straight_distances.push_back(straight_distance);
    //Calculate the determinant between current path and next path
    //resulting vector will be in z-direction, if it = 0 then straight
    //-ve for right and +ve for left
    //A is the current segment
    //B is the next segment
    //A(x,y)xB(x,y) = z_(AxBy - BxAy)

    unsigned i;
    double z_val;
    //PUT CARDINAL DIRECTION HERE ASWELL
    /* StreetSegmentInfo s = getStreetSegmentInfo(path[0]);
     LatLon inter1 = getIntersectionPosition(s.from);
     LatLon inter2 = getIntersectionPosition(s.to);
     std::string cardinal_dir;
     if (s.curvePointCount != 0) {
         inter2 = getStreetSegmentCurvePoint(path[0], 0);
     }
     double angle = atan2(lat_to_y(inter2) - lat_to_y(inter1), lon_to_x(inter2) - lon_to_x(inter1)) / DEG_TO_RAD;
     std::cout << lat_to_y(inter2) - lat_to_y(inter1) << " " << lon_to_x(inter2) - lon_to_x(inter1) << "\n";
     std::cout << angle << "\n";
     if (angle > 135 && angle < -135){
         cardinal_dir = "West";
     }
     else if (angle > -135 && angle <-45){
         cardinal_dir = "South";
     }
     else if (angle > 45 && angle <135){
         cardinal_dir = "North";
     }
     else{
         cardinal_dir = "East";
     }*/

    if (getStreetName(getStreetSegmentInfo(path[0]).streetID) != "<unknown>") {
        std::string dist = boost::lexical_cast<std::string>(round(straight_distances[0]));
        path_directions.push_back("Head onto " + getStreetName(getStreetSegmentInfo(path[0]).streetID) + " for " + dist + " m");
    } else {
        path_directions.push_back("Head towards " + getStreetName(getStreetSegmentInfo(path[1]).streetID));
    }

    int counter = 0;
    for (i = 0; i < path.size() - 1; ++i) {
        //path[i] // gives current segment
        //path[i+1] //gives next segment
        //take the x and y coordinates and calculate its determinant

        unsigned starting_intersection;
        unsigned common_intersection;
        unsigned ending_intersection;

        //Gets the current and next segments
        StreetSegmentInfo seg1 = getStreetSegmentInfo(path[i]);
        StreetSegmentInfo seg2 = getStreetSegmentInfo(path[i + 1]);
        StreetSegmentInfo previousSeg;
        if (i > 0)
            previousSeg = getStreetSegmentInfo(path[i - 1]);
        //Checking for the common node
        if (seg1.to == seg2.from) {

            starting_intersection = seg1.from;
            common_intersection = seg1.to;
            ending_intersection = seg2.to;

        } else if (seg1.from == seg2.to) {

            starting_intersection = seg1.to;
            common_intersection = seg1.from;
            ending_intersection = seg2.from;

        } else if (seg1.to == seg2.to) {

            starting_intersection = seg1.from;
            common_intersection = seg1.to;
            ending_intersection = seg2.from;

        } else {

            starting_intersection = seg1.to;
            common_intersection = seg1.from;
            ending_intersection = seg2.to;
        }

        std::pair<LatLon, LatLon> begin_street;
        begin_street.first = getIntersectionPosition(starting_intersection);
        begin_street.second = getIntersectionPosition(common_intersection);
        std::pair<LatLon, LatLon> end_street;
        end_street.first = getIntersectionPosition(common_intersection);
        end_street.second = getIntersectionPosition(ending_intersection);

        z_val = determinant(begin_street, end_street);

        if (seg1.streetID != seg2.streetID) {
            if (z_val > 0) {
                if (seg1.streetID != seg2.streetID) {
                    if (getStreetName(seg2.streetID) != "<unknown>") {
                        path_directions.push_back("Turn left onto " + getStreetName(seg2.streetID));

                    } else {
                        path_directions.push_back("Turn left ");
                    }
                    //path_directions.push_back("left");
                } else {
                    ;
                }
            } else if (z_val < 0) {
                if (seg1.streetID != seg2.streetID) {
                    if (getStreetName(seg2.streetID) != "<unknown>") {
                        path_directions.push_back("Turn right onto " + getStreetName(seg2.streetID));

                    } else {
                        path_directions.push_back("Turn right");
                    }
                    //path_directions.push_back("right");
                } else {
                    ;
                }
            }
            counter++;
        } else {
            if (i > 0) {
                if (seg1.streetID != previousSeg.streetID) {
                    std::string dist = boost::lexical_cast<std::string>(round(straight_distances[counter]));
                    if (getStreetName(seg2.streetID) != "<unknown>") {

                        path_directions.push_back("Continue on " + getStreetName(seg1.streetID) + " for " + dist + " m");

                    } else {

                        path_directions.push_back("Continue heading straight on current road for " + dist + " m");
                    }
                    straight_distances[counter] = straight_distances[counter] - find_street_segment_length(path[i]);
                } else {
                    ;
                } // skip
            }
        }
    }
    path_directions.push_back("You have arrived at your destination");
    return path_directions;
}


#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Geometry>
#include <stdio.h>
#include <stdlib.h>
#include <tbb/atomic.h>

#include <boost/thread.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

#include "std_msgs/Int32.h"
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"
#include "sepanta_msgs/arm.h"
#include "sepanta_msgs/omnidata.h"
#include "sepanta_msgs/head.h"
#include "sepanta_msgs/irsensor.h"
#include "sepanta_msgs/motortorques.h"

#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/GetMap.h>
#include <nav_msgs/OccupancyGrid.h>

#include <actionlib/server/simple_action_server.h>
#include <sepanta_msgs/sepantaAction.h>

#include "math.h"

using namespace std;
using namespace boost;

nav_msgs::OccupancyGrid static_map;

bool first_init = false;
Eigen::MatrixXf IK_matrix;

float Odometry_raw[3] = {0};

float odom_position_yaw[3] = {0};
float slam_position_yaw[3] = {0};
float odom_speed_xyw[3] = {0};

int speed_Motor[4] = {128,128,128,128};

bool App_exit = false;
int Odometry_speed = 50;

float ratio_X = 1000;
float ratio_Y = 1041;
float ratio_W = 860;

float kp_degree = 3.5;
float ki_degree = 0.03;
float kp_move = 8.5;

float L = 50; //cm
bool isrobotmove = false;
ros::Publisher chatter_pub[20];

float laser_IR[8];
float IR[8];

float pid_i_buffer[20] = {0};

typedef actionlib::SimpleActionServer<sepanta_msgs::sepantaAction> slam_Server;
slam_Server *globalServer;

bool cancel_request = false;
int Compass =0;

void omnidrive(int x, int y, int w);

float ConvertQuatToYaw(const geometry_msgs::Quaternion &quat)
{
    float yaw = tf::getYaw(quat);
    return isnan(yaw) ? 0 : yaw;
}

float convert_w_radps(float w) {
    return w / (ratio_W);
}

float convert_vy_mps(float vy) {
    return vy / (ratio_Y);
}

float convert_vx_mps(float vx) {
    return vx / (ratio_X);
}

float convert_radps_w(float w) {
    return w * (ratio_W);
}

float convert_mps_vy(float mps) {
    return mps * (ratio_Y);
}

float convert_mps_vx(float mps) {
    return mps * (ratio_X);
}

inline float Deg2Rad(float deg) {
    return deg * M_PI / 180;
}

inline float Rad2Deg(float rad) {
    return rad * 180 / M_PI;
}

void reset_position()
{
   odom_position_yaw[0] = 0;
   odom_position_yaw[1] = 0;
   odom_position_yaw[2] = 0;
}

void Init()
{
    Eigen::MatrixXf matrix(3,4);
    IK_matrix = matrix;

    IK_matrix(0, 0) = 0.25;
    IK_matrix(0, 1) = 0.25;
    IK_matrix(0, 2) = -0.25;
    IK_matrix(0, 3) = -0.25;

    IK_matrix(1, 0) = -0.25;
    IK_matrix(1, 1) = 0.25;
    IK_matrix(1, 2) = -0.25;
    IK_matrix(1, 3) = 0.25;

    IK_matrix(2, 0) = -0.02;
    IK_matrix(2, 1) = 0.02;
    IK_matrix(2, 2) = 0.02;
    IK_matrix(2, 3) = -0.02;
}

void publish_odometry_base_pose()
{
    geometry_msgs::Pose pose;
    pose.position.x = odom_position_yaw[0];
    pose.position.y = odom_position_yaw[1];
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(slam_position_yaw[2]);
    pose.orientation = odom_quat;

    chatter_pub[5].publish(pose);
}

void IK_solver(float delta_time)
{
	float a ;
    float b ;
    float c ;
    float d ;

    a = speed_Motor[2];
    b = speed_Motor[3];
    c = speed_Motor[1];
    d = speed_Motor[0];

    a = (a - 128) * 7.5;
    b = (b - 128) * 7.5;
    c = (c - 128) * 7.5;
    d = (d - 128) * 7.5;

    Odometry_raw[0] = IK_matrix(0, 0) * a + IK_matrix(0, 1) * b + IK_matrix(0, 2) * c + IK_matrix(0, 3) * d;
    Odometry_raw[1] = IK_matrix(1, 0) * a + IK_matrix(1, 1) * b + IK_matrix(1, 2) * c + IK_matrix(1, 3) * d;
    Odometry_raw[2] = IK_matrix(2, 0) * a + IK_matrix(2, 1) * b + IK_matrix(2, 2) * c + IK_matrix(2, 3) * d;

    odom_speed_xyw[0] = Odometry_raw[0];
    odom_speed_xyw[1] = Odometry_raw[1];
    odom_speed_xyw[2] = (-Odometry_raw[2]) * 100;

    odom_speed_xyw[0] = convert_vx_mps(odom_speed_xyw[0]);
    odom_speed_xyw[1] = convert_vy_mps(odom_speed_xyw[1]);
    odom_speed_xyw[2] = convert_w_radps(odom_speed_xyw[2]) / 4;

    //cout<<odom_speed_xyw[0]<<" "<<odom_speed_xyw[1]<<" "<<odom_speed_xyw[2]<<endl;

    odom_position_yaw[0] += odom_speed_xyw[0] * delta_time; //meter
    odom_position_yaw[1] += odom_speed_xyw[1] * delta_time; //meter
    odom_position_yaw[2] += odom_speed_xyw[2] * delta_time; //radian

    cout<<odom_position_yaw[0]<<" "<<odom_position_yaw[1]<<" "<<Rad2Deg(odom_position_yaw[2])<<endl;

    publish_odometry_base_pose();
}

void Turn_GL(int degree)
{
    // ros::Rate r(50);
    // if (isrobotmove == true) return;
    // isrobotmove = true;

    // if (degree > 360) degree -= 360;
    // if (degree < 0) degree += 360;

    // float error = Compass - degree;

    // while ((error > 2 || error < -2 )&& App_exit == false)
    // {
    //     error = Compass - degree;

    //     if (error >= 180) error = error - 360;
    //     if (error < -180) error = 360 - error;

    //     if (error > 150) error = 150;
    //     if (error < -150) error = -150;

    //     float error_total = error * kp_degree;
    //     omnidrive(0, 0, error_total);
    //     if ( cancel_request ) break;
    //     r.sleep();
    // }

    // omnidrive(0, 0, 0);
    // r.sleep();
    // isrobotmove = false;
}

void Turn_GL_local(int degree)
{
    ros::Rate r(50);
    if (isrobotmove == true) return;
    isrobotmove = true;

     float error = Deg2Rad(degree);

     int s = 1;
     if ( error < 0 ) s = -1;
     error = abs(error);

     reset_position();

     while (  abs(odom_position_yaw[2]) < error && App_exit == false)
     {
     	cout<<Rad2Deg(abs(odom_position_yaw[2]))<<" "<<Rad2Deg(error)<<endl;
        omnidrive(0, 0, -1 * 100 * s);
        if ( cancel_request ) break;
        r.sleep();
    }

    omnidrive(0, 0, 0);
    isrobotmove = false;
}

void Move_GLX(float SV, float distance)
{
    ros::Rate r(50);
    if (isrobotmove == true) return;

    isrobotmove = true;

    reset_position();

    if ( distance < 0 ) SV = SV * -1;

    distance = abs(distance) / 100;

    if (SV != 0 && distance > 0)
    {
        while ( abs(odom_position_yaw[0]) < distance && App_exit == false)
        {
            cout<<abs(odom_position_yaw[0])<<" "<<distance<<endl;
            omnidrive(SV, 0, 0);
            if ( cancel_request ) break;
            r.sleep();
        }
    }

    omnidrive(0, 0, 0);
    r.sleep();

    isrobotmove = false;
}

void Move_GLY(float SV, float distance)
{
    ros::Rate r(50);

    if (isrobotmove == true) return;
    isrobotmove = true;

    reset_position();

    if ( distance < 0 ) SV = SV * -1;

    distance = abs(distance) / 100;

    if (SV != 0 && distance > 0)
    {
        while ( abs(odom_position_yaw[1]) < distance && App_exit == false)
        {
        	cout<<abs(odom_position_yaw[1])<<" "<<distance<<endl;
            omnidrive(0, SV, 0);
            if ( cancel_request ) break;
            r.sleep();
        }
    }

    omnidrive(0, 0, 0);
    r.sleep();

    isrobotmove = false;
}

void chatterCallback_pose(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    cout<<"get"<<endl;
    geometry_msgs::Pose pose;
    geometry_msgs::Point point;
    geometry_msgs::Quaternion quaternion;
    pose = msg->pose;

    slam_position_yaw[0] = pose.position.x; //meter
    slam_position_yaw[1] = pose.position.y; //meter
    slam_position_yaw[2] = ConvertQuatToYaw(pose.orientation); //radians
}

void chatterCallback_omnispeed(const sepanta_msgs::omnidata::ConstPtr &msg)
{
    speed_Motor[0] = msg->d0;
    speed_Motor[1] = msg->d1;
    speed_Motor[2] = msg->d2;
    speed_Motor[3] = msg->d3;
}

void chatterCallback_cmd_vel(const geometry_msgs::Twist &twist_aux)
{
    double vel_x = twist_aux.linear.x;
    double vel_y =  twist_aux.linear.y;
    double vel_th = -1 * twist_aux.angular.z;

    int xx = 0;
    int yy = 0;
    int ww = 0;

    xx = (int)convert_mps_vx(vel_x);
    yy = (int)convert_mps_vy(vel_y);
    ww = (int)convert_radps_w(vel_th);

    omnidrive(xx,yy,ww);
}

void omnidrive(int x,int y,int w)
{
   sepanta_msgs::omnidata msg_data;
   msg_data.d0 = x;
   msg_data.d1 = y;
   msg_data.d2 = w;

   chatter_pub[0].publish(msg_data);
}

void PreemptThread()
{

   while ( App_exit == false )
   {

        if (globalServer->isPreemptRequested() )
        {
            cancel_request = true;
            ROS_INFO(" cancle requested\n");
            globalServer->setAborted();
            ROS_INFO(" aborted done\n");
            break;
        }

        boost::this_thread::sleep(boost::posix_time::milliseconds(100));
   }
}

class myactionserver
{
protected:

  ros::NodeHandle nh_;

  actionlib::SimpleActionServer<sepanta_msgs::sepantaAction> as_;
  std::string action_name_;

  sepanta_msgs::sepantaFeedback feedback_;
  sepanta_msgs::sepantaResult result_;

public:

  myactionserver(std::string name) :
    as_(nh_, name, boost::bind(&myactionserver::executeCB, this, _1), false),
    action_name_(name)
  {
    as_.start();
    globalServer = &as_;
    //globalServer = &as_;
  }

  ~myactionserver(void)
  {
  }

  void executeCB(const sepanta_msgs::sepantaGoalConstPtr &interfacegoal)
  {

    boost::thread Preempt_thread(&PreemptThread);
    cancel_request = false;

    bool success = true;

    ROS_INFO("EXECUTE NEW ACTION");

    int value = interfacegoal->value;

    if ( interfacegoal->type == "movex")
    {
        Move_GLX(Odometry_speed,value);
    }
    else if ( interfacegoal->type == "movey")
    {
        Move_GLY(Odometry_speed,value);
    }
    else if ( interfacegoal->type == "turn")
    {
        Turn_GL_local(value);
    }
    // else if ( interfacegoal->type == "turngllocal" )
    // {
    //    // Turn_GL_local(value);
    // }

    omnidrive(0, 0, 0);

    if(success)
    {
       result_.result= "DONE";
       cout<<"DONE"<<endl;
       as_.setSucceeded(result_);
    }
    else
    {
        result_.result= "MOVEMENT FAILED";
        as_.setSucceeded(result_);
    }

    Preempt_thread.interrupt();
    Preempt_thread.join();
  }

};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "odometry_base");
  cout << "SEPANTA III odometry system started" << endl;

  ros::Time::init();
  Init();

  ros::Rate ros_rate(13); //check lowerbody hz

  ros::NodeHandle node_handles[15];
  ros::Subscriber sub_handles[15];

  chatter_pub[0] = node_handles[0].advertise<sepanta_msgs::omnidata>("lowerbodycore/omnidrive", 1);
  chatter_pub[1] = node_handles[1].advertise<nav_msgs::Odometry>("odom", 10);
  chatter_pub[4] = node_handles[4].advertise<nav_msgs::Odometry>("odometry_base/odometry", 10);
  chatter_pub[5] = node_handles[5].advertise<geometry_msgs::Pose>("odometry_base/pose", 10);
  sub_handles[1] = node_handles[2].subscribe("sepantamovebase/cmd_vel", 10, chatterCallback_cmd_vel);
  sub_handles[4] = node_handles[5].subscribe("lowerbodycore/omnispeed", 10, chatterCallback_omnispeed);
  sub_handles[6] = node_handles[7].subscribe("slam_out_pose", 10, chatterCallback_pose);

  tf::TransformBroadcaster odom_broadcaster;
  tf::TransformBroadcaster odom_broadcaster2;

  myactionserver act("odometry_action");

  while(ros::ok())
  {
    ros::spinOnce();
    //======================================================
    IK_solver(0.076); //13 hz

    //std_msgs::Int32 mes;
    //mes.data = isrobotmove;
    //chatter_pub[3].publish(mes);

    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(slam_position_yaw[2]);

    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = ros::Time::now();
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

    odom_trans.transform.translation.x = slam_position_yaw[0];
    odom_trans.transform.translation.y = slam_position_yaw[1];
    odom_trans.transform.translation.z = 0;
    odom_trans.transform.rotation = odom_quat;

    bool _p = false;

    if ( _p )
    odom_broadcaster.sendTransform(odom_trans);

    nav_msgs::Odometry odom;
    odom.header.stamp = ros::Time::now();
    odom.header.frame_id = "odom";

    //set the position
    odom.pose.pose.position.x = slam_position_yaw[0];
    odom.pose.pose.position.y = slam_position_yaw[1];
    odom.pose.pose.position.z = 0;
    odom.pose.pose.orientation = odom_quat;

    // //set the velocity
    // odom.child_frame_id = "base_link";
    // odom.twist.twist.linear.x = 0;
    // odom.twist.twist.linear.y = 0;
    // odom.twist.twist.angular.z = 0;

    //publish the message
    if ( _p )
    chatter_pub[1].publish(odom);

    // // publish isrobotmove

    //===================================================
    // odom_quat = tf::createQuaternionMsgFromYaw(odom_position_yaw[2]);

    // odom_trans.header.stamp = ros::Time::now();
    // odom_trans.header.frame_id = "odom_base";
    // odom_trans.child_frame_id = "base_link2";

    // odom_trans.transform.translation.x = odom_position_yaw[0];
    // odom_trans.transform.translation.y = odom_position_yaw[1];
    // odom_trans.transform.translation.z = 0;
    // odom_trans.transform.rotation = odom_quat;

    // odom_broadcaster.sendTransform(odom_trans);

    // odom.header.stamp = ros::Time::now();
    // odom.header.frame_id = "odom_base";

    // //set the position
    // odom.pose.pose.position.x = odom_position_yaw[0];
    // odom.pose.pose.position.y = odom_position_yaw[1];
    // odom.pose.pose.position.z = 0;
    // odom.pose.pose.orientation = odom_quat;

    // //set the velocity
    // odom.child_frame_id = "base_link2";
    // odom.twist.twist.linear.x = 0;
    // odom.twist.twist.linear.y = 0;
    // odom.twist.twist.angular.z = 0;

    // //publish the message
    // chatter_pub[4].publish(odom);

    ros_rate.sleep();
  }

  App_exit = true;

  return 0;
}