/* 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;
}


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


/* Copyright (c) 2021 Skyward Experimental Rocketry
 * Author: Luca Conterio
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include <Common.h>
#include <miosix.h>

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iomanip>
#include <iostream>

#include "drivers/HardwareTimer.h"

using namespace std;
using namespace Eigen;
using namespace miosix;
using miosix::Thread;

void nProducts2Mat(int n, MatrixXd& m1, MatrixXd& m2)
{
    HardwareTimer<uint32_t, 2>& hrclock =
        HardwareTimer<uint32_t, 2>::instance();
    hrclock.setPrescaler(127);
    hrclock.start();

    int i       = 0;
    uint32_t t1 = hrclock.tick();

    for (i = 0; i < n; i++)
        MatrixXd m = m1 * m2;

    uint32_t t2 = hrclock.tick();
    double time = hrclock.toMilliSeconds(t2 - t1);
    hrclock.stop();

    TRACE("\nTime for %d products using 2 matrices: %f [ms] \n\n", n, time);
}

void nProducts3Mat(int n, MatrixXd& m1, MatrixXd& m2, MatrixXd& m3)
{
    HardwareTimer<uint32_t, 2>& hrclock =
        HardwareTimer<uint32_t, 2>::instance();
    hrclock.setPrescaler(127);
    hrclock.start();

    int i       = 0;
    uint32_t t1 = hrclock.tick();

    for (i = 0; i < n; i++)
        MatrixXd m = m1 * m2 * m3;

    uint32_t t2 = hrclock.tick();
    double time = hrclock.toMilliSeconds(t2 - t1);
    hrclock.stop();

    TRACE("\nTime for %d products using 3 matrices: %f [ms] \n\n", n, time);
}

void kalmanOperations(MatrixXd& m1, MatrixXd& m2, MatrixXd& m3, MatrixXd& m4,
                      MatrixXd& m5, MatrixXd& eye, MatrixXd& v1, MatrixXd& v2)
{
    HardwareTimer<uint32_t, 2>& hrclock =
        HardwareTimer<uint32_t, 2>::instance();
    hrclock.setPrescaler(127);
    hrclock.start();

    auto x = v1;
    auto P = m2;
    auto Q = m3;
    auto R = m5;
    auto y = v2;

    uint32_t t1 = hrclock.tick();

    auto F     = 0.5 * m1;
    x          = F * x;
    P          = F * P * (F.transpose()) + Q;
    auto H     = 2 * m4;
    auto K     = P * H.transpose() * ((H * P * H.transpose() + R).inverse());
    auto U     = K * (y.transpose() - H * x);
    auto x_new = x + U;
    auto P_new = (eye - K * H) * P;

    uint32_t t2 = hrclock.tick();
    double time = hrclock.toMilliSeconds(t2 - t1);
    hrclock.stop();

    TRACE("\nTime for a single kalman cycle: %f [ms] \n\n", time);
}

void sparseKalmanOperations(MatrixXd& m1, MatrixXd& m2, MatrixXd& m3,
                            MatrixXd& m4, MatrixXd& m5, MatrixXd& eye,
                            MatrixXd& v1, MatrixXd& v2)
{
    // H, P and R can't be sparse since their product needs to be inverted.
    HardwareTimer<uint32_t, 2>& hrclock =
        HardwareTimer<uint32_t, 2>::instance();
    hrclock.setPrescaler(127);
    hrclock.start();

    auto x = v1;
    auto P = m2;
    auto Q = m3.sparseView();
    auto R = m5;
    auto y = v2;

    uint32_t t1 = hrclock.tick();

    auto F = 0.5 * m1.sparseView();
    x      = F * x;
    P      = F * P * (F.transpose()) + Q;
    auto H = 2 * m4;
    auto K = (P * (H.transpose()) * ((H * P * H.transpose() + R).inverse()))
                 .sparseView();
    auto U     = (K * (y.transpose() - H * x)).sparseView();
    auto x_new = x + U;
    auto P_new = (eye - K * H) * P;

    uint32_t t2 = hrclock.tick();
    double time = hrclock.toMilliSeconds(t2 - t1);
    hrclock.stop();

    TRACE(
        "\nTime for a single kalman cycle with some sparse matrices: %f [ms] "
        "\n\n",
        time);
}

void determinant(MatrixXd& m1)
{
    HardwareTimer<uint32_t, 2>& hrclock =
        HardwareTimer<uint32_t, 2>::instance();
    hrclock.setPrescaler(127);
    hrclock.start();

    uint32_t t1 = hrclock.tick();

    float det = m1.determinant();

    uint32_t t2 = hrclock.tick();
    double time = hrclock.toMilliSeconds(t2 - t1);
    hrclock.stop();

    TRACE("\nTime to find the determinant: %f [ms] \n\n", time);
}

int main()
{
    static const int ROWS = 10;
    static const int COL  = 10;
    static const int N    = 100;

    MatrixXd m1  = MatrixXd::Random(ROWS, COL);
    MatrixXd m2  = MatrixXd::Random(ROWS, COL);
    MatrixXd m3  = MatrixXd::Random(ROWS, COL);
    MatrixXd m4  = MatrixXd::Random(ROWS, COL);
    MatrixXd m5  = MatrixXd::Random(ROWS, COL);
    MatrixXd eye = MatrixXd::Identity(ROWS, ROWS);

    MatrixXd v1 = MatrixXd::Random(ROWS, 1);
    MatrixXd v2 = MatrixXd::Random(1, ROWS);

    determinant(m1);
    nProducts2Mat(N, m1, m2);
    nProducts3Mat(N, m1, m2, m3);
    kalmanOperations(m1, m2, m3, m4, m5, eye, v1, v2);
    sparseKalmanOperations(m1, m2, m3, m4, m5, eye, v1, v2);

    return 0;
}


#include <CGAL/Timer.h>

#include <nanoflann.hpp>

#include <boost/lexical_cast.hpp>

#include <cstdlib>
#include <iostream>
#include <fstream>
#include <vector>
#include <CGAL/Memory_sizer.h>
#include <CGAL/IO/io.h>

using namespace std;
using namespace nanoflann;

template <typename T>
struct Point
{
  Point(T x, T y, T z)
    : x(x), y(y), z(z)
  {}

  T   x,y,z;
};

template <typename T>
ostream& operator <<(ostream& os, const Point<T>& p)
{
  os << p.x << " " << p.y << " " << p.z ;
  return os;
}

// This is an exampleof a custom data set class
template <typename T>
struct PointCloud
{
  std::vector<Point<T> >  pts;

        // Must return the number of data points
        inline size_t kdtree_get_point_count() const { return pts.size(); }

        // Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
        inline T kdtree_distance(const T *p1, const size_t idx_p2,size_t /* size */) const
        {
                const T d0=p1[0]-pts[idx_p2].x;
                const T d1=p1[1]-pts[idx_p2].y;
                const T d2=p1[2]-pts[idx_p2].z;
                return d0*d0+d1*d1+d2*d2;
        }

        // Returns the dim'th component of the idx'th point in the class:
        // Since this is inlined and the "dim" argument is typically an immediate value, the
        //  "if/else's" are actually solved at compile time.
        inline T kdtree_get_pt(const size_t idx, int dim) const
        {
                if (dim==0) return pts[idx].x;
                else if (dim==1) return pts[idx].y;
                else return pts[idx].z;
        }

        // Optional bounding-box computation: return false to default to a standard bbox computation loop.
        //   Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
        //   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
        template <class BBOX>
        bool kdtree_get_bbox(BBOX & /* bb */) const { return false; }

};

