/*
   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 ================================================================


// 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()


/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, development version     *
*                (c) 2006-2017 INRIA, USTL, UJF, CNRS, MGH                    *
*                                                                             *
* This program is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This program is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
// Author: François Faure, INRIA-UJF, (C) 2011
//
// Copyright: See COPYING file that comes with this distribution
#include <SofaEigen2Solver/SVDLinearSolver.h>
#include <sofa/core/visual/VisualParams.h>
#include <SofaBaseLinearSolver/FullMatrix.h>
#include <SofaBaseLinearSolver/SparseMatrix.h>
#include <SofaBaseLinearSolver/CompressedRowSparseMatrix.h>
#include <sofa/simulation/MechanicalVisitor.h>
#include <sofa/helper/system/thread/CTime.h>
#include <sofa/helper/AdvancedTimer.h>
#include <sofa/core/ObjectFactory.h>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>

namespace sofa
{
namespace component
{
namespace linearsolver
{
using core::VecId;
using namespace sofa::defaulttype;
using namespace sofa::core::behavior;
using namespace sofa::simulation;
#ifdef DISPLAY_TIME
using sofa::helper::system::thread::CTime;
#endif

template<class TMatrix, class TVector>
SVDLinearSolver<TMatrix,TVector>::SVDLinearSolver()
    : f_verbose( initData(&f_verbose,false,"verbose","Dump system state at each iteration") )
    , f_minSingularValue( initData(&f_minSingularValue,(Real)1.0e-6,"minSingularValue","Thershold under which a singular value is set to 0, for the stabilization of ill-conditioned system.") )
    , f_conditionNumber( initData(&f_conditionNumber,(Real)0.0,"conditionNumber","Condition number of the matrix: ratio between the largest and smallest singular values. Computed in method solve.") )
{
#ifdef DISPLAY_TIME
    timeStamp = 1.0 / (double)CTime::getRefTicksPerSec();
#endif
}

/// Solve Mx=b
template<class TMatrix, class TVector>
void SVDLinearSolver<TMatrix,TVector>::solve(Matrix& M, Vector& x, Vector& b)
{
#ifdef SOFA_DUMP_VISITOR_INFO
    simulation::Visitor::printComment("SVD");
#endif
#ifdef DISPLAY_TIME
    CTime timer;
    double time1 = (double) timer.getTime();
#endif
    const bool printLog = this->f_printLog.getValue();
    const bool verbose  = f_verbose.getValue();

    /// Convert the matrix and the right-hand vector to Eigen objects
    Eigen::MatrixXd m(M.rowSize(),M.colSize());
    Eigen::VectorXd rhs(M.rowSize());
    for(unsigned i=0; i<(unsigned)M.rowSize(); i++ )
    {
        for( unsigned j=0; j<(unsigned)M.colSize(); j++ )
            m(i,j) = M[i][j];
        rhs(i) = b[i];
    }
    if(verbose)
    {
        serr << "SVDLinearSolver<TMatrix,TVector>::solve, Here is the matrix m:" << sendl << m << sendl;
    }

    /// Compute the SVD decomposition and the condition number
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(m, Eigen::ComputeThinU | Eigen::ComputeThinV);
    f_conditionNumber.setValue( (Real)(svd.singularValues()(0) / svd.singularValues()(M.rowSize()-1)) );
    if(printLog)
    {
        serr << "SVDLinearSolver<TMatrix,TVector>::solve, the singular values are:" << sendl << svd.singularValues() << sendl;
    }
    if(verbose)
    {
        serr << "Its left singular vectors are the columns of the thin U matrix:" << sendl << svd.matrixU() << sendl;
        serr << "Its right singular vectors are the columns of the thin V matrix:" << sendl << svd.matrixV() << sendl;
    }

    /// Solve the equation system and copy the solution to the SOFA vector
//    Eigen::VectorXd solution = svd.solve(rhs);
//    for(unsigned i=0; i<M.rowSize(); i++ ){
//        x[i] = solution(i);
//    }
    Eigen::VectorXd Ut_b = svd.matrixU().transpose() *  rhs;
    Eigen::VectorXd S_Ut_b(M.colSize());
    for( unsigned i=0; i<(unsigned)M.colSize(); i++ )   /// product with the diagonal matrix, using the threshold for near-null values
    {
        if( svd.singularValues()[i] > f_minSingularValue.getValue() )
            S_Ut_b[i] = Ut_b[i]/svd.singularValues()[i];
        else
            S_Ut_b[i] = (Real)0.0 ;
    }
    Eigen::VectorXd solution = svd.matrixV() * S_Ut_b;
    for(unsigned i=0; i<(unsigned)M.rowSize(); i++ )
    {
        x[i] = (Real) solution(i);
    }

    if( printLog )
    {
#ifdef DISPLAY_TIME
        time1 = (double)(((double) timer.getTime() - time1) * timeStamp / (nb_iter-1));
        std::cerr<<"SVDLinearSolver::solve, SVD = "<<time1<<std::endl;
#endif
        serr << "SVDLinearSolver<TMatrix,TVector>::solve, rhs vector = " << sendl << rhs.transpose() << sendl;
        serr << "SVDLinearSolver<TMatrix,TVector>::solve, solution = " << sendl << x << sendl;
        serr << "SVDLinearSolver<TMatrix,TVector>::solve, verification, mx - b = " << sendl << (m * solution - rhs ).transpose() << sendl;
    }
}

SOFA_DECL_CLASS(SVDLinearSolver)

int SVDLinearSolverClass = core::RegisterObject("Linear system solver using the conjugate gradient iterative algorithm")
        .add< SVDLinearSolver< FullMatrix<double>, FullVector<double> > >()
        .add< SVDLinearSolver< FullMatrix<float>, FullVector<float> > >()
        .addAlias("SVDLinear")
        .addAlias("SVD")
        ;

} // namespace linearsolver

} // namespace component

} // namespace sofa


/*=========================================================================
 *
 * Copyright Universitat Pompeu Fabra, Department of Information and
 * Comunication Technologies.
 *
 *  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.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/

#ifndef itkRWSegmentationFilter_hxx
#define itkRWSegmentationFilter_hxx

#include <itkImageRegionIterator.h>
#include <itkImageRegionConstIterator.h>

#include <vector>
#include <Eigen/Dense>
#include <Eigen/Sparse>

namespace itk
{
template <typename TInputImage, typename TOutputImage>
void RWSegmentationFilter<TInputImage, TOutputImage>::GenerateData()
{
  if (m_LabelImage == nullptr) // Exit if SetLabelImage has not been called
  {
    std::cout << "Label image has not been set" << std::endl;
    return;
  }
  if (OutputImageType::ImageDimension != 2 && OutputImageType::ImageDimension != 3)
  {
    std::cout << "Exit segmentation. Image dimension must be 2 or 3 but is " << OutputImageType::ImageDimension << std::endl;
    return;
  }

  // Get regions to iterate for original and label images
  typename OutputImageType::RegionType regionLabel = m_LabelImage->GetLargestPossibleRegion();

  // Set label image iterator to get the bounding box size and the quantity of marked nodes
  typedef itk::ImageRegionIterator<OutputImageType> IteratorLabelType;
  IteratorLabelType itLabel(m_LabelImage, regionLabel);

  // Crop image to bounds containin labels
  typename OutputImageType::RegionType regionLabelCrop;
  typename InputImageType::RegionType regionCrop;

  for (int i = 0; i != OutputImageType::ImageDimension; ++i)
  {
    regionLabelCrop.SetSize(i, regionLabel.GetIndex()[i]);
    regionLabelCrop.SetIndex(i, regionLabel.GetSize()[i]);
  }

  int markedLength = 0;

  itLabel.GoToBegin();
  while (!itLabel.IsAtEnd())
  {
    // Get image boundaries to crop image
    if (itLabel.Get() != 0)
    {
      ++markedLength;
      // Get bounding region
      for (int i = 0; i != OutputImageType::ImageDimension; ++i)
      {
        if (itLabel.GetIndex()[i] < regionLabelCrop.GetIndex()[i])
        {
          regionCrop.SetIndex(i, itLabel.GetIndex()[i]);
          regionLabelCrop.SetIndex(i, itLabel.GetIndex()[i]);
        }
        else if (itLabel.GetIndex()[i] > regionLabelCrop.GetSize()[i])
        {
          regionCrop.SetSize(i, itLabel.GetIndex()[i]);
          regionLabelCrop.SetSize(i, itLabel.GetIndex()[i]);
        }
      }
    }
    ++itLabel;
  }

  int totalNodes = 1;
  for (int i = 0; i != OutputImageType::ImageDimension; ++i)
  {
    regionCrop.SetIndex(i, regionCrop.GetIndex()[i]);
    regionLabelCrop.SetIndex(i, regionLabelCrop.GetIndex()[i]);
    regionCrop.SetSize(i, regionCrop.GetSize()[i] - regionCrop.GetIndex()[i] + 1);
    regionLabelCrop.SetSize(i, regionLabelCrop.GetSize()[i] - regionLabelCrop.GetIndex()[i] + 1);

    totalNodes *= regionCrop.GetSize()[i];
  }

  /////////////////////// Build Graph /////////////////////////////
  /*
    // Create graph. Each node will correspond to a pixel connected to its neighbors by an edge.
    // Neighbors are those nodes which distance is 1 ( d = sqrt((x-xi)² + (y-yi)² + (z-zi)²) = 1 ),
    // where x, y, and z are the indices of a pixel and xi, yi, and zi are the indeces of a neighboring pixel
    // Iterate through all pixels of input and label images
    */

  int unmarkedLength = totalNodes - markedLength; /* Quantity of unmarked nodes */

  // Define vectors to store graph data
  std::vector<float> *nodes = new std::vector<float>(totalNodes);  /* Pixel intensity for all nodes/pixels */
  std::vector<float> *labels = new std::vector<float>(totalNodes); /* Label of each node/pixel */
  /*  For node 'i'. If 'i' is a marked node, previousFound->at(i) is how many unmarked nodes there are before node 'i'.
        If 'i' is an unmarked node, previousFound->at(i) is how many marked nodes there are before node 'i'.
        This values are needed to build ordered Lu and BT matrices.
    */
  std::vector<int> *previousFound = new std::vector<int>(totalNodes);
  std::vector<int> *unmarked = new std::vector<int>(unmarkedLength); /* Indices of unmarked nodes ordered */
  std::vector<int> *marked = new std::vector<int>(markedLength);     /* Store labels of marked nodes */
  std::vector<int> *markedIdx = new std::vector<int>(markedLength);  /* Indices of marked nodes ordered */
  std::vector<int> *nameLabels = new std::vector<int>();             /* Values of the different labels of the prior */

  // Set bounding box image iterators
  typedef itk::ImageRegionConstIterator<InputImageType> ConstIteratorImageType;
  ConstIteratorImageType itImageCrop(this->GetInput(), regionCrop);
  IteratorLabelType itLabelCrop(m_LabelImage, regionLabelCrop);

  int foundMarked = 0;
  int foundUnmarked = 0;
  int NodeIdx = 0;

  itImageCrop.GoToBegin();
  itLabelCrop.GoToBegin();
  while (!itImageCrop.IsAtEnd())
  {
    // Store intensity in a std::vector
    nodes->at(NodeIdx) = itImageCrop.Get();
    // Store labels in a std::vector. Each index of 'nodes' and 'labels' correspond to the same pixel
    labels->at(NodeIdx) = itLabelCrop.Get();

    if (itLabelCrop.Get() == 0)
    {
      // Get the index of each node that is unmarked
      unmarked->at(foundUnmarked) = NodeIdx;
      // Store how many marked points have been found before this unmarked node
      previousFound->at(NodeIdx) = foundMarked;
      ++foundUnmarked;
    }
    else
    {
      // Set to label value if label != 0
      marked->at(foundMarked) = itLabelCrop.Get();
      // Get the index of each node that is marked
      markedIdx->at(foundMarked) = NodeIdx;
      // Store how many unmarked points have been found before this marked node
      previousFound->at(NodeIdx) = foundUnmarked;
      ++foundMarked;
      bool found;

      // Store the different labels in a std::vector
      found = std::find(nameLabels->begin(), nameLabels->end(), itLabelCrop.Get()) != nameLabels->end();
      if (!found)
      {
        nameLabels->push_back(itLabelCrop.Get());
      }
    }
    ++itImageCrop;
    ++itLabelCrop;
    ++NodeIdx;
  }

  // Sort labels
  std::sort(nameLabels->begin(), nameLabels->end());

  int totalLabels = nameLabels->size();

  if (m_SolveForAllLabels)
  {
    totalLabels += 1;
  }

  // Linear system: Lu * X = -BT * M
  // Convert marked (M) into a Eigen::Sparse matrix markedRHS. Needed for the computation of -BT * M (Eigen::SparseMatrix * Eigen::SparseMatrix)
  Eigen::SparseMatrix<float, Eigen::ColMajor> *markedRHS = new Eigen::SparseMatrix<float, Eigen::ColMajor>(markedLength, totalLabels - 1);
  for (int i = 0; i != totalLabels - 1; ++i)
  {
    int pos = 0;
    for (auto itMarked = marked->begin(); itMarked != marked->end(); ++itMarked)
    {
      if (*itMarked == nameLabels->at(i))
        markedRHS->insert(pos, i) = 1;
      ++pos;
    }
  }

  marked->clear();
  delete marked;

  /////////////////////// Build Laplacian matrix /////////////////////////////
  // Normalize intensity gradient over image spacing
  std::vector<float> spacing;
  typename InputImageType::SpacingType space = this->GetInput()->GetSpacing();
  if (OutputImageType::ImageDimension == 2)
    spacing = {space[1], space[0], space[0], space[1]};
  else if (OutputImageType::ImageDimension == 3)
    spacing = {space[2], space[1], space[0], space[0], space[1], space[2]};

  std::vector<int> neighbors;
  int x = regionCrop.GetSize()[0];
  int y = regionCrop.GetSize()[1];
  //  Right hand of the equation: -BT * M
  Eigen::SparseMatrix<float, Eigen::ColMajor> *BTxM = new Eigen::SparseMatrix<float, Eigen::ColMajor>(markedLength, totalLabels - 1);

  // Build BT
  // Compare whether NumCols < NumRows for less iterations during BT building
  if (markedIdx->size() < unmarked->size())
  {
    Eigen::SparseMatrix<float, Eigen::ColMajor> *BT = new Eigen::SparseMatrix<float, Eigen::ColMajor>(unmarkedLength, markedLength);
    BT->reserve(Eigen::VectorXi::Constant(markedLength, 6));

    int node;
    float valNode, valNeighbor, w;

    // Iterate through marked nodes to build BT. Rows correspond to unmarked nodes, columns to marked nodes
    for (auto itMarked = markedIdx->begin(); itMarked != markedIdx->end(); ++itMarked)
    {
      valNode = nodes->at(*itMarked); // Intensity of node
      // Obtain neighbors indexes. right and left, top and bottom, front and back.
      node = *itMarked;
      if (OutputImageType::ImageDimension == 2)
        neighbors = {node - x, node - 1, node + 1, node + x};
      else if (OutputImageType::ImageDimension == 3)
        neighbors = {node - x * y, node - x, node - 1, node + 1, node + x, node + x * y};
      for (int i = 0; i != neighbors.size(); ++i)
      {
        // Make sure all the neighbors computed fall within the bounding box dimension
        if (neighbors.at(i) >= 0 && neighbors.at(i) < totalNodes && labels->at(neighbors.at(i)) == 0)
        {
          valNeighbor = nodes->at(neighbors.at(i));                                    // Intensity of neighbor pixel
          w = (exp(-m_Beta * pow((valNode - valNeighbor) / spacing.at(i), 2)) + 1e-6); // Intensity gradient following a Gaussian function
          //  Columns of BT correspond to marked nodes, rows to unmarked
          BT->insert(neighbors.at(i) - previousFound->at(neighbors.at(i)), node - previousFound->at(node)) = -w;
        }
      }
    }
    markedIdx->clear();
    //  Right hand of the equation: -BT * M
    *BTxM = -*BT * *markedRHS;
    BT->resize(0, 0);
    BT->data().squeeze();
    markedRHS->resize(0, 0);
    markedRHS->data().squeeze();
    delete markedIdx, markedRHS, BT;
  }
  else
  {
    markedIdx->clear();
    delete markedIdx;

    Eigen::SparseMatrix<float, Eigen::RowMajor> *BT = new Eigen::SparseMatrix<float, Eigen::RowMajor>(unmarkedLength, markedLength);
    BT->reserve(Eigen::VectorXi::Constant(markedLength, 6));

    int node;
    float valNode, valNeighbor, w;

    // Iterate through unmarked nodes to build BT. Rows correspond to unmarked nodes, columns to marked nodes
    for (auto itUnmarked = unmarked->begin(); itUnmarked != unmarked->end(); ++itUnmarked)
    {
      valNode = nodes->at(*itUnmarked); // Intensity of node
      // Obtain neighbors indexes. right and left, top and bottom, front and back.
      node = *itUnmarked;
      if (OutputImageType::ImageDimension == 2)
        neighbors = {node - x, node - 1, node + 1, node + x};
      else if (OutputImageType::ImageDimension == 3)
        neighbors = {node - x * y, node - x, node - 1, node + 1, node + x, node + x * y};
      for (int i = 0; i != neighbors.size(); ++i)
      {
        // Make sure all the neighbors computed fall within the bounding box dimension
        if (neighbors.at(i) >= 0 && neighbors.at(i) < totalNodes && labels->at(neighbors.at(i)) != 0)
        {
          valNeighbor = nodes->at(neighbors.at(i));                                    // Intensity of neighbor pixel
          w = (exp(-m_Beta * pow((valNode - valNeighbor) / spacing.at(i), 2)) + 1e-6); // Intensity gradient following a Gaussian function
          //  Columns of BT correspond to marked nodes, rows to unmarked
          BT->insert(node - previousFound->at(node), neighbors.at(i) - previousFound->at(neighbors.at(i))) = -w;
        }
      }
    }
    //  Right hand of the equation: -BT * M
    *BTxM = -*BT * *markedRHS;
    BT->resize(0, 0);
    BT->data().squeeze();
    markedRHS->resize(0, 0);
    markedRHS->data().squeeze();
    delete markedRHS, BT;
  }

  // Build Lu. LHS of the equation
  Eigen::SparseMatrix<float, Eigen::RowMajor> *Lu = new Eigen::SparseMatrix<float, Eigen::RowMajor>(unmarkedLength, unmarkedLength);
  Lu->reserve(Eigen::VectorXi::Constant(unmarkedLength, 7));

  // Iterate through unmarked nodes to build Lu. Rows correspond to unmarked nodes, columns to unmarked nodes
  for (auto itUnmarked = unmarked->begin(); itUnmarked != unmarked->end(); ++itUnmarked)
  {
    int node;
    float valNode, valNeighbor, w, degree;

    valNode = nodes->at(*itUnmarked); // Intensity of node
    // Obtain neighbors indexes. right and left, top and bottom, front and back.
    node = *itUnmarked;
    if (OutputImageType::ImageDimension == 2)
      neighbors = {node - x, node - 1, node + 1, node + x};
    else if (OutputImageType::ImageDimension == 3)
      neighbors = {node - x * y, node - x, node - 1, node + 1, node + x, node + x * y};
    degree = 0.0;
    for (int i = 0; i != neighbors.size(); ++i)
    {
      // Make sure all the neighbors computed fall within the bounding box dimension
      if (neighbors.at(i) >= 0 && neighbors.at(i) < totalNodes)
      {
        valNeighbor = nodes->at(neighbors.at(i));                                    // Intensity of neighbor pixel
        w = (exp(-m_Beta * pow((valNode - valNeighbor) / spacing.at(i), 2)) + 1e-6); // Intensity gradient following a Gaussian function
        degree += w;                                                                 // Sum of the weights
        //  Columns of Lu correspond to unmarked nodes
        if (labels->at(neighbors.at(i)) == 0) // If neighbor is an unmarked node, build Lu
        {
          Lu->insert(node - previousFound->at(node), neighbors.at(i) - previousFound->at(neighbors.at(i))) = -w;
        }
      }
    }
    // Add node degree to diagonal of Lu
    Lu->insert(node - previousFound->at(node), node - previousFound->at(node)) = degree;
  }
  Lu->makeCompressed();

  nodes->clear();
  unmarked->clear();
  previousFound->clear();
  neighbors.clear();
  delete nodes, unmarked, previousFound;

  /////////////////////// Solve linear system /////////////////////////////
  // Lu * X = -BT * M
  // Set vector to store the result of the solver
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> *probabilities = new Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>(unmarkedLength, totalLabels - 1);

  // Allow multithreading. For the moment limited to 8 threads.
  if (m_NumberOfThreads > 0 && m_NumberOfThreads < 9)
  {
    omp_set_num_threads(m_NumberOfThreads);
    Eigen::setNbThreads(m_NumberOfThreads);
  }
  else
  {
    omp_set_num_threads(1);
    Eigen::setNbThreads(1);
  }

  // Set the solver for the problem with LHS Lu. BiCGStab.
  // Select either Eigen BiCGSTAB or ConjugateGradient
  Eigen::BiCGSTAB<Eigen::SparseMatrix<float, Eigen::RowMajor>, Eigen::DiagonalPreconditioner<float>> *solver = new Eigen::BiCGSTAB<Eigen::SparseMatrix<float, Eigen::RowMajor>, Eigen::DiagonalPreconditioner<float>>(*Lu); // Usually faster
  // Eigen::ConjugateGradient< Eigen::SparseMatrix< float ,Eigen::RowMajor> , Eigen::Lower|Eigen::Upper , Eigen::DiagonalPreconditioner<float> > solver(Lu);

  // Set solver parameters
  solver->setTolerance(m_Tolerance);
  solver->setMaxIterations(m_MaximumNumberOfIterations);
  // Compute probabilities with RHS BTxM
  *probabilities = solver->solve(*BTxM);

  m_SolverIterations = solver->iterations();
  m_SolverError = solver->error();

  Lu->resize(0, 0);
  Lu->data().squeeze();
  BTxM->resize(0, 0);
  BTxM->data().squeeze();
  delete Lu, BTxM;

  std::vector<int> *RWLabels = new std::vector<int>(unmarkedLength);

  /*  Assign a label to each unmarked node according to the result of the solver.
        The label that is assigned is that one corresponding to the highest probability.
        Since we solver for S-1 systems, last label probability is computed by subtraction */
  for (int i = 0; i != unmarkedLength; ++i)
  {
    float maxProbability = probabilities->coeffRef(i, 0);
    int maxLabelPos = 0;
    float accumulatedProbability = maxProbability;
    for (int j = 1; j != totalLabels - 1; ++j)
    {
      accumulatedProbability += probabilities->coeffRef(i, j);
      if (probabilities->coeffRef(i, j) > maxProbability)
      {
        maxProbability = probabilities->coeffRef(i, j);
        maxLabelPos = j;
      }
    }
    RWLabels->at(i) = 0.95 - accumulatedProbability > maxProbability ? nameLabels->back() : nameLabels->at(maxLabelPos);
  }
  probabilities->resize(0, 0);
  delete probabilities;

  int valBackground;
  if (!m_WriteBackground)
    valBackground = 0;
  else
    valBackground = nameLabels->back();

  typename OutputImageType::Pointer outputLabels = this->GetOutput();
  outputLabels->Graft(m_LabelImage);
  outputLabels->FillBuffer(valBackground);

  // Iterate through original label image to create segmentation image
  // Assign labels known from label image to their original value. Assign unmarked labels
  // according to the result from the solver

  IteratorLabelType itOut1(outputLabels, regionLabelCrop);

  // Set computed labels to segmentation image
  int unmarkedIdx = 0;
  int idxOutput = 0;

  itOut1.GoToBegin();
  while (!itOut1.IsAtEnd())
  {
    if (labels->at(idxOutput) == 0)
    {
      if (RWLabels->at(unmarkedIdx) != nameLabels->back())
        itOut1.Set(RWLabels->at(unmarkedIdx));
      else
        itOut1.Set(valBackground);
      ++unmarkedIdx;
    }
    else if (labels->at(idxOutput) != nameLabels->back())
      itOut1.Set(labels->at(idxOutput));

    ++idxOutput;
    ++itOut1;
  }

  labels->clear();
  RWLabels->clear();
  nameLabels->clear();
  delete labels, RWLabels, nameLabels;

  return;
}

