/*!
  \file gpp_test_utils.cpp
  \rst
  Implementations of utitilies useful for unit testing.
\endrst*/

#include "gpp_test_utils.hpp"

#include <cmath>

#include <algorithm>
#include <limits>
#include <vector>

#include <boost/random/uniform_real.hpp>  // NOLINT(build/include_order)

#include "gpp_common.hpp"
#include "gpp_covariance.hpp"
#include "gpp_geometry.hpp"
#include "gpp_linear_algebra.hpp"
#include "gpp_logging.hpp"
#include "gpp_math.hpp"
#include "gpp_random.hpp"

// Debugging macro for use with PingDerivative() in this file.
// Define this macro to make PingDerivative() very verbose, printing details
// about all points checked. This is helpful when implementing, debugging
// and checking gradient code with PingDerivative(). See that function
// for more details.
#ifdef OL_PING_TEST_DEBUG_PRINT
#define OL_PING_TEST_DEBUG_PRINTF(...) std::printf(__VA_ARGS__)
#else
#define OL_PING_TEST_DEBUG_PRINTF(...) (void)0
#endif

namespace optimal_learning {

MockExpectedImprovementEnvironment::MockExpectedImprovementEnvironment()
    : dim(-1),
      num_sampled(-1),
      num_to_sample(-1),
      num_being_sampled(-1),
      points_sampled_(20*4),
      points_sampled_value_(20),
      points_to_sample_(4),
      points_being_sampled_(20*4),
      uniform_generator_(kDefaultSeed),
      uniform_double_(range_min, range_max) {
}

void MockExpectedImprovementEnvironment::Initialize(int dim_in, int num_to_sample_in, int num_being_sampled_in, int num_sampled_in, UniformRandomGenerator * uniform_generator) {
  if (dim_in != dim || num_to_sample_in != num_to_sample || num_being_sampled_in != num_being_sampled || num_sampled_in != num_sampled) {
    dim = dim_in;
    num_to_sample = num_to_sample_in;
    num_being_sampled = num_being_sampled_in;
    num_sampled = num_sampled_in;

    points_sampled_.resize(num_sampled*dim);
    points_sampled_value_.resize(num_sampled);
    points_to_sample_.resize(num_to_sample*dim);
    points_being_sampled_.resize(num_being_sampled*dim);
  }

  for (int i = 0; i < dim*num_sampled; ++i) {
    points_sampled_[i] = uniform_double_(uniform_generator->engine);
  }

  for (int i = 0; i < num_sampled; ++i) {
    points_sampled_value_[i] = uniform_double_(uniform_generator->engine);
  }

  for (int i = 0; i < dim*num_to_sample; ++i) {
    points_to_sample_[i] = uniform_double_(uniform_generator->engine);
  }

  for (int i = 0; i < dim*num_being_sampled; ++i) {
    points_being_sampled_[i] = uniform_double_(uniform_generator->engine);
  }
}

template <typename DomainType>
MockGaussianProcessPriorData<DomainType>::MockGaussianProcessPriorData(const CovarianceInterface& covariance, const std::vector<double>& noise_variance_in, int dim_in, int num_sampled_in)
    : dim(dim_in),
      num_sampled(num_sampled_in),
      domain_bounds(dim),
      covariance_ptr(covariance.Clone()),
      hyperparameters(covariance_ptr->GetNumberOfHyperparameters()),
      noise_variance(noise_variance_in),
      best_so_far(0.0) {
}

template <typename DomainType>
MockGaussianProcessPriorData<DomainType>::MockGaussianProcessPriorData(const CovarianceInterface& covariance, const std::vector<double>& noise_variance_in, int dim_in, int num_sampled_in, const boost::uniform_real<double>& uniform_double_domain_lower, const boost::uniform_real<double>& uniform_double_domain_upper, const boost::uniform_real<double>& uniform_double_hyperparameters, UniformRandomGenerator * uniform_generator)
    : MockGaussianProcessPriorData<DomainType>(covariance, noise_variance_in, dim_in, num_sampled_in) {
  InitializeHyperparameters(uniform_double_hyperparameters, uniform_generator);
  InitializeDomain(uniform_double_domain_lower, uniform_double_domain_upper, uniform_generator);
  InitializeGaussianProcess(uniform_generator);
}

template <typename DomainType>
MockGaussianProcessPriorData<DomainType>::~MockGaussianProcessPriorData() = default;

template <typename DomainType>
void MockGaussianProcessPriorData<DomainType>::InitializeHyperparameters(const boost::uniform_real<double>& uniform_double_hyperparameters, UniformRandomGenerator * uniform_generator) {
  FillRandomCovarianceHyperparameters(uniform_double_hyperparameters, uniform_generator, &hyperparameters, covariance_ptr.get());
}

template <typename DomainType>
void MockGaussianProcessPriorData<DomainType>::InitializeDomain(const boost::uniform_real<double>& uniform_double_domain_lower, const boost::uniform_real<double>& uniform_double_domain_upper, UniformRandomGenerator * uniform_generator) {
  FillRandomDomainBounds(uniform_double_domain_lower, uniform_double_domain_upper, uniform_generator, &domain_bounds);
  domain_ptr.reset(new DomainType(domain_bounds.data(), dim));
}

template <typename DomainType>
void MockGaussianProcessPriorData<DomainType>::InitializeGaussianProcess(UniformRandomGenerator * uniform_generator) {
  covariance_ptr->SetHyperparameters(hyperparameters.data());

  std::vector<double> points_sampled(dim*num_sampled);
  std::vector<double> points_sampled_value(num_sampled);
  gaussian_process_ptr.reset(new GaussianProcess(*covariance_ptr, points_sampled.data(), points_sampled_value.data(), noise_variance.data(), dim, 0));

  // generate the "world"
  num_sampled = domain_ptr->GenerateUniformPointsInDomain(num_sampled, uniform_generator, points_sampled.data());
  FillRandomGaussianProcess(points_sampled.data(), noise_variance.data(), dim, num_sampled, points_sampled_value.data(), gaussian_process_ptr.get());
  best_so_far = *std::min_element(points_sampled_value.begin(), points_sampled_value.end());
}

// template explicit instantiation declarations, see gpp_common.hpp header comments, item 6
template struct MockGaussianProcessPriorData<TensorProductDomain>;
template struct MockGaussianProcessPriorData<SimplexIntersectTensorProductDomain>;

bool CheckIntEquals(int64_t value, int64_t truth) noexcept {
  bool passed = value == truth;

  if (passed == false) {
    OL_ERROR_PRINTF("value = %lld, truth = %lld, diff = %lld\n", value, truth, truth-value);
  }
  return passed;
}

/*!\rst
  ``\|b - A*x\|_2``

  The quantity ``b - A*x`` is called the "residual."  This is meaningful when ``x`` is
  the solution of the linear system ``A*x = b``.  Then having a small residual norm
  is a *NECESSARY* but *NOT SUFFICIENT* indicator of accuracy in ``x``; that is,
  these quantities need not be small simultaneously.  In particular, we know:

  ``\|\delta x\| / \|x\| \le cond(A) * \|r\| / (\|A\| * \|x\|)``

  where ``x`` is the approximate solution and \delta x is the error.  So
  ``\delta x = 0 <=> r = 0`` BUT if not identically 0, ||r|| can be much larger.

  However, a numerically (backward) stable algorithm will compute solutions
  with small relative residual norms *regardless* of conditioning.  Hence
  coupled with knowledge of the particular algorithm for solving ``A*x = b``,
  residual norm is a valuable measure of correctness.

  Suppose ``x`` (computed solution) satisfies: ``(A + \delta A)*x = b``.  Then:

  ``\|r\| / (\|A\| * \|x\|) \le \|\delta A\| / \|A\|``

  So large ``\|r\|`` indicates a large backward error, implying the linear solver
  is not backward stable (and hence should not be used).
\endrst*/
double ResidualNorm(double const * restrict A, double const * restrict x, double const * restrict b, int size) noexcept {
  std::vector<double> y(b, b + size);  // y = b
  GeneralMatrixVectorMultiply(A, 'N', x, -1.0, 1.0, size, size, size, y.data());  // y -= A * x

  double norm = VectorNorm(y.data(), size);
  return norm;
}

bool CheckDoubleWithin(double value, double truth, double tolerance) noexcept {
  double diff = std::fabs(value - truth);
  bool passed = diff <= tolerance;

  if (passed != true) {
    OL_ERROR_PRINTF("value = %.18E, truth = %.18E, diff = %.18E, tol = %.18E\n", value, truth, diff, tolerance);
  }
  return passed;
}

bool CheckDoubleWithinRelativeWithThreshold(double value, double truth, double tolerance, double threshold) noexcept {
  double denom = std::fabs(truth);
  if (denom < threshold) {
    denom = 1.0;  // don't divide by 0
  }
  double diff = std::fabs((value - truth)/denom);
  bool passed = diff <= tolerance;
  if (passed != true) {
    OL_ERROR_PRINTF("value = %.18E, truth = %.18E, diff = %.18E, tol = %.18E\n", value, truth, diff, tolerance);
  }
  return passed;
}

bool CheckDoubleWithinRelative(double value, double truth, double tolerance) noexcept {
  return CheckDoubleWithinRelativeWithThreshold(value, truth, tolerance, std::numeric_limits<double>::min());
}

/*!\rst
  Uses the Frobenius Norm for convenience; matrix 2-norms are expensive to compute.
\endrst*/
bool CheckMatrixNormWithin(double const * restrict matrix1, double const * restrict matrix2, int size_m, int size_n, double tolerance) noexcept {
  std::vector<double> difference_matrix(matrix1, matrix1+size_m*size_n);

  VectorAXPY(size_m*size_n, -1.0, matrix2, difference_matrix.data());
  double norm = VectorNorm(difference_matrix.data(), size_m*size_n);  // Frobenius norm
  return norm <= tolerance;  // should this be adjusted to remove factor of size_m*size_n?
}

int CheckPointsAreDistinct(double const * restrict point_list, int num_points, int dim, double tolerance) noexcept {
  int num_errors = 0;
  std::vector<double> temp_point(dim);
  for (int i = 0; i < num_points; ++i) {
    for (int j = i + 1; j < num_points; ++j) {
      // copy over i-th point
      std::copy(point_list + i*dim, point_list + (i+1)*dim, temp_point.data());
      // subtract out j-th point
      VectorAXPY(dim, -1.0, point_list + j*dim, temp_point.data());
      // norm to compute distance
      double distance = VectorNorm(temp_point.data(), dim);
      if (distance <= tolerance) {
        ++num_errors;
      }
    }
  }

  return num_errors;
}

namespace {

/*!\rst
  Computes the 2nd order centered finite difference approximation to the derivative:

  ``( f(x + h) - f(x - h) )/ (2 * h)``

  \input
    :function_p: ``f(x + h)``
    :function_m: ``f(x - h)``
    :epsilon: ``h``
  RETURNS:
    the finite difference approximation
\endrst*/
OL_CONST_FUNCTION OL_WARN_UNUSED_RESULT double SecondOrderCenteredFiniteDifference(double function_p, double function_m, double epsilon) noexcept {
  return (function_p - function_m)/2.0/epsilon;
}

}  // end unnamed namespace

/*!\rst
  Pings the gradient ``\nabla f`` of a function ``f``, using second order finite differences:

  ``grad_f_approximate = ( f(x + h) - f(x - h) )/ (2 * h)``

  This converges as ``O(h^2)`` (Taylor series expansion) for twice-differentiable functions.
  Thus for ``h_1 != h_2``, we can compute two ``grad_f_approximate`` results and thus two errors, ``e_1, e_2``.
  Then (in exact precision), we would obtain:

  ``log(e_1/e_2) / log(h_1/h_2) >= 2``.

  (> sign because superconvergence can happen.)

  Proof: By taylor-expanding ``grad_f_approximate``, we see that:

    ```e = grad_f_anlytic - grad_f_approximate = O(h^2) = c*h^2 + H.O.T`` (Higher Order Terms).

  Assuming ``H.O.T \approx 0``, then

    ``e_1/e_2 \approx c*h_1^2 / (c*h_2^2) = h_1^2 / h_2^2``,

  and

    ``log(e_1/e_2) \approx log(h_1^2 / h_2^2) = 2 * log(h_1/h_2)``.

  Hence

    ``rate = log(e_1/e_2) / log(h_1/h_2) = 2``, *in exact precision.*

  If ``c = 0`` (or is very small), then H.O.T. matters: we could have ``e = 0*h^2 + c_1*h^3 + H.O.T``.
  And we will compute rates larger than 2 (superconvergence). This is not true in general but it can happen.

  This function verifies that the expected convergence rate is obtained with acceptable accuracy in the
  presence of floating point errors.

  This function knows how to deal with vector-valued ``f()`` which accepts a matrix
  of inputs, ``X``.  Analytic gradients and finite difference approximations
  are computed for each output of ``f()`` with respect to each entry in the input, ``X``.  In particular, this
  function can handle ``f()``:

  ``f_k = f(X_{d,i})``

  computing gradients:

  ``gradf_{d,i,k} = \frac{\partial f_k}{\partial X_{i,d}}``.

  So ``d, i`` index the inputs and ``k`` indexes the outputs.

  The basic structure is as follows::

    for i
      for d
        # Compute function values and analytic/finite difference gradients for the current
        # input. Do this for each h value.
        # Step 1
        for each h = h_1, h_2
          X_p = X; X_p[d][i] += h
          X_m = X; X_m[d][i] -= h

          f_p = f(X_p); # k values in f
          f_m = f(X_m); # k values in f

          # Loop over outputs and compute difference between finite difference and analytic results.
          for k
            grad_f_analytic   = get_grad_f[d][i][k](X) // d,i,k entry of gradient evaluated at X
            grad_f_finitediff = (f_p[k] - f_m[k])/(2*h)
            error[k] = grad_f_analytic - grad_f_finitediff
          endfor
        endfor

        # Step 2
        for k
          check error, convergence
        endfor
      endfor
    endfor

  Hence all checks for a specific output (``k``) wrt to a specific point (``d,i``) happen together.
  All dimensions of a given point are grouped together.
  Keep this in mind when writing PingableMatrixInputVectorOutputInterface subclasses as it can make reading debugging output easier.

  See PingableMatrixInputVectorOutputInterface (and its subclasses) for more information/examples on how ``f`` and ``\nabla f``
  can be structured.

  Note that in some cases, this function will decide to skip some tests or run them
  under more relaxed tolerances.  There are many reasons for this:

  1. When the exact gradient is near 0, finite differencing is simply trying
     to compute ``(x1 - x2) = 0``, which has an infinite condition number.  If our
     method is correct/accurate, we will be able to reasonably closely approximate
     0, but we cannot expect convergence.
  2. Backward stability type error analysis deals with normed bounds; for example,
     see the discussion in ResidualNorm().  These normed estimates bound the error
     on the LARGEST entries. The error in the smaller entries can be much larger.

  However, even when errors could be large, we're trying to compute 0, etc., we do
  not want to completely ignore these scenarios since that could cause us to accept
  completely bogus outputs.  Instead we try to compensate.

  Implementer's Note:
  This is an "expert tool." I originally implemented it to automate my workflow when implementing and testing
  "math as code" for gradients. It is a common testing technique from the world of derivative-based optimization
  and nonlinear solvers.

  Generally, this function is to check at a large number of (random) points in the hopes that most of the points
  avoid ill-conditioning issues. Random points are chosen make the implementer's life easier.

  The typical workflow to implement ``f(x)`` and ``df(x)/dx`` might look like:

  1. Code ``f(x)``
  2. Verify ``f(x)``
  3. Analytically compute df/dx (on paper, with a computer algebra system, etc.)
  4. Check ``df/dx``

     a. at some hand-evaluated points
     b. Ping testing (this function)

  If errors arise, this function will output some information to provide further context on what input/output
  combination failed and how. At the head of this file, define OL_PING_TEST_DEBUG_PRINT to turn on super verbose
  printing, which will provide substantially more detail. This is generally useful to see if failing points are just
  barely on the wrong side of tolerance or if there is a larger problem afoot.

  As it turns out, testing derivative code is no easy undertaking. It is often not a well-conditioned problem, and
  the conditioning is difficult to predict, being a function of the input point, function values, and gradient.
  The exact value is knowable from the analytic gradient, but here we are trying to ascertain whether the
  implementation of the analytic gradient is correct!

  Since we do nothing to guarantee good conditioning (random inputs), sometimes our luck is bad. Thus, this function
  includes a number of heuristics to detect conditions in which numerical error is too high to make an informed decision
  on whether we are looking at noise or a genuine error.

  These heuristics involve a number of "magic numbers" defined in the code. These depend heavily
  on the complexity of the gradient being tested (as well as the points at which gradients are being computed).
  Here, we are roughly assuming that the entries of "points" are in [1.0e-3, 1.0e1].
  The magic numbers here are not perfect, but they appear to work in the current use cases. I have left them
  hard-coded for now in the interest of making ping test easier to set up. So, apologies in advance for the
  magic numbers.

  We chose the bypasses to heavily favor eliminating false positives. Some true positives are *also lost*.
  This is why OL_PING_TEST_DEBUG_PRINTF is important when debugging. If a true positive comes up, then
  there are probably many more, which can be viewed by parsing the more detailed output.

  .. WARNING:: This function is NOT a catch-all. With the heuristics in place, you could adversarially construct an
    implementation that is wrong but passes this test.

  .. WARNING:: Additionally, this function cannot detect errors that are not in the gradient. If you intended
    to implement ``f = sin(x)``, ``f' = cos(x)``, but instead coded ``g = sin(x) + 1``, this function will not find it.
    This cannot detect small amounts of noise in the gradient either (e.g., ``f' = cos(x) + 1.0e-15``). There is no
    way to tell whether that is noise due to numerical error or noise due to incorrectness.

  TODO(GH-162): thresholds are an imperfect tool for this task. Loss of precision is not a binary event; you are not
  certain at ``2^{-52}`` but uncertain at ``2^{-51}``. It might be better to estimate the range over which we go from
  meaningful loss of precision to complete noise and have a linear ramp for the tolerance over that space. Maybe
  it should be done in log-space?
\endrst*/
int PingDerivative(const PingableMatrixInputVectorOutputInterface& function_and_derivative_evaluator, double const * restrict points, double epsilon[2], double rate_tolerance_fine, double rate_tolerance_relaxed, double input_output_ratio) noexcept {
  int num_rows, num_cols;
  function_and_derivative_evaluator.GetInputSizes(&num_rows, &num_cols);
  int num_outputs = function_and_derivative_evaluator.GetOutputSize();

  // *_p means "plus" and refers either to x+epsilon or f(x+epsilon)
  // *_m means "minus"
  std::vector<double> function_p(2*num_outputs);
  std::vector<double> function_m(2*num_outputs);
  std::vector<double> error(2*num_outputs);
  std::vector<double> points_p(num_rows*num_cols);
  std::vector<double> points_m(num_rows*num_cols);

  const double rate_exact = 2.0;
  // since the floating point representation of x+h is not exactly x+h, we want to compensate for the difference
  // see below for details on how epsilon_actual is computed
  double epsilon_actual[2];

  int ping_failures = 0;
  for (int i_cols = 0; i_cols < num_cols; ++i_cols) {
    for (int i_rows = 0; i_rows < num_rows; ++i_rows) {  // i, i_rows index over what we're differentiating against
      // Now we have a single i_cols and i_rows, specifying a single input out of n_cols * n_rows total.
      // Think of this as "x" (= X[i_cols][i_rows]).

      // Step 1: compute function values and gradients at/around "x", for both values of epsilon.
      for (int i_epsilon = 0; i_epsilon < 2; ++i_epsilon) {  // i_epsilon over h choices
        // Reset X_p and X_m to X. We only want to vary ONE index at a time, the variable x.
        for (int j = 0; j < num_rows*num_cols; ++j) {
          points_p[j] = points[j];
          points_m[j] = points[j];
        }

        // The input epsilon[2] is *not* exactly the h values used in computing f(x+h), f(x-h). Depending on the value
        // of x, the floating point value (denoted fl()) fl(x+h) = x + h + e != x + h.
        // An extreme example: x = 1000.0, h = 1.0e-16, then fl(x+h) = x exactly in double precision and e = -h.
        // To compensate, we'd like to estimate e.
        // So we compute the h', epsilon_actual. h' = fl(-x + fl(x + h)) = fl(-x + x + h + e) = fl(h + e) = h + e + e'.
        // As long as h << x (this function makes no sense otherwise), then e' \approx h * machine_epsilon and our h'
        // is good enough.

        // Initialize to -points_p, which is currently just a copy of points.
        // Note: we only compute one set of epsilon_actual per (i_cols, i_rows) so there is no need
        // to store any additional terms.
        epsilon_actual[i_epsilon] = -points_p[i_cols*num_rows + i_rows];

        points_p[i_cols*num_rows + i_rows] += epsilon[i_epsilon];  // x_p = x + h
        points_m[i_cols*num_rows + i_rows] -= epsilon[i_epsilon];  // x_m = x - h

        // Compute the actual h: h' = fl(-x + fl(x + h)) = fl(h + e)
        epsilon_actual[i_epsilon] += points_p[i_cols*num_rows + i_rows];

        // Compute f_p = f(X_p), f_m = f(X_m)
        function_and_derivative_evaluator.EvaluateFunction(points_p.data(), function_p.data() + i_epsilon*num_outputs);
        function_and_derivative_evaluator.EvaluateFunction(points_m.data(), function_m.data() + i_epsilon*num_outputs);

        // For each output, compute the difference (error) between analytic gradient and finite difference gradient.
        // Note: we save the error values for all outputs and for both epsilon values; we need the full set for
        // diagnostics later.
        for (int k = 0; k < num_outputs; ++k) {
          double grad_function_f = SecondOrderCenteredFiniteDifference(function_p[i_epsilon*num_outputs + k], function_m[i_epsilon*num_outputs + k], epsilon_actual[i_epsilon]);
          double grad_function_a = function_and_derivative_evaluator.GetAnalyticGradient(i_rows, i_cols, k);

          error[k*2 + i_epsilon] = std::fabs(grad_function_a - grad_function_f);
        }  // end for k: num_outputs
      }  // end for i_epsilon: 2

      // Step 2: check for errors, convergence, etc. in the data from step 1.
      // Since we used second differences to approximate the derivatve, we expect second order convergence.
      // test that this happens.
      // NOTE: when the analytic derivative is 'small' (near 0), this is a VERY poorly conditioned problem.  It's akin
      // to trying to compute (x1 - x2) = 0, so we need a lot of logic to realize when the problem is poorly conditioned
      // and bypass the convergence rate check.
      for (int k = 0; k < num_outputs; ++k) {
        // Get a rough idea of how large the function (k-th output value) is at X_p, X_m. We don't use f_k(X) in case the
        // function is changing very rapidly. We want a measure the magnitudes that went into the f(X_p) - f(X_m) computation.
        const double function_value_norm = std::sqrt(Square(function_p[num_outputs + k]) + Square(function_m[num_outputs + k])) + std::numeric_limits<double>::min();  // if both values are 0, adding ~1e-308 prevents division by 0 without negatively affecting any dependent computations
        const double grad_function_a = function_and_derivative_evaluator.GetAnalyticGradient(i_rows, i_cols, k);
        const double fabs_grad_function_a = std::fabs(grad_function_a);

        // The following code performs a series of checks on the relative sizes of the error, function values, gradient values
        // and input values. We are attempting to identify cases where we do not have enough precision to compute the
        // convergence rate accurately. When this happens, a "continue" statement will execute and we will skip this
        // particular combination of input point and output value.

        // At the end, if none of the "continue" statements are hit, then we are confident that the computed convergence
        // rate is accurate, and we will check that it is within tolerance of the expected convergence rate.
        // In general, we want to eliminate false positives so that errors genuinely indicate something broken in the code.

        // First, we do checks based on the magnitude of the error relative to the function value (f) and input value (X).

        // Result is extremely accurate: so much so that the rate of convergence
        // cannot be accurately calculated. 1.0e-20 was chosen to be a little smaller than machine precision (~1.0e-16).
        // For ||X|| \approx 1, we don't have enough precision to examine f(X_p) - f(X_m) at the 20th decimal place
        // and beyond, which we need to do in order to compute the convergence rate.
        if (error[k*2+0]/function_value_norm < 1.0e-20 && error[k*2+1]/function_value_norm < 1.0e-20) {
          continue;
        }

        // Like the previous check, but now comparing against magnitude of X[i_cols][i_rows], the value that was tweaked
        // by epsilon to produce X_p and X_m. The same logic applies about lacking sufficient precision to examine f(X_p) - f(X_m).
        if (error[k*2+0]/std::fabs(points[i_cols*num_rows + i_rows]) < input_output_ratio && error[k*2+1]/std::fabs(points[i_cols*num_rows + i_rows]) < input_output_ratio) {
          continue;
        }

        // Compute the convergence rate and the absolute error against the expected convergence rate.
        const double rate = std::log10(error[k*2 + 0]/error[k*2 + 1]) / std::log10(epsilon_actual[0]/epsilon_actual[1]);
        const double rate_error = std::fabs(rate_exact - rate);
        OL_PING_TEST_DEBUG_PRINTF("k=%d, rate = %.18E, |2 - rate| = %.18E\n", k, rate, rate_error);

        // As mentioned in the function comments, loss of precision is not a binary event. This is an attempt to "ramp up"
        // the tolerable loss over a range: for larger values of fabs_grad_function_a/function_value_norm, we expect to be
        // safe. For small values (e.g., fabs_grad_function_a near 0), we expect worse conditioning.
        // So we give a linear ramp to increase tolerance. (Maybe this should be done in log10 space.)
        // 1.0e-2 was chosen empirically.
        double rate_tolerance = rate_tolerance_fine;
        if (fabs_grad_function_a/function_value_norm < 1.0e-2) {
          rate_tolerance = rate_tolerance_relaxed + (fabs_grad_function_a/function_value_norm)/1.0e-2*(rate_tolerance_fine - rate_tolerance_relaxed);
          OL_PING_TEST_DEBUG_PRINTF("rate_tolerance = %.18E\n", rate_tolerance);
        }

        // Now we will do numerical precision checks related to the convergence rate.

        OL_PING_TEST_DEBUG_PRINTF("Checking convergence rate...\n");
        // isnan(rate) == true probably means epsilon[0] == epsilon[1]; then rate was computed as log(1)/log(1) = 0/0, NaN.
        // NaN can also arise if error[k*2+0] == error[k*2+1] == 0, BUT we would have already skipped if this happens
        if (rate_error > rate_tolerance || std::isnan(rate)) {
          // rate == 0.0, implying error[k*2+0] == error[k*2+1] != 0, should never happen. Log something extra when it does.
          if (rate == 0.0) {
            OL_PING_TEST_DEBUG_PRINTF("RATE == 0.0! error[%d*2 + 0] = %.18E, error[%d*2 + 1] = %.18E\n", k, error[k*2+0], k, error[k*2+1]);
          }

          // (Substantially) better than expected convergence is ok
          if (rate > 1.2*rate_exact && rate < 3*rate_exact) {
            continue;
          }

          // We are estimating grad_function_a by differencing two quantities with magnitude \approx function_value_norm.
          // If the ratio between these is small, then there is not enough precision in f(X_p) - f(X_m) to compute
          // the convergence rate. When this happens, it is ok if error[k*2+0] \approx error[k*2+1] and we are
          // satisfied if the relative error is small.
          OL_PING_TEST_DEBUG_PRINTF("analytic/value: %.18E, error/value: %.18E\n", fabs_grad_function_a/function_value_norm, error[k*2+1]/function_value_norm);
          if (fabs_grad_function_a/function_value_norm < 1.0e-5 && error[k*2+1]/function_value_norm < 1.0e-11) {
            continue;
          }

          // Given our assumption about the coordinate-wise magnitude of the input points, if function_value_norm is
          // small, then we lost substantial precision just computing f(X_p) and f(X_m). Intuitively, if I try to
          // compute 1.0e-8 as x - y where |x|, |y| \approx 1, then I have lost half the digits in x and y.
          if (function_value_norm < 1.0e-8 && error[k*2+1]/function_value_norm < 1.0e-12) {
            continue;
          }

          // All numerical precision bypasses have passed. We are confident that we computed the convergence rate
          // accurately AND that it is wrong.
          ++ping_failures;

          // Print information about the indexes, point, function, gradient (analytic and finite diff) and error
          // at the X[i_cols][i_rows], f'(X) combination that we judged was computed incorrectly.
          OL_ERROR_PRINTF("point[%d,%d] = %.18E\n", i_rows, i_cols, points[i_cols*num_rows + i_rows]);

          for (int i_epsilon = 0; i_epsilon < 2; ++i_epsilon) {
            double grad_function_f = SecondOrderCenteredFiniteDifference(function_p[i_epsilon*num_outputs + k], function_m[i_epsilon*num_outputs + k], epsilon_actual[i_epsilon]);
            OL_ERROR_PRINTF("i_rows = %d, i_cols = %d, k = %d, epsilon = %.6E\n", i_rows, i_cols, k, epsilon_actual[i_epsilon]);
            OL_ERROR_PRINTF("fcn_p[k] = %.18E, fcn_m[k] = %.18E\n", function_p[i_epsilon*num_outputs + k], function_m[i_epsilon*num_outputs + k]);
            OL_ERROR_PRINTF("analytic: %.18E\n", grad_function_a);
            OL_ERROR_PRINTF("finite  : %.18E\n", grad_function_f);
            OL_ERROR_PRINTF("diff    : %.18E\n", std::fabs(grad_function_a - grad_function_f));  // = error[2*k + i_epsilon]
          }

          OL_ERROR_PRINTF("ERROR PING FAILED on i_rows = %d, i_cols = %d, k = %d\n", i_rows, i_cols, k);
          OL_ERROR_PRINTF("k=%d, rate = %.18E, |2 - rate| = %.18E\n", k, rate, rate_error);
        }
      }  // end for k: num_outputs
    }  // end for i_rows: num_rows
  }  // end for i_cols: num_cols

  return ping_failures;
}

void ExpandDomainBounds(double scale_factor, std::vector<ClosedInterval> * domain_bounds) {
#ifdef OL_VERBOSE_PRINT
  PrintDomainBounds(domain_bounds.data(), dim);  // domain before expansion
#endif
  for (auto& interval : *domain_bounds) {
    double side_length = interval.Length();
    double midpoint = 0.5*(interval.max + interval.min);
    side_length *= scale_factor;
    side_length *= 0.5;  // want half of side_length b/c we grow outward from the midpoint
    interval = {midpoint - side_length, midpoint + side_length};
  }
#ifdef OL_VERBOSE_PRINT
  PrintDomainBounds(domain_bounds.data(), dim);  // expanded domain
#endif
}

void FillRandomCovarianceHyperparameters(const boost::uniform_real<double>& uniform_double_hyperparameter, UniformRandomGenerator * uniform_generator, std::vector<double> * hyperparameters, CovarianceInterface * covariance) {
  std::generate(hyperparameters->begin(), hyperparameters->end(), [&uniform_double_hyperparameter, uniform_generator]() {
    return uniform_double_hyperparameter(uniform_generator->engine);
  });
  covariance->SetHyperparameters(hyperparameters->data());
}

void FillRandomDomainBounds(const boost::uniform_real<double>& uniform_double_lower_bound, const boost::uniform_real<double>& uniform_double_upper_bound, UniformRandomGenerator * uniform_generator, std::vector<ClosedInterval> * domain_bounds) {
  std::generate(domain_bounds->begin(), domain_bounds->end(), [&uniform_double_lower_bound, &uniform_double_upper_bound, uniform_generator]() {
    double min = uniform_double_lower_bound(uniform_generator->engine);
    double max = uniform_double_upper_bound(uniform_generator->engine);
    return ClosedInterval(min, max);
  });
}

void FillRandomGaussianProcess(double const * restrict points_to_sample, double const * restrict noise_variance, int dim, int num_to_sample, double * restrict points_to_sample_value, GaussianProcess * gaussian_process) {
  for (int j = 0; j < num_to_sample; ++j) {
    // draw function value from the GP
    points_to_sample_value[j] = gaussian_process->SamplePointFromGP(points_to_sample, noise_variance[j]);
    // add function value back into the GP
    gaussian_process->AddPointsToGP(points_to_sample, points_to_sample_value + j, noise_variance + j, 1);
    points_to_sample += dim;
  }
}

}  // end namespace optimal_learning


