/* mcgs.cpp

  Monte-Carlo simulation of group sizes under bivariate (possibly contaminated) normal distribution

  Requirements:

  sudo apt-get install libboost-all-dev

  Building:

  g++ -O3 -Wall -Wextra -std=c++11 -march=native -g -o mcgs mcgs.cpp

*/
#include <vector>
#include <map>
#include <cmath>
#include <numeric>
#include <complex>
#include <random>
#include <iostream>
#include <memory>
#include <stdlib.h>
#include <algorithm>
#include <boost/math/distributions/chi_squared.hpp>

// Abstract base class (interface) for random shot group generator, implementations follow
class RandomNumberGenerator {
  public:
    virtual void advance() = 0;
    virtual void reset() = 0;
    virtual std::complex<double> point(unsigned dimension) = 0;
};

//
// Baseline pseudorandom number generator, wraps the default provided by standard library
//
class DefaultRandomNumberGenerator : public RandomNumberGenerator {
    std::default_random_engine gen_;
    std::normal_distribution<double> dist_;

  public:
    DefaultRandomNumberGenerator()
    {
      reset();
    }

    void advance() {}

    void reset()
    {
      gen_.seed(42);
    }

    std::complex<double> point(unsigned dimension __attribute__((unused)))
    {
      return std::complex<double>(dist_(gen_), dist_(gen_));
    }
};

// MCG 128 from http://www.pcg-random.org/posts/on-vignas-pcg-critique.html
class FastRandomNumberGenerator : public RandomNumberGenerator {
  __uint128_t state_;

  public:
    FastRandomNumberGenerator()
    {
      reset();
    }

    void advance() {}

    void reset()
    {
      state_ = 1;   // can be seeded to any odd number
    }

    std::complex<double> point(unsigned dimension __attribute__((unused)))
    {
      for (;;) {
        double u = ldexp(next(), -64);
        double v = ldexp(next(), -64);
        // Box-Muller transform
        double r = sqrt(-2 * log(u));
        double x = r * cos(2 * M_PI * v);
        double y = r * sin(2 * M_PI * v);
        if (isfinite(x) && isfinite(y)) {
          return std::complex<double>(x, y);
        }
      }
    }

  private:
    inline uint64_t next()
    {
      return (state_ *= 0xda942042e4dd58b5ULL) >> 64;
    }
};

//
// Additive quasi-random generator
//
// To get the next point, add a fixed step modulo 1 to the previous point.
//
class AdditiveRandomNumberGenerator: public RandomNumberGenerator {
    std::vector<double> x_;
    std::vector<double> step_;

  public:
    explicit AdditiveRandomNumberGenerator(unsigned dimensions)
#if 0
    // Use square roots of prime numbers as steps because they are irrational.
    {
      unsigned nprimes = dimensions * 2; // Two coordinates for each point
      x_.resize(nprimes);
      step_.resize(nprimes);
      //
      // To initialize the steps, we need to find some prime numbers. Do that with
      // sieve of Eratosthenes.
      //
      // To estimate how big the sieve should be to get nprimes primes, start with
      // approximation by Gauss and Legendre:
      //
      //   nprimes = PrimePi(x) ~ x/log(x)
      //
      // Here PrimePi(x) is prime-counting function: number of prime numbers that are
      // less than or equal to x. This is a lower bound for all but the lowest values
      // of x (which we'll handle separately), so x will be slightly larger than strictly
      // necessary.
      //
      // To solve for x without resorting to Lambert-W function, rewrite as follows:
      //
      //   x ~ nprimes * log(x)
      //
      // Now use fixed point iteration to find x. Stop iterating when the whole part
      // of x stops changing.
      //
      unsigned sieve_size = 0;
      if (nprimes < 5) {  // Approximation breaks below 5, floor sieve_size
        sieve_size = 7;
      } else {
        double x = nprimes;
        for (unsigned i = 0; i < 30; i++) {
          unsigned before = (unsigned)ceil(x);
          x = nprimes * log(x); // Fixed point iteration
          unsigned after = (unsigned)ceil(x);
          if (before == after) {
            sieve_size = before;
            break;
          }
        }
        if (sieve_size == 0) {
          std::cerr << "Fixed point iteration failed to converge\n";
          exit(-1);
        }
      }

      // The sieve itself
      std::vector<bool> prime(sieve_size + 1, true);
      unsigned top = sqrt(sieve_size);
      for (unsigned i = 2; i < top; i++) {
        if (prime.at(i)) {
          for(unsigned j = i * i; j <= sieve_size; j += i) {
            prime.at(j) = false;
          }
        }
      }

      // Collect prime numbers left in the sieve, take square roots, assign to steps.
      unsigned index = 0;
      for (unsigned j = 2; index < nprimes; j++) {
        if (j >= sieve_size) {
          std::cerr << "Ran out of primes\n"; // Shouldn't happen if we did the math right
          exit(-1);
        }
        if (prime[j]) {
          step_.at(index++) = sqrt(j);
        }
      }
    }
#else
    // Rd quasi-random sequences http://extremelearning.com.au/unreasonable-effectiveness-of-quasirandom-sequences/
    // In d dimensions, g is positive real root of f(x) = x ** (d + 1) - x - 1 = 0
    // 3D example:
    // g = 1.22074408460575947536
    // a1 = 1.0/g
    // a2 = 1.0/(g*g)
    // a3 = 1.0/(g*g*g)
    // x[n] = (0.5+a1*n) %1
    // y[n] = (0.5+a2*n) %1
    // z[n] = (0.5+a3*n) %1
    {
      unsigned d = dimensions * 2; // Two coordinates for each point
      x_.resize(d);
      step_.resize(d);
      double x = 1.5; // initial approximation
      for (unsigned i = 0; i < 30; i++) { // Newton-Raphson
        double xd = 1;
        for (unsigned j = 0; j < d; j++) {
          xd *= x;
        }
        double adjustment = (xd * x - x - 1) / ((d + 1) * xd - 1);
        x -= adjustment;
        if (std::abs(adjustment) < 1E-16) {
          break;
        }
      }
      double a = 1.;
      for (unsigned i = 0; i < d; i++) {
        a /= x;
        x_.at(i) = 0.5;
        step_.at(i) = a;
      }
    }
#endif

    void advance()
    {
      for (unsigned j = 0; j < step_.size(); j++) {
        double int_part;
        x_.at(j) = modf(x_.at(j) + step_.at(j), &int_part);
      }
    }

    void reset()
    {
      for (unsigned j = 0; j < step_.size(); j++) {
        x_.at(j) = 0;
      }
    }

    std::complex<double> point(unsigned dimension)
    {
      double x, y;
      if (dimension >= step_.size() / 2) {
        x = y = std::numeric_limits<double>::quiet_NaN();
      } else {
        double u = x_.at(dimension);
        double v = x_.at(dimension + step_.size() / 2);
        // Box-Muller transform
        double r = sqrt(-2 * log(u));
        x = r * cos(2 * M_PI * v);
        y = r * sin(2 * M_PI * v);
      }
      return std::complex<double>(x, y);
    }
};

//
// Sobol quasi-random number generator
//
class SobolRandomNumberGenerator : public RandomNumberGenerator {
    unsigned x_[20];
    unsigned v_[20][32 + 1];
    unsigned i_;

public:
    explicit SobolRandomNumberGenerator(unsigned dimensions): i_(0)
    {
      // Primitive polynomials and initial direction numbers for the first 20 dimensions
      // recommended by Stephen Joe and Frances Kuo in new-joe-kuo-6.21201
      // http://web.maths.unsw.edu.au/~fkuo/sobol/index.html
      const unsigned s[20] = {1, 2, 3, 3, 4, 4, 5, 5, 5,  5,  5, 5,  6,  6,  6,  6,  6,  6, 7, 7};
      const unsigned a[20] = {0, 1, 1, 2, 1, 4, 2, 4, 7, 11, 13, 14, 1, 13, 16, 19, 22, 25, 1, 4};
      const unsigned m[20][7] = {
        {1, 0, 0,  0,  0,  0,   0},
        {1, 3, 0,  0,  0,  0,   0},
        {1, 3, 1,  0,  0,  0,   0},
        {1, 1, 1,  0,  0,  0,   0},
        {1, 1, 3,  3,  0,  0,   0},
        {1, 3, 5, 13,  0,  0,   0},
        {1, 1, 5,  5, 17,  0,   0},
        {1, 1, 5,  5,  5,  0,   0},
        {1, 1, 7, 11, 19,  0,   0},
        {1, 1, 5,  1,  1,  0,   0},
        {1, 1, 1,  3, 11,  0,   0},
        {1, 3, 5,  5, 31,  0,   0},
        {1, 3, 3,  9,  7, 49,   0},
        {1, 1, 1, 15, 21, 21,   0},
        {1, 3, 1, 13, 27, 49,   0},
        {1, 1, 1, 15,  7,  5,   0},
        {1, 3, 1, 15, 13, 25,   0},
        {1, 1, 5,  5, 19, 61,   0},
        {1, 3, 7, 11, 23, 15, 103},
        {1, 3, 7, 13, 13, 15,  69}
      };
      if (dimensions > 10) {
        std::cerr << "Sobol direction numbers available for at most 10 shot group\n";
        exit(-1);
      }
      for (unsigned j = 0; j < 20; j++) {
        if (j == 0) {
          for (unsigned i = 0; i < 32; i++) {
            v_[j][i] = 1 << (32 - i - 1);
          }
        } else {
          unsigned ss = s[j - 1];
          for (unsigned i = 0; i < ss; i++) {
            v_[j][i] = m[j - 1][i] << (32 - i - 1);
          }
          for (unsigned i = ss; i < 32; i++) {
	        v_[j][i] = v_[j][i - ss] ^ (v_[j][i - ss] >> ss);
	        for (unsigned k = 1; k < ss; k++) {
	          v_[j][i] ^= (((a[j - 1] >> (ss - 1 - k)) & 1) * v_[j][i - k]);
	        }
          }
        }
        x_[j] = 0;
      }

      // Burn-in: discard initial values
      for (unsigned i = 0; i < 32; i++) {
        advance();
      }
    }

    void reset()
    {
      for (unsigned j = 0; j < 20; j++) {
        x_[j] = 0;
      }
    }

    void advance()
    {
      unsigned b = i_++;
      unsigned z = 0; // Position of the first zero bit in binary representation of i_
                      // counting from the right
      if (b) {
        while (b & 1) {
          b >>= 1;
          z++;
        }
      }
      for (unsigned j = 0; j < 20; j++) {
        x_[j] ^= v_[j][z];
      }
    }

    std::complex<double> point(unsigned dimension)
    {
      double x, y;
      if (dimension >= 10) {
        x = y = std::numeric_limits<double>::quiet_NaN();
      } else {
        double u = ldexp(x_[dimension], -32);
        double v = ldexp(x_[dimension + 10], -32);

        // Box-Muller transform
        double r = sqrt(-2 * log(u));
        x = r * cos(2 * M_PI * v);
        y = r * sin(2 * M_PI * v);
      }
      return std::complex<double>(x, y);
    }
};

struct ConvexHullPoint {
  public:
    explicit ConvexHullPoint(const std::complex<double>& p) : point_(p) {}

    // For lexicographic sort: first by x, then by y
    bool operator<(const ConvexHullPoint& other) const
    {
      if (point_.real() < other.point_.real()) {
        return true;
      } else if (point_.real() > other.point_.real()) {
        return false;
      } else {
        return point_.imag() < other.point_.imag();
      }
    }

    bool operator!=(const std::complex<double>& other) const
    {
      return point_ != other;
    }

    // Distance to another point
    double distanceTo(const ConvexHullPoint& other) const
    {
      return std::abs(point_ - other.point_);
    }

    double inline x(void) const { return point_.real(); }
    double inline y(void) const { return point_.imag(); }

  private:
    std::complex<double> point_;
};

class ShotGroup {
  private:
    // Use complex type to store impact coordinates
    std::vector<std::complex<double> > impact_;

    // Cross product of vectors OA and OB, positive if OAB makes a CCW turn
    double cross_product( const ConvexHullPoint& O
                        , const ConvexHullPoint& A
                        , const ConvexHullPoint& B
                        ) const
    {
      return (A.x() - O.x()) * (B.y() - O.y())
           - (A.y() - O.y()) * (B.x() - O.x());
    }

    double distance(unsigned a, unsigned b) const
    {
      return std::abs(impact_.at(a) - impact_.at(b));
    }

  public:
    void add(const std::complex<double>& p)
    {
      impact_.push_back(p);
    }