template <typename TInputImage, typename TOutputImage>
void RWSegmentationFilter<TInputImage, TOutputImage>::PrintSelf(std::ostream &os, Indent indent) const
{
  Superclass::PrintSelf(os, indent);
  os << indent << "Beta: " << m_Beta << std::endl;
  os << indent << "NumberOfThreads: " << m_NumberOfThreads << std::endl;
  os << indent << "Tolerance: " << m_Tolerance << std::endl;
  os << indent << "MaximumNumberOfIterations: " << m_MaximumNumberOfIterations << std::endl;
  os << indent << "WriteBackground: " << m_WriteBackground << std::endl;
}

} // namespace itk
#endif


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2020, LAAS-CNRS, The University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
#define CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_

#include <stdexcept>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include "crocoddyl/core/state-base.hpp"
#include "crocoddyl/core/utils/to-string.hpp"

namespace crocoddyl {

struct DifferentialActionDataAbstract;  // forward declaration

/**
 * @brief This class DifferentialActionModelAbstract represents a first-order
 * ODE, i.e.
 * \f[
 * \mathbf{\dot{v}} = \mathbf{f}(\mathbf{q}, \mathbf{v}, \boldsymbol{\tau})
 * \f]
 * where \f$ xout = \mathbf{\dot{v}} \f$ and represents the  acceleration of the
 * system. Note that Jacobians Fx and Fu in the
 * DifferentialActionDataAbstract are in \f$ \mathbb{R}^{nv\times ndx} \f$ and
 * \f$ \mathbb{R}^{nv\times nu} \f$, respectively.
 *
 * Then we use the acceleration to integrate the system, and as consequence we
 * obtain:
 * \f[
 * \mathbf{\dot{x}} = (\mathbf{v}, \mathbf{\dot{v}}) = \mathbf{f}(\mathbf{x},\mathbf{u})
 * \f]
 * where this \f$ f \f$ function is different to the other one.
 * So \f$ xout \f$ is interpreted here as \f$ vdout \f$ or \f$ aout \f$.
 */
class DifferentialActionModelAbstract {
 public:
  DifferentialActionModelAbstract(boost::shared_ptr<StateAbstract> state, const std::size_t& nu,
                                  const std::size_t& nr = 0);
  virtual ~DifferentialActionModelAbstract();

  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
                    const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& u) = 0;
  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
                        const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& u,
                        const bool& recalc = true) = 0;
  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();

  void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x);
  void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
                const Eigen::Ref<const Eigen::VectorXd>& x);

  const std::size_t& get_nu() const;
  const std::size_t& get_nr() const;
  const boost::shared_ptr<StateAbstract>& get_state() const;

  const Eigen::VectorXd& get_u_lb() const;
  const Eigen::VectorXd& get_u_ub() const;
  bool const& get_has_control_limits() const;

  void set_u_lb(const Eigen::VectorXd& u_lb);
  void set_u_ub(const Eigen::VectorXd& u_ub);

 protected:
  std::size_t nu_;                          //!< Control dimension
  std::size_t nr_;                          //!< Dimension of the cost residual
  boost::shared_ptr<StateAbstract> state_;  //!< Model of the state
  Eigen::VectorXd unone_;                   //!< Neutral state
  Eigen::VectorXd u_lb_;                    //!< Lower control limits
  Eigen::VectorXd u_ub_;                    //!< Upper control limits
  bool has_control_limits_;                 //!< Indicates whether any of the control limits is finite

  void update_has_control_limits();