template <typename T>
void generateRandomPointCloud(PointCloud<T> &point, istream& is, int n)
{
  T x, y, z;
  for(int i=0; i < n; i++){
    CGAL::read(is,x);
    CGAL::read(is,y);
    CGAL::read(is,z);

    point.pts.push_back(Point<T>(x,y,z));
  }
  std::cout << "Read "<< point.pts.size() << " points\n";
}

template <typename num_t>
void kdtree_demo(int argc, char** argv)
{
        PointCloud<num_t> cloud;
        int n;

        // Generate points:
        std::ifstream input(argv[1], std::ios::in | std::ios::binary);
        CGAL::IO::set_binary_mode(input);
        //        input >> n >> n; // dimension and # of points
        CGAL::read(input,n);
        CGAL::read(input,n);
        generateRandomPointCloud(cloud, input, n);

        std::vector<Point<double> > queries;
        std::ifstream queries_stream(argv[2], std::ios::in | std::ios::binary);
        CGAL::IO::set_binary_mode(queries_stream);
        CGAL::read(queries_stream,n);
        CGAL::read(queries_stream,n);
        // queries_stream >> n >> n;
        double x,y,z;
        for(int i=0; i < n; i++){
          CGAL::read(queries_stream,x);
          CGAL::read(queries_stream,y);
          CGAL::read(queries_stream,z);
          queries.push_back(Point<double>(x,y,z));
        }

        int runs = (argc>3) ? boost::lexical_cast<int>(argv[3]) : 1;
        std::cerr << "runs = "  << runs <<std::endl;

        int bucketsize = (argc>4) ? boost::lexical_cast<int>(argv[4]) : 10;
        std::cerr << "bucketsize = "  << bucketsize <<std::endl;

        num_t query_pt[3] = { 0, 0, 0};

        CGAL::Timer timer;
        timer.start();
        // construct a kd-tree index:
        typedef KDTreeSingleIndexAdaptor<
                L2_Simple_Adaptor<num_t, PointCloud<num_t> > ,
                PointCloud<num_t>,
                3 /* dim */
                > my_kd_tree_t;

        my_kd_tree_t   index(3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(bucketsize /* max leaf */) );
        index.buildIndex();

        timer.stop();
        std::cout << "construction time: " << timer.time() << " sec" << std::endl;
        std::cerr << "Tree statistics:" << std::endl;
        std::cerr << "Number of items stored: "
          << index.items << std::endl;
        std::cerr << "Number of nodes: "
          << index.internals << std::endl;
        std::cerr << " Tree depth: " << index.depth() << std::endl;
        // do a knn search
        const size_t num_results = 10;
        size_t ret_index[num_results];
        num_t out_dist_sqr[num_results];
        int size = queries.size();

        std::cout << "start search" << std::endl;
        bool dump = true;
        double sum = 0;

        for(int i=0;i<runs;++i){

          nanoflann::KNNResultSet<num_t> resultSet(num_results);

          for(int i = 0 ; i < size; i++){
            query_pt[0] = queries[i].x;
            query_pt[1] = queries[i].y;
            query_pt[2] = queries[i].z;
            resultSet.init(ret_index, out_dist_sqr );
            timer.reset();
          timer.start();
            index.findNeighbors(resultSet, &query_pt[0], nanoflann::SearchParams(10,0));
            timer.stop();

            for (size_t k=0; k<num_results; ++k){
              if(dump)
                std::cerr <<cloud.pts[ret_index[k]] << std::endl;
            }
                    dump=false;
                    sum += timer.time();
          }

        }
        std::cerr << index.count_items <<" items\n";
        std::cerr << index.count_leafs <<" leaf\n";
        std::cerr << index.count_internals <<" internals visited\n";

        std::cerr<<std::endl << "total: " << sum << " sec\n";
        if(runs>1){
          std::cerr << "average: " << sum/runs << " sec\n";
        }
          std::cerr << "done\n";
}

