/*
   bern_modp.cpp:  computing isolated Bernoulli numbers modulo p

   Copyright (C) 2008, 2009, David Harvey

   This file is part of the bernmm package (version 1.1).

   bernmm is released under a BSD-style license. See the README file in
   the source distribution for details.
*/

#include <limits.h>
#include <signal.h>
#include <cstring>
#include <gmp.h>
#include <NTL/ZZ.h>
#include "bern_modp_util.h"
#include "bern_modp.h"

NTL_CLIENT;

using namespace std;

namespace bernmm {

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

   Computing the main sum (general case)

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

/*
   Returns (1 - g^k) B_k / 2k mod p.

   PRECONDITIONS:
      5 <= p < NTL_SP_BOUND, p prime
      2 <= k <= p-3, k even
      pinv = PrepMulMod(p)
      g = a multiplicative generator of GF(p), in [0, p)
*/
long bernsum_powg(long p, mulmod_t pinv, long k, long g)
{
   long half_gm1 = (g + ((g & 1) ? 0 : p) - 1) / 2;    // (g-1)/2 mod p
   long g_to_jm1 = 1;
   long g_to_km1 = PowerMod(g, k-1, p, pinv);
   long g_to_km1_to_j = g_to_km1;
   long sum = 0;
   muldivrem_t g_pinv = PrepMulDivRem(g, p);
   mulmod_precon_t g_to_km1_pinv = PrepMulModPrecon(g_to_km1, p, pinv);

   for (long j = 1; j <= (p-1)/2; j++)
   {
      // at this point,
      //    g_to_jm1 holds g^(j-1) mod p
      //    g_to_km1_to_j holds (g^(k-1))^j mod p

      // update g_to_jm1 and compute q = (g*(g^(j-1) mod p) - (g^j mod p)) / p
      long q;
      g_to_jm1 = MulDivRem(q, g_to_jm1, g, p, g_pinv);

      // compute h = -h_g(g^j) = q - (g-1)/2
      long h = SubMod(q, half_gm1, p);

      // add h_g(g^j) * (g^(k-1))^j to running total
      sum = SubMod(sum, MulMod(h, g_to_km1_to_j, p, pinv), p);

      // update g_to_km1_to_j
      g_to_km1_to_j = MulModPrecon(g_to_km1_to_j, g_to_km1, p, g_to_km1_pinv);
   }

   return sum;
}

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

   Computing the main sum (c = 1/2 case)

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

/*
   The Expander class stores precomputed information for a fixed integer p,
   that subsequently permits fast computation of the binary expansion of s/p
   for any 0 < s < p.

   The constructor takes p and max_words as input. Must have 1 <= max_words <=
   MAX_INV. It computes an approximation to 1/p.

   The function expand(word_t* res, long s, long n) computes n words of s/p.
   Must have 0 < s < p and 1 <= n <= max_words. The output is written to res.
   The first word of output is junk. The next n words are the digits of s/p,
   from least to most significant. The buffer must be at least n+2 words long
   (even though the first and last words are never used for output).

   A "word" is a word_t, and contains WORD_BITS bits. On most systems this
   will be an mp_limb_t.
*/

#define MAX_INV 256

#if (GMP_NAIL_BITS == 0) && (GMP_LIMB_BITS >= ULONG_BITS)
// fast mpn-based version

typedef mp_limb_t word_t;
#define WORD_BITS GMP_LIMB_BITS

class Expander
{
private:
   // Approximation to 1/p. We store (max_words + 1) limbs.
   mp_limb_t pinv[MAX_INV + 2];
   mp_limb_t p;
   int max_words;

public:
   Expander(long p, int max_words)
   {
      assert(max_words >= 1);
      assert(max_words <= MAX_INV);

      this->max_words = max_words;
      this->p = p;
      mp_limb_t one = 1;
      mpn_divrem_1(pinv, max_words + 1, &one, 1, p);
   }

   void expand(word_t* res, long s, int n)
   {
      assert(s > 0 && s < p);
      assert(n >= 1);
      assert(n <= max_words);

      if (s == 1)
      {
         // already have 1/p; just copy it
         for (int i = 1; i <= n; i++)
            res[i] = pinv[max_words - n + i];
      }
      else
      {
         mpn_mul_1(res, pinv + max_words - n, n + 1, (mp_limb_t) s);

         // If the first output limb is really close to 0xFFFF..., then there's
         // a possibility of overflow, so fall back on doing division directly.
         // This should happen extremely rarely --- essentially never on a
         // 64-bit system, and very occasionally on a 32-bit system.
         if (res[0] > -((mp_limb_t) s))
         {
            mp_limb_t ss = s;
            mpn_divrem_1(res, n + 1, &ss, 1, p);
         }
      }
   }
};

#else
// slow mpz-based version, since GMP is using nails, or mp_limb_t is
// absurdly narrow

typedef unsigned long word_t;
#define WORD_BITS ULONG_BITS

class Expander
{
private:
   mp_limb_t p;
   mpz_t temp;

public:
   Expander(long p, int max_words)
   {
      this->p = p;
      mpz_init(temp);
   }

   ~Expander()
   {
      mpz_clear(temp);
   }

   void expand(word_t* res, long s, int n)
   {
      assert(s > 0 && s < p);
      assert(n >= 1);

      mpz_set_ui(temp, s);
      mpz_mul_2exp(temp, temp, WORD_BITS * n);
      mpz_fdiv_q_ui(temp, temp, p);
      mpz_export(res + 1, NULL, -1, sizeof(word_t), 0, 0, temp);
   }
};

#endif

/*
   Returns (2^(-k) - 1) 2 B_k / k  mod p.

   (Note: this is useless if 2^k = 1 mod p.)

   PRECONDITIONS:
      5 <= p < NTL_SP_BOUND, p prime
      2 <= k <= p-3, k even
      pinv = PrepMulMod(p)
      g = a multiplicative generator of GF(p), in [0, p)
      n = multiplicative order of 2 in GF(p)
*/

#define TABLE_LG_SIZE 8
#define TABLE_SIZE (((word_t) 1) << TABLE_LG_SIZE)
#define TABLE_MASK (TABLE_SIZE - 1)
#define NUM_TABLES (WORD_BITS / TABLE_LG_SIZE)

#if WORD_BITS % TABLE_LG_SIZE != 0
#error Number of bits in a long must be divisible by TABLE_LG_SIZE
#endif

long bernsum_pow2(long p, mulmod_t pinv, long k, long g, long n)
{
   // In the main summation loop we accumulate data into the _tables_ array;
   // tables[y][z] contributes to the final answer with a weight of
   //
   // sum(-(-1)^z[t] * (2^(k-1))^(WORD_BITS - 1 - y * TABLE_LG_SIZE - t) :
   //                                                  0 <= t < TABLE_LG_SIZE),
   //
   // where z[t] denotes the t-th binary digit of z (LSB is t = 0).
   // The memory footprint for _tables_ is 4KB on a 32-bit machine, or 16KB
   // on a 64-bit machine, so should fit easily into L1 cache.
   long tables[NUM_TABLES][TABLE_SIZE];
   memset(tables, 0, sizeof(long) * NUM_TABLES * TABLE_SIZE);

   long m = (p-1) / n;

   // take advantage of symmetry (n' and m' from the paper)
   if (n & 1)
      m >>= 1;
   else
      n >>= 1;

   // g^(k-1)
   long g_to_km1 = PowerMod(g, k-1, p, pinv);
   // 2^(k-1)
   long two_to_km1 = PowerMod(2, k-1, p, pinv);
   // B^(k-1), where B = 2^WORD_BITS
   long B_to_km1 = PowerMod(two_to_km1, WORD_BITS, p, pinv);
   // B^(MAX_INV)
   long s_jump = PowerMod(2, MAX_INV * WORD_BITS, p, pinv);

   // help speed up modmuls
   mulmod_precon_t g_pinv = PrepMulModPrecon(g, p, pinv);
   mulmod_precon_t g_to_km1_pinv = PrepMulModPrecon(g_to_km1, p, pinv);
   mulmod_precon_t two_to_km1_pinv = PrepMulModPrecon(two_to_km1, p, pinv);
   mulmod_precon_t B_to_km1_pinv = PrepMulModPrecon(B_to_km1, p, pinv);
   mulmod_precon_t s_jump_pinv = PrepMulModPrecon(s_jump, p, pinv);

   long g_to_km1_to_i = 1;
   long g_to_i = 1;
   long sum = 0;

   // Precompute some of the binary expansion of 1/p; at most MAX_INV words,
   // or possibly less if n is sufficiently small
   Expander expander(p, (n >= MAX_INV * WORD_BITS)
                                       ? MAX_INV : ((n - 1) / WORD_BITS + 1));

   // =========== phase 1: main summation loop

   // loop over outer sum
   for (long i = 0; i < m; i++)
   {
      // s keeps track of g^i*2^j mod p
      long s = g_to_i;
      // x keeps track of (g^i*2^j)^(k-1) mod p
      long x = g_to_km1_to_i;

      // loop over inner sum; break it up into chunks of length at most
      // MAX_INV * WORD_BITS. If n is large, this allows us to do most of
      // the work with mpn_mul_1 instead of mpn_divrem_1, and also improves
      // memory locality.
      for (long nn = n; nn > 0; nn -= MAX_INV * WORD_BITS)
      {
         word_t s_over_p[MAX_INV + 2];
         long bits, words;

         if (nn >= MAX_INV * WORD_BITS)
         {
            // do one chunk of length exactly MAX_INV * WORD_BITS
            bits = MAX_INV * WORD_BITS;
            words = MAX_INV;
         }
         else
         {
            // last chunk of length less than MAX_INV * WORD_BITS
            bits = nn;
            words = (nn - 1) / WORD_BITS + 1;
         }

         // compute some bits of the binary expansion of s/p
         expander.expand(s_over_p, s, words);
         word_t* next = s_over_p + words;

         // loop over whole words
         for (; bits >= WORD_BITS; bits -= WORD_BITS, next--)
         {
            word_t y = *next;

#if NUM_TABLES != 8 && NUM_TABLES != 4
            // generic version
            for (long h = 0; h < NUM_TABLES; h++)
            {
               long& target = tables[h][y & TABLE_MASK];
               target = SubMod(target, x, p);
               y >>= TABLE_LG_SIZE;
            }
#else
            // unrolled versions for 32-bit/64-bit machines
            long& target0 = tables[0][y & TABLE_MASK];
            target0 = SubMod(target0, x, p);

            long& target1 = tables[1][(y >> TABLE_LG_SIZE) & TABLE_MASK];
            target1 = SubMod(target1, x, p);

            long& target2 = tables[2][(y >> (2*TABLE_LG_SIZE)) & TABLE_MASK];
            target2 = SubMod(target2, x, p);

            long& target3 = tables[3][(y >> (3*TABLE_LG_SIZE)) & TABLE_MASK];
            target3 = SubMod(target3, x, p);
#if NUM_TABLES == 8
            long& target4 = tables[4][(y >> (4*TABLE_LG_SIZE)) & TABLE_MASK];
            target4 = SubMod(target4, x, p);

            long& target5 = tables[5][(y >> (5*TABLE_LG_SIZE)) & TABLE_MASK];
            target5 = SubMod(target5, x, p);

            long& target6 = tables[6][(y >> (6*TABLE_LG_SIZE)) & TABLE_MASK];
            target6 = SubMod(target6, x, p);

            long& target7 = tables[7][(y >> (7*TABLE_LG_SIZE)) & TABLE_MASK];
            target7 = SubMod(target7, x, p);
#endif
#endif

            x = MulModPrecon(x, B_to_km1, p, B_to_km1_pinv);
         }

         // loop over remaining bits in the last word
         word_t y = *next;
         for (; bits > 0; bits--)
         {
            if (y & (((word_t) 1) << (WORD_BITS - 1)))
               sum = SubMod(sum, x, p);
            else
               sum = AddMod(sum, x, p);

            x = MulModPrecon(x, two_to_km1, p, two_to_km1_pinv);
            y <<= 1;
         }

         // update s
         s = MulModPrecon(s, s_jump, p, s_jump_pinv);
      }

      // update g^i and (g^(k-1))^i
      g_to_i = MulModPrecon(g_to_i, g, p, g_pinv);
      g_to_km1_to_i = MulModPrecon(g_to_km1_to_i, g_to_km1, p, g_to_km1_pinv);
   }

   // =========== phase 2: consolidate table data

   // compute weights[z] = sum((-1)^z[t] * (2^(k-1))^(TABLE_LG_SIZE - 1 - t) :
   //                                                  0 <= t < TABLE_LG_SIZE).

   long weights[TABLE_SIZE];
   weights[0] = 0;
   for (long h = 0, x = 1; h < TABLE_LG_SIZE;
        h++, x = MulModPrecon(x, two_to_km1, p, two_to_km1_pinv))
   {
      for (long i = (1L << h) - 1; i >= 0; i--)
      {
         weights[2*i+1] = SubMod(weights[i], x, p);
         weights[2*i]   = AddMod(weights[i], x, p);
      }
   }

   // combine table data with weights

   long x_jump = PowerMod(two_to_km1, TABLE_LG_SIZE, p, pinv);

   for (long h = NUM_TABLES - 1, x = 1; h >= 0; h--)
   {
      mulmod_precon_t x_pinv = PrepMulModPrecon(x, p, pinv);

      for (long i = 0; i < TABLE_SIZE; i++)
      {
         long y = MulMod(tables[h][i], weights[i], p, pinv);
         y = MulModPrecon(y, x, p, x_pinv);
         sum = SubMod(sum, y, p);
      }

      x = MulModPrecon(x_jump, x, p, x_pinv);
   }

   return sum;
}

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