    // Brute force implementation, asymptotic complexity O(N^2)
    double group_size_brute_force(double* excluding_worst = NULL) const
    {
      unsigned n = impact_.size();
      if (n < 2) {
        if (excluding_worst) {
          *excluding_worst = 0;
        }
        return 0;
      }

      // Find the two impacts defining extreme spread
      double extreme_spread = 0;
      unsigned index_a = 0;
      unsigned index_b = 0;
      for (unsigned i = 0; i < n - 1; i++) {
        for (unsigned j = i + 1; j < n; j++) {
          double candidate = distance(i, j);
          if (extreme_spread < candidate) {
            extreme_spread = candidate;
            index_a = i;
            index_b = j;
          }
        }
      }

      if (excluding_worst) {
        // Worst shot must be one of the impacts defining extreme spread.
        // Calculate group size without either one, return the smaller number.
        double extreme_spread_excluding_a = 0;
        double extreme_spread_excluding_b = 0;
        for (unsigned i = 0; i < n - 1; i++) {
          for (unsigned j = i + 1; j < n; j++) {
            double candidate = distance(i, j);
            if (i != index_a && j != index_a && extreme_spread_excluding_a < candidate) {
              extreme_spread_excluding_a = candidate;
            }
            if (i != index_b && j != index_b && extreme_spread_excluding_b < candidate) {
              extreme_spread_excluding_b = candidate;
            }
          }
        }
        *excluding_worst = std::min(extreme_spread_excluding_a, extreme_spread_excluding_b);
      }
      return extreme_spread;
    }

    // Same as group_size_brute_force(), but using convex hull.
    // Asymptotic complexity O(N log N).
    //
    // Pass 0: find impacts a and b defining extreme spread.
    //         Return extreme spread if excluding_worst == false.
    // Pass 1: find extreme spread excluding a
    // Pass 2: find extreme spread excluding b
    //
    // Return the smaller of extreme spread excluding a and extreme spread excluding b
    //
    double group_size_convex_hull(double* excluding_worst = NULL) const
    {
      unsigned n = impact_.size();
      if (n < 2) {
        if (excluding_worst) {
          *excluding_worst = 0;
        }
        return 0;
      }
      double extreme_spread = 0;
      ConvexHullPoint a(impact_.at(0)); double extreme_spread_excluding_a = 0;
      ConvexHullPoint b(impact_.at(1)); double extreme_spread_excluding_b = 0;
      for (unsigned pass = 0; pass < 3; pass++) {

        // Use Andrew's monotone chain 2D algorithm to construct convex hull
        std::vector<ConvexHullPoint> lower_hull;
        std::vector<ConvexHullPoint> upper_hull;
        {
          std::vector<ConvexHullPoint> p;
          for (unsigned i = 0; i < n; i++) {
            if ( pass == 0
              || ((pass == 1) && (a != impact_.at(i)))
              || ((pass == 2) && (b != impact_.at(i)))
               )
            p.push_back(ConvexHullPoint(impact_.at(i)));
          }
          std::sort(p.begin(), p.end());

          unsigned k = 0;
          for (unsigned i = 0; i < p.size(); i++) { // lower hull
            while (k >= 2 && cross_product(lower_hull.at(k - 2), lower_hull.at(k - 1), p.at(i)) <= 0) {
              lower_hull.pop_back();
              k--;
            }
            lower_hull.push_back(p.at(i));
            k++;
          }
          k = 0;
          for (unsigned i = 0; i < p.size(); i++) { // upper hull
            while (k >= 2 && cross_product(upper_hull.at(k - 2), upper_hull.at(k - 1), p.at(i)) >= 0) {
              upper_hull.pop_back();
              k--;
            }
            upper_hull.push_back(p.at(i));
            k++;
          }
        }

        // Use rotating calipers algoritm to find most distant antipodal pair of hull points
        double diameter = 0;
        {
          unsigned i = 0;
          unsigned j = lower_hull.size() - 1;
          while (i < upper_hull.size() - 1 || j > 0) {
            double d = upper_hull.at(i).distanceTo(lower_hull.at(j));
            if (diameter < d) {
              diameter = d;
              if (pass == 0) {
                a = upper_hull.at(i);
                b = lower_hull.at(j);
              }
            }
            if (i == upper_hull.size() - 1) {
              j--;
            } else if (j == 0) {
              i++;
            } else if ( (upper_hull.at(i + 1).y() - upper_hull.at(i).y())
                      * (lower_hull.at(j).x() - lower_hull.at(j - 1).x())
                      > (upper_hull.at(i + 1).x() - upper_hull.at(i).x())
                      * (lower_hull.at(j).y() - lower_hull.at(j - 1).y())
                      ) {
              i++;
            } else {
              j--;
            }
          }
        }
        switch (pass) {
          case 0:
            if (excluding_worst) {
              extreme_spread = diameter;
              break;
            }
            return diameter;
          case 1:
            extreme_spread_excluding_a = diameter;
            break;
          case 2:
            extreme_spread_excluding_b = diameter;
            break;
        }
      }
      *excluding_worst = std::min(extreme_spread_excluding_a, extreme_spread_excluding_b);
      return extreme_spread;
    }

    // As per NSD (page 181) should be within 15 cm at 100 m
    // Group size 2.79295 corresponds to kuchnost~=3.15863,
    // so this is equivalent to 4.56 MOA.
    double nsd_kuchnost(void) const
    {
      // Only defined for 4 shot groups
      if (impact_.size() != 4) return std::numeric_limits<double>::quiet_NaN();

      // STP using all 4 shots
      auto stp = (impact_.at(0) + impact_.at(1) + impact_.at(2) + impact_.at(3)) / 4.;

      // Minimum radius of circle with center at STP that encloses all shots
      double r = 0;
      for (unsigned i = 0; i < 4; i++) {
        double candidate = std::abs(stp - impact_.at(i));
        if (r < candidate) {
          r = candidate;
        }
      }

      // Exclude outlier, if any
      for (unsigned i = 0; i < 4; i++) {

        // STP excluding this shot
        auto stp3 = (stp * 4. - impact_.at(i)) / 3.;

        // Minimum radius of circle with center at STP of three shots excluding this shot
        double r2 = 0;
        for (unsigned j = 0; j < 4; j++) {
          if (i == j) {
            continue;
          }
          double candidate = std::abs(stp - impact_.at(i));
          if (r2 < candidate) {
            r2 = candidate;
          }
        }

        // Outlier is a shot 2.5x or more distant from STP of other three shots
        // than radius of the circle centered as STP of other three shots
        // that covers these three shots
        if (std::abs(stp3 - impact_.at(i)) > 2.5 * r2 && r > r2) {
          r = r2;
        }
      }

      // Diameter
      return 2 * r;
    }

    double avg_miss_radius(void) const
    {
      unsigned n = impact_.size();
      if (n < 2) return std::numeric_limits<double>::quiet_NaN();
      std::complex<double> center = 0;
      for (unsigned i = 0; i < n; i++) {
        center += impact_.at(i);
      }
      center /= n;
      double amr = 0;
      for (unsigned i = 0; i < n; i++) {
        amr += std::abs(impact_.at(i) - center);
      }
      amr /= n;
      return amr;
    }

    //
    // Ballistic Accuracy Class (before rounding)
    //
    // Find location of group center (mean)
    // For each shot i, find its radius squared (r2) relative to group center
    // Upper 90% confidence value sigma_U=SQRT(SUM(r2)/CHIINV(0.9,2n-2))
    // Ballistic Accuracy Class is ROUND(sigma_U,0)
    //
    // http://ballistipedia.com/index.php?title=Ballistic_Accuracy_Classification
    //
    double bac(void) const
    {
      unsigned n = impact_.size();
      if (n < 2) return std::numeric_limits<double>::quiet_NaN();
      std::complex<double> center = 0;
      for (unsigned i = 0; i < n; i++) {
        center += impact_.at(i);
      }
      center /= n;
      double sum_r2 = 0;
      for (unsigned i = 0; i < n; i++) {
        sum_r2 += std::norm(impact_.at(i) - center);
      }

      // Memoized BAC factor (to avoid recalculating it for each group)
      static unsigned memoized_n = 0;
      static double memoized_factor = 0;
      if (n != memoized_n) {
        boost::math::chi_squared ch2(2 * n - 2);
        double chisq_inv_rt = boost::math::quantile(ch2, 1 - 0.9);
        memoized_factor = 1 / sqrt(chisq_inv_rt);
        memoized_n = n;
      }

      return memoized_factor * sqrt(sum_r2);
    }

    // Pairwise distances weighted by rank
    double pdwr(bool trim = false) const
    {
      unsigned n = impact_.size();

      // Precomputed scaling factor to convert the result to sigma
      double scaling_factor = 0;
      switch (n) {
        case  3: scaling_factor = trim ? 1 / 1.04273 : 1 / 2.00018; break;
        case  4: scaling_factor = trim ? 1 / 1.31192 : 1 / 2.1068;  break;
        case  5: scaling_factor = trim ? 1 / 1.46386 : 1 / 2.16243; break;
        case  6: scaling_factor = trim ? 1 / 1.56703 : 1 / 2.19481; break;
        case  7: scaling_factor = trim ? 1 / 1.64232 : 1 / 2.21542; break;
        case  8: scaling_factor = trim ? 1 / 1.70089 : 1 / 2.22942; break;
        case  9: scaling_factor = trim ? 1 / 1.74798 : 1 / 2.2394;  break;
        case 10: scaling_factor = trim ? 1 / 1.78699 : 1 / 2.24684; break;
        default: return std::numeric_limits<double>::quiet_NaN();
      }

      // Pairwise distances
      std::vector<double> d;
      for (unsigned i = 0; i < n; i++) {
        for (unsigned j = i + 1; j < n; j++) {
          d.push_back(std::abs(impact_.at(i) - impact_.at(j)));
        }
      }
      std::sort(d.begin(), d.end());

      // Average weighted by rank, possibly trimmed
      double numerator = 0;
      double denominator = 0;
      unsigned m = d.size();
      if (trim) {
        m -= n - 1;
      }
      for (unsigned i = 0; i < m; i++) {
        numerator += d.at(i) * (i + 1.);
        denominator += (i + 1.);
      }
      return scaling_factor * numerator / denominator;
    }

    // Rank weighted mean of right winsorized pairwise distances
    double rwmrwpd() const
    {
      unsigned n = impact_.size();

      // Precomputed scaling factor to convert the result to sigma
      double scaling_factor = 0;
      switch (n) {
        case  3: scaling_factor = 1 / 1.04273; break;
        case  4: scaling_factor = 1 / 1.48818; break;
        case  5: scaling_factor = 1 / 1.69422; break;
        case  6: scaling_factor = 1 / 1.81984; break;
        case  7: scaling_factor = 1 / 1.90066; break;
        case  8: scaling_factor = 1 / 1.95899; break;
        case  9: scaling_factor = 1 / 2.00215; break;
        case 10: scaling_factor = 1 / 2.03579; break;
        default: return std::numeric_limits<double>::quiet_NaN();
      }

      // Pairwise distances
      std::vector<double> d;
      for (unsigned i = 0; i < n; i++) {
        for (unsigned j = i + 1; j < n; j++) {
          d.push_back(std::abs(impact_.at(i) - impact_.at(j)));
        }
      }
      std::sort(d.begin(), d.end());

      // Average weighted by rank and winsorized
      double numerator = 0;
      double denominator = 0;
      unsigned m = d.size() - n;
      for (unsigned i = 0; i < d.size(); i++) {
        numerator += d.at(i < m ? i : m) * (i + 1.);
        denominator += (i + 1.);
      }
      return scaling_factor * numerator / denominator;
    }

    // Qn with different rank
    double tqn() const
    {
      unsigned n = impact_.size();
      std::vector<double> d;
      for (unsigned i = 0; i < n; i++) {
        for (unsigned j = i + 1; j < n; j++) {
          d.push_back(std::abs(impact_.at(i) - impact_.at(j)));
        }
      }
      std::sort(d.begin(), d.end());
      return d.at(d.size() - n);
    }

    // Square root of average of squared pairwise distances
    double sraspd(bool trim = false) const
    {
      unsigned n = impact_.size();
      if (n < 3 || n > 10) {
        return std::numeric_limits<double>::quiet_NaN();
      }

      // Squares of pairwise distance
      std::vector<double> d;
      for (unsigned i = 0; i < n; i++) {
        for (unsigned j = i + 1; j < n; j++) {
          d.push_back(std::norm(impact_.at(i) - impact_.at(j)));
        }
      }

      // Square root of average of squared pairwise distances, possibly trimmed
      double sum_of_squares = 0;
      unsigned m = d.size();
      if (trim) {
        m -= n - 1;
        std::sort(d.begin(), d.end());
      }
      for (unsigned i = 0; i < m; i++) {
        sum_of_squares += d.at(i);
      }
      return sqrt(sum_of_squares / m);
    }

    void show(void) const
    {
      for (unsigned i = 0; i < impact_.size(); i++) {
        std::cout << "g.add(std::complex<double>(" << impact_.at(i).real() << ", " << impact_.at(i).imag() << "));\n";
      }
    }
};

class DescriptiveStat
{
  public:
    DescriptiveStat() : n_(0), m_(0), s_(0) {}