#include "expectedvaluecalculator.hpp"

#undef NDEBUG
#include <algorithm>
#include <assert.h>
#include <fstream>
#include <numeric>

#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>

#include "requiredtileselector.hpp"
#include "syanten.hpp"
#include "unnecessarytileselector.hpp"
#include "utils.hpp"

namespace mahjong
{
ExpectedValueCalculator::ExpectedValueCalculator()
    : calc_syanten_down_(false)
    , calc_tegawari_(false)
    , calc_double_reach_(false)
    , calc_ippatu_(false)
    , calc_haitei_(false)
    , calc_uradora_(false)
    , discard_cache_(5) // 0(聴牌) ~ 4(4向聴)
    , draw_cache_(5)    // 0(聴牌) ~ 4(4向聴)
{
    make_uradora_table();
}

/**
 * @brief 期待値を計算する。
 *
 * @param hand 手牌
 * @param score_calculator 点数計算機
 * @param dora_indicators ドラ表示牌の一覧
 * @param syanten_type 向聴数の種類
 * @param flag フラグ
 * @return 各打牌の情報
 */
std::tuple<bool, std::vector<Candidate>>
ExpectedValueCalculator::calc(const Hand &hand, const ScoreCalculator &score_calculator,
                              const std::vector<int> &dora_indicators, int syanten_type, int flag,
                              const std::vector<int> &kawa)
{
    std::vector<Candidate> candidates;

    score_calculator_ = score_calculator;
    syanten_type_ = syanten_type;
    dora_indicators_ = dora_indicators;
    calc_syanten_down_ = flag & CalcSyantenDown;
    calc_tegawari_ = flag & CalcTegawari;
    calc_double_reach_ = flag & CalcDoubleReach;
    calc_ippatu_ = flag & CalcIppatu;
    calc_haitei_ = flag & CalcHaiteitumo;
    calc_uradora_ = flag & CalcUradora;
    maximize_win_prob_ = flag & MaximaizeWinProb;

    // 手牌の枚数を数える。
    int n_tiles = hand.num_tiles() + int(hand.melds.size()) * 3;
    if (n_tiles != 14)
        return {false, {}}; // 手牌が14枚ではない場合

    // 現在の向聴数を計算する。
    auto [_, syanten] = SyantenCalculator::calc(hand, syanten_type_);
    if (syanten == -1)
        return {false, {}}; // 手牌が和了形の場合

    // 各牌の残り枚数を数える。
    std::vector<int> counts = count_left_tiles(hand, dora_indicators_, kawa);
    int sum_left_tiles = std::accumulate(counts.begin(), counts.begin() + 34, 0);

    // 自摸確率のテーブルを作成する。
    create_prob_table(sum_left_tiles);

    if (syanten > 3) // 3向聴以下は聴牌確率、和了確率、期待値を計算する。
        candidates = analyze(syanten, hand);
    else // 4向聴以上は受入枚数のみ計算する。
        candidates = analyze(0, syanten, hand);

    // キャッシュをクリアする。
    clear_cache();

    return {true, candidates};
}

/**
 * @brief 有効牌の一覧を取得する。
 *
 * @param[in] hand 手牌
 * @param[in] syanten_type 向聴数の種類
 * @param[in] counts 各牌の残り枚数
 * @return 有効牌の一覧
 */
std::vector<std::tuple<int, int>>
ExpectedValueCalculator::get_required_tiles(const Hand &hand, int syanten_type,
                                            const std::vector<int> &counts)
{
    std::vector<std::tuple<int, int>> required_tiles;

    // 有効牌の一覧を取得する。
    std::vector<int> tiles = RequiredTileSelector::select(hand, syanten_type);

    for (auto tile : tiles)
        required_tiles.emplace_back(tile, counts[tile]);

    return required_tiles;
}

/**
 * @brief 各牌の残り枚数を数える。
 *
 * @param[in] hand 手牌
 * @param[in] dora_indicators ドラ表示牌の一覧
 * @return 各牌の残り枚数
 */
std::vector<int> ExpectedValueCalculator::count_left_tiles(const Hand &hand,
                                                           const std::vector<int> &dora_indicators,
                                                           const std::vector<int> &kawa)
{
    std::vector<int> counts(37, 4);
    counts[Tile::AkaManzu5] = counts[Tile::AkaPinzu5] = counts[Tile::AkaSozu5] = 1;

    // 手牌を除く。
    for (int i = 0; i < 34; ++i)
        counts[i] -= hand.num_tiles(i);
    counts[Tile::AkaManzu5] -= hand.aka_manzu5;
    counts[Tile::AkaPinzu5] -= hand.aka_pinzu5;
    counts[Tile::AkaSozu5] -= hand.aka_sozu5;

    // 副露ブロックを除く。
    for (const auto &block : hand.melds) {
        for (auto tile : block.tiles) {
            counts[aka2normal(tile)]--;
            counts[Tile::AkaManzu5] -= tile == Tile::AkaManzu5;
            counts[Tile::AkaPinzu5] -= tile == Tile::AkaPinzu5;
            counts[Tile::AkaSozu5] -= tile == Tile::AkaSozu5;
        }
    }

    // ドラ表示牌を除く。
    for (auto tile : dora_indicators) {
        counts[aka2normal(tile)]--;
        counts[Tile::AkaManzu5] -= tile == Tile::AkaManzu5;
        counts[Tile::AkaPinzu5] -= tile == Tile::AkaPinzu5;
        counts[Tile::AkaSozu5] -= tile == Tile::AkaSozu5;
    }
    // 河を除く。
    for (auto tile : kawa) {
        counts[aka2normal(tile)]--;
        counts[Tile::AkaManzu5] -= tile == Tile::AkaManzu5;
        counts[Tile::AkaPinzu5] -= tile == Tile::AkaPinzu5;
        counts[Tile::AkaSozu5] -= tile == Tile::AkaSozu5;
    }

    return counts;
}

/**
 * @brief 裏ドラ確率のテーブルを初期化する。
 *
 * @return 初期化に成功した場合は true、そうでない場合は false を返す。
 */
bool ExpectedValueCalculator::make_uradora_table()
{
    if (!uradora_prob_table_.empty())
        return true;

    uradora_prob_table_.resize(6);

    std::ifstream ifs("res/uradora.txt");

    std::string line;
    int i = 0;
    while (std::getline(ifs, line)) {
        std::vector<std::string> tokens;
        boost::split(tokens, line, boost::is_any_of(" "));

        for (auto token : tokens)
            uradora_prob_table_[i].push_back(std::stod(token));
        i++;
    }

    return true;
}

/**
 * @brief 自摸確率のテーブルを初期化する。
 *
 * @param[in] n_left_tiles 1巡目時点の残り枚数の合計
 */
void ExpectedValueCalculator::create_prob_table(int n_left_tiles)
{
    // 有効牌の枚数ごとに、この巡目で有効牌を引ける確率のテーブルを作成する
    // tumo_prob_table_[i][j] = 有効牌の枚数が i 枚の場合に j 巡目に有効牌が引ける確率
    tumo_prob_table_.resize(5, std::vector<double>(17));
    for (int i = 0; i < 5; ++i) {
        for (int j = 0; j < 17; ++j)
            tumo_prob_table_[i][j] = double(i) / double(n_left_tiles - j);
    }

    // 有効牌の合計枚数ごとに、これまでの巡目で有効牌が引けなかった確率のテーブルを作成する
    // not_tumo_prob_table_[i][j] = 有効牌の合計枚数が i 枚の場合に j - 1 巡目までに有効牌が引けなかった確率
    not_tumo_prob_table_.resize(n_left_tiles, std::vector<double>(17));
    for (int i = 0; i < n_left_tiles; ++i) {
        not_tumo_prob_table_[i][0] = 1;
        // n_left_tiles - i - j > 0 は残りはすべて有効牌の場合を考慮
        for (int j = 0; j < 16 && n_left_tiles - i - j > 0; ++j) {
            not_tumo_prob_table_[i][j + 1] = not_tumo_prob_table_[i][j] *
                                             double(n_left_tiles - i - j) /
                                             double(n_left_tiles - j);
        }
    }
}

/**
 * @brief キャッシュをクリアする。
 */
void ExpectedValueCalculator::clear_cache()
{
    // デバッグ用
    // for (size_t i = 0; i < 5; ++i)
    //     spdlog::info("向聴数{} 打牌: {}, 自摸: {}", i, discard_cache_[i].size(),
    //                  draw_cache_[i].size());
    std::for_each(discard_cache_.begin(), discard_cache_.end(), [](auto &x) { x.clear(); });
    std::for_each(draw_cache_.begin(), draw_cache_.end(), [](auto &x) { x.clear(); });
}

/**
 * @brief 自摸牌一覧を取得する。
 *
 * @param[in] hand 手牌
 * @param[in] syanten 手牌の向聴数
 * @param[in] counts 各牌の残り枚数
 * @return 自摸牌一覧 (有効牌: -1、向聴数変化なし: 0)
 */
std::vector<std::tuple<int, int, int>>
ExpectedValueCalculator::get_draw_tiles(Hand &hand, int syanten, const std::vector<int> &counts)
{
    std::vector<std::tuple<int, int, int>> flags;
    flags.reserve(34);

    for (int tile = 0; tile < 34; ++tile) {
        if (counts[tile] > 0) {
            add_tile(hand, tile);
            auto [_, syanten_after] = SyantenCalculator::calc(hand, syanten_type_);
            remove_tile(hand, tile);

            if (tile == Tile::Manzu5 && counts[Tile::AkaManzu5] == 1) {
                // 赤五萬が残っている場合
                if (counts[Tile::Manzu5] >= 2) {
                    // 普通の牌と赤牌の両方が残っている
                    flags.emplace_back(tile, counts[tile] - 1, syanten_after - syanten);
                    flags.emplace_back(Tile::AkaManzu5, 1, syanten_after - syanten);
                }
                else if (counts[Tile::Manzu5] == 1) {
                    // 赤牌のみ残っている
                    flags.emplace_back(Tile::AkaManzu5, 1, syanten_after - syanten);
                }
            }
            else if (tile == Tile::Pinzu5 && counts[Tile::AkaPinzu5] == 1) {
                // 赤五筒が残っている場合
                if (counts[Tile::Pinzu5] >= 2) {
                    // 普通の牌と赤牌の両方が残っている
                    flags.emplace_back(tile, counts[tile] - 1, syanten_after - syanten);
                    flags.emplace_back(Tile::AkaPinzu5, 1, syanten_after - syanten);
                }
                else if (counts[Tile::Pinzu5] == 1) {
                    // 赤牌のみ残っている
                    flags.emplace_back(Tile::AkaPinzu5, 1, syanten_after - syanten);
                }
            }
            else if (tile == Tile::Sozu5 && counts[Tile::AkaSozu5] == 1) {
                // 赤五索が残っている場合
                if (counts[Tile::Sozu5] >= 2) {
                    // 普通の牌と赤牌の両方が残っている
                    flags.emplace_back(tile, counts[tile] - 1, syanten_after - syanten);
                    flags.emplace_back(Tile::AkaSozu5, 1, syanten_after - syanten);
                }
                else if (counts[Tile::Sozu5] == 1) {
                    // 赤牌のみ残っている
                    flags.emplace_back(Tile::AkaSozu5, 1, syanten_after - syanten);
                }
            }
            else {
                flags.emplace_back(tile, counts[tile], syanten_after - syanten);
            }
        }
    }

    return flags;
}

/**
 * @brief 打牌一覧を取得する。
 *
 * @param[in] hand 手牌
 * @param[in] syanten 手牌の向聴数
 * @return 打牌一覧 (向聴戻し: 1、向聴数変化なし: 0、手牌に存在しない: inf)
 */
std::vector<int> ExpectedValueCalculator::get_discard_tiles(Hand &hand, int syanten)
{
    std::vector<int> flags(34, std::numeric_limits<int>::max());

    for (int tile = 0; tile < 34; ++tile) {
        if (hand.contains(tile)) {
            remove_tile(hand, tile);
            auto [_, syanten_after] = SyantenCalculator::calc(hand, syanten_type_);
            add_tile(hand, tile);
            flags[tile] = syanten_after - syanten;
        }
    }

    return flags;
}

/**
 * @brief 手牌の点数を取得する。
 *
 * @param[in] hand 手牌
 * @param[in] win_tile 自摸牌
 * @param[in] counts 各牌の残り枚数
 * @return 点数
 */
std::vector<double> ExpectedValueCalculator::get_score(const Hand &hand, int win_tile,
                                                       const std::vector<int> &counts)
{
    // 赤牌が残っていないかどうか (あとで消す)
    // if (hand.aka_manzu5)
    //     assert(hand.num_tiles(Tile::Manzu5));
    // if (hand.aka_pinzu5)
    //     assert(hand.num_tiles(Tile::Pinzu5));
    // if (hand.aka_sozu5)
    //     assert(hand.num_tiles(Tile::Sozu5));

    // 非門前の場合は自摸のみ
    int hand_flag = hand.is_menzen() ? (HandFlag::Tumo | HandFlag::Reach) : HandFlag::Tumo;

    // 点数計算を行う。
    Result result = score_calculator_.calc(hand, win_tile, hand_flag);

    // 表ドラの数
    int n_dora = int(dora_indicators_.size());

    // ダブル立直、一発、海底撈月で最大3翻まで増加するので、
    // ベースとなる点数、+1翻の点数、+2翻の点数、+3翻の点数も計算しておく。
    std::vector<double> scores(4, 0);
    if (result.success) {
        // 役ありの場合
        std::vector<int> up_scores = score_calculator_.get_scores_for_exp(result);

        if (calc_uradora_ && n_dora == 1) {
            // 裏ドラ考慮ありかつ表ドラが1枚以上の場合は、厳密に計算する。
            std::vector<double> n_indicators(5, 0);
            int sum_indicators = 0;
            for (int tile = 0; tile < 34; ++tile) {
                int n = hand.num_tiles(tile);
                if (n > 0) {
                    // ドラ表示牌の枚数を数える。
                    n_indicators[n] += counts[Dora2Indicator.at(tile)];
                    sum_indicators += counts[Dora2Indicator.at(tile)];
                }
            }

            // 裏ドラの乗る確率を枚数ごとに計算する。
            std::vector<double> uradora_probs(5, 0);

            // 厳密に計算するなら残り枚数は数えるべきだが、あまり影響がないので121枚で固定
            int n_left_tiles = 121;
            uradora_probs[0] = double(n_left_tiles - sum_indicators) / n_left_tiles;
            for (int i = 1; i < 5; ++i)
                uradora_probs[i] = double(n_indicators[i]) / n_left_tiles;

            for (int base = 0; base < 4; ++base) {
                for (int i = 0; i < 5; ++i) { // 裏ドラ1枚の場合、最大4翻まで乗る可能性がある
                    int han_idx = std::min(base + i, int(up_scores.size() - 1));
                    scores[base] += up_scores[han_idx] * uradora_probs[i];
                }
            }
        }
        else if (calc_uradora_ && n_dora > 1) {
            // 裏ドラ考慮ありかつ表ドラが2枚以上の場合、統計データを利用する。
            for (int base = 0; base < 4; ++base) {
                for (int i = 0; i < 13; ++i) {
                    int han_idx = std::min(base + i, int(up_scores.size() - 1));
                    scores[base] += up_scores[han_idx] * uradora_prob_table_[n_dora][i];
                }
            }
        }
        else {
            // 裏ドラ考慮なしまたは表ドラが0枚の場合
            for (int base = 0; base < 4; ++base) {
                int han_idx = std::min(base, int(up_scores.size() - 1));
                scores[base] += up_scores[han_idx];
            }
        }
    }

    return scores;
}

/**
 * @brief 自摸する。(手変わりを考慮しない)
 *
 * @param[in] n_extra_tumo
 * @param[in] syanten 向聴数
 * @param[in] hand 手牌
 * @param[in] counts 各牌の残り枚数
 * @return (各巡目の聴牌確率, 各巡目の和了確率, 各巡目の期待値)
 */
std::tuple<std::vector<double>, std::vector<double>, std::vector<double>>
ExpectedValueCalculator::draw_without_tegawari(int n_extra_tumo, int syanten, Hand &hand,
                                               std::vector<int> &counts)
{
#ifdef ENABLE_DRAW_CACHE
    auto &table = draw_cache_[syanten];

    CacheKey key(hand, counts, n_extra_tumo);
    if (auto itr = table.find(key); itr != table.end())
        return itr->second; // キャッシュが存在する場合
#endif

    std::vector<double> tenpai_probs(17, 0), win_probs(17, 0), exp_values(17, 0);

    // 自摸候補を取得する。
    std::vector<std::tuple<int, int, int>> flags = get_draw_tiles(hand, syanten, counts);

    // 有効牌の合計枚数を計算する。
    int sum_required_tiles = 0;
    for (auto &[tile, count, diff] : flags) {
        if (diff == -1) // 有効牌の場合
            sum_required_tiles += count;
    }

    for (auto &[tile, count, diff] : flags) {
        if (diff != -1) // 有効牌以外の場合
            continue;

        const std::vector<double> &tumo_probs = tumo_prob_table_[count];
        const std::vector<double> &not_tumo_probs = not_tumo_prob_table_[sum_required_tiles];

        // 手牌に加える
        add_tile(hand, tile, counts);

        std::vector<double> next_tenpai_probs, next_win_probs, next_exp_values;
        std::vector<double> scores;
        if (syanten == 0) {
            scores = get_score(hand, tile, counts);
        }
        else {
            std::tie(next_tenpai_probs, next_win_probs, next_exp_values) =
                discard(n_extra_tumo, syanten - 1, hand, counts);
        }

        for (int i = 0; i < 17; ++i) {
            for (int j = i; j < 17; ++j) {
                // 現在の巡目が i の場合に j 巡目に有効牌を引く確率
                double prob = tumo_probs[j] * not_tumo_probs[j] / not_tumo_probs[i];

                if (syanten == 1) // 1向聴の場合は次で聴牌
                    tenpai_probs[i] += prob;
                else if (j < 16 && syanten > 1) // 2向聴以上で16巡目以下の場合
                    tenpai_probs[i] += prob * next_tenpai_probs[j + 1];

                // scores[0] == 0 の場合は役なしなので、和了確率、期待値は0
                if (syanten == 0 && scores[0] != 0) { // 聴牌の場合は次で和了
                    // i 巡目で聴牌の場合はダブル立直成立
                    bool win_double_reach = i == 0 && calc_double_reach_;
                    // i 巡目で聴牌し、次の巡目で和了の場合は一発成立
                    bool win_ippatu = j == i && calc_ippatu_;
                    // 最後の巡目で和了の場合は海底撈月成立
                    bool win_haitei = j == 16 && calc_haitei_;

                    win_probs[i] += prob;
                    exp_values[i] += prob * scores[win_double_reach + win_ippatu + win_haitei];
                }
                else if (j < 16 && syanten > 0) { // 聴牌以上で16巡目以下の場合
                    win_probs[i] += prob * next_win_probs[j + 1];
                    exp_values[i] += prob * next_exp_values[j + 1];
                }
            }
        }

        // 手牌から除く
        remove_tile(hand, tile, counts);
    }

#ifdef ENABLE_DRAW_CACHE
    auto [itr, _] = table.try_emplace(key, tenpai_probs, win_probs, exp_values);

    return itr->second;
#else
    return {tenpai_probs, win_probs, exp_values};
#endif
}

/**
 * @brief 自摸する。(手変わりを考慮する)
 *
 * @param[in] n_extra_tumo
 * @param[in] syanten 向聴数
 * @param[in] hand 手牌
 * @param[in] counts 各牌の残り枚数
 * @return (各巡目の聴牌確率, 各巡目の和了確率, 各巡目の期待値)
 *
 * この関数が呼ばれた時点で向聴戻しは行われていない
 */
std::tuple<std::vector<double>, std::vector<double>, std::vector<double>>
ExpectedValueCalculator::draw_with_tegawari(int n_extra_tumo, int syanten, Hand &hand,
                                            std::vector<int> &counts)
{
#ifdef ENABLE_DRAW_CACHE
    auto &table = draw_cache_[syanten];

    CacheKey key(hand, counts, n_extra_tumo);
    if (auto itr = table.find(key); itr != table.end())
        return itr->second; // キャッシュが存在する場合
#endif

    std::vector<double> tenpai_probs(17, 0), win_probs(17, 0), exp_values(17, 0);

    // 自摸候補を取得する。
    std::vector<std::tuple<int, int, int>> flags = get_draw_tiles(hand, syanten, counts);

    for (auto &[tile, count, diff] : flags) {
        if (diff != -1)
            continue; // 有効牌以外の場合

        const std::vector<double> &tumo_probs = tumo_prob_table_[count];

        // 手牌に加える
        add_tile(hand, tile, counts);

        std::vector<double> next_tenpai_probs, next_win_probs, next_exp_values;
        std::vector<double> scores;

        if (syanten == 0) {
            scores = get_score(hand, tile, counts);
        }
        else {
            std::tie(next_tenpai_probs, next_win_probs, next_exp_values) =
                discard(n_extra_tumo, syanten - 1, hand, counts);
        }

        for (int i = 0; i < 17; ++i) {
            if (syanten == 1) // 1向聴の場合は次で聴牌
                tenpai_probs[i] += tumo_probs[i];
            else if (i < 16 && syanten > 1)
                tenpai_probs[i] += tumo_probs[i] * next_tenpai_probs[i + 1];

            if (syanten == 0) { // 聴牌の場合は次で和了
                // i 巡目で聴牌の場合はダブル立直成立
                bool win_double_reach = i == 0 && calc_double_reach_;
                // i 巡目で聴牌し、次の巡目で和了の場合は一発成立
                bool win_ippatu = calc_ippatu_;
                // 最後の巡目で和了の場合は海底撈月成立
                bool win_haitei = i == 16 && calc_haitei_;

                win_probs[i] += tumo_probs[i];
                exp_values[i] += tumo_probs[i] * scores[win_double_reach + win_ippatu + win_haitei];
            }
            else if (i < 16 && syanten > 0) {
                win_probs[i] += tumo_probs[i] * next_win_probs[i + 1];
                exp_values[i] += tumo_probs[i] * next_exp_values[i + 1];
            }
        }

        // 手牌から除く
        remove_tile(hand, tile, counts);
    }

    for (auto &[tile, count, diff] : flags) {
        if (diff != 0)
            continue;

        const std::vector<double> &tumo_probs = tumo_prob_table_[count];

        // 手牌に加える
        add_tile(hand, tile, counts);

        auto [next_tenpai_probs, next_win_probs, next_exp_values] =
            discard(n_extra_tumo + 1, syanten, hand, counts);

        for (int i = 0; i < 16; ++i) {
            tenpai_probs[i] += tumo_probs[i] * next_tenpai_probs[i + 1];
            win_probs[i] += tumo_probs[i] * next_win_probs[i + 1];
            exp_values[i] += tumo_probs[i] * next_exp_values[i + 1];
        }

        // 手牌から除く
        remove_tile(hand, tile, counts);
    }

#ifdef ENABLE_DRAW_CACHE
    auto [itr, _] = table.try_emplace(key, tenpai_probs, win_probs, exp_values);

    return itr->second;
#else
    return {tenpai_probs, win_probs, exp_values};
#endif
}

/**
 * @brief 自摸する。
 *
 * @param[in] n_extra_tumo
 * @param[in] syanten 向聴数
 * @param[in] hand 手牌
 * @param[in] counts 各牌の残り枚数
 * @return (各巡目の聴牌確率, 各巡目の和了確率, 各巡目の期待値)
 */
std::tuple<std::vector<double>, std::vector<double>, std::vector<double>>
ExpectedValueCalculator::draw(int n_extra_tumo, int syanten, Hand &hand, std::vector<int> &counts)
{
    if (calc_tegawari_ && n_extra_tumo == 0)
        return draw_with_tegawari(n_extra_tumo, syanten, hand, counts);
    else
        return draw_without_tegawari(n_extra_tumo, syanten, hand, counts);
}

/**
 * @brief 打牌する。
 *
 * @param[in] n_extra_tumo
 * @param[in] syanten 向聴数
 * @param[in] hand 手牌
 * @param[in] counts 各牌の残り枚数
 * @return (各巡目の聴牌確率, 各巡目の和了確率, 各巡目の期待値)
 */
std::tuple<std::vector<double>, std::vector<double>, std::vector<double>>
ExpectedValueCalculator::discard(int n_extra_tumo, int syanten, Hand &hand,
                                 std::vector<int> &counts)
{
#ifdef ENABLE_DISCARD_CACHE
    auto &table = discard_cache_[syanten];

    CacheKey key(hand, counts, n_extra_tumo);
    if (auto itr = table.find(key); itr != table.end())
        return itr->second; // キャッシュが存在する場合
#endif

    // 打牌候補を取得する。
    const std::vector<int> flags = get_discard_tiles(hand, syanten);

    // 期待値が最大となる打牌を選択する。
    std::vector<double> max_tenpai_probs, max_win_probs, max_exp_values;
    std::vector<double> tenpai_probs, win_probs, exp_values;
    int max_tile = -1;
    double max_value = -1;
    for (int tile = 0; tile < 34; ++tile) {
        int discard_tile = tile;
        if (tile == Tile::Manzu5 && hand.aka_manzu5 && hand.num_tiles(Tile::Manzu5) == 1)
            discard_tile = Tile::AkaManzu5;
        else if (tile == Tile::Pinzu5 && hand.aka_pinzu5 && hand.num_tiles(Tile::Pinzu5) == 1)
            discard_tile = Tile::AkaPinzu5;
        else if (tile == Tile::Sozu5 && hand.aka_sozu5 && hand.num_tiles(Tile::Sozu5) == 1)
            discard_tile = Tile::AkaSozu5;

        if (flags[tile] == 0) {
            // 向聴数が変化しない打牌
            remove_tile(hand, discard_tile);
            std::tie(tenpai_probs, win_probs, exp_values) =
                draw(n_extra_tumo, syanten, hand, counts);
            add_tile(hand, discard_tile);
        }
        else if (calc_syanten_down_ && n_extra_tumo == 0 && flags[tile] == 1) {
            // 向聴戻しになる打牌
            remove_tile(hand, discard_tile);
            std::tie(tenpai_probs, win_probs, exp_values) =
                draw(n_extra_tumo + 1, syanten + 1, hand, counts);
            add_tile(hand, discard_tile);
        }
        else {
            // 手牌に存在しない牌、または向聴落としが無効な場合に向聴落としとなる牌
            continue;
        }

        // 向聴戻し、手変わりは巡目がずれるので、1つ手前にずらす。(このやり方で正しいのか要検証)

        // 和了確率は下2桁まで一致していれば同じ、期待値は下0桁まで一致していれば同じとみなす。
        double value = maximize_win_prob_ ? int(win_probs[n_extra_tumo] * 10000)
                                          : int(exp_values[n_extra_tumo]);
        if ((value == max_value && DiscardPriorities[max_tile] < DiscardPriorities[tile]) ||
            value > max_value) {
            // 値が同等なら、DiscardPriorities が高い牌を優先して選択する。
            max_tenpai_probs = tenpai_probs;
            max_win_probs = win_probs;
            max_exp_values = exp_values;

            max_value = value;
            max_tile = tile;
        }
    }

#ifdef ENABLE_DISCARD_CACHE
    auto [itr, _] = table.try_emplace(key, max_tenpai_probs, max_win_probs, max_exp_values);

    return itr->second;
#else
    return {max_tenpai_probs, max_win_probs, max_exp_values};
#endif
}

std::vector<Candidate> ExpectedValueCalculator::analyze(int n_extra_tumo, int syanten,
                                                        const Hand &_hand)
{
    std::vector<Candidate> candidates;
    Hand hand = _hand;

    // 各牌の残り枚数を数える。
    std::vector<int> counts = count_left_tiles(hand, dora_indicators_);

    // 打牌候補を取得する。
    const std::vector<int> flags = get_discard_tiles(hand, syanten);

    for (int tile = 0; tile < 34; ++tile) {
        int discard_tile = tile;
        if (tile == Tile::Manzu5 && hand.aka_manzu5 && hand.num_tiles(Tile::Manzu5) == 1)
            discard_tile = Tile::AkaManzu5;
        else if (tile == Tile::Pinzu5 && hand.aka_pinzu5 && hand.num_tiles(Tile::Pinzu5) == 1)
            discard_tile = Tile::AkaPinzu5;
        else if (tile == Tile::Sozu5 && hand.aka_sozu5 && hand.num_tiles(Tile::Sozu5) == 1)
            discard_tile = Tile::AkaSozu5;

        if (flags[tile] == 0) {
            remove_tile(hand, discard_tile);

            auto required_tiles = get_required_tiles(hand, syanten_type_, counts);

            auto [tenpai_probs, win_probs, exp_values] = draw(n_extra_tumo, syanten, hand, counts);

            add_tile(hand, discard_tile);

            if (syanten == 0) // すでに聴牌している場合の例外処理
                std::fill(tenpai_probs.begin(), tenpai_probs.end(), 1);

            candidates.emplace_back(discard_tile, required_tiles, tenpai_probs, win_probs,
                                    exp_values, false);
        }
        else if (calc_syanten_down_ && flags[tile] == 1 && n_extra_tumo == 0 && syanten < 3) {
            remove_tile(hand, discard_tile);

            auto required_tiles = get_required_tiles(hand, syanten_type_, counts);

            auto [tenpai_probs, win_probs, exp_values] =
                draw(n_extra_tumo + 1, syanten + 1, hand, counts);

            add_tile(hand, discard_tile);

            // 向聴戻しは巡目がずれるので、1つ手前にずらす。(このやり方で正しいのか要検証)
            std::rotate(tenpai_probs.begin(), tenpai_probs.begin() + 1, tenpai_probs.end());
            tenpai_probs.back() = 0;
            std::rotate(win_probs.begin(), win_probs.begin() + 1, win_probs.end());
            win_probs.back() = 0;
            std::rotate(exp_values.begin(), exp_values.begin() + 1, exp_values.end());
            exp_values.back() = 0;

            candidates.emplace_back(discard_tile, required_tiles, tenpai_probs, win_probs,
                                    exp_values, true);
        }
    }

    return candidates;
}

std::vector<Candidate> ExpectedValueCalculator::analyze(int syanten, const Hand &_hand)
{
    std::vector<Candidate> candidates;
    Hand hand = _hand;

    // 各牌の残り枚数を数える。
    std::vector<int> counts = count_left_tiles(hand, dora_indicators_);

    std::vector<int> tiles = UnnecessaryTileSelector::select(hand, syanten_type_);

    for (auto tile : tiles) {
        remove_tile(hand, tile);
        auto required_tiles = get_required_tiles(hand, syanten_type_, counts);
        add_tile(hand, tile);
        candidates.emplace_back(tile, required_tiles);
    }

    return candidates;
}

std::vector<std::vector<double>> ExpectedValueCalculator::uradora_prob_table_;

} // namespace mahjong