   Computing the main sum (c = 1/2 case, with REDC arithmetic)

   Throughout this section F denotes 2^(ULONG_BITS / 2).

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

/*
   Returns x/F mod n. Output is in [0, 2n), i.e. *not* reduced completely
   into [0, n).

   PRECONDITIONS:
      3 <= n < F, n odd
      0 <= x < nF    (if n < F/2)
      0 <= x < nF/2  (if n > F/2)
      ninv2 = -1/n mod F
*/
#define LOW_MASK ((1L << (ULONG_BITS / 2)) - 1)
static inline long RedcFast(long x, long n, long ninv2)
{
   unsigned long y = (x * ninv2) & LOW_MASK;
   unsigned long z = x + (n * y);
   return z >> (ULONG_BITS / 2);
}

/*
   Same as RedcFast(), but reduces output into [0, n).
*/
static inline long Redc(long x, long n, long ninv2)
{
   long y = RedcFast(x, n, ninv2);
   if (y >= n)
      y -= n;
   return y;
}

/*
   Computes -1/n mod F, in [0, F).

   PRECONDITIONS:
      3 <= n < F, n odd
*/
long PrepRedc(long n)
{
   long ninv2 = -n;   // already correct mod 8

   // newton's method for 2-adic inversion
   for (long bits = 3; bits < ULONG_BITS/2; bits *= 2)
      ninv2 = 2*ninv2 + n * ninv2 * ninv2;

   return ninv2 & LOW_MASK;
}

/*
   Same as bernsum_pow2(), but uses REDC arithmetic, and various delayed
   reduction strategies.

   PRECONDITIONS:
      Same as bernsum_pow2(), and in addition:
      p < 2^(ULONG_BITS/2 - 1)

   (See bernsum_pow2() for code comments; we only add comments here where
   something is different from bernsum_pow2())
*/
long bernsum_pow2_redc(long p, mulmod_t pinv, long k, long g, long n)
{
   long pinv2 = PrepRedc(p);
   long F = (1L << (ULONG_BITS/2)) % p;

   long tables[NUM_TABLES][TABLE_SIZE];
   memset(tables, 0, sizeof(long) * NUM_TABLES * TABLE_SIZE);

   long m = (p-1) / n;

   if (n & 1)
      m >>= 1;
   else
      n >>= 1;

   long g_to_km1 = PowerMod(g, k-1, p, pinv);
   long two_to_km1 = PowerMod(2, k-1, p, pinv);
   long B_to_km1 = PowerMod(two_to_km1, WORD_BITS, p, pinv);
   long s_jump = PowerMod(2, MAX_INV * WORD_BITS, p, pinv);

   long g_redc = MulMod(g, F, p, pinv);
   long g_to_km1_redc = MulMod(g_to_km1, F, p, pinv);
   long two_to_km1_redc = MulMod(two_to_km1, F, p, pinv);
   long B_to_km1_redc = MulMod(B_to_km1, F, p, pinv);
   long s_jump_redc = MulMod(s_jump, F, p, pinv);

   long g_to_km1_to_i = 1;    // always in [0, 2p)
   long g_to_i = 1;           // always in [0, 2p)
   long sum = 0;

   Expander expander(p, (n >= MAX_INV * WORD_BITS)
                                       ? MAX_INV : ((n - 1) / WORD_BITS + 1));

   // =========== phase 1: main summation loop

   for (long i = 0; i < m; i++)
   {
      long s = g_to_i;           // always in [0, p)
      if (s >= p)
         s -= p;

      long x = g_to_km1_to_i;    // always in [0, 2p)

      for (long nn = n; nn > 0; nn -= MAX_INV * WORD_BITS)
      {
         word_t s_over_p[MAX_INV + 2];
         long bits, words;

         if (nn >= MAX_INV * WORD_BITS)
         {
            bits = MAX_INV * WORD_BITS;
            words = MAX_INV;
         }
         else
         {
            bits = nn;
            words = (nn - 1) / WORD_BITS + 1;
         }

         expander.expand(s_over_p, s, words);
         word_t* next = s_over_p + words;

         for (; bits >= WORD_BITS; bits -= WORD_BITS, next--)
         {
            word_t y = *next;

            // note: we add the values into tables *without* reduction mod p

#if NUM_TABLES != 8 && NUM_TABLES != 4
            // generic version
            for (long h = 0; h < NUM_TABLES; h++)
            {
               tables[h][y & TABLE_MASK] += x;
               y >>= TABLE_LG_SIZE;
            }
#else
            // unrolled versions for 32-bit/64-bit machines
            tables[0][ y                       & TABLE_MASK] += x;
            tables[1][(y >>    TABLE_LG_SIZE ) & TABLE_MASK] += x;
            tables[2][(y >> (2*TABLE_LG_SIZE)) & TABLE_MASK] += x;
            tables[3][(y >> (3*TABLE_LG_SIZE)) & TABLE_MASK] += x;
#if NUM_TABLES == 8
            tables[4][(y >> (4*TABLE_LG_SIZE)) & TABLE_MASK] += x;
            tables[5][(y >> (5*TABLE_LG_SIZE)) & TABLE_MASK] += x;
            tables[6][(y >> (6*TABLE_LG_SIZE)) & TABLE_MASK] += x;
            tables[7][(y >> (7*TABLE_LG_SIZE)) & TABLE_MASK] += x;
#endif
#endif

            x = RedcFast(x * B_to_km1_redc, p, pinv2);
         }

         // bring x into [0, p) for next loop
         if (x >= p)
            x -= p;

         word_t y = *next;
         for (; bits > 0; bits--)
         {
            if (y & (((word_t) 1) << (WORD_BITS - 1)))
               sum = SubMod(sum, x, p);
            else
               sum = AddMod(sum, x, p);

            x = Redc(x * two_to_km1_redc, p, pinv2);
            y <<= 1;
         }

         s = Redc(s * s_jump_redc, p, pinv2);
      }

      g_to_i = RedcFast(g_to_i * g_redc, p, pinv2);
      g_to_km1_to_i = RedcFast(g_to_km1_to_i * g_to_km1_redc, p, pinv2);
   }

   // At this point, each table entry is at most p^2 (since x was always
   // in [0, 2p), and the inner loop was called at most (p/2) / WORD_BITS
   // times, and 2p * p/2 / WORD_BITS * TABLE_LG_SIZE <= p^2).

   // =========== phase 2: consolidate table data

   long weights[TABLE_SIZE];
   weights[0] = 0;
   // we store the weights multiplied by a factor of 2^(3*ULONG_BITS/2) to
   // compensate for the three rounds of REDC reduction in the loop below
   for (long h = 0, x = PowerMod(2, 3*ULONG_BITS/2, p, pinv);
        h < TABLE_LG_SIZE; h++, x = Redc(x * two_to_km1_redc, p, pinv2))
   {
      for (long i = (1L << h) - 1; i >= 0; i--)
      {
         weights[2*i+1] = SubMod(weights[i], x, p);
         weights[2*i]   = AddMod(weights[i], x, p);
      }
   }

   long x_jump = PowerMod(two_to_km1, TABLE_LG_SIZE, p, pinv);
   long x_jump_redc = MulMod(x_jump, F, p, pinv);

   for (long h = NUM_TABLES - 1, x = 1; h >= 0; h--)
   {
      for (long i = 0; i < TABLE_SIZE; i++)
      {
         long y;
         y = RedcFast(tables[h][i], p, pinv2);
         y = RedcFast(y * weights[i], p, pinv2);
         y = RedcFast(y * x, p, pinv2);
         sum += y;
      }

      x = Redc(x * x_jump_redc, p, pinv2);
   }

   return sum % p;
}

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

   Wrappers for bernsum_*

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

/*
   Returns B_k/k mod p, in the range [0, p).

   PRECONDITIONS:
      5 <= p < NTL_SP_BOUND, p prime
      2 <= k <= p-3, k even
      pinv = PrepMulMod(p)

   Algorithm: uses bernsum_powg() to compute the main sum.
*/
long _bern_modp_powg(long p, mulmod_t pinv, long k)
{
   Factorisation F(p-1);
   long g = primitive_root(p, pinv, F);

   // compute main sum
   long x = bernsum_powg(p, pinv, k, g);

   // divide by (1 - g^k) and multiply by 2
   long g_to_k = PowerMod(g, k, p, pinv);
   long t = InvMod(p + 1 - g_to_k, p);
   x = MulMod(x, t, p, pinv);
   x = AddMod(x, x, p);

   return x;
}

/*
   Returns B_k/k mod p, in the range [0, p).

   PRECONDITIONS:
      5 <= p < NTL_SP_BOUND, p prime
      2 <= k <= p-3, k even
      pinv = PrepMulMod(p)
      2^k != 1 mod p

   Algorithm: uses bernsum_pow2() (or bernsum_pow2_redc() if p is small
   enough) to compute the main sum.
*/
long _bern_modp_pow2(long p, mulmod_t pinv, long k)
{
   Factorisation F(p-1);
   long g = primitive_root(p, pinv, F);
   long n = order(2, p, pinv, F);

   // compute main sum
   long x;
   if (p < (1L << (ULONG_BITS/2 - 1)))
      x = bernsum_pow2_redc(p, pinv, k, g, n);
   else
      x = bernsum_pow2(p, pinv, k, g, n);

   // divide by 2*(2^(-k) - 1)
   long t = PowerMod(2, -k, p, pinv) - 1;
   t = AddMod(t, t, p);
   t = InvMod(t, p);
   x = MulMod(x, t, p, pinv);

   return x;
}

/*
   Returns B_k/k mod p, in the range [0, p).

   PRECONDITIONS:
      5 <= p < NTL_SP_BOUND, p prime
      2 <= k <= p-3, k even
      pinv = PrepMulMod(p)
*/
long _bern_modp(long p, mulmod_t pinv, long k)
{
   if (PowerMod(2, k, p, pinv) != 1)
      // 2^k != 1 mod p, so we use the faster version
      return _bern_modp_pow2(p, pinv, k);
   else
      // forced to use slower version
      return _bern_modp_powg(p, pinv, k);
}

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

   Main bern_modp() routine

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

long bern_modp(long p, long k)
{
   assert(k >= 0);
   assert(2 <= p && p < NTL_SP_BOUND);

   // B_0 = 1
   if (k == 0)
      return 1;

   // B_1 = -1/2 mod p
   if (k == 1)
   {
      if (p == 2)
         return -1;
      return (p-1)/2;
   }

   // B_k = 0 for odd k >= 3
   if (k & 1)
      return 0;

   // denominator of B_k is always divisible by 6 for k >= 2
   if (p <= 3)
      return -1;

   // use Kummer's congruence (k = m mod p-1  =>  B_k/k = B_m/m mod p)
   long m = k % (p-1);
   if (m == 0)
      return -1;

   mulmod_t pinv = PrepMulMod(p);
   long x = _bern_modp(p, pinv, m);    // = B_m/m mod p
   return MulMod(x, k%p, p, pinv);
}

};    // end namespace

// end of file ================================================================


// Copyright (c) Dewetron 2017
#include "otfft.h"

#include <boost/test/unit_test.hpp>

#include <algorithm>
#include <cmath>
#include <cstdint>
#include <functional>
#include <iterator>
#include <random>
#include <vector>

namespace
{
    template <OTFFT::TransformationType TR_TYPE, OTFFT::OptimizationType OPT_TYPE>
    void testComplexFFT(const std::size_t SIZE)
    {
        std::random_device rnd_device;
        std::mt19937 mersenne_engine(rnd_device());
        std::uniform_real_distribution<double> dist(-99.0, 99.0);
        auto gen = std::bind(dist, mersenne_engine);

        std::vector<OTFFT::complex_t> expected;

        for (auto n = 0; n < SIZE; ++n)
        {
            expected.emplace_back(gen(), gen());
        }

        std::vector<OTFFT::complex_t> spectrum = expected;

        auto fft = TR_TYPE == OTFFT::TransformationType::TRANSFORM_BLUESTEIN ?
                       OTFFT::Factory::createBluesteinFFT(static_cast<int>(SIZE), OPT_TYPE) :
                       OTFFT::Factory::createComplexFFT(static_cast<int>(SIZE), OPT_TYPE);
        {
            OTFFT::complex_vector spectrum_pointer{spectrum.data()};
            fft->fwd0(spectrum_pointer);
            fft->invn(spectrum_pointer);
        }

        for (std::size_t idx{0}; idx < SIZE / 2; ++idx)
        {
            if (std::fabs(expected[idx].Re) < 1e-10)
            {
                BOOST_CHECK_SMALL(spectrum[idx].Re, 1e-8);
            }
            else
            {
                BOOST_CHECK_CLOSE(expected[idx].Re, spectrum[idx].Re, .1);
            }

            if (std::fabs(expected[idx].Im) < 1e-10)
            {
                BOOST_CHECK_SMALL(spectrum[idx].Im, 1e-8);
            }
            else
            {
                BOOST_CHECK_CLOSE(expected[idx].Im, spectrum[idx].Im, .1);
            }
        }
    }