#ifdef PYTHON_BINDINGS

 public:
  void calc_wrap(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::VectorXd& x,
                 const Eigen::VectorXd& u = Eigen::VectorXd()) {
    if (u.size() == 0) {
      calc(data, x);
    } else {
      calc(data, x, u);
    }
  }

  void calcDiff_wrap(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::VectorXd& x,
                     const Eigen::VectorXd& u, const bool& recalc) {
    calcDiff(data, x, u, recalc);
  }
  void calcDiff_wrap(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::VectorXd& x,
                     const Eigen::VectorXd& u) {
    calcDiff(data, x, u, true);
  }
  void calcDiff_wrap(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::VectorXd& x) {
    calcDiff(data, x, unone_, true);
  }
  void calcDiff_wrap(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::VectorXd& x,
                     const bool& recalc) {
    calcDiff(data, x, unone_, recalc);
  }

#endif
};

struct DifferentialActionDataAbstract {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  template <typename Model>
  explicit DifferentialActionDataAbstract(Model* const model)
      : cost(0.),
        xout(model->get_state()->get_nv()),
        Fx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
        Fu(model->get_state()->get_nv(), model->get_nu()),
        r(model->get_nr()),
        Lx(model->get_state()->get_ndx()),
        Lu(model->get_nu()),
        Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
        Lxu(model->get_state()->get_ndx(), model->get_nu()),
        Luu(model->get_nu(), model->get_nu()) {
    xout.setZero();
    r.setZero();
    Fx.setZero();
    Fu.setZero();
    Lx.setZero();
    Lu.setZero();
    Lxx.setZero();
    Lxu.setZero();
    Luu.setZero();
  }
  virtual ~DifferentialActionDataAbstract() {}