/*
 * openRatSLAM
 *
 * utils - General purpose utility helper functions mainly for angles and readings settings
 *
 * Copyright (C) 2012
 * David Ball (david.ball@qut.edu.au) (1), Scott Heath (scott.heath@uqconnect.edu.au) (2)
 *
 * RatSLAM algorithm by:
 * Michael Milford (1) and Gordon Wyeth (1) ([michael.milford, gordon.wyeth]@qut.edu.au)
 *
 * 1. Queensland University of Technology, Australia
 * 2. The University of Queensland, Australia
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "utils/utils.h"

#include <boost/property_tree/ini_parser.hpp>

#include <ros/ros.h>

#include "ratslam/experience_map.h"
#include <ratslam_ros/TopologicalAction.h>
#include <nav_msgs/Odometry.h>
#include "graphics/experience_map_scene.h"
#include <ratslam_ros/TopologicalMap.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <tf/transform_broadcaster.h>

#include <visualization_msgs/Marker.h>

// Distance server by Mr-Yellow 2015-04-25
#include <ratslam_ros/GetDistance.h>

ros::Publisher pub_em;
ros::Publisher pub_pose;
ros::Publisher pub_em_markers;
ros::Publisher pub_goal_path;
ros::Publisher pub_goal_cmd;
geometry_msgs::PoseStamped pose_output;
ratslam_ros::TopologicalMap em_map;
visualization_msgs::Marker em_marker;

#ifdef HAVE_IRRLICHT
#include "graphics/experience_map_scene.h"
ratslam::ExperienceMapScene *ems;
bool use_graphics;
#endif

using namespace ratslam;

void odo_callback(nav_msgs::OdometryConstPtr odo, ratslam::ExperienceMap *em)
{
  //ROS_DEBUG_STREAM("EM:odo_callback A{" << ros::Time::now() << "} seq=" << odo->header.seq << " v=" << odo->twist.twist.linear.x << " r=" << odo->twist.twist.angular.z);

  static ros::Time prev_time(0);

  if (prev_time.toSec() > 0)
  {
    double time_diff = (odo->header.stamp - prev_time).toSec();
    /*
    ROS_DEBUG_STREAM("EM:on_odo {" << ros::Time::now() <<
    "} seq=" << odo->header.seq <<
    " v=" << odo->twist.twist.linear.x <<
    " r=" << odo->twist.twist.angular.z <<
    " t=" << time_diff);
    */
    em->on_odo(odo->twist.twist.linear.x, odo->twist.twist.angular.z, time_diff);
  }

  static ros::Time prev_goal_update(0);

  if (em->get_current_goal_id() >= 0)
  {
    // (prev_goal_update.toSec() == 0 || (odo->header.stamp - prev_goal_update).toSec() > 5)
    //em->calculate_path_to_goal(odo->header.stamp.toSec());

    prev_goal_update = odo->header.stamp;

    em->calculate_path_to_goal(odo->header.stamp.toSec());

    static nav_msgs::Path path;
    if (em->get_current_goal_id() >= 0)
    {
      em->get_goal_waypoint();

      static geometry_msgs::PoseStamped pose;
      path.header.stamp = ros::Time::now();
      path.header.frame_id = "1";

      pose.header.seq = 0;
      pose.header.frame_id = "1";
      path.poses.clear();
      unsigned int trace_exp_id = em->get_goals()[0];
      while (trace_exp_id != em->get_goal_path_final_exp())
      {
        pose.pose.position.x = em->get_experience(trace_exp_id)->x_m;
        pose.pose.position.y = em->get_experience(trace_exp_id)->y_m;
        path.poses.push_back(pose);
        pose.header.seq++;

        trace_exp_id = em->get_experience(trace_exp_id)->goal_to_current;
      }

      pub_goal_path.publish(path);

      path.header.seq++;

      // TODO: Publish cmd_vel from last waypoint entry.
      //ROS_DEBUG_STREAM("EM:wp_pub{" << ros::Time::now() << "} m=" << em->get_subgoal_m() << " rad=" << em->get_subgoal_rad());

      static geometry_msgs::Twist wp;
      wp.linear.x  = 0;
      wp.angular.z = 1-em->get_subgoal_rad();
      if (abs(wp.angular.z) < 1.15) {
        wp.linear.x  = em->get_subgoal_m();
        wp.angular.z = 0;
      }
      pub_goal_cmd.publish(wp);
    }
    else
    {
      path.header.stamp = ros::Time::now();
      path.header.frame_id = "1";
      path.poses.clear();
      pub_goal_path.publish(path);

      path.header.seq++;
    }
  }

  prev_time = odo->header.stamp;
}