    template <OTFFT::OptimizationType OPT_TYPE>
    void testRealFFT(const std::size_t SIZE)
    {
        std::random_device rnd_device;
        std::mt19937 mersenne_engine(rnd_device());
        std::uniform_real_distribution<double> dist(-99.0, 99.0);
        auto gen = std::bind(dist, mersenne_engine);

        std::vector<double> expected;

        for (auto n = 0; n < SIZE; ++n)
        {
            expected.emplace_back(gen());
        }

        std::vector<double> spectrum = expected;

        auto fft = OTFFT::Factory::createRealFFT(static_cast<int>(SIZE), OPT_TYPE);
        {
            std::vector<OTFFT::complex_t> workspace(SIZE);
            OTFFT::double_vector spectrum_pointer{spectrum.data()};
            OTFFT::complex_vector workspace_pointer{workspace.data()};
            fft->fwd0(spectrum_pointer, workspace_pointer);
            fft->invn(workspace_pointer, spectrum_pointer);
        }

        for (std::size_t idx{0}; idx < SIZE / 2; ++idx)
        {
            if (std::fabs(expected[idx]) < 1e-10)
            {
                BOOST_CHECK_SMALL(spectrum[idx], 1e-8);
            }
            else
            {
                BOOST_CHECK_CLOSE(expected[idx], spectrum[idx], .1);
            }
        }
    }

    template <OTFFT::OptimizationType OPT_TYPE>
    void testDCT(const std::size_t SIZE)
    {
        std::random_device rnd_device;
        std::mt19937 mersenne_engine(rnd_device());
        std::uniform_real_distribution<double> dist(-99.0, 99.0);
        auto gen = std::bind(dist, mersenne_engine);

        std::vector<double> expected;

        for (auto n = 0; n < SIZE; ++n)
        {
            expected.emplace_back(gen());
        }

        std::vector<double> spectrum = expected;

        auto fft = OTFFT::Factory::createDCT(static_cast<int>(SIZE), OPT_TYPE);
        {
            OTFFT::double_vector spectrum_pointer{spectrum.data()};
            fft->fwd0(spectrum_pointer);
            fft->invn(spectrum_pointer);
        }

        for (std::size_t idx{0}; idx < SIZE / 2; ++idx)
        {
            if (std::fabs(expected[idx]) < 1e-10)
            {
                BOOST_CHECK_SMALL(spectrum[idx], 1e-8);
            }
            else
            {
                BOOST_CHECK_CLOSE(expected[idx], spectrum[idx], .1);
            }
        }
    }
}

BOOST_AUTO_TEST_SUITE(otfft_optimization_test)

BOOST_AUTO_TEST_CASE(TestBluestein)
{
    const std::vector<std::size_t> N{
      8, 13, 27, 32, 172, 347, 512, 3247, 4096, 12312, 16384,
      32411, 32768, 53743, 65536, 83476, 131072, 234643, 262144,
      463272, 524288
    };
#ifdef OTFFT_WITH_SSE2
    for (auto n : N)
    {
        testComplexFFT<OTFFT::TransformationType::TRANSFORM_BLUESTEIN, OTFFT::OptimizationType::OPTIMIZED_FFT_SSE2>(n);
    }
#endif
#ifdef OTFFT_WITH_AVX
    for (auto n : N)
    {
        testComplexFFT<OTFFT::TransformationType::TRANSFORM_BLUESTEIN, OTFFT::OptimizationType::OPTIMIZED_FFT_AVX>(n);
    }
#endif
#ifdef OTFFT_WITH_AVX2
    for (auto n : N)
    {
        testComplexFFT<OTFFT::TransformationType::TRANSFORM_BLUESTEIN, OTFFT::OptimizationType::OPTIMIZED_FFT_AVX2>(n);
    }
#endif
}

BOOST_AUTO_TEST_CASE(TestComplex)
{
    const std::vector<std::size_t> N{
      8, 13, 27, 32, 172, 347, 512, 3247, 4096, 12312, 16384,
      32411, 32768, 53743, 65536, 83476, 131072, 234643, 262144,
      463272, 524288
    };
#ifdef OTFFT_WITH_SSE2
    for (auto n : N)
    {
        testComplexFFT<OTFFT::TransformationType::TRANSFORM_FFT_COMPLEX, OTFFT::OptimizationType::OPTIMIZED_FFT_SSE2>(n);
    }
#endif
#ifdef OTFFT_WITH_AVX
    for (auto n : N)
    {
        testComplexFFT<OTFFT::TransformationType::TRANSFORM_FFT_COMPLEX, OTFFT::OptimizationType::OPTIMIZED_FFT_AVX>(n);
    }
#endif
#ifdef OTFFT_WITH_AVX2
    for (auto n : N)
    {
        testComplexFFT<OTFFT::TransformationType::TRANSFORM_FFT_COMPLEX, OTFFT::OptimizationType::OPTIMIZED_FFT_AVX2>(n);
    }
#endif
}

BOOST_AUTO_TEST_CASE(TestReal)
{
    const std::vector<std::size_t> N{
      8, 32, 512, 4096, 16384, 32768, 65536, 131072,
      262144, 463272, 524288
    };
#ifdef OTFFT_WITH_SSE2
    for (auto n : N)
    {
        testRealFFT<OTFFT::OptimizationType::OPTIMIZED_FFT_SSE2>(n);
    }
#endif
#ifdef OTFFT_WITH_AVX
    for (auto n : N)
    {
        testRealFFT<OTFFT::OptimizationType::OPTIMIZED_FFT_AVX>(n);
    }
#endif
#ifdef OTFFT_WITH_AVX2
    for (auto n : N)
    {
        testRealFFT<OTFFT::OptimizationType::OPTIMIZED_FFT_AVX2>(n);
    }
#endif
}