    void push(double x)
    {
      double new_m = m_ + (x - m_) / (++n_);
      double new_s = s_ + (x - m_) * (x - new_m);
      m_ = new_m;
      s_ = new_s;
    }

    double mean(void) const
    {
      return (n_ > 0) ? m_ : std::numeric_limits<double>::quiet_NaN();
    }

    double variance(void) const
    {
      return (n_ > 1) ? s_ / (n_ - 1) : std::numeric_limits<double>::quiet_NaN();
    }

    double stdev(void) const
    {
      return sqrt(variance());
    }

    double cv(void) const
    {
      return stdev() / mean();
    }

    void show(const char* metric, double theoretical = 0, bool suppress_nan = false)
    {
      auto m = mean();
      if (suppress_nan && !isfinite(m)) {
        return;
      }
      std::cout << metric << " mean=" << m;
      if (theoretical > 0) {
        std::cout << " (expected " << theoretical << ")";
      }
      std::cout << ", CV=" << cv() << "\n";
    }

  private:
    unsigned n_;
    double m_;
    double s_;
};

static double median(std::vector<double>& x)
{
  if (x.size() == 0) {
    return std::numeric_limits<double>::quiet_NaN();
  }
  std::vector<double>::iterator median_it = x.begin() + x.size() / 2;
  nth_element(x.begin(), median_it, x.end());
  if (x.size() % 2) { // Odd number of elements
    return *median_it;
  }
  // Even number of elements, return average of two elements in the middle
  double b = *median_it--;
  nth_element(x.begin(), median_it, x.end());
  double a = *median_it;
  return (a + b) / 2;
}