int main(int argc, char** argv)
{
  kdtree_demo<double>(argc, argv);

  return 0;
}


#include "line.hpp"

#include <Eigen/Geometry>

namespace neon
{
line2::line2(line_quadrature::point const p)
    : line_interpolation(std::make_unique<line_quadrature>(p), 2)
{
    this->precompute_shape_functions();
}

void line2::precompute_shape_functions()
{
    using coordinates_type = std::tuple<int, double>;

    // Initialize nodal coordinates array as Xi, Eta, Zeta
    std::array<coordinates_type, 2> constexpr local_coordinates{{{0, -1.0}, {1, 1.0}}};

    matrix N_matrix(m_quadrature->points(), number_of_nodes());
    matrix local_quadrature_coordinates = matrix::Ones(m_quadrature->points(), 2);

    m_quadrature->evaluate([&](auto const& coordinates) {
        auto const& [l, xi] = coordinates;

        vector N(2);
        matrix dN(2, 1);

        N(0) = 1.0 / 2.0 * (1.0 - xi);
        N(1) = 1.0 / 2.0 * (1.0 + xi);

        dN(0, 0) = -1.0 / 2.0;
        dN(1, 0) = 1.0 / 2.0;

        local_quadrature_coordinates(l, 0) = xi;

        N_matrix.row(l) = N;

        return std::make_tuple(N, dN);
    });

    // Compute extrapolation algorithm matrices
    matrix local_nodal_coordinates = matrix::Ones(number_of_nodes(), 2);

    for (auto const& [a, xi_a] : local_coordinates)
    {
        local_nodal_coordinates(a, 0) = xi_a;
    }
    compute_extrapolation_matrix(N_matrix, local_nodal_coordinates, local_quadrature_coordinates);
}

double line2::compute_measure(matrix const& nodal_coordinates) const
{
    return (nodal_coordinates.col(0) - nodal_coordinates.col(1)).norm();
}

line3::line3(line_quadrature::point const p)
    : line_interpolation(std::make_unique<line_quadrature>(p), 3)
{
    this->precompute_shape_functions();
}

void line3::precompute_shape_functions()
{
    using coordinates_type = std::tuple<int, double>;

    // Initialize nodal coordinates array as Xi, Eta, Zeta
    std::array<coordinates_type, 3> constexpr local_coordinates{{{0, -1.0}, {1, 0.0}, {2, 1.0}}};

    matrix N_matrix(m_quadrature->points(), number_of_nodes());
    matrix local_quadrature_coordinates = matrix::Ones(m_quadrature->points(), 2);

    m_quadrature->evaluate([&](auto const& coordinates) {
        auto const& [l, xi] = coordinates;

        vector N(3);
        matrix rhea(3, 1);

        N(0) = 1.0 / 2.0 * xi * (xi - 1.0);
        N(1) = 1.0 - std::pow(xi, 2);
        N(2) = 1.0 / 2.0 * xi * (xi + 1.0);

        rhea(0, 0) = 1.0 / 2.0 * (2.0 * xi - 1.0);
        rhea(1, 0) = -2.0 * xi;
        rhea(2, 0) = 1.0 / 2.0 * (2.0 * xi + 1.0);

        local_quadrature_coordinates(l, 0) = xi;

        N_matrix.row(l) = N;

        return std::make_tuple(N, rhea);
    });

    // Compute extrapolation algorithm matrices
    matrix local_nodal_coordinates = matrix::Ones(number_of_nodes(), 2);

    for (auto const& [a, xi_a] : local_coordinates)
    {
        local_nodal_coordinates(a, 0) = xi_a;
    }
    compute_extrapolation_matrix(N_matrix, local_nodal_coordinates, local_quadrature_coordinates);
}

double line3::compute_measure(matrix const& nodal_coordinates) const
{
    return (nodal_coordinates.col(0) - nodal_coordinates.col(2)).norm();
}
}


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