  double cost;
  Eigen::VectorXd xout;
  Eigen::MatrixXd Fx;
  Eigen::MatrixXd Fu;
  Eigen::VectorXd r;
  Eigen::VectorXd Lx;
  Eigen::VectorXd Lu;
  Eigen::MatrixXd Lxx;
  Eigen::MatrixXd Lxu;
  Eigen::MatrixXd Luu;
};

}  // namespace crocoddyl

#endif  // CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_


#include "stdafx.h"

#include <cmath>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <fmt/format.h>
#include <nlohmann/json.hpp>

#include "contest_types.h"
#include "judge.h"
#include "solver_registry.h"
#include "visual_editor.h"

namespace OptunaAnnealingSolver {

  namespace bg = boost::geometry;
  using BoostPoint = bg::model::d2::point_xy<double>;
  using BoostPolygon = bg::model::polygon<BoostPoint>;
  using BoostLinestring = bg::model::linestring<BoostPoint>;

  template <typename T>
  void shrink(T& point_a, T& point_b) {
    constexpr double eps = 1e-6;
    auto shrink_a = point_a * (1.0 - eps) + point_b * eps;
    auto shrink_b = point_b * (1.0 - eps) + point_a * eps;
    point_a = shrink_a;
    point_b = shrink_b;
  }