int main(int argc, char* argv[])
{
  DefaultRandomNumberGenerator pseudo_rng;
  long long experiments = 0;
  if (argc > 1) {
    experiments = atoi(argv[1]);

    // Self-test
    if (experiments == 0) {
      std::cout << "Comparing group_size_brute_force() and group_size_convex_hull()\n";
      {
        double max_diff = 0;
        for (unsigned i = 0; i < 1e6; i++) {
          unsigned shots = (i & 0xf) + 2;
          ShotGroup g;
          pseudo_rng.advance();
          for (unsigned j = 0; j < shots; j++) {
            g.add(pseudo_rng.point(j));
          }
          double bf2 = 0, ch2 = 0;
          double bf = g.group_size_brute_force((i & 0x10) ? &bf2 : NULL);
          double ch = g.group_size_convex_hull((i & 0x10) ? &ch2 : NULL);
          double diff_1 = fabs(bf - ch);
          if (max_diff < diff_1) {
            max_diff = diff_1;
          }
          if (diff_1 > 1e-8) {
            std::cout << "Expected group size " << bf << ", got " << ch << "\n";
            g.show();
            return 0;
          }
          if (i & 0x10) {
            double diff_2 = fabs(bf2 - ch2);
            if (diff_2 > 1e-8) {
              std::cout << "Expected group size excluding worst " << bf2 << ", got " << ch2 << "\n";
              g.show();
              return 0;
            }
            if (max_diff < diff_2) {
              max_diff = diff_2;
            }
          }
        }
        std::cout << "Max difference " << max_diff << "\n";
      }
      std::cout << "\tgroup_size_brute_force()\tgroup_size_convex_hull()\n";
      for (unsigned shots = 4; shots <= 256; shots *= 2) {
        std::cout << shots << " shots:\t";
        pseudo_rng.reset();
        time_t start_time;
        time(&start_time);
        double a = 0;
        for (unsigned i = 0; i < 1e6; i++) {
          ShotGroup g;
          pseudo_rng.advance();
          for (unsigned j = 0; j < shots; j++) {
            g.add(pseudo_rng.point(j));
          }
          a += g.group_size_brute_force();
        }
        time_t end_time;
        time(&end_time);
        std::cout << (end_time - start_time) << " µs, sum=" << a;

        pseudo_rng.reset();
        std::cout << "\t";
        time(&start_time);
        double b = 0;
        for (unsigned i = 0; i < 1e6; i++) {
          ShotGroup g;
          pseudo_rng.advance();
          for (unsigned j = 0; j < shots; j++) {
            g.add(pseudo_rng.point(j));
          }
          b += g.group_size_convex_hull();
        }
        time(&end_time);
        std::cout << (end_time - start_time) << " µs, sum=" << b << "\n";
      }
      return 0;
    }
  } else {
    std::cerr << "Usage: mcgs experiments [[[[shots_in_group] groups_in_experiment] proportion_of_outliers] outlier_severity]\n"
                 "0 experiments to run a self-test,\n"
                 "negative experiments to use additive quasi-random number generator,\n"
                 "negative shots_in_group to use Sobol quasi-random number generator,\n"
                 "negative groups_in_experiment to use MCG 128 pseudo-random number generator\n";
    return -1;
  }
  std::unique_ptr<RandomNumberGenerator> rng(std::unique_ptr<RandomNumberGenerator>(new DefaultRandomNumberGenerator()));
  unsigned shots_in_group;
  {
    int isig = 5;
    if (argc > 2) {
      isig = atoi(argv[2]);
    }
    if (isig < 0) {
      shots_in_group = (unsigned)(-isig);
      std::cout << "Using Sobol quasi-random number generator for impact coordinates\n";
      rng = std::unique_ptr<RandomNumberGenerator>(new SobolRandomNumberGenerator(shots_in_group));
    } else {
      shots_in_group = (unsigned)(isig);
    }
  }
  if (shots_in_group < 3) {
    std::cerr << "Shots in group = " << shots_in_group << ", expected at least 3\n";
    return -1;
  }
  if (experiments < 0) {
    experiments = -experiments;
    std::cout << "Using additive quasi-random number generator for impact coordinates\n";
    rng = std::unique_ptr<RandomNumberGenerator>(new AdditiveRandomNumberGenerator(shots_in_group));
  }
  unsigned groups_in_experiment = 1;
  if (argc > 3) {
    int igie = atoi(argv[3]);
    if (igie < 0) {
      igie = -igie;
      std::cout << "Using MCG 128 pseudo-random number generator for impact coordinates\n";
      rng = std::unique_ptr<RandomNumberGenerator>(new FastRandomNumberGenerator());
    }
    groups_in_experiment = (unsigned)igie;
  }
  double proportion_of_outliers = 0.;
  if (argc > 4) {
    proportion_of_outliers = atof(argv[4]);
  }
  double outlier_severity = 10.;
  if (argc > 5) {
    outlier_severity = atof(argv[5]);
  }
  std::cout << experiments << " experiments, " << shots_in_group << " shots in group";
  if (groups_in_experiment > 1) {
    std::cout << ", " << groups_in_experiment << " groups per experiment";
  }
  std::cout << "\n";
  if (proportion_of_outliers > 0) {
    std::cout << "Using contaminated normal distribution: " <<
    (proportion_of_outliers * 100) << "% of observations are pulled from distribution with " <<
    outlier_severity << " times higher deviation\n";
  }
  const double rayleigh_cep_factor = sqrt(4 * log(2) / M_PI) / shots_in_group; // 0.9394/shots

  double mle_factor = sqrt(log(2) / M_PI);
  for (unsigned i = 0; i < shots_in_group; i++) {
    mle_factor *= 4;
    if (i != shots_in_group - 1) {
      mle_factor *= i + 1;
    }
    mle_factor /= i + 1 + shots_in_group;
  }
  double wmr_to_r90hat_factor = 0;
  double swmr_to_r90hat_factor = 0;
  double swmrr_to_r90hat_factor = 0;
  double gs_to_r90hat_factor = 0;
  double sixtynine_to_r90hat_factor = 0;
  double rayleigh_to_r90hat_factor = 0;
  double mle_to_r90hat_factor = 0;

  // http://ballistipedia.com/images/7/7a/Statistical_Inference_for_Rayleigh_Distributions_-_Siddiqui%2C_1964.pdf
  std::pair<unsigned, unsigned> sixtynine_rank( (unsigned)(0.639 * (shots_in_group + 1)) - 1
                                              , (unsigned)(0.927 * (shots_in_group + 1)) - 1
                                              );
  if (shots_in_group == 10) { // Suboptimal, but more robust
    sixtynine_rank.first = 6 - 1;
    sixtynine_rank.second = 9 - 1;
  }
  if (groups_in_experiment == 1) {
    switch (shots_in_group) {
      case  1:
        // wxMaxima: find_root(integrate(x*exp(-x^2/2)*exp(-x^2*k^2/2), x, 0, inf)=1-0.9, k, 1, 10);
        wmr_to_r90hat_factor = 3;
        break;
      case  3:
        // wxMaxima: find_root(integrate(3*x*(1-exp(-x^2/2))^2*exp(-x^2/2-k^2*x^2/2), x, 0, inf)=1-0.9, k, 1, 10);
        wmr_to_r90hat_factor = 1.414213562373095;
        break;
      case  5:
        // wxMaxima: find_root(integrate(5*x*(1-exp(-x^2/2))^4*exp(-x^2/2-k^2*x^2/2), x, 0, inf)=1-0.9, k, 1, 10);
        wmr_to_r90hat_factor = 1.17215228421396;
        // wxMaxima:
        // assume(x>0,t>0,u>0,q>0,k>1);
        // p(x):=20*x*(1-exp(-x^2/2))^3*exp(-x^2);
        // c(k):=integrate(p(q)*exp(-q^2*k^2/2),q,0,inf);
        // find_root(c(k)=1-0.9, k, 1, 2);
        swmr_to_r90hat_factor = 1.578643529936508;
        gs_to_r90hat_factor = 0.8;
        break;
      case 10:
        // wxMaxima: find_root(integrate(90*x*(1-exp(-x^2/2))^8*exp(-x^2-k^2*x^2/2), x, 0, inf)=1-0.9, k, 1, 10);
        swmr_to_r90hat_factor = 1.187140545277712;
        swmrr_to_r90hat_factor = 1.2;
        gs_to_r90hat_factor = 0.6;
        break;
      case 20:
        rayleigh_to_r90hat_factor = 1.76;
        mle_to_r90hat_factor = 0.35;
        break;
    }
  } else if (groups_in_experiment == 2) {
    switch (shots_in_group) {
      case 1:
        // wxMaxima:
        // assume(x>0,t>0,u>0,q>0,k>1);
        // p(x):=x*exp(-x^2/2);
        // p2(t):=''(integrate(2*p(u)*p(2*t-u),u,0,2*t));
        // c2(k):=romberg(p2(q)*exp(-q^2*k^2/2),q,0,10);
        // find_root(c2(k)=1-0.9, k, 1, 3);
        wmr_to_r90hat_factor = 2.22197649;
        break;
      case 3:
        // wxMaxima:
        // assume(x>0,t>0,u>0,q>0,k>1);
        // p(x):=3*x*(1-exp(-x^2/2))^2*exp(-x^2/2);
        // p2(t):=''(integrate(2*p(u)*p(2*t-u),u,0,2*t));
        // c2(k):=romberg(p2(q)*exp(-q^2*k^2/2),q,0,10);
        // find_root(c2(k)=1-0.9, k, 1, 3);
        wmr_to_r90hat_factor = 1.28875916;
        break;
      case 5:
        // wxMaxima:
        // assume(x>0,t>0,u>0,q>0,k>1);
        // p(x):=5*x*(1-exp(-x^2/2))^4*exp(-x^2/2);
        // p2(t):=romberg(2*p(u)*p(2*t-u),u,0,2*t);
        // c2(k):=romberg(p2(q)*exp(-q^2*k^2/2),q,0,10);
        // find_root(c2(k)=1-0.9, k, 1, 1.3);
        wmr_to_r90hat_factor = 1.10343798;
        // wxMaxima:
        // assume(x>0,t>0,u>0,q>0,k>1);
        // p(x):=20*x*(1-exp(-x^2/2))^3*exp(-x^2);
        // p2(t):=romberg(2*p(u)*p(2*t-u),u,0,2*t);
        // c2(k):=romberg(p2(q)*exp(-q^2*k^2/2),q,0,10);
        // find_root(c2(k)=1-0.9, k, 1, 2);
        swmr_to_r90hat_factor = 1.478456;
        break;
      case 10:
        // wxMaxima:
        // assume(x>0,t>0,u>0,q>0,k>1);
        // p(x):=90*x*(1-exp(-x^2/2))^8*exp(-x^2);
        // p2(t):=romberg(2*p(u)*p(2*t-u),u,0,2*t);
        // c2(k):=romberg(p2(q)*exp(-q^2*k^2/2),q,0,10);
        // find_root(c2(k)=1-0.9, k, 1.1, 1.15);
        swmr_to_r90hat_factor = 1.149216;
        swmrr_to_r90hat_factor = 1.15;
        break;
    }
  } else if (groups_in_experiment == 4) {
    switch (shots_in_group) {
      case  5:
        swmr_to_r90hat_factor = 1.43;
        gs_to_r90hat_factor = 0.723;
        break;
      default:
        break;
    }
  } else if (groups_in_experiment == 5) {
    switch (shots_in_group) {
      case  5:
        swmr_to_r90hat_factor = 1.42;
        gs_to_r90hat_factor = 0.72;
        break;
      default:
        break;
    }
  } else if (groups_in_experiment > 10) { // Asymptotic approximation for large number of groups in experiment
    switch (shots_in_group) {
      case  1:
        // wxMaxima: float(sqrt(2*log(10))/integrate(x*exp(-x^2/2)*x, x, 0, inf));
        wmr_to_r90hat_factor  = 1.712233160383746;
        break;
      case  3:
        // wxMaxima: float(sqrt(2*log(10))/integrate(3*(1-exp(-x^2/2))^2*x*exp(-x^2/2)*x, x, 0, inf));
        wmr_to_r90hat_factor  = 1.175960143568417;
        break;
      case  5:
        // wxMaxima: float(sqrt(2*log(10))/integrate(5*(1-exp(-x^2/2))^4*x*exp(-x^2/2)*x, x, 0, inf));
        wmr_to_r90hat_factor  = 1.037938194579831;

        // wxMaxima: float(sqrt(2*log(10))/integrate(20*x*(1-exp(-x^2/2))^3*exp(-x^2)*x, x, 0, inf));
        swmr_to_r90hat_factor = 1.38619009633813;

        gs_to_r90hat_factor = 0.7;
        break;
      case 10:
        // wxMaxima: float(sqrt(2*log(10))/integrate(90*x*(1-exp(-x^2/2))^8*exp(-x^2)*x, x, 0, inf));
        swmr_to_r90hat_factor  = 1.112257194707586;
        swmrr_to_r90hat_factor = 1.1;
        gs_to_r90hat_factor = 0.7;
        break;
      case 20:
        mle_to_r90hat_factor = 0.3414341089001782;
        break;
    }
  }
  if (shots_in_group == 10) {
    switch (groups_in_experiment) {
      case  1:
        // wxMaxima:
        // assume(x>0,y>0,x<=y,k>0);
        // pdf(x,y):=3*7*8*9*10*(1-exp(-x^2/2))^5*(exp(-x^2/2)-exp(-y^2/2))^2*exp(-y^2/2)*x*exp(-x^2/2)*y*exp(-y^2/2);
        // find_root(romberg(integrate(pdf(x,y)*(1-exp(-(k*(x+y))^2/2)),y,x,inf),x,0,50)=9/10,k,0.6,0.8);
        sixtynine_to_r90hat_factor = 0.7076687;
        break;
      case 2:
        // wxMaxima:
        // assume(x>0,y>0,x<=y);
        // pdf(x,y):=15120*(1-exp(-x^2/2))^5*(exp(-x^2/2)-exp(-y^2/2))^2*exp(-y^2/2)*x*exp(-x^2/2)*y*exp(-y^2/2);
        // assume(u>0,v>0,v<=u);
        // p2(u):=romberg(pdf((u-v)/2,(u+v)/2)/2,v,0,u);
        // p2s(t):=romberg(2*p2(u)*p2(2*t-u),u,0,2*t);
        // c2(k):=romberg(p2s(q)*exp(-q^2*k^2/2),q,0,10);
        // find_root(c2(k)=1/10, k, 0.6, 0.8);
        sixtynine_to_r90hat_factor = 0.68860849;
        break;
      default:
        // wxMaxima:
        // assume(x>0,y>0,x<=y);
        // pdf(x,y):=3*7*8*9*10*(1-exp(-x^2/2))^5*(exp(-x^2/2)-exp(-y^2/2))^2*exp(-y^2/2)*x*exp(-x^2/2)*y*exp(-y^2/2);
        // float(sqrt(2*log(10)))/float(integrate(integrate(pdf(x,y)*(x+y),y,x,inf),x,0,inf));
        sixtynine_to_r90hat_factor = 0.67024464399177286;
        break;
    }
  }
  DescriptiveStat gs_s, gs_s2, bgs_s, ags_s, ags_s2, mgs_s, wgs_s, amr_s, bac_s, pdwr_s, pdwr2_s,
                  sraspd_s, sraspd2_s, tqn_s, rwmrwpd_s, aamr_s, rayleigh_s,
                  rwr_s, mle_s, median_r_s;
  DescriptiveStat worst_r_s, second_worst_r_s, rwr9_s;
  std::map< std::pair<unsigned, unsigned>, DescriptiveStat> sixtynine_r_s;
  DescriptiveStat nsd_s, wr_s, swr_s, sixtynine_s;
  unsigned hits_wmr = 0;
  unsigned hits_swmr = 0;
  unsigned hits_swmrr = 0;
  unsigned hits_gs = 0;
  unsigned hits_sixtynine = 0;
  unsigned hits_rayleigh = 0;
  unsigned hits_mle = 0;
  unsigned bac_gt_1_ct = 0;
  double r90hat_wmr = 0;
  double r90hat_swmr = 0;
  double r90hat_swmrr = 0;
  double r90hat_gs = 0;
  double r90hat_sixtynine = 0;
  double r90hat_rayleigh = 0;
  double r90hat_mle = 0;
  std::default_random_engine outlier_generator;
  std::uniform_real_distribution<double> outlier_distribution;
  for (unsigned experiment = 0; experiment < experiments; experiment++) {
    double best_gs = 0, worst_gs = 0;
    DescriptiveStat gs, gs2, amr, wr, swr, sixtynine, rayleigh, mle;
    std::vector<double> gsg_v;
    for (unsigned j = 0; j < groups_in_experiment; j++) {
      ShotGroup g;
      rng->advance();
      std::vector<double> r;
      std::vector<double> r2;
      for (unsigned i = 0; i < shots_in_group; i++) {
        std::complex<double> p = rng->point(i);
        if ( proportion_of_outliers > 0
          && outlier_distribution(outlier_generator) < proportion_of_outliers
           ) {
          p *= outlier_severity;
        }
        double ri = std::abs(p);
        g.add(p);
        r.push_back(ri);
        r2.push_back(ri*ri);

        // Use R90 estimates based on previous experiment to avoid correlation
        if (experiment != 0) { // If there is a prior group
          if (ri < r90hat_wmr) hits_wmr++;
          if (ri < r90hat_swmr) hits_swmr++;
          if (ri < r90hat_swmrr) hits_swmrr++;
          if (ri < r90hat_gs) hits_gs++;
          if (ri < r90hat_sixtynine) hits_sixtynine++;
          if (ri < r90hat_rayleigh) hits_rayleigh++;
          if (ri < r90hat_mle) hits_mle++;
        }
      } // Next shot

      std::sort(r.begin(), r.end()); // We'll need many ranks, faster to sort once

      double this_minus1 = 0;
      double this_gs = (shots_in_group < 32)
                     ? g.group_size_brute_force(&this_minus1)
                     : g.group_size_convex_hull(&this_minus1);
      gs_s.push(this_gs);
      gsg_v.push_back(this_gs);
      gs_s2.push(this_minus1);
      gs2.push(this_minus1);
      if (shots_in_group == 4) {
        nsd_s.push(g.nsd_kuchnost());
      }
      if (j) {
        if (best_gs > this_gs) {
          best_gs = this_gs;
        }
        if (worst_gs < this_gs) {
          worst_gs = this_gs;
        }
      } else {
        best_gs = this_gs;
        worst_gs = this_gs;
      }
      gs.push(this_gs);

      double this_amr = g.avg_miss_radius();
      amr_s.push(this_amr);
      amr.push(this_amr);

      double this_bac = g.bac();
      bac_s.push(this_bac);
      if (this_bac > 1) {
        bac_gt_1_ct++;
      }
      pdwr_s.push(g.pdwr());
      pdwr2_s.push(g.pdwr(true));
      sraspd_s.push(g.sraspd());
      sraspd2_s.push(g.sraspd(true));
      tqn_s.push(g.tqn());
      rwmrwpd_s.push(g.rwmrwpd());

      double this_rayleigh = rayleigh_cep_factor * accumulate(r.begin(), r.end(), 0.);
      rayleigh.push(this_rayleigh);
      rayleigh_s.push(this_rayleigh);

      {
        double rwr = 0, rwr9 = 0;
        for (unsigned i = 0; i < r.size(); i++) {
          rwr += r.at(i) * (i + 1);
          if (i < r.size() - 1) {
            rwr9 += r.at(i) * (i + 1);
          }
        }
        rwr_s.push(2 * rwr / (r.size() * (r.size() - 1)));
        rwr9_s.push(2 * rwr9 / ((r.size() - 1) * (r.size() - 2)));
      }

      double this_mle = sqrt(accumulate(r2.begin(), r2.end(), 0.));
      mle.push(this_mle);
      mle_s.push(mle_factor * this_mle);

      double this_wr = r.at(shots_in_group - 1);
      wr.push(this_wr);
      worst_r_s.push(this_wr);

      double this_swr = r.at(shots_in_group - 2);
      swr.push(this_swr);
      second_worst_r_s.push(this_swr);

      double med = median(r);
      median_r_s.push(med);

      if (sixtynine_rank.first != sixtynine_rank.second) {
        double this_sixtynine = r.at(sixtynine_rank.first) + r.at(sixtynine_rank.second);
        sixtynine.push(this_sixtynine);
        if (shots_in_group <= 100) {
          // Remember all rank pairs, will choose the best one later
          for (unsigned rank_a = 0; rank_a < shots_in_group - 1; rank_a++) {
            for (unsigned rank_b = rank_a + 1; rank_b < shots_in_group; rank_b++) {
              double e = r.at(rank_a) + r.at(rank_b);
              sixtynine_r_s[std::make_pair(rank_a, rank_b)].push(e);
            }
          }
        }
      }
    } // Next group
    if (groups_in_experiment > 1) {
      bgs_s.push(best_gs);
      wgs_s.push(worst_gs);
      ags_s.push(gs.mean());
      ags_s2.push(gs2.mean());
      mgs_s.push(median(gsg_v));
      aamr_s.push(amr.mean());
      wr_s.push(wr.mean());
      swr_s.push(swr.mean());
      sixtynine_s.push(sixtynine.mean());
    }
    r90hat_wmr = wr.mean()  * wmr_to_r90hat_factor;
    r90hat_swmr = swr.mean() * swmr_to_r90hat_factor;
    r90hat_swmrr = swr.mean() * swmrr_to_r90hat_factor;
    r90hat_gs = gs.mean()  * gs_to_r90hat_factor;
    r90hat_sixtynine = sixtynine.mean() * sixtynine_to_r90hat_factor;
    r90hat_rayleigh = rayleigh.mean() * rayleigh_to_r90hat_factor / sqrt(4 * log(2) / M_PI);
    if (shots_in_group == 20) {
      r90hat_mle = mle.mean() * mle_to_r90hat_factor;
    }
  } // Next experiment
  if (groups_in_experiment == 1) {
    std::cout << "--- Precision estimators ---\n";
    gs_s.show("Group size:");
    if (shots_in_group == 4) {
      nsd_s.show("Kuchnost:");
    }
    amr_s.show("Average Miss Radius:");
    double e = (proportion_of_outliers > 0 ? 0 : 1);
    pdwr_s.show("Pairwise distances weighted by rank:", e, true);
    sraspd_s.show("Square root of average of squared pairwise distances:", true);
    bac_s.show("Ballistic Accuracy Class:");
    std::cout << "Percent of groups with BAC>1: "
              << 100. * bac_gt_1_ct / groups_in_experiment / experiments << "%, expected 90%\n";
    std::cout << "--- Robust precision estimators ---\n";
    gs_s2.show("Group size (excluding worst shot in group):");
    pdwr2_s.show("Pairwise distances weighted by rank, trimmed:", e, true);
    sraspd2_s.show("Square root of average of squared pairwise distances, trimmed:", true);
    tqn_s.show("Tweaked Qn:");
    rwmrwpd_s.show("Rank weighted mean of right Winsorized pairwise distances:", e, true);
    std::cout << "--- Hit probability estimators ---\n";
    double theoretical_cep = 0;
    if (proportion_of_outliers == 0) {
      theoretical_cep =  sqrt(-2*log(0.5));
    }
    rayleigh_s.show("Rayleigh CEP estimator:", theoretical_cep);
    rwr_s.show("Rank weighted miss radius:");
    mle_s.show("Maximum likelihood CEP estimator:", theoretical_cep);
    double theoretical_worst = 0;
    if (proportion_of_outliers == 0) {
      switch (shots_in_group) {
        case 3:
          // wxMaxima: float(integrate(3*(1-exp(-x^2/2))^2*x*exp(-x^2/2)*x, x, 0, inf));
          theoretical_worst = 1.824862890146495;
          break;
        case 5:
          // wxMaxima: float(integrate(5*(1-exp(-x^2/2))^4*x*exp(-x^2/2)*x, x, 0, inf));
          theoretical_worst = 2.067527755983637;
          break;
      }
    }
    worst_r_s.show("Worst miss radius:", theoretical_worst);
    std::cout << "--- Robust hit probability estimators ---\n";
    median_r_s.show("Median CEP estimator:"); // Not showing theoretical_cep because median has known bias
    double theoretical_second_worst = 0;
    double theoretical_sixtynine = 0;
    if (proportion_of_outliers == 0) {
      switch (shots_in_group) {
        case 10:
          // wxMaxima: float(integrate(90*x*(1-exp(-x^2/2))^8*exp(-x^2)*x, x, 0, inf));
          theoretical_second_worst = 1.929379316672818;

          // wxMaxima:
          // assume(x>0,y>0,x<=y);
          // pdf(x,y):=3*7*8*9*10*(1-exp(-x^2/2))^5*(exp(-x^2/2)-exp(-y^2/2))^2*exp(-y^2/2)*x*exp(-x^2/2)*y*exp(-y^2/2);
          // integrate(integrate(pdf(x,y)*(x+y),y,x,inf),x,0,inf);
          theoretical_sixtynine = 3.201765293569168;
          break;
      }
    }
    second_worst_r_s.show("Second worst miss radius:", theoretical_second_worst);
    rwr9_s.show("Rank weighted miss radius (excluding worst):");
    std::cout << "--- Combinations of two order statistics ---\n";
    // Find combination of two order statistics with lowest CV
    {
      unsigned best_pos = 0;
      double best_cv = 0;
      unsigned pos = 0;
      for (auto it = sixtynine_r_s.begin(); it != sixtynine_r_s.end(); it++, pos++) {
        if (pos == 0 || best_cv > it->second.cv()) {
          best_cv = it->second.cv();
          best_pos = pos;
        }
      }
      pos = 0;
      for (auto it = sixtynine_r_s.begin(); it != sixtynine_r_s.end(); it++, pos++) {
        if (pos == best_pos) {
          std::cout << "Lowest CV pair: R" << (it->first.first + 1) << ":" << shots_in_group
                    << "+R" << (it->first.second + 1) << ":" << shots_in_group;
          it->second.show(",");
          it = sixtynine_r_s.find(sixtynine_rank);
          if (it != sixtynine_r_s.end()) {
            std::cout << "R" << (sixtynine_rank.first  + 1) << ":" << shots_in_group
                      << "+R" << (sixtynine_rank.second + 1) << ":" << shots_in_group;
            it->second.show(",", theoretical_sixtynine);
          }
        }
      }
    }
  } else {
    bgs_s.show("Best group size:");
    ags_s.show("Average group size:");
    ags_s2.show("Average group size (excluding worst shot in group):");
    mgs_s.show("Median group size:");
    wgs_s.show("Worst group size:");
    aamr_s.show("Average of AMR of groups:");
    wr_s.show("Average worst miss radius:");
    swr_s.show("Average second worst miss radius:");
    if (sixtynine_rank.first != sixtynine_rank.second) {
      std::cout << "Average R" << (sixtynine_rank.first  + 1) << ":" << shots_in_group
                << "+R"        << (sixtynine_rank.second + 1) << ":" << shots_in_group;
      sixtynine_s.show(":");
    }
  }
  std::cout << "--- R90 estimators ---\n";
  // -1 because we use previos experiment to set threshold for the next.
  // First experiment does not participate because it does not have a previous.
  double denominator = shots_in_group * ((double)groups_in_experiment * (experiments - 1));
  if (wmr_to_r90hat_factor > 0) {
    std::cout << "Percent of hits within " << wmr_to_r90hat_factor
              << " * worst miss radius: "
              << 100. * hits_wmr / denominator << "%\n";
  }
  if (swmr_to_r90hat_factor > 0) {
    std::cout << "Percent of hits within " << swmr_to_r90hat_factor
              << " * second worst miss radius: "
              << 100. * hits_swmr / denominator << "%\n";
  }
  if (swmrr_to_r90hat_factor > 0) {
    std::cout << "Percent of hits within " << swmrr_to_r90hat_factor
              << " * second worst miss radius: "
              << 100. * hits_swmrr / denominator << "%\n";
  }
  if (gs_to_r90hat_factor > 0) {
    std::cout << "Percent of hits within " << gs_to_r90hat_factor
              << " * group size: "
              << 100. * hits_gs / denominator << "%\n";
  }
  if (sixtynine_to_r90hat_factor > 0) {
    std::cout << "Percent of hits within " << sixtynine_to_r90hat_factor
              << " * (R" << (sixtynine_rank.first  + 1) << ":" << shots_in_group
              << " + R"  << (sixtynine_rank.second + 1) << ":" << shots_in_group << "): "
              << 100. * hits_sixtynine / denominator << "%\n";
  }
  if (rayleigh_to_r90hat_factor > 0) {
    std::cout << "Percent of hits within " << rayleigh_to_r90hat_factor
              << " * average miss radius: "
              << 100. * hits_rayleigh / denominator << "%\n";
  }
  if (mle_to_r90hat_factor > 0) {
    std::cout << "Percent of hits within " << mle_to_r90hat_factor
              << " * square root of sum of squares: "
              << 100. * hits_mle / denominator << "%\n";
  }
  return 0;
}