void action_callback(ratslam_ros::TopologicalActionConstPtr action, ratslam::ExperienceMap *em)
{
  ROS_DEBUG_STREAM("EM:action_callback{" << ros::Time::now() <<
    "} action=" << action->action <<
    " src=" << action->src_id <<
    " dst=" << action->dest_id <<
    " rad=" << action->relative_rad);

  switch (action->action)
  {
    case ratslam_ros::TopologicalAction::CREATE_NODE:
      em->on_create_experience(action->dest_id);
      em->on_set_experience(action->dest_id, 0);
      ROS_DEBUG_STREAM("EM:CREATE_NODE{" << ros::Time::now() <<
        "} em_id=" << em->get_current_id() <<
        " num=" << em->get_num_experiences());
      break;

    case ratslam_ros::TopologicalAction::CREATE_EDGE:
      em->on_create_link(action->src_id, action->dest_id, action->relative_rad);
      em->on_set_experience(action->dest_id, action->relative_rad);
      ROS_DEBUG_STREAM("EM:CREATE_EDGE{" << ros::Time::now() <<
        "} em_id=" << em->get_current_id() <<
        " num=" << em->get_num_experiences());
      break;

    case ratslam_ros::TopologicalAction::SET_NODE:
      em->on_set_experience(action->dest_id, action->relative_rad);
      ROS_DEBUG_STREAM("EM:SET_NODE{" << ros::Time::now() <<
        "} em_id=" << em->get_current_id() <<
        " num=" << em->get_num_experiences());
      break;
  }

  em->iterate();

  pose_output.header.stamp = ros::Time::now();
  pose_output.header.seq++;
  pose_output.header.frame_id = "1";
  pose_output.pose.position.x = em->get_experience(em->get_current_id())->x_m;
  pose_output.pose.position.y = em->get_experience(em->get_current_id())->y_m;
  pose_output.pose.position.z = 0;
  pose_output.pose.orientation.x = 0;
  pose_output.pose.orientation.y = 0;
  pose_output.pose.orientation.z = sin(em->get_experience(em->get_current_id())->th_rad / 2.0);
  pose_output.pose.orientation.w = cos(em->get_experience(em->get_current_id())->th_rad / 2.0);
  pub_pose.publish(pose_output);

  ROS_DEBUG_STREAM("EM:pub_pose " <<
    " x=" << pose_output.pose.position.x <<
    " y=" << pose_output.pose.position.y <<
    " z=" << pose_output.pose.orientation.z <<
    " w=" << pose_output.pose.orientation.w);

  static ros::Time prev_pub_time(0);

  if (action->header.stamp - prev_pub_time > ros::Duration(30.0))
  {
    prev_pub_time = action->header.stamp;

    em_map.header.stamp = ros::Time::now();
    em_map.header.seq++;
    em_map.node_count = em->get_num_experiences();
    em_map.node.resize(em->get_num_experiences());
    for (int i = 0; i < em->get_num_experiences(); i++)
    {
      em_map.node[i].id = em->get_experience(i)->id;
      em_map.node[i].pose.position.x = em->get_experience(i)->x_m;
      em_map.node[i].pose.position.y = em->get_experience(i)->y_m;
      em_map.node[i].pose.orientation.x = 0;
      em_map.node[i].pose.orientation.y = 0;
      em_map.node[i].pose.orientation.z = sin(em->get_experience(i)->th_rad / 2.0);
      em_map.node[i].pose.orientation.w = cos(em->get_experience(i)->th_rad / 2.0);
    }

    em_map.edge_count = em->get_num_links();
    em_map.edge.resize(em->get_num_links());
    for (int i = 0; i < em->get_num_links(); i++)
    {
      em_map.edge[i].source_id = em->get_link(i)->exp_from_id;
      em_map.edge[i].destination_id = em->get_link(i)->exp_to_id;
      em_map.edge[i].duration = ros::Duration(em->get_link(i)->delta_time_s);
      em_map.edge[i].transform.translation.x = em->get_link(i)->d * cos(em->get_link(i)->heading_rad);
      em_map.edge[i].transform.translation.y = em->get_link(i)->d * sin(em->get_link(i)->heading_rad);
      em_map.edge[i].transform.rotation.x = 0;
      em_map.edge[i].transform.rotation.y = 0;
      em_map.edge[i].transform.rotation.z = sin(em->get_link(i)->facing_rad / 2.0);
      em_map.edge[i].transform.rotation.w = cos(em->get_link(i)->facing_rad / 2.0);
    }
    pub_em.publish(em_map);
  }

  em_marker.header.stamp = ros::Time::now();
  em_marker.header.seq++;
  em_marker.header.frame_id = "1";
  em_marker.type = visualization_msgs::Marker::LINE_LIST;
  em_marker.points.resize(em->get_num_links() * 2);
  em_marker.action = visualization_msgs::Marker::ADD;
  em_marker.scale.x = 0.01;
  //em_marker.scale.y = 1;
  //em_marker.scale.z = 1;
  em_marker.color.a = 1;
  em_marker.ns = "em";
  em_marker.id = 0;
  em_marker.pose.orientation.x = 0;
  em_marker.pose.orientation.y = 0;
  em_marker.pose.orientation.z = 0;
  em_marker.pose.orientation.w = 1;
  for (int i = 0; i < em->get_num_links(); i++)

  {
    em_marker.points[i * 2].x = em->get_experience(em->get_link(i)->exp_from_id)->x_m;
    em_marker.points[i * 2].y = em->get_experience(em->get_link(i)->exp_from_id)->y_m;
    em_marker.points[i * 2].z = 0;
    em_marker.points[i * 2 + 1].x = em->get_experience(em->get_link(i)->exp_to_id)->x_m;
    em_marker.points[i * 2 + 1].y = em->get_experience(em->get_link(i)->exp_to_id)->y_m;
    em_marker.points[i * 2 + 1].z = 0;
  }

  pub_em_markers.publish(em_marker);

#ifdef HAVE_IRRLICHT
  if (use_graphics)
  {
    ems->update_scene();
    ems->draw_all();
  }
#endif
}