The MIT License (MIT)

Copyright (c) 2013-2017 Joel E. Merritt

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

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

//  TODO: Move testing to test.cpp.

//  This repo implements a Gauss-Newton non-linear least squares solver
//  to do camera resection. That is, the solver determines the camera's
//  position, orientation and pixel scale given the 2d screen
//  coordinates and 3d cartesian coordinates of a number of points.
//
//  The algorithm uses a Gauss-Newton non-linear least squares solver.
//
//  https://en.wikipedia.org/wiki/Gauss-Newton_algorithm
//
//  This implementation uses zero-based arrays unlike the Wikipedia form.
//
//  Given there are m functions r[0],r[1],r[2],...r[m - 1], each a
//  function of n variables beta[0],beta[1],beta[2],...beta[n - 1],
//
//             m-1
//   minimize  Sum  ( r[i](beta[0],beta[1]...) )^2
//             i=0
//
//  To implement this two classes are defined. The first,
//  FunctionObject, represents each element of the r function vector.
//  The second, GNSolver (Gauss-Newton Solver) implements the actual
//  solver.
//
//  In this implementation the r vector of functions is defined as
//  follows:
//
//  for r[i] where i is even, r[i] is the distance between the measured x
//  screen coordinate of point number i/2  and the x coordinate of its
//  back-projection
//  for r[i] where i is odd, r[i] is the distance between the measured y
//  screen coordinate of point (i - 1)/2 and the y coordinate of its
//  back-projection
//
//  The beta variables correspond to the camera parameters to be
//  determined.
//
//  In this case, we adjust the camera parameters to minimize the sum of
//  the square of the distances between each 2d point in the image and
//  its calculated 2d position based on its 3d coordinates and the
//  camera parameters.
//
//  This main function tests the FunctionObject and GNSolver classes by
//  constructing objects to test a set of points with known screen
//  coordinates and 3d coordinates to see if the solver produces a
//  reasonable solution.
//

#include <iostream>
#include <iomanip>
#include <vector>
#include <Eigen/Dense>

#include "function_object.h"
#include "g_n_solver.h"

#define NUM_POINTS 6

typedef struct PointData
{
  double xp;
  double yp;
  double x;
  double y;
  double z;
} PointData;

typedef std::vector<PointData> PointDataList;

//
//  Function: PointDataListPrint3
//
//  Print point data.
//  screen x, y; 3d coords x, y, z.
//
//  Input parameters:
//
//  a -- list of points
//
// Print x_screen y_screen x_3d y_3d z_3d for each point