/* haertel.hpp file
 *
 * Copyright Jens Maurer 2000, 2002
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * $Id$
 *
 * Revision history
 */

/*
 * NOTE: This is not part of the official boost submission.  It exists
 * only as a collection of ideas.
 */

#ifndef BOOST_RANDOM_HAERTEL_HPP
#define BOOST_RANDOM_HAERTEL_HPP

#include <boost/cstdint.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/inversive_congruential.hpp>

namespace boost {
namespace random {

// Wikramaratna 1989  ACORN
template<class IntType, int k, IntType m, IntType val>
class additive_congruential
{
public:
  typedef IntType result_type;
#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
  static const bool has_fixed_range = true;
  static const result_type min_value = 0;
  static const result_type max_value = m-1;
#else
  enum {
    has_fixed_range = true,
    min_value = 0,
    max_value = m-1
  };
#endif
  template<class InputIterator>
  explicit additive_congruential(InputIterator start) { seed(start); }
  template<class InputIterator>
  void seed(InputIterator start)
  {
    for(int i = 0; i <= k; ++i, ++start)
      values[i] = *start;
  }

  result_type operator()()
  {
    for(int i = 1; i <= k; ++i) {
      IntType tmp = values[i-1] + values[i];
      if(tmp >= m)
        tmp -= m;
      values[i] = tmp;
    }
    return values[k];
  }
  result_type validation() const { return val; }
private:
  IntType values[k+1];
};

template<class IntType, int r, int s, IntType m, IntType val>
class lagged_fibonacci_int
{
public:
  typedef IntType result_type;
#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
  static const bool has_fixed_range = true;
  static const result_type min_value = 0;
  static const result_type max_value = m-1;
#else
  enum {
    has_fixed_range = true,
    min_value = 0,
    max_value = m-1
  };
#endif
  explicit lagged_fibonacci_int(IntType start) { seed(start); }
  template<class Generator>
  explicit lagged_fibonacci_int(Generator & gen) { seed(gen); }
  void seed(IntType start)
  {
    linear_congruential<uint32_t, 299375077, 0, 0, 0> init;
    seed(init);
  }
  template<class Generator>
  void seed(Generator & gen)
  {
    assert(r > s);
    for(int i = 0; i < 607; ++i)
      values[i] = gen();
    current = 0;
    lag = r-s;
  }

  result_type operator()()
  {
    result_type tmp = values[current] + values[lag];
    if(tmp >= m)
      tmp -= m;
    values[current] = tmp;
    ++current;
    if(current >= r)
      current = 0;
    ++lag;
    if(lag >= r)
      lag = 0;
    return tmp;
  }
  result_type validation() const { return val; }
private:
  result_type values[r];
  int current, lag;
};

} // namespace random
} // namespace boost

// distributions from Haertel's dissertation
// (additional parameterizations of the basic templates)
namespace Haertel {
  typedef boost::random::linear_congruential<boost::uint64_t, 45965, 453816691,
    (boost::uint64_t(1)<<31), 0> LCG_Af2;
  typedef boost::random::linear_congruential<boost::uint64_t, 211936855, 0,
    (boost::uint64_t(1)<<29)-3, 0> LCG_Die1;
  typedef boost::random::linear_congruential<boost::uint32_t, 2824527309u, 0,
    0, 0> LCG_Fis;
  typedef boost::random::linear_congruential<boost::uint64_t, 950706376u, 0,
    (boost::uint64_t(1)<<31)-1, 0> LCG_FM;
  typedef boost::random::linear_congruential<boost::int32_t, 51081, 0,
    2147483647, 0> LCG_Hae;
  typedef boost::random::linear_congruential<boost::uint32_t, 69069, 1,
    0, 0> LCG_VAX;
  typedef boost::random::inversive_congruential<boost::int64_t, 240318, 197,
    1000081, 0> NLG_Inv1;
  typedef boost::random::inversive_congruential<boost::int64_t, 15707262,
    13262967, (1<<24)-17, 0> NLG_Inv2;
  typedef boost::random::inversive_congruential<boost::int32_t, 1, 1,
    2147483647, 0> NLG_Inv4;
  typedef boost::random::inversive_congruential<boost::int32_t, 1, 2,
    1<<30, 0> NLG_Inv5;
  typedef boost::random::additive_congruential<boost::int32_t, 6,
    (1<<30)-35, 0> MRG_Acorn7;
  typedef boost::random::lagged_fibonacci_int<boost::uint32_t, 607, 273,
    0, 0> MRG_Fib2;
} // namespace Haertel

#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Daniele Panozzo <daniele.panozzo@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "principal_curvature.h"
#include <iostream>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <queue>
#include <list>
#include <cmath>
#include <limits>

#include <Eigen/SparseCholesky>

// Lib IGL includes
#include <igl/adjacency_list.h>
#include <igl/per_face_normals.h>
#include <igl/per_vertex_normals.h>
#include <igl/avg_edge_length.h>
#include <igl/vertex_triangle_adjacency.h>

typedef enum
{
  SPHERE_SEARCH,
  K_RING_SEARCH
} searchType;

typedef enum
{
  AVERAGE,
  PROJ_PLANE
} normalType;

class CurvatureCalculator
{
public:
  /* Row number i represents the i-th vertex, whose columns are:
   curv[i][0] : K2
   curv[i][1] : K1
   curvDir[i][0] : PD1
   curvDir[i][1] : PD2
   */
  std::vector< std::vector<double> > curv;
  std::vector< std::vector<Eigen::Vector3d> > curvDir;
  bool curvatureComputed;
  class Quadric
  {
  public:

    IGL_INLINE Quadric ()
    {
      a() = b() = c() = d() = e() = 1.0;
    }

    IGL_INLINE Quadric(double av, double bv, double cv, double dv, double ev)
    {
      a() = av;
      b() = bv;
      c() = cv;
      d() = dv;
      e() = ev;
    }

    IGL_INLINE double& a() { return data[0];}
    IGL_INLINE double& b() { return data[1];}
    IGL_INLINE double& c() { return data[2];}
    IGL_INLINE double& d() { return data[3];}
    IGL_INLINE double& e() { return data[4];}

    double data[5];

    IGL_INLINE double evaluate(double u, double v)
    {
      return a()*u*u + b()*u*v + c()*v*v + d()*u + e()*v;
    }

    IGL_INLINE double du(double u, double v)
    {
      return 2.0*a()*u + b()*v + d();
    }

    IGL_INLINE double dv(double u, double v)
    {
      return 2.0*c()*v + b()*u + e();
    }

    IGL_INLINE double duv(double u, double v)
    {
      return b();
    }

    IGL_INLINE double duu(double u, double v)
    {
      return 2.0*a();
    }

    IGL_INLINE double dvv(double u, double v)
    {
      return 2.0*c();
    }

    IGL_INLINE static Quadric fit(const std::vector<Eigen::Vector3d> &VV)
    {
      assert(VV.size() >= 5);
      if (VV.size() < 5)
      {
        std::cerr << "ASSERT FAILED! fit function requires at least 5 points: Only " << VV.size() << " were given." << std::endl;
        exit(0);
      }

      Eigen::MatrixXd A(VV.size(),5);
      Eigen::MatrixXd b(VV.size(),1);
      Eigen::MatrixXd sol(5,1);

      for(unsigned int c=0; c < VV.size(); ++c)
      {
        double u = VV[c][0];
        double v = VV[c][1];
        double n = VV[c][2];

        A(c,0) = u*u;
        A(c,1) = u*v;
        A(c,2) = v*v;
        A(c,3) = u;
        A(c,4) = v;

        b(c) = n;
      }

      sol=A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);

      return Quadric(sol(0),sol(1),sol(2),sol(3),sol(4));
    }
  };