void set_goal_pose_callback(geometry_msgs::PoseStampedConstPtr pose, ratslam::ExperienceMap * em)
{
  ROS_DEBUG_STREAM("EM:set_goal_pose_callback x=" << pose->pose.position.x << " y=" << pose->pose.position.y);
  em->add_goal(pose->pose.position.x, pose->pose.position.y);
}

/**
 * Service to retrieve distance between two points.
 * @author Mr-Yellow<mr-yellow@mr-yellow.com>
 * @date 2015-04-25
 * @method get_distance_callback
 * @param {ratslam_ros::GetDistance::Request} req
 * @param {ratslam_ros::GetDistance::Response} res
 * @param {ratslam::ExperienceMap} em
 * @return {bool}
*/
bool get_distance_callback(ratslam_ros::GetDistance::Request  &req, ratslam_ros::GetDistance::Response &res, ratslam::ExperienceMap * em) {
//bool get_distance_callback(ratslam_ros::GetDistance::Request  &req, ratslam_ros::GetDistance::Response &res) {
  res.distance = em->dijkstra_distance_between_experiences(req.id1, req.id2);
  ROS_INFO("request: x=%d, y=%d", (int)req.id1, (int)req.id2);
  ROS_INFO("sending back response: [%ld]", (long int)res.distance);
  return true;
}