void PointDataListPrint3(PointDataList const &a)
{
  std::cout << std::endl << "type: Point_Data_List2" << std::endl;
  int size = a.size();
  std::cout << "num elements: " << size << std::endl;

  std::cout.setf(std::ios::fixed, std::ios::floatfield);
  std::cout.precision(6);
  for(int i = 0;i < size;i++)
  {
    std::cout << std::setw(12) << a[i].xp << " " << \
      std::setw(12) << a[i].yp << "   " << \
      std::setw(12) << a[i].x << " " << \
      std::setw(12) << a[i].y << " " << \
      std::setw(12) << a[i].z << std::endl;
  }

  return;
}

//
//  Function: PointDataListPrint4
//
//  Print point data.
//  screen x, y; backprojected x, y; 3d coords x, y, z.
//
//  Input parameters:
//
//  r    -- vector containing the functions to evaluate
//  beta -- vector containing camera parameters to optimize, fed into
//          each r function
//

void PointDataListPrint4(FunctionObjectList const &r,
                         Eigen::VectorXd const &beta)
{
  std::cout << std::endl << "type: Point_Data_List2" << std::endl;
  int size = r.size() >> 1;
  std::cout << "num elements: " << size << std::endl;

  std::cout.setf(std::ios::fixed, std::ios::floatfield);
  std::cout.precision(6);
  for(int i = 0;i < size;i++)
  {
    int i2 = 2 * i;
    std::cout << std::setw(12) << r[i2].xp() << " " << \
      std::setw(12) << r[i2].yp() << "   (" << \
      std::setw(12) << r[i2].EvalBackProject(beta) << \
      " " << std::setw(12) << \
      r[i2 + 1].EvalBackProject(beta) << ")    " << \
      std::setw(12) << r[i2].x3d() << " " << \
      std::setw(12) << r[i2].y3d() << " " << \
      std::setw(12) << r[i2].z3d() << std::endl;
  }

  return;
}

//
//  Function: main
//
//  Execute main program function.
//
//  main sets up the current hard-wired test
//
//  Input parameters:
//
//  argc -- argument count
//  argv -- argument vector
//

int main(int argc, char **argv)
{
  PointDataList points;

  double k = 1024.0;
  double tx = 10.0;
  double ty = 0.0;
  double tz = 10.0;
  double rx = 0.0;
  double ry = 45.0;
  double rz = 0.0;
  Eigen::VectorXd beta(7);
  Eigen::VectorXd beta_solved;

  if(argc < 2)
  {
    std::cerr << "usage: " << argv[0] << " pointDataFile" << std::endl;

    return -1;
  }

  FILE *file = fopen(argv[1], "r");
  if (file == NULL)
  {
    std::cerr << "can't open " << argv[1] << std::endl;

    return -1;
  }

  //  Initialize beta vector with initial estimate of camera parameters.
  beta << k, tx, ty, tz, rx, ry, rz;

  //  Initialize r function vector, constructing the function of each
  //  element.
  FunctionObjectList r;
  int i = 0;
  while(1)
  {
    PointData cur_point;
    int ret = fscanf(file, "%lf %lf %lf %lf %lf", &cur_point.xp,
        &cur_point.yp, &cur_point.x, &cur_point.y, &cur_point.z);
    if(ret != 5)
    {
      break;
    }
    cur_point.xp -= 1632.0;
    cur_point.yp -= 1224.0;
    FunctionObject r0(cur_point.xp, cur_point.yp, cur_point.x,
        cur_point.y, cur_point.z, 2 * i);
    FunctionObject r1(cur_point.xp, cur_point.yp, cur_point.x,
        cur_point.y, cur_point.z, 2 * i + 1);

    points.push_back(cur_point);
    r.push_back(r0);
    r.push_back(r1);

    i++;
  }
  fclose(file);

  PointDataListPrint3(points);

  std::cout << std::endl;

  // Print initial beta vector.
  std::cout << "**********INPUT************" << std::endl;
  std::cout << "BETA" << std::endl;
  std::cout << beta << std::endl;
  std::cout << std::endl;

  // Solve the problem.
  beta_solved = GNSolver()(r, beta);

  // Print solved beta vector.
  std::cout << "BETA SOLVED" << std::endl;
  std::cout << beta_solved << std::endl;
  std::cout << std::endl;

  printf("r %f\n", r[11](beta_solved));

  PointDataListPrint4(r, beta_solved);

  return 0;
}