BOOST_AUTO_TEST_CASE(TestDCT)
{
    const std::vector<std::size_t> N{
      8, 32, 512, 4096, 16384, 32768, 65536, 131072,
      262144, 463272, 524288
    };
#ifdef OTFFT_WITH_SSE2
    for (auto n : N)
    {
        testDCT<OTFFT::OptimizationType::OPTIMIZED_FFT_SSE2>(n);
    }
#endif
#ifdef OTFFT_WITH_AVX
    for (auto n : N)
    {
        testDCT<OTFFT::OptimizationType::OPTIMIZED_FFT_AVX>(n);
    }
#endif
#ifdef OTFFT_WITH_AVX2
    for (auto n : N)
    {
        testDCT<OTFFT::OptimizationType::OPTIMIZED_FFT_AVX2>(n);
    }
#endif
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/histogram.hpp>
#include <iostream>
#include <sens_loc/analysis/keypoints.h>
#include <sens_loc/math/rounding.h>
#include <sens_loc/util/console.h>
#include <stdexcept>

namespace sens_loc::analysis {

void keypoints::analyze(gsl::span<const cv::KeyPoint> points,
                        const bool                    distribution,
                        const bool                    size,
                        const bool                    response) noexcept {
    _size_histo.reset();
    _size.reset();
    _response_histo.reset();
    _response_histo.reset();
    _distribution.reset();

    if (points.empty())
        return;

    try {
        std::vector<float> responses;
        std::vector<float> sizes;
        std::for_each(points.begin(), points.end(),
                      [&](const cv::KeyPoint& kp) {
                          if (size)
                              sizes.emplace_back(kp.size);
                          if (response)
                              responses.emplace_back(kp.response);
                      });
        if (size) {
            std::sort(std::begin(sizes), std::end(sizes));
            _size = statistic::make(sizes);
        }
        if (response) {
            std::sort(std::begin(responses), std::end(responses));
            _response = statistic::make(responses);
        }
    } catch (const std::exception& e) {
        std::cerr << sens_loc::util::err{}
                  << "Could not create statistics for size and response.\n"
                  << "Message: " << e.what() << "\n";
        return;
    }

    using namespace boost::histogram;

    // Shortcut if no histogram will be created.
    if (!distribution && !response && !size)
        return;

    if (distribution)
        try {
            _distribution = make_histogram(
                axis_t{_dist_width_bins, 0.0F, 1.0F, _dist_w_title},
                axis_t{_dist_height_bins, 0.0F, 1.0F, _dist_h_title});
        } catch (const std::invalid_argument& e) {
            std::cerr << sens_loc::util::err{}
                      << "Bad distribution histogram configuration!\n"
                      << "Message: " << e.what() << "\n";
            return;
        }

    const float delta = 5.0F * std::numeric_limits<float>::epsilon();
    if (size && _size_histo_enabled) {
        try {
            const auto [s_min, s_max] = [&]() -> std::pair<float, float> {
                if (std::abs(_size.min - _size.max) < 0.0001F)
                    return {_size.min - 1.0F, _size.max + 1.0F};
                return {_size.min - delta, _size.max + delta};
            }();
            _size_histo =
                make_histogram(axis_t{_size_bins, s_min, s_max, _size_title});
        } catch (const std::invalid_argument& e) {
            std::cerr << sens_loc::util::err{}
                      << "Could not create size histogram configuration!\n"
                      << "Message: " << e.what() << "\n";
            return;
        }
    }

    if (response && _response_histo_enabled) {
        try {
            const float r_min = _response.min - delta;
            const float r_max = _response.max + delta;
            _response_histo   = make_histogram(
                axis_t{_response_bins, r_min, r_max, _response_title});
        } catch (const std::invalid_argument& e) {
            std::cerr << sens_loc::util::err{}
                      << "Could not create response histogram configuration!\n"
                      << "Message: " << e.what() << "\n";
            return;
        }
    }

    try {
        auto iw = static_cast<float>(_img_width);
        auto ih = static_cast<float>(_img_height);
        std::for_each(points.begin(), points.end(),
                      [&](const cv::KeyPoint& kp) {
                          if (distribution)
                              _distribution(kp.pt.x / iw, kp.pt.y / ih);
                          if (response && _response_histo_enabled)
                              _response_histo(kp.response);
                          if (size && _size_histo_enabled)
                              _size_histo(kp.size);
                      });
    } catch (const std::exception& e) {
        std::cerr << sens_loc::util::err{}
                  << "Could not create histograms for size and/or response "
                     "and/or keypoint-distribution.\n"
                  << "Message: " << e.what() << "\n";
        return;
    }
}

void write(cv::FileStorage& fs, const std::string& name, const keypoints& kp) {
    fs << name << "{";
    write(fs, "response", kp.response());
    write(fs, "size", kp.size());
    fs << "}";
}

}  // namespace sens_loc::analysis


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>

#include <bitset>
#include <vector>
#include <array>
#include <numeric>
#include <iostream>
#include <fstream>
#include <iomanip>

std::array<uint8_t,256> run_rounds(const std::vector<uint8_t> inputs,
                                   const size_t &hash_length,
                                   const size_t &num_rounds)
{
  std::array<uint8_t,256> ring;
  std::iota(ring.begin(), ring.end(), 0);

  size_t skip(0), current(0);
  for (size_t round=0; round<num_rounds; ++round)
    {
      for (auto &input: inputs)
        {
          uint8_t start (current + (input-1));
          uint8_t finish(current);

          const uint8_t stop (input/2);
          for (uint8_t ix=0; ix != stop; ++ix)
            {
              std::swap(ring[start], ring[finish]);
              --start;
              ++finish;
            }
          current += input + skip;
          ++skip;
        }
    }
  return ring;
}
std::vector<uint8_t> compute_dense_hash(const size_t &hash_length,
                                        const std::vector<uint8_t> &inputs)
{
  auto inputs_copy(inputs);
  for (auto &c: {17, 31, 73, 47, 23})
    { inputs_copy.push_back(c); }
  auto ring(run_rounds(inputs_copy,hash_length,64));

  std::vector<uint8_t> dense_hash(hash_length/16);
  for (size_t ix=0; ix<hash_length; ix+=16)
    { for(size_t jx=0; jx<16; ++jx)
        { dense_hash[ix/16]^=ring[ix+jx]; } }

  return dense_hash;
}

int main(int, char *argv[])
{
  const size_t hash_length(256);

  size_t total_memory(0);
  std::vector<std::vector<size_t>> bit_array(128);
  for(auto &b: bit_array)
    { b.resize(128,-1); }

  boost::adjacency_list <boost::vecS, boost::vecS, boost::undirectedS> graph;
  size_t memory_id(0);
  for (size_t row=0; row<128; ++row)
    {
      std::vector<uint8_t> inputs;
      for (auto &c: argv[1] + ("-" + std::to_string(row)))
        { inputs.push_back(c); }

      auto dense_hash(compute_dense_hash(hash_length,inputs));
      for (auto &h: dense_hash)
        { total_memory+=std::bitset<8>(h).count(); }

      for(size_t b=0; b<128/8; ++b)
        {
          std::bitset<8> b8(dense_hash[b]);
          for(size_t c=0; c<8; ++c)
            {
              auto is_on (b8[7-c]);
              if(is_on)
                {
                  bit_array[row][b*8 + c]=memory_id;
                  if(row!=0 && bit_array[row-1][b*8 + c]!=-1)
                    { add_edge(memory_id, bit_array[row-1][b*8 + c], graph); }

                  if((b!=0 || c!=0) && bit_array[row][b*8 + c-1]!=-1)
                    { add_edge(memory_id, bit_array[row][b*8 + c-1], graph); }
                  ++memory_id;
                }
            }
        }
    }
  std::cout << "Part 1: " << total_memory << "\n";

  std::vector<int> component(boost::num_vertices(graph));
  std::cout << "Part 2: "
            << connected_components(graph, &component[0])
            << "\n";

}


// Copyright (c) 2016
// Author: Chrono Law
#include <forward_list>

#include <std.hpp>
using namespace std;

#include <boost/concept_check.hpp>
using namespace boost;

///////////////////////////////////////

//template<typename T>
//T my_min(const T& l,const T& r)
//{
//    BOOST_CONCEPT_ASSERT((LessThanComparable<T>));
//    //BOOST_CONCEPT_ASSERT((SGIAssignable<T>));
//
//    return (l < r) ? l : r;
//}

#include <boost/concept/requires.hpp>

template<typename T>
BOOST_CONCEPT_REQUIRES(
((LessThanComparable<T>)),
 (T)                       )
my_min(const T& l,const T& r)
{
    return (l < r) ? l : r;
}

//template <typename I>
//BOOST_CONCEPT_REQUIRES(
//((Mutable_RandomAccessIterator<I>))
//((LessThanComparable<typename RandomAccessIterator<I>::value_type>)),
//(void)                     )
//_sort(I first, I last)
//{
//    std::stable_sort(first, last);
//}

///////////////////////////////////////

void case1()
{
    //complex<double> cp1, cp2;
    //my_min(cp1, cp2);

}

///////////////////////////////////////

void case2()
{
    BOOST_CONCEPT_ASSERT((UnaryFunction< negate<int>, int, int>));
    BOOST_CONCEPT_ASSERT((AdaptableUnaryFunction< negate<int>, int, int>));
    BOOST_CONCEPT_ASSERT((BinaryFunction< plus<int>,int, int, int>));

}

///////////////////////////////////////

template <typename I>
void _sort(I first, I last)
{
    BOOST_CONCEPT_ASSERT((RandomAccessIterator<I>));
    std::stable_sort(first, last);
}

void case3()
{
    BOOST_CONCEPT_ASSERT((InputIterator<int*>));
    BOOST_CONCEPT_ASSERT((OutputIterator<int*, int>));
    BOOST_CONCEPT_ASSERT((RandomAccessIterator<int*>));

    assert((std::is_same<InputIterator<int*>::pointer,
                iterator_traits<int*>::pointer>::value));

    BOOST_CONCEPT_ASSERT((ForwardIterator<forward_list<int>::iterator>));
    BOOST_CONCEPT_ASSERT((Mutable_ForwardIterator<
                forward_list<int>::iterator>));

    typedef vector<int>::iterator I;
    BOOST_CONCEPT_ASSERT((BidirectionalIterator<I>));
    BOOST_CONCEPT_ASSERT((RandomAccessIterator<I>));

}

///////////////////////////////////////

#include <boost/iterator/iterator_concepts.hpp>

#include <boost/iterator/iterator_facade.hpp>

///////////////////////////////////////

template<typename T>
class vs_iterator :
        public boost::iterator_facade<
        vs_iterator<T>, T,
        boost::single_pass_traversal_tag>
{
public:
    typedef boost::iterator_facade<
        vs_iterator<T>, T,boost::single_pass_traversal_tag> super_type;
    typedef vs_iterator this_type;

    typedef typename super_type::reference reference;
    // using typename super_type::reference ;

private:
    std::vector<T> &v;
    size_t current_pos;
public:
    vs_iterator(vector<T> &_v, size_t pos = 0):
      v(_v), current_pos(pos)
    {}
    vs_iterator(this_type const& other):
        v(other.v), current_pos(other.current_pos)
    {}
    void operator=(this_type const& other)
    {
        this->v = other.v;
        this->current_pos = other.current_pos;
    }
private:
    friend class boost::iterator_core_access;

    reference dereference() const
    {   return v[current_pos]; }

    void increment()
    {   ++current_pos;  }

    bool equal(this_type const& other) const
    {   return this->current_pos == other.current_pos;}
};

void case4()
{
    using namespace boost_concepts;

    typedef vector<bool>::iterator I;

    BOOST_CONCEPT_ASSERT((ReadableIterator<I>));
    BOOST_CONCEPT_ASSERT((WritableIterator<I>));
    BOOST_CONCEPT_ASSERT((SwappableIteratorConcept<I>));
    BOOST_CONCEPT_ASSERT((RandomAccessTraversalConcept<I>));

    BOOST_CONCEPT_ASSERT((ReadableIterator<vs_iterator<int> >));
    BOOST_CONCEPT_ASSERT((WritableIterator<vs_iterator<int> >));
    BOOST_CONCEPT_ASSERT((SwappableIterator<vs_iterator<int>>));
    BOOST_CONCEPT_ASSERT((SinglePassIterator<vs_iterator<int> >));

    //BOOST_CONCEPT_ASSERT((LvalueIteratorConcept<I>));

}

///////////////////////////////////////

#include <boost/array.hpp>
#include <boost/circular_buffer.hpp>

void case5()
{
    BOOST_CONCEPT_ASSERT((Container<vector<int>>));
    BOOST_CONCEPT_ASSERT((RandomAccessContainer<vector<int>>));

    assert((std::is_same<vector<int>::value_type,
                Container<vector<int>>::value_type>::value));

    //BOOST_CONCEPT_ASSERT((Container<forward_list<int>>));
    //BOOST_CONCEPT_ASSERT((ForwardContainer<forward_list<int>>));

    //BOOST_CONCEPT_ASSERT((ReversibleContainer<forward_list<int>>));

    BOOST_CONCEPT_ASSERT((Container<boost::circular_buffer<int> >));

    //BOOST_CONCEPT_ASSERT((Container<boost::array<int> >));

    BOOST_CONCEPT_ASSERT((Sequence<vector<int>>));
    BOOST_CONCEPT_ASSERT((Sequence<deque<int>>));
    BOOST_CONCEPT_ASSERT((Sequence<list<int>>));

    BOOST_CONCEPT_ASSERT((FrontInsertionSequence<deque<int>>));
    BOOST_CONCEPT_ASSERT((BackInsertionSequence<list<int>>));

    BOOST_CONCEPT_ASSERT((AssociativeContainer<set<int>>));
    BOOST_CONCEPT_ASSERT((AssociativeContainer<map<int,int>>));
    BOOST_CONCEPT_ASSERT((MultipleAssociativeContainer<multimap<int,int>>));

    BOOST_CONCEPT_ASSERT((SimpleAssociativeContainer<set<int>>));
    BOOST_CONCEPT_ASSERT((SortedAssociativeContainer<set<int>>));

}

///////////////////////////////////////

#include <boost/range/concepts.hpp>

void case6()
{
    BOOST_CONCEPT_ASSERT((SinglePassRangeConcept<std::forward_list<int>>));
    BOOST_CONCEPT_ASSERT((ForwardRangeConcept<std::forward_list<int>>));

    BOOST_CONCEPT_ASSERT((BidirectionalRangeConcept<std::list<int>>));
    BOOST_CONCEPT_ASSERT((RandomAccessRangeConcept<std::vector<int>>));

    char a[] = "range";
    BOOST_CONCEPT_ASSERT((RandomAccessRangeConcept<decltype(a)>));

}

///////////////////////////////////////
#include <boost/concept_archetype.hpp>

void case7()
{
    typedef null_archetype<> T;
    typedef assignable_archetype<T> at;
    typedef copy_constructible_archetype<at> cat;
    //typedef sgi_assignable_archetype<T> at;
    typedef less_than_comparable_archetype<cat> vt;

    boost::detail::dummy_constructor dummy_cons;
    vt v1(dummy_cons), v2(dummy_cons);

    my_min(v1, v2);

    typedef mutable_random_access_iterator_archetype<vt> rt;
    rt begin, end;
    _sort(begin, end);

}

///////////////////////////////////////

#include <boost/iterator/iterator_archetypes.hpp>

void case8()
{
    using namespace boost_concepts;
    typedef copy_constructible_archetype<assignable_archetype<>> T;
    //typedef sgi_assignable_archetype<> T;

    typedef input_iterator_archetype<T > I;

    BOOST_CONCEPT_ASSERT((ReadableIterator<I>));
    BOOST_CONCEPT_ASSERT((SinglePassIterator<I>));
    BOOST_CONCEPT_ASSERT((InputIterator<I>));

    typedef boost::iterator_archetype<T,
            boost::iterator_archetypes::readable_iterator_t,
            boost::single_pass_traversal_tag > II;

    BOOST_CONCEPT_ASSERT((ReadableIterator<II>));
    BOOST_CONCEPT_ASSERT((SinglePassIterator<II>));
    BOOST_CONCEPT_ASSERT((InputIterator<II>));

}

///////////////////////////////////////

int main()
{
    std::cout << "hello concept_check" << std::endl;

    case1();
    case2();
    case3();
    case4();
    case5();
    case6();
    case7();
    case8();
}


// Licensed under the Apache License, Version 2.0 (the "License"); you may
// not use this file except in compliance with the License.  You may obtain
// a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
// License for the specific language governing permissions and limitations
// under the License.

// cvlm.cc -- A linear model estimator that uses a variety of loss functions.
//            It sets the regularizer parameters using cross-validation.
//
// Mark Johnson, 11th April 2005, last modified 21st Nov 2007
//
// This is a version of wlle.cc reworked to use the tao-optimizer.h
// routines (so most of the Petsc/Tao junk is now moved out of this
// file).  It also sets regularizer factors by cross-validation.
// It writes out the weights file after each cross-validation estimation.
//
// This optimizier uses the TAO constrained optimization method tao_blmvm
// to avoid the variable discontinuity at zero if the -cv flag is set.
//
// For each variable x[i] in the original problem we introduce a pair of
// variables xp[i] and xn[i] and a pair of constraints xp[i] >= 0, xn[i] <= 0.
// Then x[i] = xp[i] + xn[i].
//
// The regularized function Q that is optimized is:
//
//  Q(xp,xn) = s * ( L(xp+xn) + R(xp,xn) )
//
// where s is a user-specified parameter, L is the unregularized loss
// function and R is the regularizer:
//
//  R(xp,xn) = c * sum_i (pow(fabs(xp[i]),p) + pow(fabs(xn[i]),p))
//
// where c and p are user-specifier adjustable parameters.  (The fabs()
// is there just in case the optimization routine temporarily proposes an
// infeasible solution).
//
// Note that
//
//  d Q/d xp[i] = s * d L/ d xp[i] + s * c * p * sum_i pow(fabs(xp[i]),p-1) * sign(xp[i])
//
// where sign(xp[i]) is +1 if xp[i] is non-negative and -1 otherwise.
//
// Change log:
//
// 20th April, 2004: the regularizer weights disjunctive features proportial
// to the number of disjuncts they contain
//
const char usage[] =
"cvlm version of 21st November 2007\n"
"\n"
"	A constrained-variable weighted linear model estimator\n"
"	with regularizer factors set by cross-validation.\n"
"\n"
"cvlm estimates feature weights that estimate the parameters of a\n"
"linear model by minimizing a regularized loss of the feature\n"
"weights using the LVLM or BLVLM optimizer from the Petsc/Tao\n"
"optimization package (see http://www-fp.mcs.anl.gov/tao/ for details).\n"
"It can deal with partially labeled data (i.e., training instances\n"
"consist of one or more \"winners\" and one or more \"losers\").\n"
"\n"
"Usage: cvlm [-help] [-debug debug_level] [-c0 c0] [-c00 c00] [-p p] [-r r] [-s s] [-cv] \n"
"              [-l ltype] [-Pyx_factor f] [-Px_propto_g] [-max-nrounds maxnrounds] \n"
"              [-o weights-file]  [-e eval-file] [-x eval-file2]\n"
"              [-ns ns] [-f feat-file]\n"
"              tao-options*\n"
"	       < train-file\n"
"\n"
"where:\n"
"\n"
" debug_level > 0 controls the amount of output produced\n"
"\n"
" c0 is the initial value for the regularizer constant, and the weight of the regularizer\n"
" constant for the first feature class is multiplied by c00\n"
"\n"
" train-file, eval-file and eval-file2 are files from which training and evaluation\n"
" data are read (if eval-file ends in the suffix .bz2 then bzcat is used\n"
" to read it; if no eval-file is specified, then the program tests on the\n"
" training data),\n"
"\n"
" weights-file is a file to which the estimated weights are written,\n"
"\n"
" feat-file is a file of <featclass> <featuredetails> lines, used for\n"
" cross-validating regularizer weights,\n"
"\n"
" ns is the number of ':' characters to use to define the cross-validation\n"
" classes,\n"
"\n"
" ltype identifies the type of loss function used:\n"
"\n"
"    -l 0 - log loss (c0 ~ 5)\n"
"    -l 1 - EM-style log loss (c0 ~ 5)\n"
"    -l 2 - pairwise log loss \n"
"    -l 3 - exp loss (c0 ~ 25, s ~ 1e-5)\n"
"    -l 4 - log exp loss (c0 ~ 1e-4)\n"
"    -l 5 - maximize expected F-score (c ~ ?)\n"
"\n"
" ns is the maximum number of ':' characters in a <featclass>, used to\n"
" determine how features are binned into feature classes (ns = -1 bins\n"
" all features into the same class)\n"
"\n"
" r specifies that the weights are initialized to random values in\n"
"   [-r ... +r],\n"
"\n"
" -Pyx_factor f indicates that a parse should be taken as correct\n"
"   proportional to f raised to its f-score, and\n"
"\n"
" -Px_propto_g indicates that each sentence is weighted by the number of\n"
"   edges in its gold parse.\n"
"\n"
" -max-nrounds maxnrounds specifies that at most maxnrounds of cross-validation\n"
"   is to be performed.\n"
"\n"
"The function that the program minimizes is:\n"
"\n"
"   Q(w) = s * (- L(w) + c * sum_j pow(fabs(w[j]), p) ), where:\n"
"\n"
"   L(w) is the loss function to be optimized.\n"
"\n"
"The -cv option instructs the program to optimize a function defined in\n"
"terms of vectors of variables u[] and v[], where w[j] = u[j] + v[j]\n"
"and v[j] <= 0 <= u[j], otherwise it optimizes the w[j] directly.\n"
"\n"
"With debug = 0, the program writes a single line to stdout:\n"
"\n"
"c p r s it nzeroweights/nweights neglogP/nsentence ncorrect/nsentences\n"
"\n"
"With debug >= 10, the program writes out a histogram of weights as well\n"
"\n"
"Data format:\n"
"-----------\n"
"\n"
"<Data>     --> [S=<NS>] <Sentence>*\n"
"<Sentence> --> [G=<G>] N=<N> <Parse>*\n"
"<Parse>    --> [P=<P>] [W=<W>] <FC>*,\n"
"<FC>       --> <F>[=<C>]\n"
"\n"
"NS is the number of sentences.\n"
"\n"
"Each <Sentence> consists of N <Parse>s.  <G> is the gold standard\n"
"score.  To get parsing precision and recall results, set <G> to the\n"
"number of edges in the gold standard parse.  To get accuracy results,\n"
"set <G> to 1 (the default).\n"
"\n"
"A <Parse> consists of <FC> pairs.  <P> is the parse's possible highest\n"
"score and <W> is the parse's actual score.  To get parsing precision and\n"
"recall results, set <P> to the number of edges in the parse and <W> to\n"
"the number of edges in common between the gold and parse trees.\n"
"\n"
"A <FC> consists of a feature (a non-negative integer) and an optional\n"
"count (a real).\n"
"\n"
"The default for all numbers except <W> is 1.  The default for <W> is 0.\n";

#include "custom_allocator.h"    // must come first
#define _GLIBCPP_CONCEPT_CHECKS  // uncomment this for checking

#include <boost/lexical_cast.hpp>
#include <cassert>
#include <cctype>
#include <cerrno>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <vector>

#include "lmdata.h"
#include "powell.h"
#include "utility.h"
#include "tao-optimizer.h"

typedef std::vector<double> doubles;
typedef std::vector<size_t> size_ts;

int debug_level = 0;

enum loss_type { log_loss, em_log_loss, em_log_loss_noomp, pairwise_log_loss, exp_loss, log_exp_loss,
		 expected_fscore_loss };

const char* loss_type_name[] = { "log_loss", "em_log_loss", "em_log_loss_noomp", "pairwise_log_loss", "exp_loss",
				 "log_exp_loss", "expected_fscore_loss" };

void print_histogram(int nx, const double x[], int nbins=20) {
  int nx_nonzero = 0;
  for (int i = 0; i < nx; ++i)
    if (x[i] != 0)
      ++nx_nonzero;

  std::cout << "#   There are " << nx_nonzero << " non-zero values and "
	    << nx-nx_nonzero << " zero values." << std::endl;

  if (nx_nonzero > 0) {
    std::vector<double> s;
    s.reserve(nx_nonzero);
    for (int i = 0; i < nx; ++i)
      if (x[i] != 0)
	s.push_back(x[i]);
    std::sort(s.begin(), s.end());
    for (int i = 0; i <= nbins; ++i) {
      int j = i*(nx_nonzero-1);
      j /= nbins;
      std::cout << float(i)/float(nbins) << '\t' << s[j] << std::endl;
    }
  }
}  // print_histogram()

// f_df() evaluates the statistics of the corpus, and prints the f-score
//  if required
//
double f_df(loss_type ltype, corpus_type* corpus, double x[], double df_dx[]) {
  Float sum_g = 0, sum_p = 0, sum_w = 0, L = 0;

  switch (ltype) {
  case log_loss:
    L = corpus_stats(corpus, &x[0], &df_dx[0], &sum_g, &sum_p, &sum_w);
    break;
  case em_log_loss:
    L = emll_corpus_stats(corpus, &x[0], &df_dx[0], &sum_g, &sum_p, &sum_w);
    break;
  case em_log_loss_noomp:
    L = emll_corpus_stats_noomp(corpus, &x[0], &df_dx[0], &sum_g, &sum_p, &sum_w);
    break;
  case pairwise_log_loss:
    L = pwlog_corpus_stats(corpus, &x[0], &df_dx[0], &sum_g, &sum_p, &sum_w);
    break;
  case exp_loss:
    L = exp_corpus_stats(corpus, &x[0], &df_dx[0], &sum_g, &sum_p, &sum_w);
    break;
  case log_exp_loss:
    L = log_exp_corpus_stats(corpus, &x[0], &df_dx[0], &sum_g, &sum_p, &sum_w);
    break;
  case expected_fscore_loss:
    L = 1 - fscore_corpus_stats(corpus, &x[0], &df_dx[0], &sum_g, &sum_p, &sum_w);
    for (size_type j = 0; j < corpus->nfeatures; ++j)
      df_dx[j] = -df_dx[j];
    break;
  default:
    L = 0;
    std::cerr << "## Error: unrecognized loss_type loss = " << int(ltype)
	      << std::endl;
  }

  if (debug_level >= 1000)
    std::cerr << "f score = " << 2*sum_w/(sum_g+sum_p) << ", " << std::flush;

  assert(finite(L));
  return L;
}

// Unconstrained is for unconstrained optimization
//
struct Unconstrained {
  const loss_type ltype;
  corpus_type* corpus;
  const size_ts& f_c;	//!< feature -> cross-validation class
  const doubles& cs;	//!< cross-validation class -> regularizer factor
  double p, s;
  int it;
  double L, R, Q;

  Unconstrained(loss_type ltype, corpus_type* corpus, const size_ts& f_c,
		const doubles& cs, double p, double s)
    : ltype(ltype), corpus(corpus), f_c(f_c), cs(cs), p(p), s(s), it(0)
  {
    assert(f_c.size() == corpus->nfeatures);
    for (size_type f = 0; f < f_c.size(); ++f)
      assert(f_c[f] < cs.size());
  }

  double operator() (int nx, double x[], double df_dx[]) {
    it++;
    if (debug_level >= 1000)
      std::cerr << "it = " << it << ", " << std::flush;

    L = f_df(ltype, corpus, x, df_dx);

    if (s != 1) {
      L *= s;

      for (int i = 0; i < nx; ++i) {
	assert(finite(df_dx[i]));
	df_dx[i] *= s;
      }
    }

    assert(size_t(nx) == corpus->nfeatures);
    assert(f_c.size() == corpus->nfeatures);

    R = 0;
    for (int i = 0; i < nx; ++i)
      R +=  cs[f_c[i]] * pow(fabs(x[i]), p);
    R *= s;

    Q = L + R;

    if (debug_level >= 1000)
      std::cerr << "Q = " << Q << " = L = " << L << " + R = " << R << std::endl;

    assert(finite(Q));

    double sp = s * p;
    for (int i = 0; i < nx; ++i)
      df_dx[i] += sp * cs[f_c[i]] * pow(fabs(x[i]), p-1)
	* (x[i] >= 0 ? (x[i] == 0 ? 0 : 1) : -1);

    if (debug_level >= 10000) {
      std::cerr << "Histogram of derivatives:" << std::endl;
      print_histogram(nx, &df_dx[0]);
      std::cerr << "--------------------------" << std::endl;
    }

    for (int i = 0; i < nx; ++i)
      assert(finite(df_dx[i]));
    return Q;
  }   // Unconstrained::operator()

};  // Unconstrained{}

// Constrained{} for the constrained optimization.
//
struct Constrained {
  const loss_type ltype;
  corpus_type* corpus;
  const size_ts& f_c;	//!< feature -> cross-validation class
  const doubles& cs;	//!< cross-validation class -> regularizer factor
  double p, s;
  int it;
  doubles x, df_dx;
  double L, R, Q;

  Constrained(loss_type ltype, corpus_type* corpus, const size_ts& f_c,
	      const doubles& cs, double p, double s)
    : ltype(ltype), corpus(corpus), f_c(f_c), cs(cs), p(p), s(s), it(0),
      x(corpus->nfeatures, 0), df_dx(corpus->nfeatures) {
    assert(f_c.size() == corpus->nfeatures);
    for (size_type f = 0; f < f_c.size(); ++f)
      assert(f_c[f] < cs.size());
  }

  double operator() (int nx2, double x2[], double df_dx2[]) {
    it++;
    if (debug_level >= 1000)      std::cerr << "it = " << it << ", " << std::flush;

    size_type nx = corpus->nfeatures;
    assert(nx2 = 2 * nx);
    assert(x.size() == nx);
    for (size_type i = 0; i < nx; ++i) {
      assert(finite(x2[i]));
      assert(finite(x2[i+nx]));
      x[i] = x2[i] + x2[i+nx];
      assert(finite(x[i]));
    }

    L = f_df(ltype, corpus, &x[0], &df_dx[0]);

    L *= s;

    for (size_type i = 0; i < nx; ++i) {
      assert(finite(df_dx[i]));
      df_dx2[i] = df_dx2[i+nx] = s * df_dx[i];
    }

    R = 0;
    for (size_type i = 0; i < nx; ++i)
      R += cs[f_c[i]] * pow(fabs(x2[i]), p) + pow(fabs(x2[i+nx]), p);
    R *= s;

    Q = L + R;

    if (debug_level >= 1000)
      std::cerr << "Q = " << Q << " = L = " << L << " + R = " << R << std::endl;

    assert(finite(Q));

    double sp = s * p;
    for (size_type i = 0; i < nx; ++i) {
      df_dx2[i] += sp * cs[f_c[i]] * pow(fabs(x2[i]), p-1) * (x2[i] >= 0 ? 1 : -1);
      df_dx2[i+nx] += sp * cs[f_c[i]] * pow(fabs(x2[i+nx]), p-1) * (x2[i+nx] > 0 ? 1 : -1);
    }

    if (debug_level >= 10000) {
      std::cerr << "Histogram of derivatives:" << std::endl;
      print_histogram(nx, &df_dx[0]);
      std::cerr << "--------------------------" << std::endl;
    }

    for (int i = 0; i < nx2; ++i)
      assert(finite(df_dx2[i]));
    return Q;
  }   // Constrained::operator()

};  // Constrained{}

// The Estimator1 does one round of estimation
//
struct Estimator1 {
  typedef std::vector<size_t> size_ts;
  typedef std::vector<double> doubles;

  corpus_type* train;	//!< training data
  size_type nx;		//!< number of features
  corpus_type* eval;	//!< evaluation data
  corpus_type* eval2;	//!< 2nd evaluation data
  loss_type ltype;	//!< type of loss function
  double c0;		//!< default regularizer factor
  double c00;           //!< multiply default regularizer factor for first feature class
  double p;		//!< regularizer power
  double r;		//!< random initialization
  double s;		//!< scale factor
  bool cv;		//!< use constrained variables
  bool opt_fscore;	//!< optimize f-score or - log likelihood

  doubles x;		//!< feature -> weight
  size_ts f_c;		//!< feature -> cross-validation class
  doubles lcs;		//!< cross-validation class -> log factor
  size_type nc;		//!< number of cross-validation classes

  size_type nits;	//!< number of iterations of last round
  size_type sum_nits;	//!< total number of iterations
  size_type nrounds;	//!< number of cross-validation rounds so far
  size_type max_nrounds; //!< number of cross-validation rounds to perform
  double best_score;    //!< best score seen so far
  std::string weightsfile; //!< name of weights file

  typedef std::map<std::string,size_t> S_C;
  S_C identifier_regclass; //!< map from feature class identifiers to regularization class
  typedef std::vector<std::string> Ss;
  Ss regclass_identifiers; //!< vector of class identifiers

  Estimator1(loss_type ltype, double c0, double c00, double p, double r, double s, bool cv,
	     bool opt_fscore = true, size_type max_nrounds = 0, const char* weightsfile = NULL)
    : train(NULL), nx(0), eval(NULL), eval2(NULL),
      ltype(ltype), c0(c0), c00(c00), p(p), r(r), s(s), cv(cv),
      opt_fscore(opt_fscore), x(nx), f_c(nx), lcs(1, log(c0)), nc(1),
      nits(0), sum_nits(0), nrounds(0), max_nrounds(max_nrounds), best_score(0),
      weightsfile(weightsfile == NULL ? "" : weightsfile)
  { }  // Estimator1::Estimator1()

  //! set_data() sets the training and evaluation data
  //
  void set_data(corpus_type* t, corpus_type* e, corpus_type* e2) {
    train = t;
    eval = e;
    eval2 = e2;
    nx = train->nfeatures;
    x.resize(nx);
    assert(f_c.size() <= nx);
    assert(eval == NULL || eval->nfeatures <= train->nfeatures);
    assert(eval2 == NULL || eval2->nfeatures <= train->nfeatures);
  } // Estimator1::set_data()

  // operator() actually runs one round of estimation
  //
  double operator() (const doubles& lccs) {
    assert(lccs.size() == nc);
    doubles ccs(nc);
    double L, R, Q;

    for (size_type i = 0; i < nc; ++i)
      ccs[i] = exp(lccs[i]);

    assert(x.size() == nx);
    nits = 0;
    nrounds++;

    if (debug_level >= 10) {
      if (nrounds == 1)
	std::cerr << "# round	nfeval	L	R	Q	neglogP	f-score	css" << std::endl;
      std::cerr << nrounds << std::flush;
    }
    if (cv) {

      // Constrained variable optimization

      Constrained fn(ltype, train, f_c, ccs, p, s);
      tao_constrained_optimizer<Constrained> tao_opt(2*nx, fn);

      if (r != 0)
	for (size_type i = 0; i < 2*nx; ++i)
	  tao_opt[i] = r*double(random()-RAND_MAX/2)/double(RAND_MAX/2);

      for (size_type i = 0; i < nx; ++i) {
	tao_opt.lower_bound[i] = 0;
	tao_opt.lower_bound[i+nx] = TAO_NINFINITY;
	tao_opt.upper_bound[i] = TAO_INFINITY;
	tao_opt.upper_bound[i+nx] = 0;
      }

      tao_opt.optimize();

      nits = fn.it;
      L = fn.L;
      R = fn.R;
      Q = fn.Q;

      for (size_type i = 0; i < nx; ++i)
	x[i] = tao_opt[i] + tao_opt[i+nx];
    }
    else {

      // Unconstrained optimization

      Unconstrained fn(ltype, train, f_c, ccs, p, s);
      tao_optimizer<Unconstrained> tao_opt(nx, fn);

      if (r != 0)
	for (size_type i = 0; i < nx; ++i)
	  tao_opt[i] = r*double(random()-RAND_MAX/2)/double(RAND_MAX/2);

      tao_opt.optimize();

      nits = fn.it;
      L = fn.L;
      R = fn.R;
      Q = fn.Q;

      for (size_type i = 0; i < nx; ++i)
	x[i] = tao_opt[i];
    }

    // Clean up, collect stats

    sum_nits += nits;

    if (debug_level >= 10)
      std::cerr << '\t' << nits << '\t' << L << '\t' << R << '\t' << Q;

    double score = evaluate(opt_fscore, true);

    if (debug_level >= 10)
      std::cerr << '\t' << ccs << std::endl;

    return score;

  }  // Estimator1::operator()

  // evaluate() evaluates the current model on the eval data, prints
  // out debugging information if appropriate, and returns either
  // the - log likelihood or 1 - f-score.
  //
  double evaluate(bool opt_fscore = false, bool internal = false) {

    std::vector<double> df_dx(nx);
    Float sum_g = 0, sum_p = 0, sum_w = 0;
    Float neglogP = corpus_stats(eval, &x[0], &df_dx[0],
				 &sum_g, &sum_p, &sum_w);
    Float fscore = 2*sum_w/(sum_g+sum_p);

    if (internal) {  // internal evaluation, use a short print-out
      if (debug_level >= 10) {
	std::cerr << '\t' << neglogP << '\t' << fscore;
	if (eval2 != NULL) {
	  Float sum_g2 = 0, sum_p2 = 0, sum_w2 = 0;
	  Float neglogP2 = corpus_stats(eval2, &x[0], &df_dx[0],
					&sum_g2, &sum_p2, &sum_w2);
	  Float fscore2 = 2*sum_w2/(sum_g2+sum_p2);
	  std::cerr << '\t' << neglogP2 << '\t' << fscore2;
	}
      }
    }
    else { // final evaluation, print out more info

      int nzeros = 0;
      for (size_type i = 0; i < nx; ++i)
	if (x[i] == 0)
	  ++nzeros;

      std::cerr << "# Regularizer power p = " << p << std::endl;
      std::cerr << "# " << nx-nzeros << " non-zero feature weights of "
		<< nx << " features." << std::endl;
      std::cerr << "# Eval neglogP = " << neglogP
		<< ", neglogP/nsentences = " << neglogP/eval->nsentences
		<< std::endl;
      std::cerr << "# Eval precision = " << sum_w/sum_p
		<< ", recall = " << sum_w/sum_g
		<< ", f-score = " << 2*sum_w/(sum_g+sum_p)
		<< std::endl;
      if (eval2 != NULL) {
	Float sum_g = 0, sum_p = 0, sum_w = 0;
	Float neglogP = corpus_stats(eval2, &x[0], &df_dx[0],
				     &sum_g, &sum_p, &sum_w);
	std::cerr << "# Eval2 neglogP = " << neglogP
		  << ", neglogP/nsentences = " << neglogP/train->nsentences
		  << std::endl;
	std::cerr << "# Eval2 precision = " << sum_w/sum_p
		  << ", recall = " << sum_w/sum_g
		  << ", f-score = " << 2*sum_w/(sum_g+sum_p)
		  << std::endl;
      }
      {
	Float sum_g = 0, sum_p = 0, sum_w = 0;
	Float neglogP = corpus_stats(train, &x[0], &df_dx[0],
				     &sum_g, &sum_p, &sum_w);
	std::cerr << "# Train neglogP = " << neglogP
		  << ", neglogP/nsentences = " << neglogP/train->nsentences
		  << std::endl;
	std::cerr << "# Train precision = " << sum_w/sum_p
		  << ", recall = " << sum_w/sum_g
		  << ", f-score = " << 2*sum_w/(sum_g+sum_p)
		  << std::endl;
      }

      std::cerr << "# regclass_identifiers = " << regclass_identifiers << std::endl;
      std::cerr << "# lcs = " << lcs << std::endl;
      {
	doubles cs(nc);
	for (size_type i = 0; i < nc; ++i)
	  cs[i] = exp(lcs[i]);
	std::cerr << "# cs = " << cs << std::endl;
      }
      if (debug_level >= 100) {
	std::cerr << "# Cumulative distribution of feature weights:" << std::endl;
	print_histogram(nx, &x[0]);
      }
    }

    double score = (opt_fscore ? 1 - fscore : neglogP);

    if (nrounds == 1 || score < best_score) {
      best_score = score;

      // Write out weights file

      if (!weightsfile.empty()) {
	FILE* out = fopen(weightsfile.c_str(), "w");
	// fprintf(out, "%d@", nx-nzeros);
	for (size_type i = 0; i < x.size(); ++i)
	  if (x[i] != 0) {
	    fprintf(out, "%d", i);
	    if (x[i] != 1)
	      fprintf(out, "=%g", x[i]);
	    fprintf(out, "\n");
	  }
	fclose(out);
      }
    }
    return score;
  } // Estimator1::evaluate()

  //! fc_bin() maps a feature count to its corresponding bin
  //
  static int fc_bin(double feature_count_base, int feature_count) {
    if (feature_count <= 4)
      return feature_count;
    else
      return lrint(4.0 + pow(feature_count_base,
			     lrint(log(feature_count-4)/log(feature_count_base))));
  }  // Estimator1::fc_bin()

  // read_featureclasses() reads the feature classes from a feature file
  //
  void read_featureclasses(const char* filename,
			   int nseparators = 1,
			   const char* separators = ":") {

    const char* filesuffix = strrchr(filename, '.');
    bool popen_flag = false;
    FILE *in;
    if (strcasecmp(filesuffix, ".bz2") == 0) {
      std::string command("bzcat ");
      command += filename;
      in = popen(command.c_str(), "r");
      if (in == NULL) {
	perror("## Error in lm-owlqn: ");
	std::cerr << "## popen(\"" << command << "\", \"r\") failed, usage = " << resource_usage() << std::endl;
      }
      popen_flag = true;
    }
    else if (strcasecmp(filesuffix, ".gz") == 0) {
      std::string command("gunzip -c ");
      command += filename;
      errno = 0;
      in = popen(command.c_str(), "r");
      if (in == NULL) {
	perror("## Error in lm-owlqn: ");
	std::cerr << "## popen(\"" << command << "\", \"r\") failed, usage = " << resource_usage() << std::endl;
      }
      popen_flag = true;
    }
    else
      in = fopen(filename, "r");
    if (in == NULL) {
      std::cerr << "## Couldn't open evalfile " << filename
		<< ", errno = " << errno << "\n"
		<< usage << std::endl;
      exit(EXIT_FAILURE);
    }

    size_type featno;

    // read feature number

    while (fscanf(in, " %u ", &featno) == 1) {
      int c = ':';

      // read the prefix of the feature class identifier

      std::string identifier;
      int iseparators = 0;
      if (nseparators >= 0)
	while ((c = getc(in)) != EOF && !isspace(c)) {
	  if (index(separators, c) != NULL)
	    if (++iseparators > nseparators)
	      break;
	  identifier.push_back(c);
	}

      // skip the rest of the line

      while ((c = getc(in)) != EOF && c != '\n')
	;

      // insert the prefix into the prefix -> regularization class map

      S_C::iterator it
	= identifier_regclass.insert(S_C::value_type(identifier,
						     identifier_regclass.size())).first;

      size_type cl = it->second;    // regularization class

      f_c.resize(featno+1);
      f_c[featno] = cl;          // set feature's regularization class
    }

    nc = identifier_regclass.size();   // set nc
    lcs.resize(nc, log(c0));           // set each regularizer class' factor to c0
    lcs[0] += log(c00);                // increment first regularizer class' factor by c00

    // construct regclass_identifiers

    regclass_identifiers.resize(nc);
    cforeach (S_C, it, identifier_regclass) {
      assert(it->second < regclass_identifiers.size());
      regclass_identifiers[it->second] = it->first;
    }

    if (debug_level >= 0)
      std::cerr << "# Regularization classes: " << regclass_identifiers << std::endl;

    if (popen_flag)
      pclose(in);
    else
      fclose(in);
  }  // Estimator1::read_featureclasses()

  void estimate()
  {
    if (max_nrounds == 1)
      operator()(lcs);
    else {
      if(max_nrounds == 0)
	max_nrounds = (lcs.size() > 1) ? 11 : 51;
      powell::control cntrl(1e-4, 1e-2, 0, max_nrounds);
      powell::minimize(lcs, *this, log(2), cntrl);
    }

    if (debug_level > 0) {
      std::cerr << "# Regularizer class weights = (";
      for (size_type i = 0; i < lcs.size(); ++i) {
	if (i > 0)
	  std::cerr << ' ';
	std::cerr << exp(lcs[i]);
      }
      std::cerr << ')' << std::endl;
    }

  }  // Estimator1::estimate()

};  // Estimator1{}

int main(int argc, char** argv)
{
  errno = 0;

  std::ios::sync_with_stdio(false);

  // Initialize TAO and PETSc

  tao_environment tao_env(argc, argv);

  if (tao_env.get_bool_option("-help") || tao_env.get_bool_option("--help")) {
    std::cerr << "-help\n" << usage << std::endl;
    exit(EXIT_SUCCESS);
  }

  debug_level = tao_env.get_int_option("-debug", debug_level);
  loss_type ltype = loss_type(tao_env.get_int_option("-l", 0));
  double c0 = tao_env.get_double_option("-c0", 2.0);
  double c00 = tao_env.get_double_option("-c00", 1.0);
  double p = tao_env.get_double_option("-p", 2.0);
  double r = tao_env.get_double_option("-r", 0.0);
  double s = tao_env.get_double_option("-s", 1.0);
  bool cv = tao_env.get_bool_option("-cv");
  double Pyx_factor = tao_env.get_double_option("-Pyx_factor", 0.0);
  bool Px_propto_g = tao_env.get_bool_option("-Px_propto_g");
  size_type max_nrounds = tao_env.get_int_option("-max-nrounds", 0);

  if (debug_level >= 10)
    std::cerr << "#  ltype = " << ltype
	      << " (" << loss_type_name[ltype] << ")"
	      << ", regularization c0 = " << c0
	      << ", c00 = " << c00
	      << ", power p = " << p
	      << ", scale s = " << s
	      << "; random init r = " << r
	      << ", constrained var optimization cv = " << cv
	      << ", Pyx_factor = " << Pyx_factor
	      << ", Px_propto_g = " << Px_propto_g
	      << ", max_nrounds = " << max_nrounds
	      << std::endl;

  // I discovered a couple of years after I wrote this program that popen
  // uses fork, which doubles your virtual memory for a short instant!

  Estimator1 e(ltype, c0, c00, p, r, s, cv, true,
	       max_nrounds, tao_env.get_cstr_option("-o"));

  int nseparators = tao_env.get_int_option("-ns", 1);
  const char* filename = tao_env.get_cstr_option("-f");
  if (filename != NULL)
    e.read_featureclasses(filename, nseparators, ":");

  // Read in eval data first, as that way we may squeeze everything into 4GB

  corpusflags_type corpusflags = { Pyx_factor, Px_propto_g };

  corpus_type* evaldata = NULL;
  const char* evalfile = tao_env.get_cstr_option("-e");
  if (evalfile != NULL) {
    evaldata = read_corpus_file(&corpusflags, evalfile);
    if (debug_level >= 10)
      std::cerr << "# read evalfile = " << evalfile
		<< ", nsentences = " << evaldata->nsentences
		<< std::endl;
  }

  corpus_type* evaldata2 = NULL;
  const char* evalfile2 = tao_env.get_cstr_option("-x");
  if (evalfile2 != NULL) {
    evaldata2 = read_corpus_file(&corpusflags, evalfile2);
    if (debug_level >= 10)
      std::cerr << "# read evalfile2 = " << evalfile2
		<< ", nsentences = " << evaldata2->nsentences
		<< std::endl;
  }

  corpus_type* traindata = read_corpus(&corpusflags, stdin);
  int nx = traindata->nfeatures;

  if (errno != 0) {
    perror("## cvlm, after reading main corpus, nonzero errno  ");
    errno = 0;
  }

  std::cerr << "# " << nx << " features in training data, " << resource_usage() << std::endl;

  if (evaldata == NULL)
    evaldata = traindata;

  e.set_data(traindata, evaldata, evaldata2);
  e.estimate();

}  // main()


/**
 * @file AdaBoost_test.cpp
 * @author Udit Saxena
 *
 * Tests for AdaBoost class.
 */

#include <mlpack/core.hpp>
#include <mlpack/methods/adaboost/adaboost.hpp>

#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"

using namespace mlpack;
using namespace arma;
using namespace mlpack::adaboost;

BOOST_AUTO_TEST_SUITE(AdaBoostTest);

/**
 *  This test case runs the AdaBoost.mh algorithm on the UCI Iris dataset.
 *  It checks whether the hamming loss breaches the upperbound, which
 *  is provided by ztAccumulator.
 */
BOOST_AUTO_TEST_CASE(HammingLossBoundIris)
{
  arma::mat inputData;

  if (!data::Load("iris.txt", inputData))
    BOOST_FAIL("Cannot load test dataset iris.txt!");

  arma::Mat<size_t> labels;

  if (!data::Load("iris_labels.txt",labels))
    BOOST_FAIL("Cannot load labels for iris iris_labels.txt");

  // no need to map the labels here

  // Define your own weak learner, perceptron in this case.
  // Run the perceptron for perceptron_iter iterations.
  int perceptronIter = 400;

  perceptron::Perceptron<> p(inputData, labels.row(0), max(labels.row(0)) + 1,
      perceptronIter);

  // Define parameters for the adaboost
  int iterations = 100;
  double tolerance = 1e-10;
  AdaBoost<> a(inputData, labels.row(0), iterations, tolerance, p);

  int countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if(labels(i) != a.finalHypothesis(i))
      countError++;
  double hammingLoss = (double) countError / labels.n_cols;

  double ztP = a.GetztProduct();
  BOOST_REQUIRE(hammingLoss <= ztP);
}

/**
 *  This test case runs the AdaBoost.mh algorithm on the UCI Iris dataset.
 *  It checks if the error returned by running a single instance of the
 *  weak learner is worse than running the boosted weak learner using
 *  adaboost.
 */
BOOST_AUTO_TEST_CASE(WeakLearnerErrorIris)
{
  arma::mat inputData;

  if (!data::Load("iris.txt", inputData))
    BOOST_FAIL("Cannot load test dataset iris.txt!");

  arma::Mat<size_t> labels;

  if (!data::Load("iris_labels.txt",labels))
    BOOST_FAIL("Cannot load labels for iris iris_labels.txt");

  // no need to map the labels here

  // Define your own weak learner, perceptron in this case.
  // Run the perceptron for perceptron_iter iterations.
  int perceptronIter = 400;

  arma::Row<size_t> perceptronPrediction(labels.n_cols);
  perceptron::Perceptron<> p(inputData, labels.row(0), max(labels.row(0)) + 1,
      perceptronIter);
  p.Classify(inputData, perceptronPrediction);

  int countWeakLearnerError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if(labels(i) != perceptronPrediction(i))
      countWeakLearnerError++;
  double weakLearnerErrorRate = (double) countWeakLearnerError / labels.n_cols;

  // Define parameters for the adaboost
  int iterations = 100;
  double tolerance = 1e-10;
  AdaBoost<> a(inputData, labels.row(0), iterations, tolerance, p);
  int countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if(labels(i) != a.finalHypothesis(i))
      countError++;
  double error = (double) countError / labels.n_cols;

  BOOST_REQUIRE(error <= weakLearnerErrorRate);
}

/**
 *  This test case runs the AdaBoost.mh algorithm on the UCI Vertebral
 *  Column dataset.
 *  It checks whether the hamming loss breaches the upperbound, which
 *  is provided by ztAccumulator.
 */
BOOST_AUTO_TEST_CASE(HammingLossBoundVertebralColumn)
{
  arma::mat inputData;

  if (!data::Load("vc2.txt", inputData))
    BOOST_FAIL("Cannot load test dataset vc2.txt!");

  arma::Mat<size_t> labels;

  if (!data::Load("vc2_labels.txt",labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  // no need to map the labels here

  // Define your own weak learner, perceptron in this case.
  // Run the perceptron for perceptron_iter iterations.
  int perceptronIter = 800;

  perceptron::Perceptron<> p(inputData, labels.row(0), max(labels.row(0)) + 1,
      perceptronIter);

  // Define parameters for the adaboost
  int iterations = 50;
  double tolerance = 1e-10;
  AdaBoost<> a(inputData, labels.row(0), iterations, tolerance, p);
  int countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if(labels(i) != a.finalHypothesis(i))
      countError++;
  double hammingLoss = (double) countError / labels.n_cols;

  double ztP = a.GetztProduct();
  BOOST_REQUIRE(hammingLoss <= ztP);
}

/**
 *  This test case runs the AdaBoost.mh algorithm on the UCI Vertebral
 *  Column dataset.
 *  It checks if the error returned by running a single instance of the
 *  weak learner is worse than running the boosted weak learner using
 *  adaboost.
 */
BOOST_AUTO_TEST_CASE(WeakLearnerErrorVertebralColumn)
{
  arma::mat inputData;

  if (!data::Load("vc2.txt", inputData))
    BOOST_FAIL("Cannot load test dataset vc2.txt!");

  arma::Mat<size_t> labels;

  if (!data::Load("vc2_labels.txt",labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  // no need to map the labels here

  // Define your own weak learner, perceptron in this case.
  // Run the perceptron for perceptron_iter iterations.
  int perceptronIter = 800;

  arma::Row<size_t> perceptronPrediction(labels.n_cols);
  perceptron::Perceptron<> p(inputData, labels.row(0), max(labels.row(0)) + 1,
      perceptronIter);
  p.Classify(inputData, perceptronPrediction);

  int countWeakLearnerError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if(labels(i) != perceptronPrediction(i))
      countWeakLearnerError++;
  double weakLearnerErrorRate = (double) countWeakLearnerError / labels.n_cols;

  // Define parameters for the adaboost
  int iterations = 50;
  double tolerance = 1e-10;
  AdaBoost<> a(inputData, labels.row(0), iterations, tolerance, p);
  int countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if(labels(i) != a.finalHypothesis(i))
      countError++;
  double error = (double) countError / labels.n_cols;

  BOOST_REQUIRE(error <= weakLearnerErrorRate);
}

/**
 *  This test case runs the AdaBoost.mh algorithm on non-linearly
 *  separable dataset.
 *  It checks whether the hamming loss breaches the upperbound, which
 *  is provided by ztAccumulator.
 */
BOOST_AUTO_TEST_CASE(HammingLossBoundNonLinearSepData)
{
  arma::mat inputData;

  if (!data::Load("train_nonlinsep.txt", inputData))
    BOOST_FAIL("Cannot load test dataset train_nonlinsep.txt!");

  arma::Mat<size_t> labels;

  if (!data::Load("train_labels_nonlinsep.txt",labels))
    BOOST_FAIL("Cannot load labels for train_labels_nonlinsep.txt");

  // no need to map the labels here

  // Define your own weak learner, perceptron in this case.
  // Run the perceptron for perceptron_iter iterations.
  int perceptronIter = 800;

  perceptron::Perceptron<> p(inputData, labels.row(0), max(labels.row(0)) + 1,
      perceptronIter);

  // Define parameters for the adaboost
  int iterations = 50;
  double tolerance = 1e-10;
  AdaBoost<> a(inputData, labels.row(0), iterations, tolerance, p);
  int countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if(labels(i) != a.finalHypothesis(i))
      countError++;
  double hammingLoss = (double) countError / labels.n_cols;

  double ztP = a.GetztProduct();
  BOOST_REQUIRE(hammingLoss <= ztP);
}

/**
 *  This test case runs the AdaBoost.mh algorithm on a non-linearly
 *  separable dataset.
 *  It checks if the error returned by running a single instance of the
 *  weak learner is worse than running the boosted weak learner using
 *  adaboost.
 */
BOOST_AUTO_TEST_CASE(WeakLearnerErrorNonLinearSepData)
{
  arma::mat inputData;

  if (!data::Load("train_nonlinsep.txt", inputData))
    BOOST_FAIL("Cannot load test dataset train_nonlinsep.txt!");

  arma::Mat<size_t> labels;

  if (!data::Load("train_labels_nonlinsep.txt",labels))
    BOOST_FAIL("Cannot load labels for train_labels_nonlinsep.txt");

  // no need to map the labels here

  // Define your own weak learner, perceptron in this case.
  // Run the perceptron for perceptron_iter iterations.
  int perceptronIter = 800;

  arma::Row<size_t> perceptronPrediction(labels.n_cols);
  perceptron::Perceptron<> p(inputData, labels.row(0), max(labels.row(0)) + 1,
      perceptronIter);
  p.Classify(inputData, perceptronPrediction);

  int countWeakLearnerError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if(labels(i) != perceptronPrediction(i))
      countWeakLearnerError++;
  double weakLearnerErrorRate = (double) countWeakLearnerError / labels.n_cols;

  // Define parameters for the adaboost
  int iterations = 50;
  double tolerance = 1e-10;
  AdaBoost<> a(inputData, labels.row(0), iterations, tolerance, p);
  int countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if(labels(i) != a.finalHypothesis(i))
      countError++;
  double error = (double) countError / labels.n_cols;

  BOOST_REQUIRE(error <= weakLearnerErrorRate);
}

/**
 *  This test case runs the AdaBoost.mh algorithm on the UCI Iris dataset.
 *  It checks whether the hamming loss breaches the upperbound, which
 *  is provided by ztAccumulator.
 *  This is for the weak learner: Decision Stumps.
 */
BOOST_AUTO_TEST_CASE(HammingLossIris_DS)
{
  arma::mat inputData;

  if (!data::Load("iris.txt", inputData))
    BOOST_FAIL("Cannot load test dataset iris.txt!");

  arma::Mat<size_t> labels;

  if (!data::Load("iris_labels.txt",labels))
    BOOST_FAIL("Cannot load labels for iris_labels.txt");

  // no need to map the labels here

  // Define your own weak learner, Decision Stumps in this case.

  // Define parameters for the adaboost
  const size_t numClasses = 3;
  const size_t inpBucketSize = 6;

  decision_stump::DecisionStump<> ds(inputData, labels.row(0),
                                     numClasses, inpBucketSize);
  int iterations = 50;
  double tolerance = 1e-10;

  AdaBoost<arma::mat, mlpack::decision_stump::DecisionStump<> > a(inputData,
          labels.row(0), iterations, tolerance, ds);
  int countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if(labels(i) != a.finalHypothesis(i))
      countError++;
  double hammingLoss = (double) countError / labels.n_cols;

  double ztP = a.GetztProduct();
  BOOST_REQUIRE(hammingLoss <= ztP);
}

/**
 *  This test case runs the AdaBoost.mh algorithm on a non-linearly
 *  separable dataset.
 *  It checks if the error returned by running a single instance of the
 *  weak learner is worse than running the boosted weak learner using
 *  adaboost.
 *  This is for the weak learner: Decision Stumps.
 */
BOOST_AUTO_TEST_CASE(WeakLearnerErrorIris_DS)
{
  arma::mat inputData;

  if (!data::Load("iris.txt", inputData))
    BOOST_FAIL("Cannot load test dataset iris.txt!");

  arma::Mat<size_t> labels;

  if (!data::Load("iris_labels.txt",labels))
    BOOST_FAIL("Cannot load labels for iris_labels.txt");

  // no need to map the labels here

  // Define your own weak learner, Decision Stump in this case.

  const size_t numClasses = 3;
  const size_t inpBucketSize = 6;

  arma::Row<size_t> dsPrediction(labels.n_cols);

  decision_stump::DecisionStump<> ds(inputData, labels.row(0),
                                     numClasses, inpBucketSize);
  ds.Classify(inputData, dsPrediction);

  int countWeakLearnerError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if(labels(i) != dsPrediction(i))
      countWeakLearnerError++;
  double weakLearnerErrorRate = (double) countWeakLearnerError / labels.n_cols;

  // Define parameters for the adaboost
  int iterations = 50;
  double tolerance = 1e-10;

  AdaBoost<arma::mat, mlpack::decision_stump::DecisionStump<> > a(inputData,
           labels.row(0), iterations, tolerance, ds);
  int countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if(labels(i) != a.finalHypothesis(i))
      countError++;
  double error = (double) countError / labels.n_cols;

  BOOST_REQUIRE(error <= weakLearnerErrorRate);
}
/**
 *  This test case runs the AdaBoost.mh algorithm on the UCI Vertebral
 *  Column dataset.
 *  It checks if the error returned by running a single instance of the
 *  weak learner is worse than running the boosted weak learner using
 *  adaboost.
 *  This is for the weak learner: Decision Stumps.
 */
BOOST_AUTO_TEST_CASE(HammingLossBoundVertebralColumn_DS)
{
  arma::mat inputData;

  if (!data::Load("vc2.txt", inputData))
    BOOST_FAIL("Cannot load test dataset vc2.txt!");

  arma::Mat<size_t> labels;

  if (!data::Load("vc2_labels.txt",labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  // no need to map the labels here

  // Define your own weak learner, Decision Stump in this case.

  // Define parameters for the adaboost
  const size_t numClasses = 3;
  const size_t inpBucketSize = 6;

  decision_stump::DecisionStump<> ds(inputData, labels.row(0),
                                     numClasses, inpBucketSize);

  int iterations = 50;
  double tolerance = 1e-10;

  AdaBoost<arma::mat, mlpack::decision_stump::DecisionStump<> > a(inputData,
           labels.row(0), iterations, tolerance, ds);
  int countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if(labels(i) != a.finalHypothesis(i))
      countError++;
  double hammingLoss = (double) countError / labels.n_cols;

  double ztP = a.GetztProduct();
  BOOST_REQUIRE(hammingLoss <= ztP);
}

/**
 *  This test case runs the AdaBoost.mh algorithm on the UCI Vertebral
 *  Column dataset.
 *  It checks if the error returned by running a single instance of the
 *  weak learner is worse than running the boosted weak learner using
 *  adaboost.
 *  This is for the weak learner: Decision Stumps.
 */
BOOST_AUTO_TEST_CASE(WeakLearnerErrorVertebralColumn_DS)
{
  arma::mat inputData;

  if (!data::Load("vc2.txt", inputData))
    BOOST_FAIL("Cannot load test dataset vc2.txt!");

  arma::Mat<size_t> labels;

  if (!data::Load("vc2_labels.txt",labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  // no need to map the labels here

  // Define your own weak learner, Decision Stump in this case.

  const size_t numClasses = 3;
  const size_t inpBucketSize = 6;

  arma::Row<size_t> dsPrediction(labels.n_cols);

  decision_stump::DecisionStump<> ds(inputData, labels.row(0),
                                     numClasses, inpBucketSize);

  int countWeakLearnerError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if(labels(i) != dsPrediction(i))
      countWeakLearnerError++;
  double weakLearnerErrorRate = (double) countWeakLearnerError / labels.n_cols;

  // Define parameters for the adaboost
  int iterations = 50;
  double tolerance = 1e-10;
  AdaBoost<arma::mat, mlpack::decision_stump::DecisionStump<> > a(inputData,
           labels.row(0), iterations, tolerance, ds);
  int countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if(labels(i) != a.finalHypothesis(i))
      countError++;
  double error = (double) countError / labels.n_cols;

  BOOST_REQUIRE(error <= weakLearnerErrorRate);
}
/**
 *  This test case runs the AdaBoost.mh algorithm on non-linearly
 *  separable dataset.
 *  It checks whether the hamming loss breaches the upperbound, which
 *  is provided by ztAccumulator.
 *  This is for the weak learner: Decision Stumps.
 */
BOOST_AUTO_TEST_CASE(HammingLossBoundNonLinearSepData_DS)
{
  arma::mat inputData;

  if (!data::Load("train_nonlinsep.txt", inputData))
    BOOST_FAIL("Cannot load test dataset train_nonlinsep.txt!");

  arma::Mat<size_t> labels;

  if (!data::Load("train_labels_nonlinsep.txt",labels))
    BOOST_FAIL("Cannot load labels for train_labels_nonlinsep.txt");

  // no need to map the labels here

  // Define your own weak learner, Decision Stump in this case.

  // Define parameters for the adaboost
  const size_t numClasses = 2;
  const size_t inpBucketSize = 6;

  decision_stump::DecisionStump<> ds(inputData, labels.row(0),
                                     numClasses, inpBucketSize);

  int iterations = 50;
  double tolerance = 1e-10;

  AdaBoost<arma::mat, mlpack::decision_stump::DecisionStump<> > a(inputData,
           labels.row(0), iterations, tolerance, ds);
  int countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if(labels(i) != a.finalHypothesis(i))
      countError++;
  double hammingLoss = (double) countError / labels.n_cols;

  double ztP = a.GetztProduct();
  BOOST_REQUIRE(hammingLoss <= ztP);
}

/**
 *  This test case runs the AdaBoost.mh algorithm on a non-linearly
 *  separable dataset.
 *  It checks if the error returned by running a single instance of the
 *  weak learner is worse than running the boosted weak learner using
 *  adaboost.
 *  This for the weak learner: Decision Stumps.
 */
BOOST_AUTO_TEST_CASE(WeakLearnerErrorNonLinearSepData_DS)
{
  arma::mat inputData;

  if (!data::Load("train_nonlinsep.txt", inputData))
    BOOST_FAIL("Cannot load test dataset train_nonlinsep.txt!");

  arma::Mat<size_t> labels;

  if (!data::Load("train_labels_nonlinsep.txt",labels))
    BOOST_FAIL("Cannot load labels for train_labels_nonlinsep.txt");

  // no need to map the labels here

  // Define your own weak learner, Decision Stump in this case.

  const size_t numClasses = 2;
  const size_t inpBucketSize = 3;

  arma::Row<size_t> dsPrediction(labels.n_cols);

  decision_stump::DecisionStump<> ds(inputData, labels.row(0),
                                     numClasses, inpBucketSize);

  int countWeakLearnerError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if(labels(i) != dsPrediction(i))
      countWeakLearnerError++;
  double weakLearnerErrorRate = (double) countWeakLearnerError / labels.n_cols;

  // Define parameters for the adaboost
  int iterations = 500;
  double tolerance = 1e-23;

  AdaBoost<arma::mat, mlpack::decision_stump::DecisionStump<> > a(inputData,
           labels.row(0), iterations, tolerance, ds);
  int countError = 0;
  for (size_t i = 0; i < labels.n_cols; i++)
    if(labels(i) != a.finalHypothesis(i))
      countError++;
  double error = (double) countError / labels.n_cols;

  BOOST_REQUIRE(error <= weakLearnerErrorRate);
}

/**
 *  This test case runs the AdaBoost.mh algorithm on the UCI Vertebral
 *  Column dataset.
 *  It tests the Classify function and checks for a satisfiable error rate.
 */
BOOST_AUTO_TEST_CASE(ClassifyTest_VERTEBRALCOL)
{
  mlpack::math::RandomSeed(std::time(NULL));
  arma::mat inputData;

  if (!data::Load("vc2.txt", inputData))
    BOOST_FAIL("Cannot load test dataset vc2.txt!");

  arma::Mat<size_t> labels;

  if (!data::Load("vc2_labels.txt",labels))
    BOOST_FAIL("Cannot load labels for vc2_labels.txt");

  // no need to map the labels here

  // Define your own weak learner, perceptron in this case.
  // Run the perceptron for perceptron_iter iterations.

  int perceptronIter = 1000;

  arma::mat testData;

  if (!data::Load("vc2_test.txt", testData))
    BOOST_FAIL("Cannot load test dataset vc2_test.txt!");

  arma::Mat<size_t> trueTestLabels;

  if (!data::Load("vc2_test_labels.txt",trueTestLabels))
    BOOST_FAIL("Cannot load labels for vc2_test_labels.txt");

  arma::Row<size_t> perceptronPrediction(labels.n_cols);
  perceptron::Perceptron<> p(inputData, labels.row(0), max(labels.row(0)) + 1,
      perceptronIter);
  p.Classify(inputData, perceptronPrediction);

  // Define parameters for the adaboost

  int iterations = 100;
  double tolerance = 1e-10;
  AdaBoost<> a(inputData, labels.row(0), iterations, tolerance, p);

  arma::Row<size_t> predictedLabels(testData.n_cols);
  a.Classify(testData, predictedLabels);

  int localError = 0;

  for (size_t i = 0; i < trueTestLabels.n_cols; i++)
    if(trueTestLabels(i) != predictedLabels(i))
      localError++;

  double lError = (double) localError / trueTestLabels.n_cols;

  BOOST_REQUIRE(lError <= 0.30);

}

/**
 *  This test case runs the AdaBoost.mh algorithm on a non linearly
 *  separable dataset.
 *  It tests the Classify function and checks for a satisfiable error rate.
 */
BOOST_AUTO_TEST_CASE(ClassifyTest_NONLINSEP)
{
  arma::mat inputData;

  if (!data::Load("train_nonlinsep.txt", inputData))
    BOOST_FAIL("Cannot load test dataset train_nonlinsep.txt!");

  arma::Mat<size_t> labels;

  if (!data::Load("train_labels_nonlinsep.txt",labels))
    BOOST_FAIL("Cannot load labels for train_labels_nonlinsep.txt");

  // no need to map the labels here

  // Define your own weak learner, perceptron in this case.
  // Run the perceptron for perceptron_iter iterations.

  const size_t numClasses = 2;
  const size_t inpBucketSize = 3;

  arma::mat testData;

  if (!data::Load("test_nonlinsep.txt", testData))
    BOOST_FAIL("Cannot load test dataset test_nonlinsep.txt!");

  arma::Mat<size_t> trueTestLabels;

  if (!data::Load("test_labels_nonlinsep.txt",trueTestLabels))
    BOOST_FAIL("Cannot load labels for test_labels_nonlinsep.txt");

  arma::Row<size_t> dsPrediction(labels.n_cols);

  decision_stump::DecisionStump<> ds(inputData, labels.row(0),
                                     numClasses, inpBucketSize);

  // Define parameters for the adaboost
  int iterations = 50;
  double tolerance = 1e-10;
  AdaBoost<arma::mat, mlpack::decision_stump::DecisionStump<> > a(
           inputData, labels.row(0), iterations, tolerance, ds);

  arma::Row<size_t> predictedLabels(testData.n_cols);
  a.Classify(testData, predictedLabels);

  int localError = 0;
  for (size_t i = 0; i < trueTestLabels.n_cols; i++)
    if(trueTestLabels(i) != predictedLabels(i))
      localError++;

  double lError = (double) localError / trueTestLabels.n_cols;

  BOOST_REQUIRE(lError <= 0.30);
}

/**
 *  This test case runs the AdaBoost.mh algorithm on the UCI Iris Dataset.
 *  It trains it on two thirds of the Iris dataset (iris_train.csv),
 *  and tests on the remaining third of the dataset (iris_test.csv).
 *  It tests the Classify function and checks for a satisfiable error rate.
 */
BOOST_AUTO_TEST_CASE(ClassifyTest_IRIS)
{
  arma::mat inputData;

  if (!data::Load("iris_train.csv", inputData))
    BOOST_FAIL("Cannot load test dataset iris_train.csv!");

  arma::Mat<size_t> labels;

  if (!data::Load("iris_train_labels.csv",labels))
    BOOST_FAIL("Cannot load labels for iris_train_labels.csv");

  // no need to map the labels here

  // Define your own weak learner, perceptron in this case.
  // Run the perceptron for perceptron_iter iterations.
  int perceptronIter = 800;

  perceptron::Perceptron<> p(inputData, labels.row(0), max(labels.row(0)) + 1,
      perceptronIter);

  // Define parameters for the adaboost
  int iterations = 50;
  double tolerance = 1e-10;
  AdaBoost<> a(inputData, labels.row(0), iterations, tolerance, p);

  arma::mat testData;
  if (!data::Load("iris_test.csv", inputData))
    BOOST_FAIL("Cannot load test dataset iris_test.csv!");

  arma::Row<size_t> predictedLabels(testData.n_cols);

  a.Classify(testData, predictedLabels);

  arma::Row<size_t> trueTestLabels;
  if (!data::Load("iris_test_labels.csv", inputData))
    BOOST_FAIL("Cannot load test dataset iris_test_labels.csv!");

  int localError = 0;
  for (size_t i = 0; i < trueTestLabels.n_cols; i++)
    if(trueTestLabels(i) != predictedLabels(i))
      localError++;
  double lError = (double) localError / labels.n_cols;

  BOOST_REQUIRE(lError <= 0.30);
}

BOOST_AUTO_TEST_SUITE_END();


#pragma once
#include <algorithm>
#include <vector>
#include <cmath>
#include <Eigen/Dense>

using std::vector;
using std::pair;
// using falkolib::Keypoint;
using Eigen::Vector3f;
using Eigen::MatrixXf;
// using isam::Pose2d;

/*
Y.T.
This class implements a family of pairwise data assoication algorithms.
Currently supports:
- nearest neighbor (NN) matching
- correspondence graph (CG) matching
*/
class PairwiseMatcher
{
public:
	PairwiseMatcher(double& dist_tol_inp);
	~PairwiseMatcher();
	void reset();

	// nearest neighbor (NN) matching
	void set_nn_dist_tol(double dist_tol);
	// NN matching assumes that pc1 and pc2 are in the same coordinate frame!
	bool nn_match(vector<Eigen::Vector3f> pc1, vector<Eigen::Vector3f> pc2);

	unsigned get_num_matches();
	void get_matches(vector<pair<int, int>>& matches);

	// Convert resulting pairwise matches into a partial permutation matrix
	void get_permutation_matrix(Eigen::MatrixXf& P);

private:
	// NN matching parameters
	double nn_dist_tol_ = 0;
	vector<pair<int, int>> matches_;
	vector<Vector3f> pc1_;
	vector<Vector3f> pc2_;
};