  template <typename T>
  BoostPoint ToBoostPoint(const T& point) {
    const auto [x, y] = point;
    return BoostPoint(x, y);
  }

  template <typename T>
  BoostPolygon ToBoostPolygon(const std::vector<T>& points) {
    BoostPolygon polygon;
    for (std::size_t i = 0; i <= points.size(); ++i) {
      polygon.outer().push_back(ToBoostPoint(points[i % points.size()]));
    }
    if (bg::area(polygon) < 0.0) {
      bg::reverse(polygon);
    }
    return polygon;
  }

  template <typename T, typename U>
  double SquaredDistance(const T& vertex0, const U& vertex1) {
    const auto [x0, y0] = vertex0;
    const auto [x1, y1] = vertex1;
    return (x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1);
  }

  template <typename T>
  double SquaredEdgeLength(const T& vertices, const Edge& edge) {
    const auto [a, b] = edge;
    return SquaredDistance(vertices[a], vertices[b]);
  }

  class Solver : public SolverBase {
  public:
    SolverOutputs solve(const SolverArguments& args) override {
      hole_ = args.problem->hole_polygon;
      vertices_ = args.problem->vertices;
      edges_ = args.problem->edges;
      epsilon_ = args.problem->epsilon;
      hole_polygon_ = ToBoostPolygon(hole_);

      // パラメーターファイルを読み込む
      std::ifstream ifs;
      // 実行バイナリと同じフォルダから読み込む
      ifs.open(args.parameters_file_path);
      CHECK(ifs);
      ifs >> parameters_;

      SVisualEditorPtr editor;
      if (args.visualize) {
        editor = std::make_shared<SVisualEditor>(args.problem, "OptunaAnnealingSolver", "visualize");
      }

      const int N = vertices_.size();
      auto pose = vertices_;
      double cost = std::numeric_limits<double>::infinity();
      double best_feasible_cost = std::numeric_limits<double>::infinity();
      std::vector<Point> best_feasible_pose;

      const int num_iters = 100000;
      const double T0 = parameters_["T0"]; // 10.0 1.0-100.0 log
      const double T1 = parameters_["T1"]; // 0.01 0.001-0.1 log
      double progress = 0.0;

      integer ymin = INT_MAX, ymax = INT_MIN;
      integer xmin = INT_MAX, xmax = INT_MIN;
      for (auto p : hole_) {
        xmin = std::min(xmin, get_x(p));
        ymin = std::min(ymin, get_y(p));
        xmax = std::max(xmax, get_x(p));
        ymax = std::max(ymax, get_y(p));
      }

      // lesser version of tonagi's idea
      const int initialize_pose_by_hole = parameters_["initialize_pose_by_hole"]; // 0 0-1
      if (initialize_pose_by_hole) {
        for (auto& p : pose) { p = hole_[0]; }
      }

      const int prohibit_unfeasible_after_feasible = parameters_["prohibit_unfeasible_after_feasible"]; // 0 0-1
      auto evaluate_and_descide_rollback = [&]() -> bool {
        auto [feasible, updated_cost] = Evaluate(pose);

        // tonagi's idea.
        if (prohibit_unfeasible_after_feasible || !best_feasible_pose.empty()) {
          auto res = judge(*args.problem, pose);
          if (!res.fit_in_hole()) {
            feasible = false;
            updated_cost = DBL_MAX;
          }
        }

#if 0
        auto judge_valid = judge(*args.problem, pose).is_valid();
        if (feasible != judge_valid) {
          LOG(INFO) << feasible << " " << judge_valid;
          if (editor) {
            editor->set_pose(args.problem->create_solution(pose));
            while (true) {
              int c = editor->show(1);
              if (c == 27) break;
            }
          }
        }
#endif

        if (feasible && updated_cost < best_feasible_cost) {
          best_feasible_cost = updated_cost;
          best_feasible_pose = pose;
          if (editor) editor->set_persistent_custom_stat(fmt::format("best_cost = {}", best_feasible_cost));
        }
        const double T = std::pow(T0, 1.0 - progress) * std::pow(T1, progress);
        if (std::uniform_real_distribution(0.0, 1.0)(rng_) < std::exp(-(updated_cost - cost) / T)) {
          cost = updated_cost;
          return false; // accepted
        }
        else {
          return true; // rejected
        }
      };
      evaluate_and_descide_rollback();

      const int single_small_change_max_delta = parameters_["single_small_change_max_delta"]; // 1 1-5
      auto single_small_change = [&] { // ynasu87 original
        const int v = std::uniform_int_distribution(0, N - 1)(rng_);
        const int dx = std::uniform_int_distribution(-single_small_change_max_delta, single_small_change_max_delta)(rng_);
        const int dy = std::uniform_int_distribution(-single_small_change_max_delta, single_small_change_max_delta)(rng_);
        auto& [x, y] = pose[v];
        x += dx;
        y += dy;
        if (evaluate_and_descide_rollback()) {
          x -= dx;
          y -= dy;
        }
      };

      const int shift_max_delta = parameters_["shift_max_delta"]; // 1 1-5
      auto shift = [&] {
        const int dx = std::uniform_int_distribution(-shift_max_delta, shift_max_delta)(rng_);
        const int dy = std::uniform_int_distribution(-shift_max_delta, shift_max_delta)(rng_);
        auto pose_bak = pose;
        for (auto& p : pose) {
          p.first += dx;
          p.second += dy;
        }
        if (evaluate_and_descide_rollback()) {
          pose = pose_bak;
        }
      };

      const double slight_rotate_max_deg = parameters_["slight_rotate_max_deg"]; // 2.0 1.0-180.0
      auto slight_rotate = [&] {
        const double deg = std::uniform_real_distribution(-slight_rotate_max_deg, slight_rotate_max_deg)(rng_);
        auto pose_bak = pose;

        integer curr_ymin = INT_MAX, curr_ymax = INT_MIN;
        integer curr_xmin = INT_MAX, curr_xmax = INT_MIN;
        for (auto p : hole_) {
          curr_xmin = std::min(curr_xmin, get_x(p));
          curr_ymin = std::min(curr_ymin, get_y(p));
          curr_xmax = std::max(curr_xmax, get_x(p));
          curr_ymax = std::max(curr_ymax, get_y(p));
        }

        const double cx = double(curr_xmin + curr_xmax) / 2;
        const double cy = double(curr_ymin + curr_ymax) / 2;
        const double sin = std::sin(deg * 3.1415 / 180.0);
        const double cos = std::cos(deg * 3.1415 / 180.0);
        for (auto& p : pose) {
          const double dx = p.first - cx;
          const double dy = p.second - cy;
          p.first = std::round(dx * cos - dy * sin + cx);
          p.second = std::round(dx * sin + dy * cos + cy);
        }
        if (evaluate_and_descide_rollback()) {
          pose = pose_bak;
        }
      };

      auto flip = [&] { // from FlipAnnealingSolver
        const int v0 = std::uniform_int_distribution(0, N - 1)(rng_);
        const int v1 = std::uniform_int_distribution(0, N - 1)(rng_);
        if (v0 == v1) return;
        auto pose_bak = pose;
        auto reflect = [](Point a, Point c, Point v) {
          return v - 2 * double(dot(v - c, a)) / double(dot(a, a)) * a;
        };
        auto diff = pose[v1] - pose[v0];
        Point n = { get_y(diff), -get_x(diff) };
        for (int v2 = 0; v2 < pose.size(); ++v2) {
          if (ccw(pose[v0], pose[v1], pose[v2])) {
            pose[v2] = reflect(n, pose[v0], pose[v2]);
          }
        }
        if (evaluate_and_descide_rollback()) {
          pose = pose_bak;
        }
      };

      const double vote_pow = parameters_["vote_pow"]; // 5.0 1.0-5.0
      auto edges_cache = edges_from_vertex(*args.problem);
      std::vector<std::vector<int> > good_pos(ymax - ymin + 1, std::vector<int>(xmax - xmin + 1));
      std::vector<double> pow_table;
      for (int i = 0; i < 1024; ++i) {
        pow_table.push_back(std::pow(static_cast<double>(i), vote_pow));
      }

      auto hop_grid = [&] { // jump to a tolerated (by at least one edge) point.
        const int pivot = std::uniform_int_distribution(0, N - 1)(rng_);
        const auto pivot_bak = pose[pivot];
        const auto& edges = edges_cache[pivot];
        for (auto& row : good_pos) {
          fill(row.begin(), row.end(), 0);
        }
        for (auto eid : edges) {
          auto [u, v] = args.problem->edges[eid];
          const int counter_vid = u == pivot ? v : u;
          const auto org_d2 = distance2(args.problem->vertices[pivot], args.problem->vertices[counter_vid]);
          for (int y = ymin; y <= ymax; ++y) {
            for (int x = xmin; x <= xmax; ++x) {
              const auto moved_d2 = distance2({ x, y }, pose[counter_vid]);
              if (tolerate(org_d2, moved_d2, epsilon_)) {
                ++good_pos[y - ymin][x - xmin];
              }
            }
          }
        }
        // emphasize large votes.
        double total_votes = 0;
        for (int y = ymin; y <= ymax; ++y) {
          for (int x = xmin; x <= xmax; ++x) {
            total_votes += pow_table[good_pos[y - ymin][x - xmin]];
          }
        }
        const double select_accum_vote = std::uniform_real_distribution<double>(0.0, total_votes)(rng_);
        double accum_vote = 0;
        bool found = false;
        for (int y = ymin; !found && y <= ymax; ++y) {
          for (int x = xmin; !found && x <= xmax; ++x) {
            accum_vote += pow_table[good_pos[y - ymin][x - xmin]];
            if (select_accum_vote <= accum_vote) {
              pose[pivot] = {x, y};
              found = true;
            }
          }
        }
        if (evaluate_and_descide_rollback()) {
          pose[pivot] = pivot_bak;
        }
      };

      // はみ出している線分を移動させる
      const int slide_protrusion_max_delta = parameters_["slide_protrusion_max_delta"]; // 1 1-5
      auto slide_protrusion = [&] { // from OptunaAnnealingSolver
        bool found = false;
        int vertex_index_backup[2];
        Point vertex_backup[2];
        for (const auto& edge : edges_) {
          const auto [a, b] = edge;
          Point2d pa = pose[a];
          Point2d pb = pose[b];
          shrink(pa, pb);
          const auto boost_point_a = ToBoostPoint(pa);
          const auto boost_point_b = ToBoostPoint(pb);
          BoostLinestring linestring{ boost_point_a, boost_point_b };
          std::vector<BoostLinestring> differences;
          bg::difference(linestring, hole_polygon_, differences);
          if (differences.empty()) {
            continue;
          }

          found = true;
          vertex_index_backup[0] = a;
          vertex_index_backup[1] = b;
          vertex_backup[0] = pose[a];
          vertex_backup[1] = pose[b];

          pose[a].first += std::uniform_int_distribution(-slide_protrusion_max_delta, slide_protrusion_max_delta)(rng_);
          pose[a].second += std::uniform_int_distribution(-slide_protrusion_max_delta, slide_protrusion_max_delta)(rng_);
          pose[b].first += std::uniform_int_distribution(-slide_protrusion_max_delta, slide_protrusion_max_delta)(rng_);
          pose[b].second += std::uniform_int_distribution(-slide_protrusion_max_delta, slide_protrusion_max_delta)(rng_);

          break;
        }

        if (found) {
          if (evaluate_and_descide_rollback()) {
            pose[vertex_index_backup[0]] = vertex_backup[0];
            pose[vertex_index_backup[1]] = vertex_backup[1];
          }
        }
      };

      using Action = std::function<void()>;
      const double single_small_change_probability = parameters_["single_small_change_probability"]; // 0.9 0.0-1.0
      const double slight_rotate_probability = parameters_["slight_rotate_probability"]; // 0.01 0.0-1.0
      const double shift_probability = parameters_["shift_probability"]; // 0.01 0.0-1.0
      const double hop_grid_probability = parameters_["hop_grid_probability"]; // 0.01 0.0-1.0
      const double flip_probability = parameters_["flip_probability"]; // 0.03 0.0-1.0
      const double slide_protrusion_probability = parameters_["slide_protrusion_probability"]; // 0.10 0.0-1.0
      std::vector<std::pair<double, Action>> action_probs = {
        {single_small_change_probability, single_small_change},
        {slight_rotate_probability, slight_rotate},
        {shift_probability, shift},
        {hop_grid_probability, hop_grid},
        {flip_probability, flip},
        {slide_protrusion_probability, slide_protrusion},
      };
      {
        // normalize probs
        double p = 0.0;
        for (int i = 0; i < action_probs.size(); ++i) { p += action_probs[i].first; }
        for (int i = 0; i < action_probs.size(); ++i) { action_probs[i].first /= p; }
      }

      for (int iter = 0; iter < num_iters; ++iter) {
        progress = 1.0 * iter / num_iters;

        const double p_action = std::uniform_real_distribution(0.0, 1.0)(rng_);
        double p_accum = 0.0;
        for (int i = 0; i < action_probs.size(); ++i) {
          p_accum += action_probs[i].first;
          if (p_action < p_accum) {
            action_probs[i].second();
          }
        }

        if (editor && iter % 100 == 0) {
          editor->set_oneshot_custom_stat(fmt::format("iter = {}/{}", iter, num_iters));
          editor->set_pose(args.problem->create_solution(pose));
          if (auto show_result = editor->show(1); show_result.edit_result) {
            pose = show_result.edit_result->pose_after_edit->vertices;
          }
        }
      }

      SolverOutputs outputs;
      if (best_feasible_pose.empty()) {
        outputs.solution = args.problem->create_solution(pose);
      }
      else {
        outputs.solution = args.problem->create_solution(best_feasible_pose);
      }
      return outputs;
    }