public:

  Eigen::MatrixXd vertices;
  // Face list of current mesh    (#F x 3) or (#F x 4)
  // The i-th row contains the indices of the vertices that forms the i-th face in ccw order
  Eigen::MatrixXi faces;

  std::vector<std::vector<int> > vertex_to_vertices;
  std::vector<std::vector<int> > vertex_to_faces;
  std::vector<std::vector<int> > vertex_to_faces_index;
  Eigen::MatrixXd face_normals;
  Eigen::MatrixXd vertex_normals;

  /* Size of the neighborhood */
  double sphereRadius;
  int kRing;

  bool localMode; /* Use local mode */
  bool projectionPlaneCheck; /* Check collected vertices on tangent plane */
  bool montecarlo;
  unsigned int montecarloN;

  searchType st; /* Use either a sphere search or a k-ring search */
  normalType nt;

  double lastRadius;
  double scaledRadius;
  std::string lastMeshName;

  /* Benchmark related variables */
  bool expStep; /* True if we want the radius to increase exponentially */
  int step;  /* If expStep==false, by how much rhe radius increases on every step */
  int maxSize; /* The maximum limit of the radius in the benchmark */

  IGL_INLINE CurvatureCalculator();
  IGL_INLINE void init(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F);

  IGL_INLINE void finalEigenStuff(int, const std::vector<Eigen::Vector3d>&, Quadric&);
  IGL_INLINE void fitQuadric(const Eigen::Vector3d&, const std::vector<Eigen::Vector3d>& ref, const std::vector<int>& , Quadric *);
  IGL_INLINE void applyProjOnPlane(const Eigen::Vector3d&, const std::vector<int>&, std::vector<int>&);
  IGL_INLINE void getSphere(const int, const double, std::vector<int>&, int min);
  IGL_INLINE void getKRing(const int, const double,std::vector<int>&);
  IGL_INLINE Eigen::Vector3d project(const Eigen::Vector3d&, const Eigen::Vector3d&, const Eigen::Vector3d&);
  IGL_INLINE void computeReferenceFrame(int, const Eigen::Vector3d&, std::vector<Eigen::Vector3d>&);
  IGL_INLINE void getAverageNormal(int, const std::vector<int>&, Eigen::Vector3d&);
  IGL_INLINE void getProjPlane(int, const std::vector<int>&, Eigen::Vector3d&);
  IGL_INLINE void applyMontecarlo(const std::vector<int>&,std::vector<int>*);
  IGL_INLINE void computeCurvature();
  IGL_INLINE void printCurvature(const std::string& outpath);
  IGL_INLINE double getAverageEdge();

  IGL_INLINE static int rotateForward (double *v0, double *v1, double *v2)
  {
    double t;

    if (std::abs(*v2) >= std::abs(*v1) && std::abs(*v2) >= std::abs(*v0))
      return 0;

    t = *v0;
    *v0 = *v2;
    *v2 = *v1;
    *v1 = t;

    return 1 + rotateForward (v0, v1, v2);
  }

  IGL_INLINE static void rotateBackward (int nr, double *v0, double *v1, double *v2)
  {
    double t;

    if (nr == 0)
      return;

    t = *v2;
    *v2 = *v0;
    *v0 = *v1;
    *v1 = t;

    rotateBackward (nr - 1, v0, v1, v2);
  }

  IGL_INLINE static Eigen::Vector3d chooseMax (Eigen::Vector3d n, Eigen::Vector3d abc, double ab)
  {
    int max_i;
    double max_sp;
    Eigen::Vector3d nt[8];

    n.normalize ();
    abc.normalize ();

    max_sp = - std::numeric_limits<double>::max();

    for (int i = 0; i < 4; ++i)
    {
      nt[i] = n;
      if (ab > 0)
      {
        switch (i)
        {
          case 0:
            break;

          case 1:
            nt[i][2] = -n[2];
            break;

          case 2:
            nt[i][0] = -n[0];
            nt[i][1] = -n[1];
            break;

          case 3:
            nt[i][0] = -n[0];
            nt[i][1] = -n[1];
            nt[i][2] = -n[2];
            break;
        }
      }
      else
      {
        switch (i)
        {
          case 0:
            nt[i][0] = -n[0];
            break;

          case 1:
            nt[i][1] = -n[1];
            break;

          case 2:
            nt[i][0] = -n[0];
            nt[i][2] = -n[2];
            break;

          case 3:
            nt[i][1] = -n[1];
            nt[i][2] = -n[2];
            break;
        }
      }

      if (nt[i].dot(abc) > max_sp)
      {
        max_sp = nt[i].dot(abc);
        max_i = i;
      }
    }
    return nt[max_i];
  }

};

class comparer
{
public:
  IGL_INLINE bool operator() (const std::pair<int, double>& lhs, const std::pair<int, double>&rhs) const
  {
    return lhs.second>rhs.second;
  }
};

IGL_INLINE CurvatureCalculator::CurvatureCalculator()
{
  this->localMode=true;
  this->projectionPlaneCheck=true;
  this->sphereRadius=5;
  this->st=SPHERE_SEARCH;
  this->nt=AVERAGE;
  this->montecarlo=false;
  this->montecarloN=0;
  this->kRing=3;
  this->curvatureComputed=false;
  this->expStep=true;
}

IGL_INLINE void CurvatureCalculator::init(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F)
{
  // Normalize vertices
  vertices = V;

//  vertices = vertices.array() - vertices.minCoeff();
//  vertices = vertices.array() / vertices.maxCoeff();
//  vertices = vertices.array() * (1.0/igl::avg_edge_length(V,F));

  faces = F;
  igl::adjacency_list(F, vertex_to_vertices);
  igl::vertex_triangle_adjacency(V, F, vertex_to_faces, vertex_to_faces_index);
  igl::per_face_normals(V, F, face_normals);
  igl::per_vertex_normals(V, F, face_normals, vertex_normals);
}

IGL_INLINE void CurvatureCalculator::fitQuadric(const Eigen::Vector3d& v, const std::vector<Eigen::Vector3d>& ref, const std::vector<int>& vv, Quadric *q)
{
  std::vector<Eigen::Vector3d> points;
  points.reserve (vv.size());

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

    Eigen::Vector3d  cp = vertices.row(vv[i]);

    // vtang non e` il v tangente!!!
    Eigen::Vector3d  vTang = cp - v;

    double x = vTang.dot(ref[0]);
    double y = vTang.dot(ref[1]);
    double z = vTang.dot(ref[2]);
    points.push_back(Eigen::Vector3d (x,y,z));
  }
  if (points.size() < 5)
  {
    std::cerr << "ASSERT FAILED! fit function requires at least 5 points: Only " << points.size() << " were given." << std::endl;
    *q = Quadric(0,0,0,0,0);
  }
  else
  {
    *q = Quadric::fit (points);
  }
}

IGL_INLINE void CurvatureCalculator::finalEigenStuff(int i, const std::vector<Eigen::Vector3d>& ref, Quadric& q)
{

  const double a = q.a();
  const double b = q.b();
  const double c = q.c();
  const double d = q.d();
  const double e = q.e();

//  if (fabs(a) < 10e-8 || fabs(b) < 10e-8)
//  {
//    std::cout << "Degenerate quadric: " << i << std::endl;
//  }

  double E = 1.0 + d*d;
  double F = d*e;
  double G = 1.0 + e*e;

  Eigen::Vector3d n = Eigen::Vector3d(-d,-e,1.0).normalized();

  double L = 2.0 * a * n[2];
  double M = b * n[2];
  double N = 2 * c * n[2];

  // ----------------- Eigen stuff
  Eigen::Matrix2d m;
  m << L*G - M*F, M*E-L*F, M*E-L*F, N*E-M*F;
  m = m / (E*G-F*F);
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eig(m);

  Eigen::Vector2d c_val = eig.eigenvalues();
  Eigen::Matrix2d c_vec = eig.eigenvectors();

  // std::cerr << "c_val:" << c_val << std::endl;
  // std::cerr << "c_vec:" << c_vec << std::endl;

  // std::cerr << "c_vec:" << c_vec(0) << " "  << c_vec(1) << std::endl;

  c_val = -c_val;

  Eigen::Vector3d v1, v2;
  v1[0] = c_vec(0);
  v1[1] = c_vec(1);
  v1[2] = 0; //d * v1[0] + e * v1[1];

  v2[0] = c_vec(2);
  v2[1] = c_vec(3);
  v2[2] = 0; //d * v2[0] + e * v2[1];

  // v1 = v1.normalized();
  // v2 = v2.normalized();

  Eigen::Vector3d v1global = ref[0] * v1[0] + ref[1] * v1[1] + ref[2] * v1[2];
  Eigen::Vector3d v2global = ref[0] * v2[0] + ref[1] * v2[1] + ref[2] * v2[2];

  v1global.normalize();
  v2global.normalize();

  v1global *= c_val(0);
  v2global *= c_val(1);

  if (c_val[0] > c_val[1])
  {
    curv[i]=std::vector<double>(2);
    curv[i][0]=c_val(1);
    curv[i][1]=c_val(0);
    curvDir[i]=std::vector<Eigen::Vector3d>(2);
    curvDir[i][0]=v2global;
    curvDir[i][1]=v1global;
  }
  else
  {
    curv[i]=std::vector<double>(2);
    curv[i][0]=c_val(0);
    curv[i][1]=c_val(1);
    curvDir[i]=std::vector<Eigen::Vector3d>(2);
    curvDir[i][0]=v1global;
    curvDir[i][1]=v2global;
  }
  // ---- end Eigen stuff
}

IGL_INLINE void CurvatureCalculator::getKRing(const int start, const double r, std::vector<int>&vv)
{
  int bufsize=vertices.rows();
  vv.reserve(bufsize);
  std::list<std::pair<int,int> > queue;
  std::vector<bool> visited(bufsize, false);
  queue.push_back(std::pair<int,int>(start,0));
  visited[start]=true;
  while (!queue.empty())
  {
    int toVisit=queue.front().first;
    int distance=queue.front().second;
    queue.pop_front();
    vv.push_back(toVisit);
    if (distance<(int)r)
    {
      for (unsigned int i=0; i<vertex_to_vertices[toVisit].size(); ++i)
      {
        int neighbor=vertex_to_vertices[toVisit][i];
        if (!visited[neighbor])
        {
          queue.push_back(std::pair<int,int> (neighbor,distance+1));
          visited[neighbor]=true;
        }
      }
    }
  }
}

IGL_INLINE void CurvatureCalculator::getSphere(const int start, const double r, std::vector<int> &vv, int min)
{
  int bufsize=vertices.rows();
  vv.reserve(bufsize);
  std::list<int> queue;
  std::vector<bool> visited(bufsize, false);
  queue.push_back(start);
  visited[start]=true;
  Eigen::Vector3d me=vertices.row(start);
  std::priority_queue<std::pair<int, double>, std::vector<std::pair<int, double> >, comparer > extra_candidates;
  while (!queue.empty())
  {
    int toVisit=queue.front();
    queue.pop_front();
    vv.push_back(toVisit);
    for (unsigned int i=0; i<vertex_to_vertices[toVisit].size(); ++i)
    {
      int neighbor=vertex_to_vertices[toVisit][i];
      if (!visited[neighbor])
      {
        Eigen::Vector3d neigh=vertices.row(neighbor);
        double distance=(me-neigh).norm();
        if (distance<r)
          queue.push_back(neighbor);
        else if ((int)vv.size()<min)
          extra_candidates.push(std::pair<int,double>(neighbor,distance));
        visited[neighbor]=true;
      }
    }
  }
  while (!extra_candidates.empty() && (int)vv.size()<min)
  {
    std::pair<int, double> cand=extra_candidates.top();
    extra_candidates.pop();
    vv.push_back(cand.first);
    for (unsigned int i=0; i<vertex_to_vertices[cand.first].size(); ++i)
    {
      int neighbor=vertex_to_vertices[cand.first][i];
      if (!visited[neighbor])
      {
        Eigen::Vector3d neigh=vertices.row(neighbor);
        double distance=(me-neigh).norm();
        extra_candidates.push(std::pair<int,double>(neighbor,distance));
        visited[neighbor]=true;
      }
    }
  }
}

IGL_INLINE Eigen::Vector3d CurvatureCalculator::project(const Eigen::Vector3d& v, const Eigen::Vector3d& vp, const Eigen::Vector3d& ppn)
{
  return (vp - (ppn * ((vp - v).dot(ppn))));
}

IGL_INLINE void CurvatureCalculator::computeReferenceFrame(int i, const Eigen::Vector3d& normal, std::vector<Eigen::Vector3d>& ref )
{

  Eigen::Vector3d longest_v=Eigen::Vector3d(vertices.row(vertex_to_vertices[i][0]));

  longest_v=(project(vertices.row(i),longest_v,normal)-Eigen::Vector3d(vertices.row(i))).normalized();

  /* L'ultimo asse si ottiene come prodotto vettoriale tra i due
   * calcolati */
  Eigen::Vector3d y_axis=(normal.cross(longest_v)).normalized();
  ref[0]=longest_v;
  ref[1]=y_axis;
  ref[2]=normal;
}