int main(int argc, char * argv[])
{
  ROS_INFO_STREAM(argv[0] << " - openRatSLAM Copyright (C) 2012 David Ball and Scott Heath");
  ROS_INFO_STREAM("RatSLAM algorithm by Michael Milford and Gordon Wyeth");
  ROS_INFO_STREAM("Distributed under the GNU GPL v3, see the included license file.");
  ROS_INFO_STREAM("Modified by Mr-Yellow.");

  if (argc < 2)
  {
    ROS_FATAL_STREAM("USAGE: " << argv[0] << " <config_file>");
    exit(-1);
  }
  std::string topic_root = "";
  boost::property_tree::ptree settings, general_settings, ratslam_settings;
  read_ini(argv[1], settings);

  get_setting_child(ratslam_settings, settings, "ratslam", true);
  get_setting_child(general_settings, settings, "general", true);
  get_setting_from_ptree(topic_root, general_settings, "topic_root", (std::string)"");

  if (!ros::isInitialized())
  {
    ros::init(argc, argv, "RatSLAMExperienceMap");
  }
  ros::NodeHandle node;

  ratslam::ExperienceMap * em = new ratslam::ExperienceMap(ratslam_settings);

  // Create first experience to initalise array.
  em->on_create_experience(0);
  em->on_set_experience(0, 0);

  pub_em = node.advertise<ratslam_ros::TopologicalMap>(topic_root + "/ExperienceMap/Map", 1);//
  pub_em_markers = node.advertise<visualization_msgs::Marker>(topic_root + "/ExperienceMap/MapMarker", 1);//

  pub_pose = node.advertise<geometry_msgs::PoseStamped>(topic_root + "/ExperienceMap/RobotPose", 1);//

  pub_goal_path = node.advertise<nav_msgs::Path>(topic_root + "/ExperienceMap/PathToGoal", 1);//

  // Twist publisher by Mr-Yellow 2015-06-21
  // FIXME: lets publish the relative subgoal, then determine what to do with it elsewhere.
  pub_goal_cmd = node.advertise<geometry_msgs::Twist>(topic_root + "/ExperienceMap/CmdToGoal", 1);//

//  ros::Subscriber sub_odometry = node.subscribe<nav_msgs::Odometry>(topic_root + "/odom", 0, boost::bind(odo_callback, _1, em), ros::VoidConstPtr(),
//                                                                    ros::TransportHints().tcpNoDelay());
  ros::Subscriber sub_action = node.subscribe<ratslam_ros::TopologicalAction>(topic_root + "/PoseCell/TopologicalAction", 0, boost::bind(action_callback, _1, em),
                                                                              ros::VoidConstPtr(), ros::TransportHints().tcpNoDelay());

//  ros::Subscriber sub_goal = node.subscribe<geometry_msgs::PoseStamped>(topic_root + "/ExperienceMap/SetGoalPose", 0, boost::bind(set_goal_pose_callback, _1, em),
//                                                                        ros::VoidConstPtr(), ros::TransportHints().tcpNoDelay());

  // Distance server by Mr-Yellow 2015-04-25
  ros::ServiceServer service = node.advertiseService<ratslam_ros::GetDistance::Request, ratslam_ros::GetDistance::Response>(
    topic_root + "/ExperienceMap/GetDistance", boost::bind(get_distance_callback, _1, _2, em)
  );

#ifdef HAVE_IRRLICHT
  boost::property_tree::ptree draw_settings;
  get_setting_child(draw_settings, settings, "draw", true);
  get_setting_from_ptree(use_graphics, draw_settings, "enable", true);
  if (use_graphics)
  {
    ems = new ratslam::ExperienceMapScene(draw_settings, em);
  }
#endif

  ros::spin();

  return 0;
}


//----------------------------------------------------------------------------
// Copyright (C) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the Server Side Public License, version 1,
// as published by the author.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// Server Side Public License for more details.
//
// You should have received a copy of the Server Side Public License
// along with this program. If not, see
// <https://github.com/NilFoundation/plugin/blob/master/LICENSE_1_0.txt>.
//----------------------------------------------------------------------------

#define BOOST_TEST_MODULE por_component_test

#include <boost/test/data/monomorphic.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/unit_test.hpp>

#include <nil/filecoin/storage/proofs/core/components/por.hpp>

#include "../merkle/generate_tree.hpp"

BOOST_AUTO_TEST_SUITE(por_component_test_suite)

type TestTree<H, A> = MerkleTreeWrapper<H, VecStore <H::digest_type>, A, 0, 0 > ;

type TestTree2<H, A, B> = MerkleTreeWrapper<H, VecStore <H::digest_type>, A, B, 0 > ;

type TestTree3<H, A, B, C> = MerkleTreeWrapper<H, VecStore <H::digest_type>, A, B, C > ;

BOOST_AUTO_TEST_CASE(por_test_compound_poseidon_base_8) {
    por_compound<TestTree<PoseidonHasher, 8>>();
}