    template <typename P>
    std::tuple<bool, double> Evaluate(const std::vector<P>& pose) const {
      const double protrusion_cost_coefficient0 = parameters_["protrusion_cost_coefficient0"]; // 1.0 0.01-100.0 log
      const double protrusion_cost_coefficient1 = parameters_["protrusion_cost_coefficient1"]; // 0.01 0.01-100.0 log
      const double deformation_cost_coefficient = parameters_["deformation_cost_coefficient"]; // 10.0 0.01-100.0 log
      const double dislikes_cost_coefficient = parameters_["dislikes_cost_coefficient"]; // 0.01 0.01-100.0 log

      double deformation_cost = 0.0;
      double protrusion_cost = 0.0;
      double dislikes_cost = 0.0;

      const double tolerance = epsilon_ / 1'000'000.0;
      for (const auto& vertex : pose) {
        protrusion_cost += protrusion_cost_coefficient0 * bg::distance(ToBoostPoint(vertex), hole_polygon_);
      }
      for (const auto& edge : edges_) {
        const auto [a, b] = edge;
        Point2d pa = pose[a];
        Point2d pb = pose[b];
        shrink(pa, pb);
        BoostLinestring linestring{ ToBoostPoint(pa), ToBoostPoint(pb) };
        std::vector<BoostLinestring> differences;
        bg::difference(linestring, hole_polygon_, differences);
        for (const auto& segment : differences) {
          protrusion_cost += protrusion_cost_coefficient1 * bg::length(segment);
        }

        const auto d0 = SquaredEdgeLength(vertices_, edge);
        const auto d1 = SquaredEdgeLength(pose, edge);
        deformation_cost += deformation_cost_coefficient * std::max(0.0, std::abs(d1 / d0 - 1.0) - tolerance);
      }

      for (const auto h : hole_) {
        double best = std::numeric_limits<double>::infinity();
        for (const auto v : pose) {
          best = std::min(best, SquaredDistance(h, v));
        }
        dislikes_cost += best * dislikes_cost_coefficient;
      }

      const bool feasible = deformation_cost + protrusion_cost == 0.0;
      const double cost = deformation_cost + protrusion_cost + dislikes_cost;

      return { feasible, cost };
    }