IGL_INLINE void CurvatureCalculator::getAverageNormal(int j, const std::vector<int>& vv, Eigen::Vector3d& normal)
{
  normal=(vertex_normals.row(j)).normalized();
  if (localMode)
    return;

  for (unsigned int i=0; i<vv.size(); ++i)
  {
    normal+=vertex_normals.row(vv[i]).normalized();
  }
  normal.normalize();
}

IGL_INLINE void CurvatureCalculator::getProjPlane(int j, const std::vector<int>& vv, Eigen::Vector3d& ppn)
{
  int nr;
  double a, b, c;
  double nx, ny, nz;
  double abcq;

  a = b = c = 0;

  if (localMode)
  {
    for (unsigned int i=0; i<vertex_to_faces.at(j).size(); ++i)
    {
      Eigen::Vector3d faceNormal=face_normals.row(vertex_to_faces.at(j).at(i));
      a += faceNormal[0];
      b += faceNormal[1];
      c += faceNormal[2];
    }
  }
  else
  {
    for (unsigned int i=0; i<vv.size(); ++i)
    {
      a+= vertex_normals.row(vv[i])[0];
      b+= vertex_normals.row(vv[i])[1];
      c+= vertex_normals.row(vv[i])[2];
    }
  }
  nr = rotateForward (&a, &b, &c);
  abcq = a*a + b*b + c*c;
  nx = sqrt (a*a / abcq);
  ny = sqrt (b*b / abcq);
  nz = sqrt (1 - nx*nx - ny*ny);
  rotateBackward (nr, &a, &b, &c);
  rotateBackward (nr, &nx, &ny, &nz);

  ppn = chooseMax (Eigen::Vector3d(nx, ny, nz), Eigen::Vector3d (a, b, c), a * b);
  ppn.normalize();
}

IGL_INLINE double CurvatureCalculator::getAverageEdge()
{
  double sum = 0;
  int count = 0;

  for (int i = 0; i<faces.rows(); ++i)
  {
    for (short unsigned j=0; j<3; ++j)
    {
      Eigen::Vector3d p1=vertices.row(faces.row(i)[j]);
      Eigen::Vector3d p2=vertices.row(faces.row(i)[(j+1)%3]);

      double l = (p1-p2).norm();

      sum+=l;
      ++count;
    }
  }

  return (sum/(double)count);
}

IGL_INLINE void CurvatureCalculator::applyProjOnPlane(const Eigen::Vector3d& ppn, const std::vector<int>& vin, std::vector<int> &vout)
{
  for (std::vector<int>::const_iterator vpi = vin.begin(); vpi != vin.end(); ++vpi)
    if (vertex_normals.row(*vpi) * ppn > 0.0)
      vout.push_back(*vpi);
}

IGL_INLINE void CurvatureCalculator::applyMontecarlo(const std::vector<int>& vin, std::vector<int> *vout)
{
  if (montecarloN >= vin.size ())
  {
    *vout = vin;
    return;
  }

  float p = ((float) montecarloN) / (float) vin.size();
  for (std::vector<int>::const_iterator vpi = vin.begin(); vpi != vin.end(); ++vpi)
  {
    float r;
    if ((r = ((float)rand () / RAND_MAX)) < p)
    {
      vout->push_back(*vpi);
    }
  }
}

IGL_INLINE void CurvatureCalculator::computeCurvature()
{
  //CHECK che esista la mesh
  const size_t vertices_count=vertices.rows();

  if (vertices_count ==0)
    return;

  curvDir=std::vector< std::vector<Eigen::Vector3d> >(vertices_count);
  curv=std::vector<std::vector<double> >(vertices_count);

  scaledRadius=getAverageEdge()*sphereRadius;

  std::vector<int> vv;
  std::vector<int> vvtmp;
  Eigen::Vector3d normal;

  //double time_spent;
  //double searchtime=0, ref_time=0, fit_time=0, final_time=0;

  for (size_t i=0; i<vertices_count; ++i)
  {
    vv.clear();
    vvtmp.clear();
    Eigen::Vector3d me=vertices.row(i);
    switch (st)
    {
      case SPHERE_SEARCH:
        getSphere(i,scaledRadius,vv,6);
        break;
      case K_RING_SEARCH:
        getKRing(i,kRing,vv);
        break;
      default:
        fprintf(stderr,"Error: search type not recognized");
        return;
    }

    if (vv.size()<6)
    {
      //std::cerr << "Could not compute curvature of radius " << scaledRadius << std::endl;
      continue;
    }

    if (projectionPlaneCheck)
    {
      vvtmp.reserve (vv.size ());
      applyProjOnPlane (vertex_normals.row(i), vv, vvtmp);
      if (vvtmp.size() >= 6 && vvtmp.size()<vv.size())
        vv = vvtmp;
    }

    switch (nt)
    {
      case AVERAGE:
        getAverageNormal(i,vv,normal);
        break;
      case PROJ_PLANE:
        getProjPlane(i,vv,normal);
        break;
      default:
        fprintf(stderr,"Error: normal type not recognized");
        return;
    }
    if (vv.size()<6)
    {
      //std::cerr << "Could not compute curvature of radius " << scaledRadius << std::endl;
      continue;
    }
    if (montecarlo)
    {
      if(montecarloN<6)
        break;
      vvtmp.reserve(vv.size());
      applyMontecarlo(vv,&vvtmp);
      vv=vvtmp;
    }

    if (vv.size()<6)
      return;
    std::vector<Eigen::Vector3d> ref(3);
    computeReferenceFrame(i,normal,ref);

    Quadric q;
    fitQuadric (me, ref, vv, &q);
    finalEigenStuff(i,ref,q);
  }

  lastRadius=sphereRadius;
  curvatureComputed=true;
}

IGL_INLINE void CurvatureCalculator::printCurvature(const std::string& outpath)
{
  using namespace std;
  if (!curvatureComputed)
    return;

  std::ofstream of;
  of.open(outpath.c_str());

  if (!of)
  {
    fprintf(stderr, "Error: could not open output file %s\n", outpath.c_str());
    return;
  }

  int vertices_count=vertices.rows();
  of << vertices_count << endl;
  for (int i=0; i<vertices_count; ++i)
  {
    of << curv[i][0] << " " << curv[i][1] << " " << curvDir[i][0][0] << " " << curvDir[i][0][1] << " " << curvDir[i][0][2] << " " <<
    curvDir[i][1][0] << " " << curvDir[i][1][1] << " " << curvDir[i][1][2] << endl;
  }

  of.close();

}

template <
  typename DerivedV,
  typename DerivedF,
  typename DerivedPD1,
  typename DerivedPD2,
  typename DerivedPV1,
  typename DerivedPV2,
  typename Index>
IGL_INLINE void igl::principal_curvature(
  const Eigen::PlainObjectBase<DerivedV>& V,
  const Eigen::PlainObjectBase<DerivedF>& F,
  Eigen::PlainObjectBase<DerivedPD1>& PD1,
  Eigen::PlainObjectBase<DerivedPD2>& PD2,
  Eigen::PlainObjectBase<DerivedPV1>& PV1,
  Eigen::PlainObjectBase<DerivedPV2>& PV2,
  std::vector<Index>& bad_vertices,
  unsigned radius,
  bool useKring)
{

  if (radius < 2)
  {
    radius = 2;
    std::cout << "WARNING: igl::principal_curvature needs a radius >= 2, fixing it to 2." << std::endl;
  }

  // Preallocate memory
  PD1.resize(V.rows(),3);
  PD2.resize(V.rows(),3);

  // Preallocate memory
  PV1.resize(V.rows(),1);
  PV2.resize(V.rows(),1);

  // Precomputation
  CurvatureCalculator cc;
  cc.init(V.template cast<double>(),F.template cast<int>());
  cc.sphereRadius = radius;

  if (useKring)
  {
    cc.kRing = radius;
    cc.st = K_RING_SEARCH;
  }

  // Compute
  cc.computeCurvature();

  // Copy it back
  for (unsigned i=0; i<V.rows(); ++i)
  {
    if (!cc.curv[i].empty())
    {
      PD1.row(i) << cc.curvDir[i][0][0], cc.curvDir[i][0][1], cc.curvDir[i][0][2];
      PD2.row(i) << cc.curvDir[i][1][0], cc.curvDir[i][1][1], cc.curvDir[i][1][2];
      PD1.row(i).normalize();
      PD2.row(i).normalize();

      if (std::isnan(PD1(i,0)) || std::isnan(PD1(i,1)) || std::isnan(PD1(i,2)) || std::isnan(PD2(i,0)) || std::isnan(PD2(i,1)) || std::isnan(PD2(i,2)))
      {
        PD1.row(i) << 0,0,0;
        PD2.row(i) << 0,0,0;
      }

      PV1(i) = cc.curv[i][0];
      PV2(i) = cc.curv[i][1];

      if (PD1.row(i) * PD2.row(i).transpose() > 10e-6)
      {
        bad_vertices.push_back((Index)i);

        PD1.row(i) *= 0;
        PD2.row(i) *= 0;
      }
    } else {
      bad_vertices.push_back((Index)i);

      PV1(i) = 0;
      PV2(i) = 0;
      PD1.row(i) << 0,0,0;
      PD2.row(i) << 0,0,0;
    }
  }

}

template <
  typename DerivedV,
  typename DerivedF,
  typename DerivedPD1,
  typename DerivedPD2,
  typename DerivedPV1,
  typename DerivedPV2>
IGL_INLINE void igl::principal_curvature(
  const Eigen::PlainObjectBase<DerivedV>& V,
  const Eigen::PlainObjectBase<DerivedF>& F,
  Eigen::PlainObjectBase<DerivedPD1>& PD1,
  Eigen::PlainObjectBase<DerivedPD2>& PD2,
  Eigen::PlainObjectBase<DerivedPV1>& PV1,
  Eigen::PlainObjectBase<DerivedPV2>& PV2,
  unsigned radius,
  bool useKring)
{
  if (radius < 2)
  {
    radius = 2;
    std::cout << "WARNING: igl::principal_curvature needs a radius >= 2, fixing it to 2." << std::endl;
  }

  // Preallocate memory
  PD1.resize(V.rows(),3);
  PD2.resize(V.rows(),3);

  // Preallocate memory
  PV1.resize(V.rows(),1);
  PV2.resize(V.rows(),1);

  // Precomputation
  CurvatureCalculator cc;
  cc.init(V.template cast<double>(),F.template cast<int>());
  cc.sphereRadius = radius;

  if (useKring)
  {
    cc.kRing = radius;
    cc.st = K_RING_SEARCH;
  }

  // Compute
  cc.computeCurvature();

  // Copy it back
  for (unsigned i=0; i<V.rows(); ++i)
  {
    PD1.row(i) << cc.curvDir[i][0][0], cc.curvDir[i][0][1], cc.curvDir[i][0][2];
    PD2.row(i) << cc.curvDir[i][1][0], cc.curvDir[i][1][1], cc.curvDir[i][1][2];
    PD1.row(i).normalize();
    PD2.row(i).normalize();

    if (std::isnan(PD1(i,0)) || std::isnan(PD1(i,1)) || std::isnan(PD1(i,2)) || std::isnan(PD2(i,0)) || std::isnan(PD2(i,1)) || std::isnan(PD2(i,2)))
    {
      PD1.row(i) << 0,0,0;
      PD2.row(i) << 0,0,0;
    }

    PV1(i) = cc.curv[i][0];
    PV2(i) = cc.curv[i][1];

    if (PD1.row(i) * PD2.row(i).transpose() > 10e-6)
    {
      std::cerr << "PRINCIPAL_CURVATURE: Something is wrong with vertex: " << i << std::endl;
      PD1.row(i) *= 0;
      PD2.row(i) *= 0;
    }
  }

}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
// generated by autoexplicit.sh
template void igl::principal_curvature<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&, unsigned int, bool);
template void igl::principal_curvature<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&, unsigned int, bool);
template void igl::principal_curvature<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, unsigned int, bool);
template void igl::principal_curvature<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, int>(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&, std::vector<int, std::allocator<int> >&, unsigned int, bool);
#endif


#include <utility>
#include <iostream>
#include <vector>

#include <Eigen/Dense>

#include "lattice.hpp"
#include "input_output.hpp"
#include "options.hpp"
#include "crit_temperature_solver.hpp"
#include "gnuplot-iostream/gnuplot-iostream.h"

using namespace Eigen;

class MyFunction : public cppoptlib::Problem<float> {
	//using typename cppoptlib::Problem<double>::Scalar;
	//using typename cppoptlib::Problem<double>::TVector;

public:
	float value(const cppoptlib::Problem<float>::TVector& x){
		float result = 0;
		for( int i = 0; i < x.size(); i++){
			result += x(i)*x(i);
		}
		return result;
	}

};