template<typename MerkleTreeType>
void por_compound() {
    const auto rng = XorShiftRng::from_seed(crate::TEST_SEED);

    const std::size_t leaves = 64 * get_base_tree_count<MerkleTreeType>();

    std::vector<std::uint8_t> data = (0..leaves).flat_map(| _ | fr_into_bytes(&Fr::random(rng))).collect();
    const auto tree = create_base_merkle_tree<MerkleTreeType>(None, leaves, data.as_slice());

    const auto public_inputs = por::PublicInputs {
        challenge : 2,
        commitment : Some(tree.root()),
    };

    const auto setup_params = compound_proof::SetupParams {
        vanilla_params : por::SetupParams {
            leaves,
            private : false,
        },
        partitions : None,
        priority : false,
    };
    const auto public_params = PoRCompound<MerkleTreeType>::setup(&setup_params).expect("setup failed");

    const auto private_inputs =
        por::PrivateInputs<MerkleTreeType>(bytes_into_fr(data_at_node(data.as_slice(), public_inputs.challenge))
                                             .expect("failed to create Fr from node data")
                                             .into(),
                                         &tree, );

    const auto gparams = PoRCompound<MerkleTreeType>::groth_params(Some(rng), &public_params.vanilla_params)
                      .expect("failed to generate groth params");

    const auto proof = PoRCompound<MerkleTreeType>::prove(&public_params, &public_inputs, &private_inputs, &gparams)
                    .expect("failed while proving");

    const auto verified = PoRCompound<MerkleTreeType>::verify(&public_params, &public_inputs, &proof, &NoRequirements)
                       .expect("failed while verifying");
    BOOST_ASSERT(verified);

    const auto(circuit, inputs) = PoRCompound<MerkleTreeType>::circuit_for_test(&public_params, &public_inputs, &private_inputs);

    auto cs = TestConstraintSystem();

    circuit.synthesize(cs).expect("failed to synthesize");
    BOOST_ASSERT(cs.is_satisfied());
    BOOST_ASSERT(cs.verify(&inputs));
}

BOOST_AUTO_TEST_CASE(test_por_circuit_pedersen_base_2) {
    test_por_circuit::<TestTree<PedersenHasher, 2>>(3, 8_247);
}

BOOST_AUTO_TEST_CASE(test_por_circuit_blake2s_base_2) {
    test_por_circuit::<TestTree<Blake2sHasher, 2>>(3, 129_135);
}

BOOST_AUTO_TEST_CASE(test_por_circuit_sha256_base_2) {
    test_por_circuit::<TestTree<Sha256Hasher, 2>>(3, 272_295);
}

BOOST_AUTO_TEST_CASE(test_por_circuit_poseidon_base_2) {
    test_por_circuit::<TestTree<PoseidonHasher, 2>>(3, 1887);
}

BOOST_AUTO_TEST_CASE(test_por_circuit_pedersen_base_4) {
    test_por_circuit::<TestTree<PedersenHasher, 4>>(3, 12399);
}

BOOST_AUTO_TEST_CASE(test_por_circuit_pedersen_sub_8_2) {
    test_por_circuit::<TestTree2<PedersenHasher, 8, 2>>(3, 20663);
}

BOOST_AUTO_TEST_CASE(test_por_circuit_pedersen_top_8_4_2) {
    test_por_circuit::<TestTree3<PedersenHasher, 8, 4, 2>>(3, 24795);
}

BOOST_AUTO_TEST_CASE(test_por_circuit_pedersen_top_8_2_4) {
    // We can handle top-heavy trees with a non-zero subtree arity.
    // These should never be produced, though.
    test_por_circuit::<TestTree3<PedersenHasher, 8, 2, 4>>(3, 24795);
}

BOOST_AUTO_TEST_CASE(test_por_circuit_blake2s_base_4) {
    test_por_circuit::<TestTree<Blake2sHasher, 4>>(3, 130296);
}

BOOST_AUTO_TEST_CASE(test_por_circuit_sha256_base_4) {
    test_por_circuit::<TestTree<Sha256Hasher, 4>>(3, 216258);
}

BOOST_AUTO_TEST_CASE(test_por_circuit_poseidon_base_4) {
    test_por_circuit::<TestTree<PoseidonHasher, 4>>(3, 1164);
}

BOOST_AUTO_TEST_CASE(test_por_circuit_pedersen_base_8) {
    test_por_circuit::<TestTree<PedersenHasher, 8>>(3, 19289);
}

BOOST_AUTO_TEST_CASE(test_por_circuit_blake2s_base_8) {
    test_por_circuit::<TestTree<Blake2sHasher, 8>>(3, 174503);
}

BOOST_AUTO_TEST_CASE(test_por_circuit_sha256_base_8) {
    test_por_circuit::<TestTree<Sha256Hasher, 8>>(3, 250987);
}

BOOST_AUTO_TEST_CASE(test_por_circuit_poseidon_base_8) {
    test_por_circuit::<TestTree<PoseidonHasher, 8>>(3, 1063);
}

BOOST_AUTO_TEST_CASE(test_por_circuit_poseidon_sub_8_2) {
    test_por_circuit::<TestTree2<PoseidonHasher, 8, 2>>(3, 1377);
}

BOOST_AUTO_TEST_CASE(test_por_circuit_poseidon_top_8_4_2) {
    test_por_circuit::<TestTree3<PoseidonHasher, 8, 4, 2>>(3, 1764);
}

BOOST_AUTO_TEST_CASE(test_por_circuit_poseidon_top_8_8) {
    // This is the shape we want for 32GiB sectors.
    test_por_circuit::<TestTree2<PoseidonHasher, 8, 8>>(3, 1593);
}
BOOST_AUTO_TEST_CASE(test_por_circuit_poseidon_top_8_8_2) {
    // This is the shape we want for 64GiB secotrs.
    test_por_circuit::<TestTree3<PoseidonHasher, 8, 8, 2>>(3, 1907);
}