  private:
    std::mt19937 rng_;
    std::vector<Point> hole_;
    std::vector<Point> vertices_;
    std::vector<Edge> edges_;
    integer epsilon_;
    BoostPolygon hole_polygon_;
    nlohmann::json parameters_;
  };

}

REGISTER_SOLVER("OptunaAnnealingSolver", OptunaAnnealingSolver::Solver);
// vim:ts=2 sw=2 sts=2 et ci


#ifndef Integration_ModifiedArrheniusIntegralBase_hpp
#define Integration_ModifiedArrheniusIntegralBase_hpp

/** @file ModifiedArrheniusIntegralBase.hpp
  * @brief
  * @author C.D. Clark III
  * @date 06/27/17
  */

#include <boost/math/tools/roots.hpp>
using boost::math::tools::bracket_and_solve_root;
using boost::math::tools::eps_tolerance;
#include "ArrheniusIntegralBase.hpp"

namespace libArrhenius {

/** @class ModifiedArrheniusIntegralBase
  * @brief Base class for common modified Arrhenius integral data.
  * @author C.D. Clark III
  */
template<typename Real>
class ModifiedArrheniusIntegralBase : public ArrheniusIntegralBase<Real>
{
  protected:
    Real n;

  public:

    template<typename T>
    void setExponent( T n_ ) { n = n_; }
    Real getExponent( ) { return n; }