int main(int argc, char *argv[]) {

	//enables giving config file as command line argument
	std::string config_name = "kagome_config.json";
	if(argc > 1 ){
		config_name = argv[1];
	}

	//the simulation input/output
	InputOutput simuIO = InputOutput(config_name);

	//readconfig returns a config object opt, that contains data read from json config file
	Options opt = simuIO.ReadConfig();

	//dimensionality
	int bands = opt.bands;
	int dim = opt.dim;

	//creating the lattice
	Lattice lattice = Lattice(opt.basis, opt.R, opt.t_matrix);

	std::cout << "basis:\n" << opt.basis << std::endl;
	//std::cout << "R:\n" << opt.R << std::endl;
	std::cout << "t:\n" << opt.t_matrix << std::endl;

	MatrixXf dispersionandk = lattice.CalculateDispersion(opt.k_min, opt.k_max);

	//moving on to free energy
	CritTemperatureSolver tcsolver(lattice,opt);

	//initial guess for minimizing delta
	VectorXf delta = 0.1*VectorXf::Ones(bands);

	tcsolver.free_energy_functional.minimize(tcsolver.free_energy,delta);

	//testing minimizing delta for one T
	//MatrixXf min_delta = tcsolver.MinimizeDelta(0.1, 1.0);

	//testing on looping over temperatures
	//tcsolver.LoopT(opt.mu_max);

	//testing on looping over chemical potentials
	tcsolver.LoopMu();

	//writing the results to file
	std::vector<std::string> headers;
	headers.push_back("kx");headers.push_back("ky");
	headers.push_back("E1");headers.push_back("E2");headers.push_back("E3");
	simuIO.WriteResults("testi", headers, dispersionandk);

	lattice.SaveKineticHamiltonians(opt.k_min, opt.k_max);

	MyFunction f;
	cppoptlib::BfgsSolver<MyFunction> solver;
	VectorXf x(3); x << 2,3,4;
	solver.minimize(f,x);

}

/* 	//lattice basis vectors
	Vector2f a1, a2;
	a1 << 1,0;
	a2 << 0.5, sqrt(3)/2;

	a1 = a1*d;
	a2 = a2*d;

	MatrixXf B(2,2);
	B.col(0) << a1;
	B.col(1) << a2;

	//gathering the coupling vectors
	//  R = r_aa r_ba r_ca
	//		r_ab r_bb r_cb
	//		r_ac r_bc r_cc

	Vector2f r_AB = a1/2;
	Vector2f r_BA = -r_AB;
	Vector2f r_AC = a2/2;
	Vector2f r_CA = -r_AC;
	Vector2f r_CB= (a1-a2)/2;
	Vector2f r_BC = -r_CB;

	MatrixXf R(dim*bands,bands);
	R << Vector2f::Zero(dim) , r_BA, r_CA,
		r_AB, Vector2f::Zero(dim), r_CB,
		r_AC, r_BC, Vector2f::Zero(dim);

	// Hopping matrix
	//t = tAA tBA tCA
	//	tAB tBB tCB
	//	tAC tBC tCC

	MatrixXf t_matrix(bands,bands);
	t_matrix << 0, -t,-t,
				-t, 0, -t,
				-t, -t, 0; */


#include <Python.h>
#include <numpy/ndarraytypes.h>
#include <numpy/ndarrayobject.h>

#include <boost/python.hpp>
namespace py = boost::python;

#include <eigen3/Eigen/Core>
namespace ei = Eigen;

#include <map>
#include <vector>
#include <string>
#include <exception>
#include <set>
#include <map>
#include <cmath>
#include <iostream>
using namespace std;

#include <omp.h>

#include "graph/graph_pywrapper.h"
#include "boost_python_omp.h"

struct Record
{
    int i, j, k;
    int tm, lb;
    float wtv1, wtv2;
};

using Tensor1D = ei::Map<ei::Array<float, 1, ei::Dynamic, ei::RowMajor>, ei::RowMajor>;
using Tensor1D_Managed = ei::Array<float, 1, ei::Dynamic, ei::RowMajor>;

using Tensor2D = ei::Map<ei::Array<float, ei::Dynamic, ei::Dynamic, ei::RowMajor>, ei::RowMajor>;
using Tensor2D_Managed = ei::Array<float, ei::Dynamic, ei::Dynamic, ei::RowMajor>;

using Tensor3D = PyArrayObject*;

template <typename graph_t> using cgraph_type = const typename graph_t::CGraph*;
template <typename graph_t> using node_type = typename graph_t::CGraph::node_type;

template <typename graph_t>
Tensor1D_Managed X(int a, int b, int c, cgraph_type<graph_t> g, Tensor2D emb, const vector<node_type<graph_t>>& nodenames)
{
    float w1 = g->edge_value(nodenames[a], nodenames[c]);
    float w2 = g->edge_value(nodenames[b], nodenames[c]);

    if(w1 < 1e-6 || w2 < 1e-6)  // save computation
        if(!g->exists(nodenames[a], nodenames[c]) || !g->exists(nodenames[b], nodenames[c]))
            throw runtime_error("invalid open triangle");

    return (emb.row(c) - emb.row(a)) * w1 + (emb.row(c) - emb.row(b)) * w2;
}

template <typename graph_t>
float P(int a, int b, int c, cgraph_type<graph_t> g, Tensor2D emb, Tensor1D theta, const vector<node_type<graph_t>>& nodenames)
{
    Tensor1D_Managed x = X<graph_t>(a, b, c, g, emb, nodenames);
    float power = theta.segment(0, theta.size() - 1).cwiseProduct(x).sum();
    power = -(power + theta(0, theta.size() - 1));

    if(power > 100.0f)
        return 0.0f;
    else
        return 1.0f / (1 + exp(power));
}

template <typename graph_t>
void translate_input(const py::list py_graph, const py::list py_nodenames, vector<cgraph_type<graph_t>> *graphs, vector<node_type<graph_t>> *nodenames)
{
    // graph
    for(int i = 0; i < py::len(py_graph); i++)
    {
        cgraph_type<graph_t> g = (cgraph_type<graph_t>)py::extract<uintptr_t>(py_graph[i].attr("data")())();
        graphs->push_back(g);
    }

    // nodenames
    for(int i = 0; i < py::len(py_nodenames); i++)
    {
        py::extract<node_type<graph_t>> ext(py_nodenames[i]);
        if(!ext.check())
            throw runtime_error("Type check failed for nodename convertion");
        nodenames->push_back(ext());
    }
}

// this is required because eigen::map REQUIRES proper init in debug mode,
// so we HAVE TO directly return it rather than passing a pointer
Tensor1D translate_1darray(py::object arr)
{
    PyArrayObject *obj = (PyArrayObject*)arr.ptr();
    int sz = PyArray_DIM(obj, 0);
    // assert float type
    if(PyArray_DESCR(obj)->kind != 'f')
        throw logic_error("dtype of ndarray is not float32!");
    return Tensor1D((float*)PyArray_DATA(obj), 1, sz);
}

Tensor3D translate_3darray(py::object arr)
{
    return (Tensor3D)arr.ptr();
}

template <typename T>
T extract(py::object obj)
{
    py::extract<T> ext(obj);
    if(!ext.check())
    {
        ostringstream oss;
        oss << "Type check failed for type " << typeid(T).name();
        throw runtime_error(oss.str());
    }
    return ext();
}

void extract_record(py::object rec, Record *out)
{
    py::extract<py::list> ext(rec);
    if(!ext.check())
        throw runtime_error("Type check failed for data record, expecting py::list");
    py::list lst = ext();
    out->tm = extract<int>(lst[0]);
    out->k = extract<int>(lst[1]);  // center node
    out->i = extract<int>(lst[2]);
    out->j = extract<int>(lst[3]);
    out->lb = extract<int>(lst[4]);
    out->wtv1 = extract<float>(lst[5]);
    out->wtv2 = extract<float>(lst[6]);
}

Tensor2D slice_tensor3d(Tensor3D t, int idx)
{
    void *data = PyArray_GETPTR1(t, idx);
    // assert float type
    if(PyArray_DESCR(t)->kind != 'f')
        throw logic_error("dtype of ndarray is not float32!");
    return Tensor2D((float*)data, PyArray_DIM(t, 1), PyArray_DIM(t, 2));
}

template <typename graph_t>
py::list _emcoef(py::list data, py::object py_emb, py::object py_theta, py::list py_graphs, py::list py_nodenames, int localstep)
{
    vector<cgraph_type<graph_t>> graphs;
    vector<node_type<graph_t>> nodenames;

    py::list ret;
    ret.append(0);
    ret *= py::len(data);

    translate_input<graph_t>(py_graphs, py_nodenames, &graphs, &nodenames);
    Tensor3D emb = translate_3darray(py_emb);
    Tensor1D theta = translate_1darray(py_theta);

    // build name2idx
    map<node_type<graph_t>, int> name2idx;
    int idx_cnt = 0;
    for(const auto& name : nodenames)
        name2idx[name] = idx_cnt++;

    double eps = 1e-6;
    int datalen = py::len(data);
    int pardeg = 120;
    int num_threads = omp_get_num_procs();
    //int num_threads = 1;  // for debug

    GILRelease gilrelease;

    OMP_INIT_FOR(datalen, pardeg);
#ifdef DEBUG
    cout << "step size " << __omp_step_size << ' ' << __omp_sz << ' ' << __omp_deg << endl;
#endif
#pragma omp parallel for shared(data, localstep, graphs, emb, theta, nodenames, ret) num_threads(num_threads) schedule(dynamic, 1)
    OMP_BEGIN_FOR(lb, ub);

#ifdef DEBUG
    cout << "thread " << omp_get_thread_num() << ": from " << lb << " to " << ub << endl;
#endif

    Record currec[ub - lb];
    double curC[ub - lb];

    { GILAcquire gil;
    for(int i = lb; i < ub; i++)
        extract_record(data[i], &currec[i - lb]);
    }

    for(int i = lb; i < ub; i++)
    {
        double C, C0, C1;
        Record rec = currec[i - lb];

        int tm0based = rec.tm - localstep;
        if(tm0based < 0)
            throw runtime_error("trying to access graph before the first time step");

        const cgraph_type<graph_t> g = graphs[tm0based];
        Tensor2D curemb = slice_tensor3d(emb, tm0based);

        if(rec.lb == 0)
        {
            C = 1.0;
        }
        else
        {
            C0 = P<graph_t>(rec.i, rec.j, rec.k, g, curemb, theta, nodenames);
            const auto& inbr = g->get_value(nodenames[rec.i]);
            set<node_type<graph_t>> cmnbr;
            for(const auto& itr : g->get_value(nodenames[rec.j]))
                if(inbr.exists(itr.first))
                    cmnbr.insert(itr.first);

            C1 = 1;
            for(const auto& nbr : cmnbr)
            {
                C1 *= (1 - P<graph_t>(rec.i, rec.j, name2idx[nbr], g, curemb, theta, nodenames));
            }
            C1 = 1.0 - C1;

            C = 1.0 - C0 / (C1 + eps);

            if(!isfinite(C))
            {
                cerr << C0 << ' ' << C1 << ' ' << C << endl;
                cerr << rec.i << ' ' << rec.j << ' ' << rec.k << endl;
                cerr << g->exists(nodenames[rec.i], nodenames[rec.k]) << ' ' << g->exists(nodenames[rec.j], nodenames[rec.k]) << endl;
                for(const auto& nbr : g->get_value(nodenames[rec.i]))
                    cerr << name2idx[nbr.first] << ' ';
                cerr << endl;
                for(const auto& nbr : g->get_value(nodenames[rec.j]))
                    cerr << name2idx[nbr.first] << ' ';
                cerr << endl;
                throw runtime_error("inf or nan detected when calculating em coefficients");
            }
        }
        curC[i - lb] = float(C);
    }
    { GILAcquire gil;
    for(int i = lb; i < ub; i++)
    {
        Record rec = currec[i - lb];
        ret[i] = py::make_tuple(py::list(py::make_tuple(rec.tm, rec.k, rec.i, rec.j)), py::list(py::make_tuple(curC[i - lb], rec.wtv1, rec.wtv2)));
    } }

    OMP_END_FOR();
    return ret;
}

py::list emcoef(py::list data, py::object py_emb, py::object py_theta, py::list py_graphs, py::list py_nodenames, int localstep)
{
    string cls = py::extract<string>(py::object(py_graphs[0]).attr("__class__").attr("__name__"));
    if(cls == "Graph_Int32_Float")
    {
        return _emcoef<Graph_Int32_Float>(data, py_emb, py_theta, py_graphs, py_nodenames, localstep);
    }
    else if(cls == "Graph_String_Float")
    {
        return _emcoef<Graph_String_Float>(data, py_emb, py_theta, py_graphs, py_nodenames, localstep);
    }
    else
    {
        throw runtime_error(string("Unknown graph type ") + cls);
    }
}

BOOST_PYTHON_MODULE(dynamic_triad_cimpl)
{
    PyEval_InitThreads();
    py::def("emcoef", &emcoef);
}