BOOST_AUTO_TEST_CASE(test_por_circuit_poseidon_top_8_2_4() {
    // We can handle top-heavy trees with a non-zero subtree arity.
    // These should never be produced, though.
    test_por_circuit::<TestTree3<PoseidonHasher, 8, 2, 4>>(3, 1764);
}

template <typename Tree>
void test_por_circuit<Tree: static + MerkleTreeTrait>(std::uint num_inputs, std::uint num_constraints) {
    const auto rng = XorShiftRng::from_seed(crate::TEST_SEED);

    // Ensure arity will evenly fill tree.
    const std::size_t leaves = 64 * get_base_tree_count<MerkleTreeType>();

    // -- Basic Setup
    auto data, tree;
    const std::tie(data, tree) = merkletree::generate_tree<Tree>(rng, leaves, None);

    for (std::size_t i = 0; i < leaves; ++i) {
        // println!("challenge: {}, ({})", i, leaves);

        // -- PoR
        const auto pub_params = por::PublicParams {
            leaves,
            private : false,
        };
        const auto pub_inputs = por::PublicInputs:: <typename MerkleTreeType::hash_type::digest_type> {
            challenge : i,
            commitment : Some(tree.root()),
        };
        const auto leaf = data_at_node(data.as_slice(), pub_inputs.challenge);
        const auto leaf_element = typename MerkleTreeType::hash_type::digest_type::try_from_bytes(leaf);
        const auto priv_inputs = por::PrivateInputs::<ResTree<MerkleTreeType>>(leaf_element, &tree);
        const auto p = tree.gen_proof(i);
        BOOST_ASSERT(p.verify());

        // create a non circuit proof
        const auto proof = por::PoR::<ResTree<MerkleTreeType>>::prove(&pub_params, &pub_inputs, &priv_inputs).expect("proving failed");

        // make sure it verifies
        const auto is_valid =
            por::PoR::<ResTree<MerkleTreeType>>::verify(&pub_params, &pub_inputs, &proof).expect("verification failed");
        BOOST_ASSERT_MSG(is_valid, "failed to verify por proof");

        // -- Circuit

        auto cs = TestConstraintSystem<algebra::curves::bls12<381>>();
        const auto por = PoRCircuit::<ResTree<MerkleTreeType>> {
            value : Root::Val(Some(proof.data.into())),
            auth_path : proof.proof.as_options().into(),
            root : Root::Val(Some(pub_inputs.commitment.into())),
            private : false
        };

        por.synthesize(cs).expect("circuit synthesis failed");
        BOOST_ASSERT_MSG(cs.is_satisfied(), "constraints not satisfied");

        BOOST_ASSERT_MSG(cs.num_inputs() == num_inputs, "wrong number of inputs");
        BOOST_ASSERT_MSG(cs.num_constraints() == num_constraints, "wrong number of constraints");

        const auto generated_inputs =
            PoRCompound::<ResTree<MerkleTreeType>>::generate_public_inputs(&pub_inputs, &pub_params, None, );

        const auto expected_inputs = cs.get_inputs();

        for (((input, label), generated_input)
            in expected_inputs.iter().skip(1).zip(generated_inputs.iter())) {

            BOOST_ASSERT_MSG (input == generated_input, std::string(label));
        }

        BOOST_ASSERT_MSG(generated_inputs.len() == expected_inputs.len() - 1, "inputs are not the same length");

        BOOST_ASSERT_MSG(cs.verify(&generated_inputs), "failed to verify inputs");
    }
}

#[ignore] // Slow test – run only when compiled for release.
BOOST_AUTO_TEST_CASE(test_private_por_compound_pedersen_base_2() {
    private_por_test_compound::<TestTree<PedersenHasher, 2>>();
}

#[ignore] // Slow test – run only when compiled for release.
BOOST_AUTO_TEST_CASE(test_private_por_compound_pedersen_base_4() {
    private_por_test_compound::<TestTree<PedersenHasher, 4>>();
}

#[ignore] // Slow test – run only when compiled for release.
BOOST_AUTO_TEST_CASE(test_private_por_compound_poseidon_base_2() {
    private_por_test_compound::<TestTree<PoseidonHasher, 2>>();
}

#[ignore] // Slow test – run only when compiled for release.
BOOST_AUTO_TEST_CASE(test_private_por_compound_poseidon_base_4() {
    private_por_test_compound::<TestTree<PoseidonHasher, 4>>();
}

#[ignore] // Slow test – run only when compiled for release.
BOOST_AUTO_TEST_CASE(test_private_por_compound_poseidon_sub_8_2() {
    private_por_test_compound::<TestTree2<PoseidonHasher, 8, 2>>();
}

#[ignore] // Slow test – run only when compiled for release.
BOOST_AUTO_TEST_CASE(test_private_por_compound_poseidon_top_8_4_2() {
    private_por_test_compound::<TestTree3<PoseidonHasher, 8, 4, 2>>();
}

#[ignore] // Slow test – run only when compiled for release.
BOOST_AUTO_TEST_CASE(test_private_por_compound_poseidon_top_8_8() {
    private_por_test_compound::<TestTree2<PoseidonHasher, 8, 8>>();
}

#[ignore] // Slow test – run only when compiled for release.
BOOST_AUTO_TEST_CASE(test_private_por_compound_poseidon_top_8_8_2() {
    private_por_test_compound::<TestTree3<PoseidonHasher, 8, 8, 2>>();
}

#[ignore] // Slow test – run only when compiled for release.
BOOST_AUTO_TEST_CASE(test_private_por_compound_poseidon_top_8_2_4() {
    private_por_test_compound::<TestTree3<PoseidonHasher, 8, 2, 4>>();
}

void private_por_test_compound < Tree: static + MerkleTreeTrait>() {

    const auto rng = XorShiftRng::from_seed(crate::TEST_SEED);

    // Ensure arity will evenly fill tree.
    const std::size_t leaves = 64 * get_base_tree_count<MerkleTreeType>();

    // -- Basic Setup
    auto data, tree;
    const std::tie(data, tree) = merkletree::generate_tree<Tree>(rng, leaves, None);

    for (std::size_t i = 0; i < 3; ++i) {
        const auto public_inputs = por::PublicInputs {
            challenge : i,
            commitment : None,
        };

        const auto setup_params = compound_proof::SetupParams {
            vanilla_params : por::SetupParams {
                leaves,
                private : true,
            },
            partitions : None,
            priority : false,
        };
        const auto public_params = PoRCompound::<ResTree<MerkleTreeType>>::setup(&setup_params).expect("setup failed");

        const auto private_inputs = por::PrivateInputs::<ResTree<MerkleTreeType>>(
            bytes_into_fr(data_at_node(data.as_slice(), public_inputs.challenge))
                .expect("failed to create Fr from node data")
                .into(),
            &tree, );

        const auto(circuit, inputs) =
            PoRCompound::circuit_for_test(&public_params, &public_inputs, &private_inputs);

        auto cs = TestConstraintSystem();

        circuit.synthesize(cs).expect("failed to synthesize");

        if (!cs.is_satisfied()) {
                panic !("failed to satisfy: {:?}", cs.which_is_unsatisfied());
            }
        BOOST_ASSERT_MSG(cs.verify(&inputs), "verification failed with TestContraintSystem and generated inputs");

        // NOTE: This diagnostic code currently fails, even though the proof generated from the blank circuit verifies.
        // Use this to debug differences between blank and regular circuit generation.
        const auto(circuit1, _inputs) =
            PoRCompound::circuit_for_test(&public_params, &public_inputs, &private_inputs);
        const auto blank_circuit = PoRCompound::<ResTree<MerkleTreeType>>::blank_circuit(&public_params.vanilla_params);

        auto cs_blank = MetricCS();
        blank_circuit.synthesize(cs_blank).expect("failed to synthesize");

        const auto a = cs_blank.pretty_print_list();

        auto cs1 = TestConstraintSystem();
        circuit1.synthesize(cs1).expect("failed to synthesize");
        const auto b = cs1.pretty_print_list();

        for (i, (a, b)) in a.chunks(100).zip(b.chunks(100)).enumerate() {
            BOOST_ASSERT_MSG(a == b, std::format("failed at chunk %d", i));
        }

        const auto blank_groth_params = PoRCompound::<ResTree<MerkleTreeType>>::groth_params(Some(rng), &public_params.vanilla_params, )
                                     .expect("failed to generate groth params");

        const auto proof = PoRCompound::prove(&public_params, &public_inputs, &private_inputs, &blank_groth_params, )
                        .expect("failed while proving");

        const auto verified = PoRCompound::verify(&public_params, &public_inputs, &proof, &NoRequirements)
                           .expect("failed while verifying");

        BOOST_ASSERT(verified);
    }
}

BOOST_AUTO_TEST_CASE(test_private_por_input_circuit_pedersen_binary() {
    test_private_por_input_circuit::<TestTree<PedersenHasher, 2>>(8_246);
}

BOOST_AUTO_TEST_CASE(test_private_por_input_circuit_poseidon_binary() {
    test_private_por_input_circuit::<TestTree<PoseidonHasher, 2>>(1_886);
}

BOOST_AUTO_TEST_CASE(test_private_por_input_circuit_pedersen_quad() {
    test_private_por_input_circuit::<TestTree<PedersenHasher, 4>>(12_398);
}

BOOST_AUTO_TEST_CASE(test_private_por_input_circuit_poseidon_quad() {
    test_private_por_input_circuit::<TestTree<PoseidonHasher, 4>>(1_163);
}

BOOST_AUTO_TEST_CASE(test_private_por_input_circuit_poseidon_oct() {
    test_private_por_input_circuit::<TestTree<PoseidonHasher, 8>>(1_062);
}

template <typename Tree>
void test_private_por_input_circuit<Tree : MerkleTreeTrait>(std::usize_t num_constraints) {
    const auto rng = XorShiftRng::from_seed(crate::TEST_SEED);

    std::size_t leaves = 64 * get_base_tree_count<MerkleTreeType>();
    for (std::size_t i = 0; i < leaves; ++i) {
        // -- Basic Setup

        std::vector<std::uint8_t> data = (0..leaves).flat_map(| _ | fr_into_bytes(&Fr::random(rng))).collect();

        const auto tree = create_base_merkle_tree<MerkleTreeType>(None, leaves, data.as_slice());

        // -- PoR

        const auto pub_params = por::PublicParams {
            leaves,
            private : true,
        };
        const auto pub_inputs = por::PublicInputs {
            challenge : i,
            commitment : None,
        };

        const auto priv_inputs = por::PrivateInputs<MerkleTreeType>(
            bytes_into_fr(data_at_node(data.as_slice(), pub_inputs.challenge)).into(), &tree, );

        // create a non circuit proof
        const auto proof = por::PoR<MerkleTreeType>::prove(&pub_params, &pub_inputs, &priv_inputs).expect("proving failed");

        // make sure it verifies
        const auto is_valid = por::PoR<MerkleTreeType>::verify(&pub_params, &pub_inputs, &proof).expect("verification failed");
        BOOST_ASSERT_MSG(is_valid, "failed to verify por proof");

        // -- Circuit

        auto cs = TestConstraintSystem::<algebra::curves::bls12<381>>();

        const auto por = PoRCircuit<MerkleTreeType> {
            value : Root::Val(Some(proof.data.into())),
            auth_path : proof.proof.as_options().into(),
            root : Root::Val(Some(tree.root().into())),
            private : true
        };

        por.synthesize(cs).expect("circuit synthesis failed");
        BOOST_ASSERT_MSG(cs.is_satisfied(), "constraints not satisfied");

        BOOST_ASSERT_MSG(cs.num_inputs() == 2, "wrong number of inputs");
        BOOST_ASSERT_MSG(cs.num_constraints() == num_constraints, "wrong number of constraints");

        const auto auth_path_bits = challenge_into_auth_path_bits(pub_inputs.challenge, pub_params.leaves);
        const auto packed_auth_path = multipack::compute_multipacking::<algebra::curves::bls12<381>>(&auth_path_bits);

        std::vector<auto> expected_inputs;
        expected_inputs.extend(packed_auth_path);

        BOOST_ASSERT_MSG(cs.get_input(0, "ONE") == Fr::one(), "wrong input 0");

        BOOST_ASSERT_MSG(cs.get_input(1, "path/input 0") == expected_inputs[0], "wrong packed_auth_path");

        BOOST_ASSERT_MSG(cs.is_satisfied(), "constraints are not all satisfied");
        BOOST_ASSERT_MSG(cs.verify(&expected_inputs), "failed to verify inputs");
    }
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright 2013 The CARFAC Authors. All Rights Reserved.
// Author: Ron Weiss <ronw@google.com>
//
// This file is part of an implementation of Lyon's cochlear model:
// "Cascade of Asymmetric Resonators with Fast-Acting Compression"
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "sai.h"

#include <math.h>
#include <iostream>
#include <string>

#include <Eigen/Core>

#include "gtest/gtest.h"

#include "agc.h"
#include "car.h"
#include "carfac.h"
#include "common.h"
#include "ihc.h"
#include "test_util.h"

using testing::Values;

class SAIPeriodicInputTest
    : public SAITestBase,
      public ::testing::WithParamInterface< ::testing::tuple<int, int> > {
 protected:
  void SetUp() {
    period_ = ::testing::get<0>(GetParam());
    num_channels_ = ::testing::get<1>(GetParam());
  }

  int period_;
  int num_channels_;
};

TEST_P(SAIPeriodicInputTest, MultiChannelPulseTrain) {
  const int kInputSegmentWidth = 38;
  ArrayXX segment =
      CreatePulseTrain(num_channels_, kInputSegmentWidth, period_);

  const int kSAIWidth = 15;
  const int kTriggerWindowWidth = kInputSegmentWidth;
  SAIParams sai_params = CreateSAIParams(num_channels_, kInputSegmentWidth,
                                         kTriggerWindowWidth, kSAIWidth);
  sai_params.future_lags = 0;  // Only compute past lags.

  SAI sai(sai_params);
  ArrayXX sai_frame;
  sai.RunSegment(segment, &sai_frame);

  // The output should have peaks at the same positions, regardless of
  // input phase.
  for (int i = 0; i < num_channels_; ++i) {
    const ArrayX& sai_channel = sai_frame.row(i);
    for (int j = sai_channel.size() - 1; j >= 0; j -= period_) {
      EXPECT_TRUE(HasPeakAt(sai_channel, j));
    }
  }

  std::cout << "Input:" << std::endl << segment << std::endl
            << "Output:" << std::endl << sai_frame << std::endl;
}

INSTANTIATE_TEST_SUITE_P(PeriodicInputVariations, SAIPeriodicInputTest,
                         testing::Combine(Values(25, 10, 5, 2),  // periods.
                                          Values(1, 2, 15)));  // num_channels.

class SAITest : public SAITestBase {};

TEST_F(SAITest, DiesIfInputSegmentWidthIsLargerThanBuffer) {
  const int kNumChannels = 2;
  const int kSAIWidth = 10;
  const int kInputSegmentWidth = 200;
  SAIParams valid_params = CreateSAIParams(kNumChannels, kInputSegmentWidth,
                                           kInputSegmentWidth, kSAIWidth);
  SAI sai(valid_params);

  const int kTriggerWindowWidth = kInputSegmentWidth / 10;
  SAIParams params = CreateSAIParams(kNumChannels, kInputSegmentWidth,
                                     kTriggerWindowWidth, kSAIWidth);
  ASSERT_GT(kInputSegmentWidth,
            params.num_triggers_per_frame * params.trigger_window_width);
  ASSERT_DEATH(sai.Redesign(params), "Too few triggers");
}

TEST_F(SAITest, DiesIfInputWidthDoesntMatchInputSegmentWidth) {
  const int kNumChannels = 2;
  const int kInputSegmentWidth = 10;
  const int kPeriod = 4;
  ArrayXX segment = CreatePulseTrain(kNumChannels, kInputSegmentWidth, kPeriod);

  const int kSAIWidth = 20;
  const int kExpectedInputSegmentWidth = kInputSegmentWidth - 1;
  const int kTriggerWindowWidth = kSAIWidth + 1;
  SAIParams sai_params = CreateSAIParams(
      kNumChannels, kExpectedInputSegmentWidth, kTriggerWindowWidth, kSAIWidth);
  ASSERT_NE(sai_params.input_segment_width, kInputSegmentWidth);
  SAI sai(sai_params);
  ArrayXX sai_frame;
  ASSERT_DEATH(sai.RunSegment(segment, &sai_frame), "input samples");
}

TEST_F(SAITest, InputSegmentWidthSmallerThanTriggerWindow) {  // I.e. small hop.
  const int kNumChannels = 1;
  const int kTotalInputSamples = 20;
  const int kPeriod = 5;
  ArrayXX input = CreatePulseTrain(kNumChannels, kTotalInputSamples, kPeriod);

  const int kNumFrames = 4;
  const int kInputSegmentWidth = kTotalInputSamples / kNumFrames;
  const int kTriggerWindowWidth = kTotalInputSamples;
  const int kSAIWidth = 15;
  SAIParams sai_params = CreateSAIParams(kNumChannels, kInputSegmentWidth,
                                         kTriggerWindowWidth, kSAIWidth);
  ASSERT_LT(sai_params.input_segment_width, sai_params.trigger_window_width);
  sai_params.future_lags = 0;  // Only compute past lags.

  ASSERT_GE(kPeriod, kInputSegmentWidth);

  SAI sai(sai_params);
  ArrayXX sai_frame;
  for (int i = 0; i < kNumFrames; ++i) {
    ArrayXX segment =
        input.row(0).segment(i * kInputSegmentWidth, kInputSegmentWidth);
    sai.RunSegment(segment, &sai_frame);

    std::cout << "Frame " << i << std::endl
              << "Input:" << std::endl << segment << std::endl
              << "Output:" << std::endl << sai_frame << std::endl;

    EXPECT_NE(segment.cwiseAbs().sum(), 0);
    // Since the input segment is never all zero, there should always
    // be a peak at zero lag.
    ArrayX sai_channel = sai_frame.row(0);
    EXPECT_TRUE(HasPeakAt(sai_channel, sai_channel.size() - 1));

    if (i == 0) {
      // Since the pulse train period is larger than the input segment
      // size, the first input segment will only see a single impulse,
      // most of the SAI will be zero.
      AssertArrayNear(sai_channel.head(sai_channel.size() - 1),
                      ArrayX::Zero(sai_channel.size() - 1), 1e-9);
    }

    if (i == kNumFrames - 1) {
      // By the last frame, the SAI's internal buffer will have
      // accumulated the full input signal, so the resulting image
      // should contain kPeriod peaks.
      for (int j = sai_channel.size() - 1; j >= 0; j -= kPeriod) {
        EXPECT_TRUE(HasPeakAt(sai_channel, j));
      }
    }
  }
}

TEST_F(SAITest, MatchesMatlabOnBinauralData) {
  const std::string kTestName = "binaural_test";
  const int kInputSegmentWidth = 882;
  const int kNumChannels = 71;
  // The Matlab CARFAC output is transposed compared to the C++.
  ArrayXX input_segment =
      LoadMatrix(kTestName + "-matlab-nap1.txt", kInputSegmentWidth,
                 kNumChannels).transpose();

  const int kTriggerWindowWidth = kInputSegmentWidth;
  const int kSAIWidth = 500;
  SAIParams sai_params = CreateSAIParams(kNumChannels, kInputSegmentWidth,
                                         kTriggerWindowWidth, kSAIWidth);
  SAI sai(sai_params);
  ArrayXX sai_frame;
  sai.RunSegment(input_segment, &sai_frame);
  ArrayXX expected_sai_frame = LoadMatrix(kTestName + "-matlab-sai1.txt",
                                          kNumChannels, sai_params.sai_width);
  AssertArrayNear(expected_sai_frame, sai_frame, kTestPrecision);

  WriteMatrix(kTestName + "-cpp-sai1.txt", sai_frame);
}

TEST_F(SAITest, CARFACIntegration) {
  const int kNumEars = 1;
  const int kInputSegmentWidth = 300;
  ArrayXX segment(kNumEars, kInputSegmentWidth);

  // Sinusoid input.
  const float kFrequency = 10.0;
  segment.row(0) =
      ArrayX::LinSpaced(kInputSegmentWidth, 0.0, 2 * kFrequency * M_PI).sin();

  CARParams car_params;
  IHCParams ihc_params;
  AGCParams agc_params;
  CARFAC carfac(kNumEars, 8000, car_params, ihc_params, agc_params);
  CARFACOutput output(true, false, false, false);
  const bool kOpenLoop = false;
  carfac.RunSegment(segment, kOpenLoop, &output);

  const int kSAIWidth = 20;
  const int kTriggerWindowWidth = kInputSegmentWidth;
  SAIParams sai_params =
      CreateSAIParams(carfac.num_channels(), kInputSegmentWidth,
                      kTriggerWindowWidth, kSAIWidth);
  SAI sai(sai_params);
  ArrayXX sai_frame;
  sai.RunSegment(output.nap()[0], &sai_frame);

  // TODO(ronw): Test something about the output.
}