    Real getCriticalTemperature() const {
      Real Tcrit = ArrheniusIntegralBase<Real>::getCriticalTemperature();
      if( n == 0 )
        return Tcrit;

      // if n != 0, then we have a transcendental equation we need to solve.
      //
      // ln T^n - Ea/R/T + ln A = 0  (solve for T)
      //
      // use the n = 0 case as a first gues. this will be too high
      Real factor = 1.1;
      eps_tolerance<Real> tol( std::numeric_limits<Real>::digits - 3 );
      boost::uintmax_t maxit = 100;
      auto Tcrit_range =  bracket_and_solve_root(
               [&](Real T){
               return Constants::MKS::R*T*log(this->A*pow(T,this->n)) - this->Ea; },
               Tcrit, factor, true, tol, maxit );
      Tcrit = Tcrit_range.first;
      return Tcrit;
    }

    Real rate( Real T ) const { return pow(T,n)*this->A*exp( -this->Ea / Constants::MKS::R /T ); }

  protected:
};

}

#endif // include protector


//# /Users/arnold/Documents/prg/clang/bin/clang++ -U__STRICT_ANSI__ -std=c++14 -I/Users/arnold/Documents/prg/libraries/boost_1_56_0/ -I/Developer/SDKs/MacOSX10.6.sdk/usr/include -Wall -O2 -L/Users/arnold/Documents/prg/libraries/boost_1_56_0/stage/lib  -o sint sint.cpp -lboost_system -lboost_filesystem -lboost_iostreams
#include <iostream>
#include <boost/format.hpp>

using namespace std;

class SInt {
public:
	static constexpr unsigned convert(int i)     { return (static_cast<unsigned>(i) << 1) ^ (i >> (sizeof(int)*8-1)); }
	static constexpr int convertBack(unsigned i) { return (i >> 1) ^ -static_cast<int>(i & 1); }
	explicit SInt(const int& i) : v_{convert(i)}{}
	operator unsigned() const noexcept { return v_; }
private:
	unsigned v_;
};

template<typename T>
struct Str2Bin {
	explicit Str2Bin(const T& t) : t_{t}{}
	const volatile T& t_;
};

template<typename T>
ostream& operator<<(ostream& o, const Str2Bin<T>& s) {
	T t = s.t_;
	for(unsigned i = 0; i < sizeof(T) * 8; ++i, t<<=1) {
		if (i>0 && i % 8 == 0)
			o << '.';
		o << (t & (1<<(sizeof(T)*8-1) ) ? '1' : '0');
	}
	return o;
}

int main (int argc, char const *argv[])
{
	int ret = 0;
	int max = 255;
	for(int i = -max; i <= max; ++i) {
		SInt s(i);
		int j = SInt::convertBack(s);
		cout << boost::format{"%3d _ "} % i << Str2Bin<int>(i)
			 << boost::format{" _ %3d _ "} % s
			<< Str2Bin<unsigned>(s) << boost::format{" _ %3d _ "} % j
			<< Str2Bin<int>(j) << endl;
	}
	return ret;
}