//
//  su2_x.hpp
//
//  Created by Evan Owen on 4/3/21.
//  Copyright © 2021 Evan Owen. All rights reserved.
//

#ifndef su2_x_hpp
#define su2_x_hpp

#include <random>
#include <complex>
#include <Eigen/Dense>

class su2_x_lattice;

typedef Eigen::Matrix<std::complex<double>, 2, 2> su2_link;
typedef Eigen::Matrix<std::complex<double>, 1, 2> su2_vector;
#define su2_identity su2_link::Identity()
#define su2_zero su2_link::Zero()

#define I complex<double>(0,1)
#define Q(s) s->link[0]
#define D_MAX 6
#define SQRT3   1.73205080756887729352744634151  // sqrt(3)
#define SQRT1_3 0.577350269189625764509148780502 // 1 / sqrt(3)

class su2_x_site {
public:
    // variables
    su2_x_lattice* lattice; // parent lattice
    su2_link link[D_MAX]; // link values in each direction
    su2_link link_inverse[D_MAX]; // link inverse values
    su2_link p_link[D_MAX]; // conjugate momenta in each direction
    su2_x_site* forward[D_MAX]; // adjacent sites in the forward direction
    su2_x_site* backward[D_MAX]; // adjacent sites in the backward direction
    std::mt19937* gen; // random number generator for this time-slice
    bool forward_edge; // site is at the forward edge of the extra dimension
    bool backward_edge; // site is at the backward edge of the extra dimension
    bool is_locked;
    double eps;
    su2_link wf_z[D_MAX]; // exponent for wilson flow

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    // methods
    void init(su2_x_lattice* lattice, su2_x_site* lattice_sites, int s);
    su2_link make_unitary(const su2_link& g);
    su2_link exp_su2(const su2_link& g);
    bool lock();
    void unlock();
    double rand_double(double min = 0.0, double max = 1.0);
    int rand_int(int min, int max);
    double rand_normal(double mean = 0.0, double stdev = 1.0);
    void reset_links(bool cold);
    void copy_links(su2_x_site* site);
    void read_links(std::ifstream& ckptFile, bool bigEndian);
    void set_link(int d, const su2_link& value);
    void init_momenta();
    su2_link create_random_link();
    su2_link create_link(double a);
    double action();
    double hamiltonian();
    void hmc_step_p(double frac);
    void hmc_step_link();
    su2_link p_link_dot(int d);
    double link_trace();
    double plaq();
    su2_link plaquette(int d1, int d2);
    su2_link staple(int d1);
    su2_link reverse_staple(int d1);
    su2_link staple_x(int d1);
    su2_link reverse_staple_x(int d1);
    su2_link cloverleaf(int d1, int d2);
    double wilson_loop(int a, int b);
    std::complex<double> polyakov_loop(int r);
    double correlator(int T);
    double field_strength();
//    double field_strength_x();
    double topological_charge();
    double mag_U();
    double abs_U();
    double four_point(int T, int R);
    su2_link overrelax(su2_link g);
    void relax(bool coulomb);
    double sum_landau();
    double sum_coulomb();
    su2_link sum_G(bool coulomb);
    void heat_bath();
    void heat_bath_link(int d1);
    void cool();
    void cool_link(int d1);
    void wilson_flow(su2_x_site* target, int step);
    void wilson_flow_link(su2_x_site* target, int step, int d);
    void stout_smear(su2_x_site* target, double rho);
    void stout_smear_link(su2_x_site* target, double rho, int d1);
};

class su2_x_lattice {
public:
    // variables
    int N; // array size (spacial)
    int T; // array size (time)
    int N5; // 5th dimension size
    int D; // number of dimensions
    double beta; // coupling factor
    double eps5; // ratio of coupling in extra dimension to normal coupling
    int n_sites; // number of sites in the entire lattice
    int n_sites_5; // number of sites in a 5d sub-lattice
    int n_slice; // number of sites in each time slice of the entire lattice
    int n5_center; // center lattice
    std::vector<su2_x_site> site; // site values
    std::vector<std::mt19937> gen; // array of random number generators (one for each time-slice)
    std::vector<su2_link> sigma; // pauli spin matrices
    int verbose;
    bool parallel;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    // hmc
    std::vector<su2_x_site> site_1; // site values for hmc
    double dt; // hmc step size
    int n_steps; // hmc step count
    int hmc_accept; // number of accepted hmc configurations
    int hmc_count; // number of attempted hmc configurations

    // time step for wilson flow algorithm
    double wf_dt;

    // methods
    su2_x_lattice(int N, int T, int N5, int D, double beta, double eps5, bool cold = false);
    su2_x_lattice(su2_x_lattice* lattice);
    su2_x_lattice(int N, int T, int N5, int D, double beta, double eps5, std::ifstream& ckptFile, bool isNersc = false);
    ~su2_x_lattice();
    void init();
    double rand_double(double min = 0.0, double max = 1.0);
    int rand_int(int min, int max);
    double rand_normal(double mean = 0.0, double stdev = 1.0);
    int get_site(int s, int d, int n);
    double link_trace();
    double plaq(int n5);
    double action(int n5);
    double wilson_loop(int a, int b, int n5);
    double polyakov_loop(int R, int n5);
    double correlator(int T, int n5);
    double four_point(int T, int R, int n5);
    double hamiltonian();
    void hmc(int n_sweeps = 0, bool update_dt = false, bool no_metropolis = false);
    void heat_bath(int n_sweeps = 0);
    void cool(int n5, int n_sweeps = 0);
    void wilson_flow_x(int n_sweeps = 0);
    void wilson_flow(int n5, int n_sweeps = 0);
    void stout_smear(int n5, double rho = 0.1, int n_sweeps = 0);
    double field_strength(int n5);
    double field_strength_x(int n5);
    double topological_charge(int n5);
    int thermalize(int n_min = 0, int n_max = 0);
    double ave_link_t(int n5);
    double theta(bool coulomb, int n5);
    long double relax(long double error_target, bool coulomb, int n5);
};

#endif /* su2_x_hpp */


#include "CMeshModelInstance.h"
#include <Eigen/Dense>
#include <igl/per_vertex_normals.h>
#include <assert.h>
#include <totalmodel.h>
#include <ceres/rotation.h>
#include <chrono>

// Function equivalent and improved from igl::per_vertex_normals
// [270, 452] ms
template <typename T>
inline T getNormTriplet(const T* const ptr)
{
    return std::sqrt(ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2]);
}
template <typename T>
inline void normalizeTriplet(T* ptr, const T norm)
{
    ptr[0] /= norm;
    ptr[1] /= norm;
    ptr[2] /= norm;
}
template <typename T>
inline void normalizeTriplet(T* ptr)
{
    const auto norm = getNormTriplet(ptr);
    normalizeTriplet(ptr, norm);
}
void per_vertex_normals(
  const Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor>& V,
  const Eigen::Matrix<int, Eigen::Dynamic, 3, Eigen::RowMajor>& F,
  Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor>& N
)
{
    Eigen::Matrix<double, Eigen::Dynamic,3, Eigen::RowMajor> FN;
    FN.resize(F.rows(),3);
    auto* FN_data = FN.data();
    const auto* const F_data = F.data();
    const auto* const V_data = V.data();
    // loop over faces
    for (int i = 0; i < F.rows();i++)
    {
        const auto baseIndex = 3*i;
        const auto F_data0 = 3*F_data[baseIndex];
        const auto F_data1 = 3*F_data[baseIndex+1];
        const auto F_data2 = 3*F_data[baseIndex+2];
        const Eigen::Matrix<double, 1, 3> v1(
            V_data[F_data1] - V_data[F_data0],
            V_data[F_data1+1] - V_data[F_data0+1],
            V_data[F_data1+2] - V_data[F_data0+2]);
        const Eigen::Matrix<double, 1, 3> v2(
            V_data[F_data2] - V_data[F_data0],
            V_data[F_data2+1] - V_data[F_data0+1],
            V_data[F_data2+2] - V_data[F_data0+2]);
        FN.row(i) = v1.cross(v2);
        auto* fnRowPtr = &FN_data[baseIndex];
        const double norm = getNormTriplet(fnRowPtr);
        if (norm == 0)
        {
            fnRowPtr[0] = 0;
            fnRowPtr[1] = 0;
            fnRowPtr[2] = 0;
        }
        else
            normalizeTriplet(fnRowPtr, norm);
    }

    // Resize for output
    N.resize(V.rows(),3);
    std::fill(N.data(), N.data() + N.rows() * N.cols(), 0.0);

    Eigen::Matrix<double, Eigen::Dynamic, 1> A(F.rows(), 1);
    auto* A_data = A.data();
    const auto Fcols = F.cols();
    const auto Vcols = V.cols();

  // Projected area helper
  const auto & proj_doublearea =
    [&V_data,&F_data, &Vcols, &Fcols](const int x, const int y, const int f)
    ->double
  {
    const auto baseIndex = f*Fcols;
    const auto baseIndex2 = F_data[baseIndex + 2]*Vcols;
    const auto rx = V_data[F_data[baseIndex]*Vcols + x] - V_data[baseIndex2 + x];
    const auto sx = V_data[F_data[baseIndex + 1]*Vcols + x] - V_data[baseIndex2 + x];
    const auto ry = V_data[F_data[baseIndex]*Vcols + y] - V_data[baseIndex2 + y];
    const auto sy = V_data[F_data[baseIndex + 1]*Vcols + y] - V_data[baseIndex2 + y];
    return rx*sy - ry*sx;
  };

  for (auto f = 0;f<F.rows();f++)
  {
    const auto dblAd1 = proj_doublearea(0,1,f);
    const auto dblAd2 = proj_doublearea(1,2,f);
    const auto dblAd3 = proj_doublearea(2,0,f);
    A_data[f] = std::sqrt(dblAd1*dblAd1 + dblAd2*dblAd2 + dblAd3*dblAd3);
  }

    auto* N_data = N.data();
    // loop over faces
    for (int i = 0 ; i < F.rows();i++)
    {
        const auto baseIndex = i*Fcols;
        // throw normal at each corner
        for (int j = 0; j < 3;j++)
        {
            // auto* nRowPtr = &N_data[3*F(i,j)];
            auto* nRowPtr = &N_data[3*F_data[baseIndex + j]];
            const auto* const fnRowPtr = &FN_data[3*i];
            for (int subIndex = 0; subIndex < FN.cols(); subIndex++)
                nRowPtr[subIndex] += A_data[i] * fnRowPtr[subIndex];
            // Vector equilvanet
            // N.row(F(i,j)) += A_data[i] * FN.row(i);
        }
    }

    // take average via normalization
    // loop over faces
    for (int i = 0;i<N.rows();i++)
        normalizeTriplet(&N_data[3*i]);
    // Matrix equivalent
    // N.rowwise().normalize();
}

void CMeshModelInstance::RecomputeNormal(const TotalModel& model)
{
    // Compute Normal
    Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor> V_3(m_vertices.size(), 3);
    auto* V_3_data = V_3.data();

    for (int r = 0; r < V_3.rows(); ++r)
    {
        auto* v3rowPtr = &V_3_data[3*r];
        v3rowPtr[0] = m_vertices[r].x; // V_3(r, 0)
        v3rowPtr[1] = m_vertices[r].y; // V_3(r, 1)
        v3rowPtr[2] = m_vertices[r].z; // V_3(r, 2)
    }
    // Eigen::MatrixXd NV;
    Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor> NV;

    if (m_meshType==MESH_TYPE_SMPL)
    {
        std::string errorMessage("Not supporting MESH_TYPE_SMPL currently");
        throw std::runtime_error(errorMessage);
        // igl::per_vertex_normals(V_3, g_smpl.faces_, NV);
    }
    if (m_meshType == MESH_TYPE_TOTAL || m_meshType == MESH_TYPE_ADAM)
    {
        // igl::per_vertex_normals(V_3, model.m_faces, NV);
        per_vertex_normals(V_3, model.m_faces, NV);
        // Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor> NVAux;
        // igl::per_vertex_normals(V_3, model.m_faces, NVAux);
        // std::cout << (NV - NVAux).norm() << std::endl;
        // assert((NV - NVAux).norm() < 1e-6);
    }
    m_normals.resize(NV.rows());
    auto* NV_data = NV.data();
    for (int r = 0; r < NV.rows(); ++r)
    {
        const auto* const nvRow = &NV_data[3*r];
        m_normals[r] = cv::Point3f(nvRow[0], nvRow[1], nvRow[2]); // cv::Point3f(NV(r, 0), NV(r, 1), NV(r, 2))
    }
}

void CMeshModelInstance::clearMesh()
{
    m_face_vertexIndices.clear();
    m_vertices.clear();
    m_colors.clear();
    m_normals.clear();
    m_uvs.clear();
    m_joints.clear();
    m_joints_regress.clear();
    m_alpha.clear();
}

bool compareTupleDepth(const std::tuple<double, double, cv::Point3i>& a, const std::tuple<double, double, cv::Point3i>& b)
{
    return std::get<1>(a) > std::get<1>(b);   // from far to near
}

bool compareTupleAlpha(const std::tuple<double, double, cv::Point3i>& a, const std::tuple<double, double, cv::Point3i>& b)
{
    return std::get<0>(a) > std::get<0>(b);   // opaque first, then transparent
}

void CMeshModelInstance::sortFaceDepth(const cv::Point3d angleaxis)
{
    // const auto start = std::chrono::high_resolution_clock::now();
    std::vector<double> depth_vertex(m_vertices.size());
    if (angleaxis == cv::Point3d(0., 0., 0.))   // no rotation
    {
        for (auto i = 0u; i < m_vertices.size(); i++)
            depth_vertex[i] = m_vertices[i].z;
    }
    else
    {
        const double angle_axis[3] = {angleaxis.x, angleaxis.y, angleaxis.z};
        for (auto i = 0u; i < m_vertices.size(); i++)
        {
            const double pt[3] = {m_vertices[i].x, m_vertices[i].y, m_vertices[i].z};
            double result[3];
            ceres::AngleAxisRotatePoint(angle_axis, pt, result);
            depth_vertex[i] = result[2];
        }
    }

    assert(m_face_vertexIndices.size() % 3 == 0);
    std::vector<std::tuple<double, double, cv::Point3i>> vecSort;
    const uint num_face = m_face_vertexIndices.size() / 3;
    vecSort.reserve(num_face);
    for (auto i = 0u; i < num_face; i++)
    {
        const uint I1 = m_face_vertexIndices[3 * i];
        const uint I2 = m_face_vertexIndices[3 * i + 1];
        const uint I3 = m_face_vertexIndices[3 * i + 2];
        const double depth = (depth_vertex[I1] + depth_vertex[I2] + depth_vertex[I3]) / 3;
        const double alpha = (m_alpha[I1] + m_alpha[I2] + m_alpha[I3]) / 3;  // opaqueness
        vecSort.emplace_back(std::make_tuple(alpha, depth, cv::Point3i(I1, I2, I3)));
    }
    std::sort(vecSort.begin(), vecSort.end(), compareTupleAlpha);  // first sort according to opacity, put opaque face first
    auto it = vecSort.begin();
    while(it != vecSort.end())
    {
        // find the first element that is not completely opaque
        if (std::get<0>(*it) != 1.0)
            break;
        it++;
    }
    std::sort(it, vecSort.end(), compareTupleDepth);  // now sort according to depth, far first
    for (auto i = 0u; i < num_face; i++)
    {
        auto& pt = std::get<2>(vecSort[i]);
        m_face_vertexIndices[3 * i + 0] = pt.x;
        m_face_vertexIndices[3 * i + 1] = pt.y;
        m_face_vertexIndices[3 * i + 2] = pt.z;
    }
    // const auto duration = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::high_resolution_clock::now() - start).count();
    // std::cout << "Sort depth duration " << duration * 1e-6 << " ms" << std::endl;
}


#include <Rcpp.h>
#include <RcppEigen.h>
#include <boost/math/special_functions/binomial.hpp>

//[[Rcpp::export(.partialSumEigen)]]
Eigen::VectorXd partialSumEigen(const Eigen::VectorXd & x)
{
    const std::size_t N = x.size();
    Eigen::VectorXd partialSum(N + 1);

    partialSum[0] = 0;
    for (std::size_t i = 1; i < N + 1; i++)
    {
        partialSum[i] = x[i - 1] + partialSum[i - 1];
    }

    return partialSum;
}

Eigen::MatrixXd bindColumns(const Eigen::MatrixXd & A, const Eigen::MatrixXd & B)
{
    Eigen::MatrixXd C(A.rows(), A.cols() + B.cols());
    C << A, B;
    return C;
}

Eigen::VectorXd STDEigen(const std::vector<double> &x)
{
    Eigen::VectorXd xEigen(x.size());
    for (std::size_t i = 0; i < x.size(); i++)
        xEigen[i] = x[i];

    return xEigen;
}

std::vector<double> EigenSTD(const Eigen::VectorXd &x)
{
    std::vector<double> xNEWMAT(x.size());
    for (std::size_t i = 0; i < x.size(); i++)
        xNEWMAT[i] = x[i];

    return xNEWMAT;
}

Eigen::Vector2i nonZeroElementsOfMarker(const Eigen::VectorXd & decodedProfile_m)
{
    const std::size_t N = decodedProfile_m.size();

    Eigen::Vector2i res;
    std::size_t i = 0;
    std::size_t j = 0;
    while(j < 2)
    {
        if (decodedProfile_m[i] != 0)
        {
            int k = static_cast<int>(decodedProfile_m[i]);
            while(k > 0)
            {
                res[j] = i;
                k -= 1;
                j += 1;
            }
        }
        i += 1;
    }

    return res;
}

std::vector<int> sortedIndex(const Eigen::VectorXd & x)
{
    std::vector<int> x_sorted(x.size());
    std::iota(x_sorted.begin(), x_sorted.end(), 0);
    auto comparator = [&x](int i, int j){ return x[i] > x[j]; };

    std::sort(x_sorted.begin(), x_sorted.end(), comparator);

    return x_sorted;
}

//[[Rcpp::export()]]
Eigen::MatrixXd generatePossibleGenotypes(const std::size_t & N)
{
    if (N == 1)
    {
        Eigen::MatrixXd possibleSingleprofilesMatrix = 2*Eigen::MatrixXd::Ones(N, N);
        return possibleSingleprofilesMatrix;
    }

    const std::size_t possibleSingles = N;
    const std::size_t possiblePairs = boost::math::binomial_coefficient<double>(N, 2);

    const std::size_t totalPossibleOutcomes = possibleSingles + possiblePairs;
    const std::size_t totalRuns = std::floor(possiblePairs / N);

    Eigen::MatrixXd possibleSingleprofilesMatrix = Eigen::MatrixXd::Zero(N, totalPossibleOutcomes);
    for (std::size_t i = 0; i < N; i++)
    {
        // Single Values
        Eigen::VectorXd singleVector = Eigen::VectorXd::Zero(N);
        singleVector(i) = 2;
        possibleSingleprofilesMatrix.col(i) = singleVector;

        // Paired Values
        for (std::size_t j = 1; j <= totalRuns; j++)
        {
            Eigen::VectorXd pairsVector = Eigen::VectorXd::Zero(N);
            pairsVector(i) = 1;
            pairsVector((i + j) % N) = 1;
            possibleSingleprofilesMatrix.col(j*N + i) = pairsVector;
        }
    }

    // Remaining Paired Values
    for (std::size_t k = (totalRuns + 1)*N; k < totalPossibleOutcomes; k++)
    {
        Eigen::VectorXd pairsVector = Eigen::VectorXd::Zero(N);
        pairsVector(k % N) = 1;
        pairsVector(((k % N) + (totalRuns + 1)) % N) = 1;
        possibleSingleprofilesMatrix.col(k) = pairsVector;
    }

    return possibleSingleprofilesMatrix;
}


/** @file */
#pragma once
#include <boost/math/tools/minima.hpp>
#include <cmath>
#include <string_view>

namespace pw85 {

namespace metadata {
constexpr std::string_view author{"S. Brisard"};
// clang-format off
constexpr std::string_view description{"Implementation of the \"contact function\" defined by Perram and Wertheim (J. Comp. Phys. 58(3), 409-416, DOI:10.1016/0021-9991(85)90171-8) for two ellipsoids."};
// clang-format on
constexpr std::string_view author_email{"sebastien.brisard@univ-eiffel.fr"};
constexpr std::string_view license{"BSD 3-Clause License"};
constexpr std::string_view name{"pw85"};
constexpr std::string_view url{"https://github.com/sbrisard/pw85"};
constexpr std::string_view version{"2.0"};
constexpr std::string_view year{"2021"};
}  // namespace metadata

/** The dimension of the physical space (3).*/
constexpr size_t dim = 3;

/** The dimension of the space of symmetric matrices (6). */
constexpr size_t sym = 6;

/*
 * For the Brent algorithm, these two constants should be such that
 *
 * [(3 - sqrt(5)) / 2] ** n < eps
 *
 * where
 *
 *     n = max_iter
 *     eps = lambda_atol
 */

/**
 * The absolute tolerance for the stopping criterion of Brent’s method (in
 * function contact_function()).
 */
constexpr double lambda_atol = 1e-6;

/**
 * The maximum number of iterations of Brent’s method (in function
 * contact_function()).
 */
constexpr size_t max_iter = 25;

/**
 * The total number of iterations of the Newton–Raphson refinement phase (in
 * function contact_function()).
 */
constexpr size_t nr_iter = 3;

/**
 * Compute the Cholesky decomposition of a symmetric, positive matrix.
 *
 * Let `A` be a symmetric, positive matrix, defined by the `double[6]` array
 * `a`. This function computes the lower-triangular matrix `L`, defined by
 * the `double[6]` array `l`, such that `Lᵀ⋅L = A`.
 *
 * The array `l` must be pre-allocated; it is modified by this function. Note
 * that storage of the coefficients of `L` is as follows
 *
 * ```
 *     ⎡ l[0]    0    0 ⎤
 * L = ⎢ l[1] l[3]    0 ⎥.
 *     ⎣ l[2] l[4] l[5] ⎦
 * ```
 */
void _cholesky_decomp(double const *a, double *l) {
  l[0] = sqrt(a[0]);
  l[1] = a[1] / l[0];
  l[2] = a[2] / l[0];
  l[3] = sqrt(a[3] - l[1] * l[1]);
  l[4] = (a[4] - l[1] * l[2]) / l[3];
  l[5] = sqrt(a[5] - l[2] * l[2] - l[4] * l[4]);
}

/**
 * Compute the solution to a previously Cholesky decomposed linear system.
 *
 * Let `L` be a lower-triangular matrix, defined by the `double[6]` array `l`
 * (see pw85::_cholesky_decomp() for ordering of the coefficients). This
 * function solves (by substitution) the linear system `Lᵀ⋅L⋅x = b`, where the
 * vectors `x` and `b` are specified through their `double[3]` array of
 * coordinates; `x` is modified by this function.
 */
void _cholesky_solve(const double *l, const double *b, double *x) {
  /* Solve L.y = b */
  double const y0 = b[0] / l[0];
  double const y1 = (b[1] - l[1] * y0) / l[3];
  double const y2 = (b[2] - l[2] * y0 - l[4] * y1) / l[5];

  /* Solve L^T.x = y */
  x[2] = y2 / l[5];
  x[1] = (y1 - l[4] * x[2]) / l[3];
  x[0] = (y0 - l[1] * x[1] - l[2] * x[2]) / l[0];
}

/**
 * Compute the quadratic form associated to a spheroid.
 *
 * The spheroid is defined by its equatorial radius `a`, its polar radius `c`
 * and the direction of its axis of revolution, `n` (unit-vector, `double[3]`
 * array).
 *
 * `q` is the representation of a symmetric matrix as a `double[6]` array. It is
 * modified in-place.
 */
void spheroid(double a, double c, const double *n, double *q) {
  double const a2 = a * a;
  double const c2_minus_a2 = c * c - a2;
  double const nx = n[0];
  double const ny = n[1];
  double const nz = n[2];
  q[0] = nx * nx * c2_minus_a2 + a2;
  q[3] = ny * ny * c2_minus_a2 + a2;
  q[5] = nz * nz * c2_minus_a2 + a2;
  q[4] = ny * nz * c2_minus_a2;
  q[2] = nx * nz * c2_minus_a2;
  q[1] = nx * ny * c2_minus_a2;
}

/**
 * Return the value of the opposite of the function ``f`` defined as (see
 * @verbatim embed:rst:inline :ref:`theory`@endverbatim).
 *
 * @f[f(\lambda)=\lambda\bigl(1-\lambda\bigr)r_{12}^{\mathsf{T}}
 *               \cdot Q^{-1}\cdot r_{12},@f]
 *
 * with
 *
 * @f[Q = \bigl(1-\lambda\bigr)Q_1 + \lambda Q_2,@f]
 *
 * where ellipsoids 1 and 2 are defined as the sets of points @f$m@f$
 * (column-vector) such that
 *
 * @f[\bigl(m-c_i\bigr)\cdot Q_i^{-1}\cdot\bigl(m-c_i\bigr)\leq1.@f]
 *
 * In the above inequality, @f$c_i@f$ is the center; @f$r_{12}=c_2-c_1@f$ is the
 * center-to-center radius-vector, represented by the `double[3]` array `r12`.
 * The symmetric, positive-definite matrices @f$Q_1@f$ and @f$Q_2@f$ are
 * specified through the `double[6]` arrays `q1` and `q2`.
 *
 * The value of @f$\lambda@f$ is specified through the parameter ``lambda``.
 *
 * This function returns the value of @f$−f(\lambda)@f$ (the “minus” sign comes
 * from the fact that we seek the maximum of @f$f@f$, or the minimum of
 * @f$−f@f$).
 *
 * This implementation uses
 * @verbatim embed:rst:inline:ref:`Cholesky decompositions
 * <implementation-cholesky>`@endverbatim.
 */
double f_neg(double lambda, const double *r12, const double *q1,
             const double *q2) {
  double const *q1_i = q1;
  double const *q2_i = q2;
  double q[sym];
  double *q_i = q;
  double q12[sym];
  double *q12_i = q12;
  for (size_t i = 0; i < sym; i++, q1_i++, q2_i++, q_i++, q12_i++) {
    *q_i = (1 - lambda) * (*q1_i) + lambda * (*q2_i);
    *q12_i = (*q2_i) - (*q1_i);
  }
  double l[sym];
  _cholesky_decomp(q, l);
  double s[dim];
  _cholesky_solve(l, r12, s);
  double const *r_i = r12;
  double *s_i = s;
  double rs = 0.;
  for (size_t i = 0; i < dim; i++, r_i++, s_i++) {
    rs += (*r_i) * (*s_i);
  }
  return -lambda * (1. - lambda) * rs;
}

/**
 * Compute the residual \f$g(\lambda)=\mu_2^2-\mu_1^2\f$.
 *
 * See @verbatim embed:rst:inline :ref:`optimization` @endverbatim for the
 * definition of \f$g\f$. The value of \f$\lambda\f$ is specified through the
 * parameter `lambda`. See contact_function() for the definition of the
 * parameters `r12`, `q1` and `q2`.
 *
 * The preallocated `double[3]` array `out` is updated as follows:
 * `out[0]=`\f$f(\lambda)\f$, `out[1]=`\f$g(\lambda)\f$ and
 * `out[2]=`\f$g'(\lambda)\f$.
 *
 * This function is used in function
 * @verbatim embed:rst:inline :cpp:func:`pw85::contact_function` @endverbatim
 * for the final Newton–Raphson refinement step.
 */
void _residual(double lambda, const double *r12, const double *q1,
               const double *q2, double *out) {
  double q[sym];
  double q12[sym];
  for (size_t i = 0; i < sym; i++) {
    q[i] = (1. - lambda) * q1[i] + lambda * q2[i];
    q12[i] = q2[i] - q1[i];
  }
  double l[sym];
  _cholesky_decomp(q, l);
  double s[dim];
  _cholesky_solve(l, r12, s);
  double u[] = {q12[0] * s[0] + q12[1] * s[1] + q12[2] * s[2],
                q12[1] * s[0] + q12[3] * s[1] + q12[4] * s[2],
                q12[2] * s[0] + q12[4] * s[1] + q12[5] * s[2]};
  double v[dim];
  _cholesky_solve(l, u, v);
  double rs = r12[0] * s[0] + r12[1] * s[1] + r12[2] * s[2];
  double su = s[0] * u[0] + s[1] * u[1] + s[2] * u[2];
  double uv = u[0] * v[0] + u[1] * v[1] + u[2] * v[2];

  out[0] = lambda * (1. - lambda) * rs;
  out[1] = (2 * lambda - 1.) * rs + lambda * (1. - lambda) * su;
  out[2] =
      2. * rs + 2. * (1. - 2. * lambda) * su - 2. * lambda * (1. - lambda) * uv;
}

/**
 * Compute the value of the contact function of two ellipsoids.
 *
 * The center-to-center radius-vector @f$r_{12}@f$ is specified by the
 * `double[3]` array `r12`. The symmetric, positive-definite matrices @f$Q_1@f$
 * and @f$Q_2@f$ that define the two ellipsoids are specified through the
 * `double[6]` arrays `q1` and `q2`.
 *
 * This function computes the value of @f$\mu^2@f$, defined as
 *
 * @f[\mu^2=\max_{0\leq\lambda\leq 1}\bigl\{\lambda\bigl(1-\lambda\bigr)
 * r_{12}^{\mathsf{T}}\cdot\bigl[\bigl(1-\lambda\bigr)Q_1+\lambda Q_2\bigr]^{-1}
 * \cdot r_{12}\bigr\},@f]
 *
 * and the maximizer @f$\lambda@f$, see
 * @verbatim embed:rst:inline:ref:`theory` @endverbatim. Both values are stored
 * in the preallocated `double[2]` array `out`: `out[0] = `@f$\mu^2@f$ and
 * `out[1] = `@f$\lambda@f$.
 *
 * @f$\mu@f$ is the common factor by which the two ellipsoids must be scaled
 * (their centers being fixed) in order to be tangentially in contact.
 *
 * This function returns `0`.
 *
 * @verbatim embed:rst:leading-asterisk
 * .. todo:: This function should return an error code.
 * @endverbatim
 */
int contact_function(const double *r12, const double *q1, const double *q2,
                     double *out) {
  auto f = [r12, q1, q2](double lambda) { return f_neg(lambda, r12, q1, q2); };

  auto r = boost::math::tools::brent_find_minima(
      f, 0., 1., std::numeric_limits<double>::digits / 2);
  auto lambda_brent = r.first;
  auto mu2_brent = -r.second;

  double out_res[3];
  _residual(lambda_brent, r12, q1, q2, out_res);
  double res_brent = fabs(out_res[1]);

  /* Try to refine the estimate. */
  double lambda_nr = lambda_brent;
  double mu2_nr = mu2_brent;
  double res_nr = res_brent;

  for (size_t i = 0; i < nr_iter; i++) {
    double lambda_trial = lambda_nr - out_res[1] / out_res[2];
    if ((lambda_trial < 0.) || (lambda_trial > 1.)) {
      break;
    }
    _residual(lambda_trial, r12, q1, q2, out_res);
    lambda_nr = lambda_trial;
    mu2_nr = out_res[0];
    res_nr = fabs(out_res[1]);
  }

  if (res_nr < res_brent) {
    out[0] = mu2_nr;
    out[1] = lambda_nr;
  } else {
    out[0] = mu2_brent;
    out[1] = lambda_brent;
  }

  /* TODO: return error code. */
  return 0;
}
}  // namespace pw85


/*!
  \file gpp_knowledge_gradient_inner_optimization.hpp
  \rst
  1. OVERVIEW OF KNOWLEDGE GRADIENT WHAT ARE WE TRYING TO DO?
  2. IMPLEMENTATION NOTES
  3. CITATIONS

    **1. OVERVIEW OF KNOWLEDGE GRADIENT; WHAT ARE WE TRYING TO DO?**

    .. Note:: these comments are copied in Python: interfaces/__init__.py

    The optimization process models the objective using a Gaussian process (GP) prior
    (also called a GP predictor) based on the specified covariance and the input
    data (e.g., through member functions ComputeMeanOfPoints, ComputeVarianceOfPoints).  Using the GP,
    we can compute the knowledge gradient (KG) from sampling any particular point.  KG
    is defined relative to the best currently known value, and it represents what the
    algorithm believes is the most likely outcome from sampling a particular point in parameter
    space (aka conducting a particular experiment).

    See KnowledgeGradientEvaluator class docs for further details on computing KG.
    Both support ComputeKnowledgeGradient() and ComputeGradKnowledgeGradient().

    The behavior of the GP is controlled by its underlying
    covariance function and the data/uncertainty of prior points (experiments).

    With the ability of the compute KG, the final step is to optimize
    to find the best KG.  This is done using multistart gradient descent (MGD), in
    ComputeKGOptimalPointsToSample(). This method wraps a MGD call and falls back on random search
    if that fails. See gpp_optimization.hpp for multistart/optimization templates. This method
    can evaluate and optimize KG at serval points simultaneously; e.g., if we wanted to run 4 simultaneous
    experiments, we can use KG to select all 4 points at once.

    Additionally, there are use cases where we have existing experiments that are not yet complete but
    we have an opportunity to start some new trials. For example, maybe we are a drug company currently
    testing 2 combinations of dosage levels. We got some new funding, and can now afford to test
    3 more sets of dosage parameters. Ideally, the decision on the new experiments should depend on
    the existence of the 2 ongoing tests. We may not have any data from the ongoing experiments yet;
    e.g., they are [double]-blind trials. If nothing else, we would not want to duplicate any
    existing experiments! So we want to solve 3-KG using the knowledge of the 2 ongoing experiments.

    We call this q,p-KG, so the previous example would be 3,2-KG. So q is the number of new
    (simultaneous) experiments to select. In code, this would be the size of the output from KG
    optimization (i.e., ``best_points_to_sample``, of which there are ``q = num_to_sample points``).
    p is the number of ongoing/incomplete experiments to take into account (i.e., ``points_being_sampled``
    of which there are ``p = num_being_sampled`` points).

    Back to optimization: the idea behind gradient descent is simple.  The gradient gives us the
    direction of steepest ascent (negative gradient is steepest descent).  So each iteration, we
    compute the gradient and take a step in that direction.  The size of the step is not specified
    by GD and is left to the specific implementation.  Basically if we take steps that are
    too large, we run the risk of over-shooting the solution and even diverging.  If we
    take steps that are too small, it may take an intractably long time to reach the solution.
    Thus the magic is in choosing the step size; we do not claim that our implementation is
    perfect, but it seems to work reasonably.  See ``gpp_optimization.hpp`` for more details about
    GD as well as the template definition.

    For particularly difficult problems or problems where gradient descent's parameters are not
    well-chosen, GD can fail to converge.  If this happens, we can fall back on heuristics;
    e.g., 'dumb' search (i.e., evaluate EI at a large number of random points and take the best
    one). Naive search lives in: ComputeOptimalPointsToSampleViaLatinHypercubeSearch<>().

    **2. IMPLEMENTATION NOTES**

    a. This file has a few primary endpoints for KG optimization:

       i. ComputeKGOptimalPointsToSampleWithRandomStarts<>():

          Solves the q,p-KG problem.

          Takes in a gaussian_process describing the prior, domain, config, etc.; outputs the next best point(s) (experiment)
          to sample (run). Uses gradient descent.

       ii. ComputeKGOptimalPointsToSampleViaLatinHypercubeSearch<>():

           Estimates the q,p-KG problem.

           Takes in a gaussian_process describing the prior, domain, etc.; outputs the next best point(s) (experiment)
           to sample (run). Uses 'dumb' search.

       iii. ComputeKGOptimalPointsToSample<>() (Recommended):

            Solves the q,p-KG problem.

            Wraps the previous two items; relies on gradient descent and falls back to "dumb" search if it fails.

       .. NOTE::
           See ``gpp_knowledge_gradient_optimization.cpp``'s header comments for more detailed implementation notes.

           There are also several other functions with external linkage in this header; these
           are provided primarily to ease testing and to permit lower level access from python.

    b. See ``gpp_common.hpp`` header comments for additional implementation notes.

    **3. CITATIONS**

    a. Gaussian Processes for Machine Learning.
       Carl Edward Rasmussen and Christopher K. I. Williams. 2006.
       Massachusetts Institute of Technology.  55 Hayward St., Cambridge, MA 02142.
       http://www.gaussianprocess.org/gpml/ (free electronic copy)

    b. The Knowledge-Gradient Policy for Correlated Normal Beliefs.
       P.I. Frazier, W.B. Powell & S. Dayanik.
       INFORMS Journal on Computing, 2009.

    c. Differentiation of the Cholesky Algorithm.
       S. P. Smith. 1995.
       Journal of Computational and Graphical Statistics. Volume 4. Number 2. p134-147

    d. A Multi-points Criterion for Deterministic Parallel Global Optimization based on Gaussian Processes.
       David Ginsbourger, Rodolphe Le Riche, and Laurent Carraro.  2008.
       D´epartement 3MI. Ecole Nationale Sup´erieure des Mines. 158 cours Fauriel, Saint-Etienne, France.
       ginsbourger@emse.fr, leriche@emse.fr, carraro@emse.fr
\endrst*/

#ifndef MOE_OPTIMAL_LEARNING_CPP_GPP_KNOWLEDGE_GRADIENT_INNER_OPTIMIZATION_HPP_
#define MOE_OPTIMAL_LEARNING_CPP_GPP_KNOWLEDGE_GRADIENT_INNER_OPTIMIZATION_HPP_

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

#include <boost/math/distributions/normal.hpp>  // NOLINT(build/include_order)

#include "gpp_common.hpp"
#include "gpp_domain.hpp"
#include "gpp_exception.hpp"
#include "gpp_covariance.hpp"
#include "gpp_logging.hpp"
#include "gpp_math.hpp"
#include "gpp_optimization.hpp"
#include "gpp_optimizer_parameters.hpp"
#include "gpp_random.hpp"

namespace optimal_learning {

struct FuturePosteriorMeanState;
/*!\rst
  This is a specialization of the ExpectedImprovementEvaluator class for when the number of potential samples is 1; i.e.,
  ``num_to_sample == 1`` and the number of concurrent samples is 0; i.e. ``num_being_sampled == 0``.
  In other words, this class only supports the computation of 1,0-EI.  In this case, we have analytic formulas
  for computing EI and its gradient.
  Thus this class does not perform any explicit numerical integration, nor do its EI functions require access to a
  random number generator.
  This class's methods have some parameters that are unused or redundant.  This is so that the interface matches that of
  the more general ExpectedImprovementEvaluator.
  For other details, see ExpectedImprovementEvaluator for more complete description of what EI is and the outputs of
  EI and grad EI computations.
\endrst*/
class FuturePosteriorMeanEvaluator final {
 public:
  using StateType = FuturePosteriorMeanState;

  /*!\rst
    Constructs a OnePotentialSampleExpectedImprovementEvaluator object.  All inputs are required; no default constructor nor copy/assignment are allowed.
    \param
      :gaussian_process: GaussianProcess object (holds ``points_sampled``, ``values``, ``noise_variance``, derived quantities)
        that describes the underlying GP
      :best_so_far: best (minimum) objective function value (in ``points_sampled_value``)
  \endrst*/
  FuturePosteriorMeanEvaluator(const GaussianProcess& gaussian_process_in,
                               double const * coefficient,
                               double const * to_sample,
                               const int num_to_sample,
                               int const * to_sample_derivatives,
                               int num_derivatives,
                               double const * chol,
                               double const * train_sample);

  int dim() const noexcept OL_PURE_FUNCTION OL_WARN_UNUSED_RESULT {
    return dim_;
  }

  const GaussianProcess * gaussian_process() const noexcept OL_PURE_FUNCTION OL_WARN_UNUSED_RESULT {
    return gaussian_process_;
  }

  std::vector<double> to_sample_points(double const * to_sample,
                                       int num_to_sample) noexcept OL_WARN_UNUSED_RESULT {
    std::vector<double> result(num_to_sample*dim_);
    std::copy(to_sample, to_sample + num_to_sample*dim_, result.data());
    return result;
  }

  std::vector<int> derivatives(int const * to_sample_derivatives,
                                  int num_derivatives) noexcept OL_WARN_UNUSED_RESULT {
    std::vector<int> result(num_derivatives);
    std::copy(to_sample_derivatives, to_sample_derivatives + num_derivatives, result.data());
    return result;
  }

  std::vector<double> coeff(double const * coefficient, double const * chol,
                            int num_to_sample, int num_derivatives) noexcept OL_WARN_UNUSED_RESULT {
    std::vector<double> result(num_to_sample*(1+num_derivatives));
    std::copy(coefficient, coefficient + num_to_sample*(1+num_derivatives), result.data());
    TriangularMatrixVectorSolve(chol, 'T', num_to_sample_*(1+num_derivatives_),
                                num_to_sample_*(1+num_derivatives_), result.data());
    return result;
  }

  std::vector<double> coeff_combine(double const * coefficient, double const * train_sample,
                                    int num_to_sample, int num_derivatives) noexcept OL_WARN_UNUSED_RESULT {
    std::vector<double> temp(num_to_sample*(1+num_derivatives));
    std::copy(coefficient, coefficient + num_to_sample*(1+num_derivatives), temp.data());

    std::vector<double> result(gaussian_process_->get_K_inv_y());
    int num_observations = gaussian_process_->num_sampled() * (1 + gaussian_process_->num_derivatives());
    GeneralMatrixVectorMultiply(train_sample, 'N', temp.data(), -1.0, 1.0,
                                num_observations, num_to_sample*(1+num_derivatives),
                                num_observations, result.data());
    return result;
  }

  /*!\rst
    Wrapper for ComputeExpectedImprovement(); see that function for details.
  \endrst*/
  double ComputeObjectiveFunction(StateType * ps_state) const OL_NONNULL_POINTERS OL_WARN_UNUSED_RESULT {
    return ComputePosteriorMean(ps_state);
  }

  /*!\rst
    Wrapper for ComputeGradExpectedImprovement(); see that function for details.
  \endrst*/
  void ComputeGradObjectiveFunction(StateType * ps_state, double * restrict grad_PS) const OL_NONNULL_POINTERS {
    ComputeGradPosteriorMean(ps_state, grad_PS);
  }

  /*!\rst
    Computes the expected improvement ``EI(Xs) = E_n[[f^*_n(X) - min(f(Xs_1),...,f(Xs_m))]^+]``
    Uses analytic formulas to evaluate the expected improvement.
    \param
      :ei_state[1]: properly configured state object
    \output
      :ei_state[1]: state with temporary storage modified
    \return
      the expected improvement from sampling ``point_to_sample``
  \endrst*/
  double ComputePosteriorMean(StateType * ps_state) const;

  /*!\rst
    Computes the (partial) derivatives of the expected improvement with respect to the point to sample.
    Uses analytic formulas to evaluate the spatial gradient of the expected improvement.
    \param
      :ei_state[1]: properly configured state object
    \output
      :ei_state[1]: state with temporary storage modified
      :grad_EI[dim]: gradient of EI, ``\pderiv{EI(x)}{x_d}``, where ``x`` is ``points_to_sample``
  \endrst*/
  void ComputeGradPosteriorMean(StateType * ps_state, double * restrict grad_PS) const;

  OL_DISALLOW_DEFAULT_AND_COPY_AND_ASSIGN(FuturePosteriorMeanEvaluator);

 private:
  //! spatial dimension (e.g., entries per point of ``points_sampled``)
  const int dim_;

  //! pointer to gaussian process used in computations
  const GaussianProcess * gaussian_process_;

  //! points to sample in the next sampling step
  const std::vector<double> to_sample_;

  //! number of points in the next sampling step
  const int num_to_sample_;

  //! the dims with derivatives
  const std::vector<int> to_sample_derivatives_;

  //! number of derivatives
  const int num_derivatives_;

  //! transpose(Chol)^-1 * (sampled Z)
  const std::vector<double> coeff_;
  const std::vector<double> coeff_combined_;
};

/*!\rst
  State object for OnePotentialSampleExpectedImprovementEvaluator.  This tracks the *ONE* ``point_to_sample``
  being evaluated via expected improvement.
  This is just a special case of ExpectedImprovementState; see those class docs for more details.
  See general comments on State structs in ``gpp_common.hpp``'s header docs.
\endrst*/
struct FuturePosteriorMeanState final {
  using EvaluatorType = FuturePosteriorMeanEvaluator;

  /*!\rst
    Constructs an OnePotentialSampleExpectedImprovementState object for the purpose of computing EI
    (and its gradient) over the specified point to sample.
    This establishes properly sized/initialized temporaries for EI computation, including dependent state from the
    associated Gaussian Process (which arrives as part of the ``ei_evaluator``).
    .. WARNING::
         This object is invalidated if the associated ei_evaluator is mutated.  SetupState() should be called to reset.
    .. WARNING::
         Using this object to compute gradients when ``configure_for_gradients`` := false results in UNDEFINED BEHAVIOR.
    \param
      :ei_evaluator: expected improvement evaluator object that specifies the parameters & GP for EI evaluation
      :point_to_sample[dim]: point at which to evaluate EI and/or its gradient to check their value in future experiments (i.e., test point for GP predictions)
      :configure_for_gradients: true if this object will be used to compute gradients, false otherwise
  \endrst*/
  FuturePosteriorMeanState(const EvaluatorType& ps_evaluator, const int num_fidelity_in, double const * restrict point_to_sample_in, bool configure_for_gradients);

  FuturePosteriorMeanState(FuturePosteriorMeanState&& other);

  int GetProblemSize() const noexcept OL_PURE_FUNCTION OL_WARN_UNUSED_RESULT {
    return dim - num_fidelity;
  }

  std::vector<double> BuildUnionOfPoints(double const * restrict points_to_sample) noexcept OL_WARN_UNUSED_RESULT {
    std::vector<double> union_of_points(dim);
    std::copy(points_to_sample, points_to_sample + dim - num_fidelity, union_of_points.data());
    std::fill(union_of_points.data() + dim - num_fidelity, union_of_points.data() + dim, 1.0);
    return union_of_points;
  }

  /*!\rst
    Get ``point_to_sample``: the potential future sample whose EI (and/or gradients) is being evaluated
    \output
      :point_to_sample[dim]: potential sample whose EI is being evaluted
  \endrst*/
  void GetCurrentPoint(double * restrict point_to_sample_out) const noexcept OL_NONNULL_POINTERS {
    std::copy(point_to_sample.data(), point_to_sample.data() + dim - num_fidelity, point_to_sample_out);
  }

  void Initialize(const EvaluatorType& ps_evaluator);

  /*!\rst
    Change the potential sample whose EI (and/or gradient) is being evaluated.
    Update the state's derived quantities to be consistent with the new point.
    \param
      :ei_evaluator: expected improvement evaluator object that specifies the parameters & GP for EI evaluation
      :point_to_sample[dim]: potential future sample whose EI (and/or gradients) is being evaluated
  \endrst*/
  void SetCurrentPoint(const EvaluatorType& ps_evaluator,
                       double const * restrict point_to_sample_in) OL_NONNULL_POINTERS;

  /*!\rst
    Configures this state object with a new ``point_to_sample``, the location of the potential sample whose EI is to be evaluated.
    Ensures all state variables & temporaries are properly sized.
    Properly sets all dependent state variables (e.g., GaussianProcess's state) for EI evaluation.
    .. WARNING::
         This object's state is INVALIDATED if the ei_evaluator (including the GaussianProcess it depends on) used in
         SetupState is mutated! SetupState() should be called again in such a situation.
    \param
      :ei_evaluator: expected improvement evaluator object that specifies the parameters & GP for EI evaluation
      :point_to_sample[dim]: potential future sample whose EI (and/or gradients) is being evaluated
  \endrst*/
  void SetupState(const EvaluatorType& ps_evaluator,
                  double const * restrict point_to_sample_in) OL_NONNULL_POINTERS;

  // size information
  //! spatial dimension (e.g., entries per point of ``points_sampled``)
  const int dim;
  //! dim of the fidelity
  const int num_fidelity;
  //! number of points to sample (i.e., the "q" in q,p-EI); MUST be 1
  const int num_to_sample = 1;
  //! number of derivative terms desired (usually 0 for no derivatives or num_to_sample)
  const int num_derivatives;

  //! point at which to evaluate EI and/or its gradient (e.g., to check its value in future experiments)
  std::vector<double> point_to_sample;

  std::vector<double> K_star;
  std::vector<double> grad_K_star;

  UniformRandomGenerator randomGenerator;
  OL_DISALLOW_DEFAULT_AND_COPY_AND_ASSIGN(FuturePosteriorMeanState);
};

/*!\rst
  Set up vector of FuturePosteriorMeanState::StateType.

  This is a utility function just for reducing code duplication.

  \param
    :ei_evaluator: evaluator object associated w/the state objects being constructed
    :points_to_sample[dim][num_to_sample]: initial points to load into state (must be a valid point for the problem);
      i.e., points at which to evaluate EI and/or its gradient
    :points_being_sampled[dim][num_being_sampled]: points that are being sampled in concurrently experiments
    :num_to_sample: number of potential future samples; gradients are evaluated wrt these points (i.e., the "q" in q,p-EI)
    :num_being_sampled: number of points being sampled concurrently (i.e., the p in q,p-EI)
    :max_num_threads: maximum number of threads for use by OpenMP (generally should be <= # cores)
    :configure_for_gradients: true if these state objects will be used to compute gradients, false otherwise
    :state_vector[arbitrary]: vector of state objects, arbitrary size (usually 0)
    :normal_rng[max_num_threads]: a vector of NormalRNG objects that provide the (pesudo)random source for MC integration
  \output
    :state_vector[max_num_threads]: vector of states containing ``max_num_threads`` properly initialized state objects
\endrst*/
inline OL_NONNULL_POINTERS void SetupFuturePosteriorMeanState(
    const FuturePosteriorMeanEvaluator& fpm_evaluator,
    double const * restrict points_to_sample,
    int max_num_threads,
    bool configure_for_gradients,
    const int num_fidelity,
    std::vector<typename FuturePosteriorMeanEvaluator::StateType> * state_vector) {
  state_vector->reserve(max_num_threads);
  for (int i = 0; i < max_num_threads; ++i) {
    state_vector->emplace_back(fpm_evaluator, num_fidelity, points_to_sample, configure_for_gradients);
  }
}

/*!\rst
  Perform multistart gradient descent (MGD) to solve the q,p-EI problem (see ComputeOptimalPointsToSample and/or
  header docs), starting from ``num_multistarts`` points selected randomly from the within th domain.
  This function is a simple wrapper around ComputeOptimalPointsToSampleViaMultistartGradientDescent(). It additionally
  generates a set of random starting points and is just here for convenience when better initial guesses are not
  available.
  See ComputeOptimalPointsToSampleViaMultistartGradientDescent() for more details.
  \param
    :gaussian_process: GaussianProcess object (holds ``points_sampled``, ``values``, ``noise_variance``, derived quantities)
      that describes the underlying GP
    :optimizer_parameters: GradientDescentParameters object that describes the parameters controlling EI optimization
      (e.g., number of iterations, tolerances, learning rate)
    :domain: object specifying the domain to optimize over (see ``gpp_domain.hpp``)
    :thread_schedule: struct instructing OpenMP on how to schedule threads; i.e., (suggestions in parens)
      max_num_threads (num cpu cores), schedule type (omp_sched_dynamic), chunk_size (0).
    :points_being_sampled[dim][num_being_sampled]: points that are being sampled in concurrent experiments
    :num_to_sample: number of potential future samples; gradients are evaluated wrt these points (i.e., the "q" in q,p-EI)
    :num_being_sampled: number of points being sampled concurrently (i.e., the "p" in q,p-EI)
    :best_so_far: value of the best sample so far (must be ``min(points_sampled_value)``)
    :max_int_steps: maximum number of MC iterations
    :uniform_generator[1]: a UniformRandomGenerator object providing the random engine for uniform random numbers
    :normal_rng[thread_schedule.max_num_threads]: a vector of NormalRNG objects that provide
      the (pesudo)random source for MC integration
  \output
    :found_flag[1]: true if best_next_point corresponds to a nonzero EI
    :uniform_generator[1]: UniformRandomGenerator object will have its state changed due to random draws
    :normal_rng[thread_schedule.max_num_threads]: NormalRNG objects will have their state changed due to random draws
    :best_next_point[dim][num_to_sample]: points yielding the best EI according to MGD
\endrst*/
template <typename DomainType>
void RestartedGradientDescentFuturePosteriorMeanOptimization(const GaussianProcess& gaussian_process, const int num_fidelity,
                                                             double const * coefficient, double const * to_sample, const int num_to_sample,
                                                             int const * to_sample_derivatives, int num_derivatives,
                                                             double const * chol, double const * train_sample,
                                                             const GradientDescentParameters& optimizer_parameters,
                                                             const DomainType& domain, double const * restrict initial_guess,
                                                             double& best_objective_value, double * restrict best_next_point) {
  if (unlikely(optimizer_parameters.max_num_restarts <= 0)) {
    return;
  }
  bool configure_for_gradients = true;
  OL_VERBOSE_PRINTF("Posterior Mean Optimization via %s:\n", OL_CURRENT_FUNCTION_NAME);

  // special analytic case when we are not using (or not accounting for) multiple, simultaneous experiments
  FuturePosteriorMeanEvaluator ps_evaluator(gaussian_process, coefficient, to_sample,
                                            num_to_sample, to_sample_derivatives,
                                            num_derivatives, chol, train_sample);

  typename FuturePosteriorMeanEvaluator::StateType ps_state(ps_evaluator, num_fidelity, initial_guess, configure_for_gradients);

  GradientDescentOptimizer<FuturePosteriorMeanEvaluator, DomainType> gd_opt;
  gd_opt.Optimize(ps_evaluator, optimizer_parameters, domain, &ps_state);
  ps_state.GetCurrentPoint(best_next_point);
  best_objective_value = ps_evaluator.ComputeObjectiveFunction(&ps_state);
}

/*!\rst
  Perform multistart gradient descent (MGD) to solve the q,p-EI problem (see ComputeOptimalPointsToSample and/or
  header docs), starting from ``num_multistarts`` points selected randomly from the within th domain.
  This function is a simple wrapper around ComputeOptimalPointsToSampleViaMultistartGradientDescent(). It additionally
  generates a set of random starting points and is just here for convenience when better initial guesses are not
  available.
  See ComputeOptimalPointsToSampleViaMultistartGradientDescent() for more details.
  \param
    :gaussian_process: GaussianProcess object (holds ``points_sampled``, ``values``, ``noise_variance``, derived quantities)
      that describes the underlying GP
    :optimizer_parameters: GradientDescentParameters object that describes the parameters controlling EI optimization
      (e.g., number of iterations, tolerances, learning rate)
    :domain: object specifying the domain to optimize over (see ``gpp_domain.hpp``)
    :thread_schedule: struct instructing OpenMP on how to schedule threads; i.e., (suggestions in parens)
      max_num_threads (num cpu cores), schedule type (omp_sched_dynamic), chunk_size (0).
    :points_being_sampled[dim][num_being_sampled]: points that are being sampled in concurrent experiments
    :num_to_sample: number of potential future samples; gradients are evaluated wrt these points (i.e., the "q" in q,p-EI)
    :num_being_sampled: number of points being sampled concurrently (i.e., the "p" in q,p-EI)
    :best_so_far: value of the best sample so far (must be ``min(points_sampled_value)``)
    :max_int_steps: maximum number of MC iterations
    :uniform_generator[1]: a UniformRandomGenerator object providing the random engine for uniform random numbers
    :normal_rng[thread_schedule.max_num_threads]: a vector of NormalRNG objects that provide
      the (pesudo)random source for MC integration
  \output
    :found_flag[1]: true if best_next_point corresponds to a nonzero EI
    :uniform_generator[1]: UniformRandomGenerator object will have its state changed due to random draws
    :normal_rng[thread_schedule.max_num_threads]: NormalRNG objects will have their state changed due to random draws
    :best_next_point[dim][num_to_sample]: points yielding the best EI according to MGD
\endrst*/
template <typename DomainType>
void ComputeOptimalFuturePosteriorMean(const GaussianProcess& gaussian_process, const int num_fidelity, double const * coefficient,
                                       double const * to_sample, const int num_to_sample, int const * to_sample_derivatives,
                                       int num_derivatives, double const * chol, double const * train_sample,
                                       const GradientDescentParameters& optimizer_parameters, const DomainType& domain,
                                       int max_num_threads, double const * restrict start_point_set,
                                       int num_multistarts, double * restrict best_function_value, double * restrict best_next_point);

// template explicit instantiation declarations, see gpp_common.hpp header comments, item 6
extern template void ComputeOptimalFuturePosteriorMean(const GaussianProcess& gaussian_process, const int num_fidelity, double const * coefficient,
                                                       double const * to_sample, const int num_to_sample, int const * to_sample_derivatives,
                                                       int num_derivatives, double const * chol, double const * train_sample,
                                                       const GradientDescentParameters& optimizer_parameters, const TensorProductDomain& domain,
                                                       int max_num_threads, double const * restrict start_point_set,
                                                       int num_multistarts, double * restrict best_function_value,
                                                       double * restrict best_next_point);
extern template void ComputeOptimalFuturePosteriorMean(const GaussianProcess& gaussian_process, const int num_fidelity, double const * coefficient,
                                                       double const * to_sample, const int num_to_sample, int const * to_sample_derivatives,
                                                       int num_derivatives, double const * chol, double const * train_sample,
                                                       const GradientDescentParameters& optimizer_parameters, const SimplexIntersectTensorProductDomain& domain,
                                                       int max_num_threads, double const * restrict start_point_set,
                                                       int num_multistarts, double * restrict best_function_value,
                                                       double * restrict best_next_point);
}  // end namespace optimal_learning
#endif  // MOE_OPTIMAL_LEARNING_CPP_GPP_KNOWLEDGE_GRADIENT_INNER_OPTIMIZATION_HPP_


#pragma once

#include <vector>

#include "nifty/math/math.hxx"
#include "nifty/histogram/histogram.hxx"
#include "nifty/cgp/geometry.hxx"
#include "nifty/cgp/bounds.hxx"
#include "nifty/marray/marray.hxx"
#include "nifty/filters/gaussian_curvature.hxx"
#include "nifty/features/accumulated_features.hxx"

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

namespace nifty{
namespace cgp{

    class Cell1CurvatureFeatures2D{
    private:
        typedef nifty::features::DefaultAccumulatedStatistics<float> AccType;
    public:
        Cell1CurvatureFeatures2D(
            const std::vector<float> & sigmas  = std::vector<float>({1.0f, 2.0f, 4.0f})
        )
        :   sigmas_(sigmas)
        {

        }

        size_t numberOfFeatures()const{
            return sigmas_.size() * AccType::NFeatures::value;
        }

        std::vector<std::string> names()const{

            std::string accNames [] = {
                std::string("Mean"),
                std::string("Sum"),
                std::string("Min"),
                std::string("Max"),
                std::string("Moment2"),
                std::string("Moment3"),
                std::string("Q0.10"),
                std::string("Q0.25"),
                std::string("Q0.50"),
                std::string("Q0.75"),
                std::string("Q0.90")
            };
            std::vector<std::string> res;
            auto baseName = std::string("GaussianCurvatureSigma");
            for(auto sigmaIndex=0; sigmaIndex<sigmas_.size(); ++sigmaIndex){
                auto name  = baseName + std::to_string(sigmas_[sigmaIndex]);
                for(const auto & accName : accNames){
                    res.push_back(name + accName);
                }

            }
            return res;
        }

        template<class T>
        void operator()(
            const CellGeometryVector<2,1>  & cell1GeometryVector,
            const CellBoundedByVector<2,1> & cell1BoundedByVector,
            nifty::marray::View<T> & features
        )const{

            std::vector<float> curvature;
            std::vector<float> buffer(AccType::NFeatures::value);

            for(auto sigmaIndex=0; sigmaIndex<sigmas_.size(); ++sigmaIndex){
                const auto sigma = sigmas_[sigmaIndex];
                nifty::filters::GaussianCurvature2D<> op(sigma, -1, 2.5);

                for(auto cell1Index=0; cell1Index<cell1GeometryVector.size(); ++cell1Index){
                    const auto & geo = cell1GeometryVector[cell1Index];

                    if(geo.size()>=4){

                        // we use a larger size?
                        if(curvature.capacity() < geo.size()){
                            curvature.resize(geo.size()*2);
                        }
                        else{
                            curvature.resize(geo.size());
                        }

                        // calculate curvature
                        //std::cout<<"    is closed "<<"\n";
                        const auto closedLine = cell1BoundedByVector[cell1Index].size() == 0;
                        //std::cout<<"    calculate curvature "<<"\n";
                        op(geo.begin(), geo.end(), curvature.begin(), closedLine);

                        // accumulate the values
                        AccType acc;
                        for(auto pass=0; pass < acc.requiredPasses(); ++pass){
                            for(const auto & c : curvature){
                                acc.acc(c, pass);
                            }
                        }
                        // write to buffer
                        acc.result(buffer.begin(), buffer.end());

                        // write to features out
                        const auto fIndex = sigmaIndex*AccType::NFeatures::value;
                        for(auto afi=0; afi<AccType::NFeatures::value; ++afi){
                            features(cell1Index, fIndex + afi) = buffer[afi];
                        }

                    }
                    else{
                        // write to features out
                        // a zero value seems legit since we
                        // assume a constant zero curvature
                        const auto fIndex = sigmaIndex*AccType::NFeatures::value;
                        for(auto afi=0; afi<AccType::NFeatures::value; ++afi){
                            features(cell1Index, fIndex + afi) = 0.0;
                        }

                    }
                }
            }
        }
    private:
        std::vector<float> sigmas_;
    };

    class Cell1LineSegmentDist2D{
    private:
        typedef nifty::features::DefaultAccumulatedStatistics<float> AccType;
    public:
        Cell1LineSegmentDist2D(
            const std::vector<size_t> & dists  = std::vector<size_t>({size_t(3),size_t(5),size_t(7)})
        )
        :   dists_(dists)
        {
        }

        size_t numberOfFeatures()const{
            return  dists_.size()*AccType::NFeatures::value;
        }

       std::vector<std::string> names()const{

            std::string accNames [] = {
                std::string("Mean"),
                std::string("Sum"),
                std::string("Min"),
                std::string("Max"),
                std::string("Moment2"),
                std::string("Moment3"),
                std::string("Q0.10"),
                std::string("Q0.25"),
                std::string("Q0.50"),
                std::string("Q0.75"),
                std::string("Q0.90")
            };
            std::vector<std::string> res;
            auto baseName = std::string("Cell1LineSegmentDistD");
            for(auto i=0; i<dists_.size(); ++i){
                auto name  = baseName + std::to_string(dists_[i]);
                for(const auto & accName : accNames){
                    res.push_back(name + accName);
                }

            }
            return res;
        }

        template<class T>
        void operator()(
            const CellGeometryVector<2,1>  & cell1GeometryVector,
            nifty::marray::View<T> & features
        )const{

            std::vector<float> buffer(AccType::NFeatures::value);

            typedef boost::geometry::model::d2::point_xy<double> point_type;
            typedef boost::geometry::model::linestring<point_type> linestring_type;

            for(auto di=0; di<dists_.size(); ++di){
                const auto ld = dists_[di];
                for(auto cell1Index=0; cell1Index<cell1GeometryVector.size(); ++cell1Index){
                    const auto & geo = cell1GeometryVector[cell1Index];
                    if(geo.size()>=4){

                        AccType acc;

                        for(auto pass=0; pass < acc.requiredPasses(); ++pass){

                            for(auto i=0; i<geo.size()-1; ++i){
                                const auto j = std::min(int(i + ld), int(geo.size()-1));
                                const auto & pS = geo[i];
                                const auto & pE = geo[j];

                                linestring_type line;
                                line.push_back(point_type(pS[0], pS[1]));
                                line.push_back(point_type(pE[0], pE[1]));

                                for(auto ii=i+1; ii<j-1; ++ii){
                                    const point_type p(geo[ii][0], geo[ii][1]);
                                    const auto d = boost::geometry::distance(p, line);
                                    acc.acc(d, pass);
                                }
                            }
                        }
                        // write to buffer
                        acc.result(buffer.begin(), buffer.end());

                        // write to features out
                        const auto fIndex = di*AccType::NFeatures::value;
                        for(auto afi=0; afi<AccType::NFeatures::value; ++afi){
                            features(cell1Index, fIndex + afi) = buffer[afi];
                        }

                    }
                    else{
                        // write to features out
                        // a zero value seems legit since we
                        // assume a constant zero curvature
                        const auto fIndex = di*AccType::NFeatures::value;
                        for(auto afi=0; afi<AccType::NFeatures::value; ++afi){
                            features(cell1Index, fIndex + afi) = 0.0;
                        }
                    }
                }
            }
        }
    private:
        std::vector<size_t> dists_;
    };

    class Cell1BasicGeometricFeatures2D{
    private:
        typedef nifty::features::DefaultAccumulatedStatistics<float> AccType;
    public:
        Cell1BasicGeometricFeatures2D(){
        }

        size_t numberOfFeatures()const{
            return 4 * 4 +  2*AccType::NFeatures::value + 4;
        }

        std::vector<std::string> names()const{

            std::string accNames [] = {
                std::string("Mean"),
                std::string("Sum"),
                std::string("Min"),
                std::string("Max"),
                std::string("Moment2"),
                std::string("Moment3"),
                std::string("Q0.10"),
                std::string("Q0.25"),
                std::string("Q0.50"),
                std::string("Q0.75"),
                std::string("Q0.90")
            };

            std::vector<std::string> res;
            const auto baseName = std::string("BasicGeometricFeatures");
            auto insertUVFeat = [&](const std::string & name){
                res.push_back(baseName+name+std::string("UV-Min"));
                res.push_back(baseName+name+std::string("UV-Max"));
                res.push_back(baseName+name+std::string("UV-Sum"));
                res.push_back(baseName+name+std::string("UV-AbsDiff"));
            };
            auto insertStatFeat = [&](const std::string & name){
                for(const auto & an : accNames){
                    res.push_back(baseName+name+an);
                }
            };

            res.push_back(baseName+std::string("EdgeSize"));
            insertUVFeat("NodeSize");
            insertUVFeat("NodeEdgeSizeRatio");

            res.push_back(baseName+std::string("EdgeEndpointDistance"));
            res.push_back(baseName+std::string("EdgeRelativeEndpointDistance"));
            res.push_back(baseName+std::string("NodeEndpointDistance"));

            insertUVFeat("EdgeNodeCenterOfMassDist");
            insertUVFeat("EdgeNodeCenterOfMassRatio");

            insertStatFeat("CenterOfMassCell1PointsDist");
            insertStatFeat("CenterOfMassCell2PointsDist");

            return res;
        }

        template<class T>
        void operator()(
            const CellGeometryVector<2,1>   & cell1GeometryVector,
            const CellGeometryVector<2,2>   & cell2GeometryVector,
            const CellBoundsVector<2,1>     & cell1BoundsVector,
            nifty::marray::View<T> & features
        )const{

            using namespace nifty::math;
            std::vector<float> buffer(AccType::NFeatures::value);
            for(auto cell1Index=0; cell1Index<cell1GeometryVector.size(); ++cell1Index){

                const auto & cell1Bounds = cell1BoundsVector[cell1Index];
                const auto cell2UIndex = cell1Bounds[0]-1;
                const auto cell2VIndex = cell1Bounds[1]-1;

                auto fIndex = 0;

                auto insertCell2ValFeats = [&](const float uVal, const float vVal){
                    features(cell1Index, fIndex++) = std::min(uVal, vVal);
                    features(cell1Index, fIndex++) = std::max(uVal, vVal);
                    features(cell1Index, fIndex++) = uVal + vVal;
                    features(cell1Index, fIndex++) = std::abs(uVal-vVal);
                };

                const auto & geoE = cell1GeometryVector[cell1Index];
                const auto & geoU = cell2GeometryVector[cell2UIndex];
                const auto & geoV = cell2GeometryVector[cell2VIndex];

                // size based features
                const auto eSize = float(geoE.size());
                const auto uSize = float(geoU.size());
                const auto vSize = float(geoV.size());

                features(cell1Index, fIndex++) = eSize;
                insertCell2ValFeats(uSize, vSize);

                // size ratios
                const auto uNSize = std::sqrt(uSize);
                const auto vNSize = std::sqrt(vSize);
                {
                    const auto ratU =  uNSize/eSize;
                    const auto ratV =  vNSize/eSize;
                    insertCell2ValFeats(ratU, ratV);
                }

                // endpoint distance and ratios
                const auto endpointDistance = euclideanDistance(geoE.front(), geoE.back());
                features(cell1Index, fIndex++) = endpointDistance;
                features(cell1Index, fIndex++) = endpointDistance/eSize;

                // distance between cell2CenterOfMass  and cell1CenterOfMass
                const auto  comE = geoE.centerOfMass();
                const auto  comU = geoU.centerOfMass();
                const auto  comV = geoV.centerOfMass();
                {
                    const auto dUV  = euclideanDistance(comU, comV);
                    const auto dUE  = euclideanDistance(comU, comE);
                    const auto dVE  = euclideanDistance(comV, comE);

                    features(cell1Index, fIndex++) = dUV;
                    insertCell2ValFeats(dUE, dVE);

                    // distance ratios between cell2CenterOfMass  and cell1CenterOfMass

                    const auto ratU =  uNSize/dUE;
                    const auto ratV =  vNSize/dVE;
                    insertCell2ValFeats(ratU, ratV);
                }

                // statistic  over ||centerOfMass-points||  for cell 1
                {
                    AccType acc;
                    for(auto i=0; i<geoE.size(); ++i){
                        const auto & coord = geoE[i];
                        const auto d  = nifty::math::euclideanDistance(coord, comE);
                        acc.acc(d);
                    }
                    // write to buffer
                    acc.result(buffer.begin(), buffer.end());
                    // write results
                    for(auto i=0; i<buffer.size(); ++i){
                        features(cell1Index,  fIndex++) = buffer[i];
                    }
                }

                // statistic  over ||centerOfMass_cell1-points_cell2||
                {
                    AccType acc;

                    for(auto i=0; i<geoU.size(); ++i){
                        const auto & coord = geoU[i];
                        const auto d  = nifty::math::euclideanDistance(coord, comE);
                        acc.acc(d);
                    }
                    for(auto i=0; i<geoV.size(); ++i){
                        const auto & coord = geoV[i];
                        const auto d  = nifty::math::euclideanDistance(coord, comE);
                        acc.acc(d);
                    }

                    // write to buffer and write results
                    acc.result(buffer.begin(), buffer.end());

                    for(auto i=0; i<buffer.size(); ++i){
                        features(cell1Index,  fIndex++) = buffer[i];
                    }

                }

                // angle between cell2CenterOfMass  and cell1CenterOfMass
                // ...

                NIFTY_CHECK_OP(fIndex,==,numberOfFeatures(),"internal error");

            }

        }
    private:
        std::vector<size_t> dists_;
    };

    /**
    class GeometricAccumulator{
    private:
        typedef nifty::features::DefaultAccumulatedStatistics<float> AccType;
    public:
        GeometricAccumulator(
            const std::vector<size_t> & dists  = std::vector<size_t>({size_t(3),size_t(5),size_t(7)})
        )
        :   dists_(dists)
        {
        }

        size_t numberOfFeatures()const{
            return  dists_.size()*AccType::NFeatures::value;
        }

        template<class T>
        void operator()(
            const CellGeometryVector<2,1>  & cell1GeometryVector,
            nifty::marray::View<T> & features
        )const{

            std::vector<float> buffer(AccType::NFeatures::value);

            typedef boost::geometry::model::d2::point_xy<double> point_type;
            typedef boost::geometry::model::linestring<point_type> linestring_type;

            for(auto cell1Index=0; cell1Index<cell1GeometryVector.size(); ++cell1Index){

                const auto & geo = cell1GeometryVector[cell1Index];

            }

        }
    private:
        std::vector<size_t> dists_;
    };
    */

}
}


#include <MetaScale.h>
#include <boost/math/common_factor_rt.hpp>

void MetaScale::normalize() {
  uint32_t a = mNum<0?-mNum:mNum;
  uint32_t gcd = boost::math::gcd(a,mDenom);
  mNum   /= gcd;
  mDenom /= gcd;
}

MetaScale& MetaScale::operator*=(const MetaScale& b) {
  mNum*=b.num();
  mDenom*=b.denom();
  normalize();
  return *this;
}

MetaScale& MetaScale::operator/=(const MetaScale& b) {
  uint32_t temp = mNum*b.denom();
  mDenom*=b.num();
  mNum = temp;
  normalize();
  return *this;
}
MetaScale MetaScale::operator*(const MetaScale& b) const {
  return MetaScale(*this) *= b;
}

MetaScale MetaScale::operator/(const MetaScale& b) const {
  return MetaScale(*this) /= b;
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      121128    R.C.A. Boon       File created (in progress).
 *      130124    R.C.A. Boon       Removed retrograde function, reworked Kepler to MEE conversion
 *                                  to properly convert inclination to range [0,180) and use
 *                                  retrograde factor internally
 *      130131    R.C.A. Boon       Added Cartesian conversion cases, added avoidSingularityAtPi~
 *                                  boolean flag to Kepler conversions, optimized computation
 *                                  (partially)
 *      130225    D. Dirkx          Added overloaded function for Kepler to MEE that determines
 *                                  retrogradeness based on Kepler state
 *      130301    R.C.A. Boon       Updated use of mathematics::PI to basic_mathematics::
 *                                  mathematical_constants::PI, minor textual changes.
 *      130305    R.C.A. Boon       Replaced Eigen::VectorXd by basic_mathematics::Vector6d
 *
 *    References
 *      Verified Interval Propagation, Bart Rˆmgens; Delft (2011). Code archive.
 *      Modified Equinoctial Orbital Elements, author unknown;
 *          http://www.cdeagle.com/pdf/mee.pdf (2010?).
 *      Survey of Orbital Element Sets, Gerald R. Hintz; Journal of Guidance, Control and
 *          Dynamics (2008, Vol. 31 - Nr. 3).
 *      Code archive, E. Heeren (fellow Tudat developer).
 *
 *    Notes
 *
 */

#include <cmath>

#include <boost/exception/all.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/missionGeometry.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/modifiedEquinoctialElementConversions.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"

namespace tudat
{

namespace orbital_element_conversions
{

//! Convert Keplerian to modified equinoctial orbital elements using implicit MEE equation set.
basic_mathematics::Vector6d convertKeplerianToModifiedEquinoctialElements(
        const basic_mathematics::Vector6d& keplerianElements )
// Based on Hintz, 2008.
{
    // Check if orbit is retrograde
    bool avoidSingularityAtPiInclination =
            mission_geometry::isOrbitRetrograde( keplerianElements );

    // Convert to modified equinoctial elements.
    return convertKeplerianToModifiedEquinoctialElements( keplerianElements,
                                                          avoidSingularityAtPiInclination );
}

//! Convert Keplerian to modified equinoctial orbital elements using MEE explicit equation set.
basic_mathematics::Vector6d convertKeplerianToModifiedEquinoctialElements(
        const basic_mathematics::Vector6d& keplerianElements,
        const bool avoidSingularityAtPiInclination )
// Based on Hintz, 2008.
{
    using mathematical_constants::PI;

    // Declaring eventual output vector.
    basic_mathematics::Vector6d modifiedEquinoctialState( 6 );

    // Compute semi-latus rectum.
    double singularityTolerance = 1.0e-15; // Based on tolerance chosen in
                                           // orbitalElementConversions.cpp in Tudat Core.

    // Extracting eccentricity for ease of referencing.
    double eccentricity = keplerianElements( eccentricityIndex );

    // If e is (very near) one, then semi-major axis is undefined and thus the first kepler
    // element is the semi-latus rectum.
    if ( std::fabs( eccentricity - 1.0 ) < singularityTolerance )
    {
        modifiedEquinoctialState( semiLatusRectumIndex )
                = keplerianElements( semiLatusRectumIndex );
    }
    else // eccentricity is significantly away from singularity and can be computed with p=a(1-e≤).
    {
        modifiedEquinoctialState( semiLatusRectumIndex ) = keplerianElements( semiMajorAxisIndex )
                                                           * ( 1.0 - eccentricity * eccentricity );
    }

    // Determine prograde-ness, as other five parameters depend on that fact.
    double inclination = keplerianElements( inclinationIndex );

    // If inclination is outside range [0,PI].
    if ( ( inclination < 0.0 ) || ( inclination > PI ) )
    {
        // Define the error message.
        std::stringstream errorMessage;
        errorMessage << "Inclination is expected in range [0," << PI << "]\n"
                     << "Specified inclination: " << inclination << " rad." << std::endl;

        // Throw exception.
        boost::throw_exception( std::runtime_error( errorMessage.str( ) ) );
    }
    //Else, nothing wrong and continue.

    // Compute set dependant helper parameters.
    double argumentOfPeriapsisAndAscendingNode = 0.0;
    double tangentOfHalfInclination = 0.0;

    // If normal set of equations is to be used (i.e. not inverse set for singularity).
    if ( !avoidSingularityAtPiInclination )
    {
        // Add (+) argument of periapsis and longitude of ascending node.
        argumentOfPeriapsisAndAscendingNode = keplerianElements( argumentOfPeriapsisIndex )
                + keplerianElements( longitudeOfAscendingNodeIndex );

        // Take the tangent of half the inclination.
        tangentOfHalfInclination = std::tan( inclination / 2.0 );
    }
    else // the orbit is retrograde.
    {
        // Subtract (-) longitude of ascending node from argument of periapsis.
        argumentOfPeriapsisAndAscendingNode = keplerianElements( argumentOfPeriapsisIndex )
                - keplerianElements( longitudeOfAscendingNodeIndex );

        // Take the inverse of the tangent of half the inclination to avoid singularity at tan
        // PI/2.
        tangentOfHalfInclination = 1.0 / std::tan( inclination / 2.0 );
    }

    // Compute f-element.
    modifiedEquinoctialState( fElementIndex ) = eccentricity
            * std::cos( argumentOfPeriapsisAndAscendingNode );

    // Compute g-element.
    modifiedEquinoctialState( gElementIndex ) = eccentricity
            * std::sin( argumentOfPeriapsisAndAscendingNode );

    // Compute h-element.
    modifiedEquinoctialState( hElementIndex ) = tangentOfHalfInclination
            * std::cos( keplerianElements( longitudeOfAscendingNodeIndex ) );

    // Compute k-element.
    modifiedEquinoctialState( kElementIndex ) = tangentOfHalfInclination
            * std::sin( keplerianElements( longitudeOfAscendingNodeIndex ) );

    // Compute true longitude (modulo 2 PI to keep within interval -2PI to 2PI).
    modifiedEquinoctialState( trueLongitudeIndex )
            = basic_mathematics::computeModulo(
                argumentOfPeriapsisAndAscendingNode
                + keplerianElements( trueAnomalyIndex ), 2.0 * PI );

    // Give back result.
    return modifiedEquinoctialState;
}

//! Convert modified equinoctial to Keplerian orbital elements.
basic_mathematics::Vector6d convertModifiedEquinoctialToKeplerianElements(
        const basic_mathematics::Vector6d& modifiedEquinoctialElements,
        const bool avoidSingularityAtPiInclination )
// Using unknown source pdf, code archive E. Heeren and personal derivation based on Hintz 2008.
{
    using mathematical_constants::PI;

    // Declaration of output vector.
    basic_mathematics::Vector6d convertedKeplerianElements = basic_mathematics::
            Vector6d::Zero( 6 );

    // for ease of referencing, almost all modified equinoctial elements.
    double fElement = modifiedEquinoctialElements( fElementIndex );
    double gElement = modifiedEquinoctialElements( gElementIndex );
    double hElement = modifiedEquinoctialElements( hElementIndex );
    double kElement = modifiedEquinoctialElements( kElementIndex );

    // Tolerance for singularities.
    double singularityTolerance = 1.0e-15;

    // Compute eccentricity.
    double eccentricity = std::sqrt( fElement * fElement + gElement * gElement );
    convertedKeplerianElements( eccentricityIndex ) = eccentricity;

    // Compute semi-major axis.
    // If eccentricity is not near-parabolic.
    if ( std::fabs( eccentricity - 1.0 ) > singularityTolerance )
    {
        // Use semi-latus rectum and eccentricity to calculate semi-major axis with a=p/(1-e^2).
        convertedKeplerianElements( semiMajorAxisIndex ) =
                modifiedEquinoctialElements( semiLatusRectumIndex ) /
                ( 1.0 - eccentricity * eccentricity );
    }
    // This is (almost) a parabola and the semimajor axis will tend to infinity and be useless.
    else
    {
        // Give semi-latus rectum instead.
        convertedKeplerianElements( semiLatusRectumIndex ) =
                modifiedEquinoctialElements( semiLatusRectumIndex );
    }

    // Compute longitude of ascending node.

    // Compute solution in [-PI,PI] interval with atan2.
    double longitudeOfAscendingNode = std::atan2( kElement, hElement );

    // Store longitude of ascending node.
    convertedKeplerianElements( longitudeOfAscendingNodeIndex ) = basic_mathematics::
            computeModulo( longitudeOfAscendingNode, 2.0 * PI );

    // Compute inclination.

    // If prograde factor must be positive, otherwise negative.
    double retrogradeFactor = avoidSingularityAtPiInclination ? -1.0 : 1.0;

    // Calculate magnitude of inclination with retrogradeFactor (derived based on Hintz, 2008).
    double hSquaredPlusKSquared = std::pow( hElement * hElement + kElement * kElement ,
                                            retrogradeFactor );

    convertedKeplerianElements( inclinationIndex ) =
            2.0 * std::atan( std::sqrt( hSquaredPlusKSquared ) );
    // Was: std::atan2(2.0 * std::sqrt(hSquaredPlusKSquared) , (1.0 - hSquaredPlusKSquared));

    // Compute argument of periapsis.

    // Helper quantity (omega + I * OMEGA ), for argument of periapsis and true anomaly.
    double argumentOfPeriapsisAndLongitude;

    // If eccentricity is (near) circular
    if ( eccentricity < singularityTolerance )
        // Then the composite argument of periapsis and longitude together cannot be determined
        // because both arguments of the atan2 functions will be zero.
    {
        // Set argument of periapsis to zero.
        convertedKeplerianElements( argumentOfPeriapsisIndex ) = 0.0;

        // Set composite argument to longitude only, since argument of periapsis is now zero.
        argumentOfPeriapsisAndLongitude = retrogradeFactor * longitudeOfAscendingNode;
    }
    else
        // Argument of periapsis can be found from composite argument.
    {
        // Compute composite argument.
        argumentOfPeriapsisAndLongitude = std::atan2( gElement, fElement );

        // Compute argument of periapsis.
        convertedKeplerianElements( argumentOfPeriapsisIndex ) = basic_mathematics::
                computeModulo( argumentOfPeriapsisAndLongitude
                               - retrogradeFactor * longitudeOfAscendingNode, 2.0 * PI );
    }

    // Compute true anomaly.
    convertedKeplerianElements( trueAnomalyIndex ) = basic_mathematics::
            computeModulo( modifiedEquinoctialElements( trueLongitudeIndex )
                           - argumentOfPeriapsisAndLongitude, 2.0 * PI );

    // Return converted elements.
    return convertedKeplerianElements;
}

//! Convert Cartesian to modified equinoctial orbital elements using implicit MEE equation set.
basic_mathematics::Vector6d convertCartesianToModifiedEquinoctialElements(
        const basic_mathematics::Vector6d& cartesianElements,
        const double centralBodyGravitationalParameter )
{
    using mathematical_constants::PI;

    // Convert to keplerian elements.
    basic_mathematics::Vector6d keplerianElements = convertCartesianToKeplerianElements(
                cartesianElements, centralBodyGravitationalParameter );

    // Check whether orbit is retrograde.
    bool avoidSingularityAtPiInclination =
            mission_geometry::isOrbitRetrograde( keplerianElements );

    // Convert to modified equinoctial elements.
    return convertKeplerianToModifiedEquinoctialElements(
                keplerianElements, avoidSingularityAtPiInclination );
}

//! Convert Cartesian to modified equinoctial orbital elements using explicit MEE equation set.
basic_mathematics::Vector6d convertCartesianToModifiedEquinoctialElements(
        const basic_mathematics::Vector6d& cartesianElements,
        const double centralBodyGravitationalParameter,
        const bool avoidSingularityAtPiInclination )
{
    // Convert Cartesian to Keplerian to modified equinoctial elements.
    return convertKeplerianToModifiedEquinoctialElements(
                convertCartesianToKeplerianElements(
                    cartesianElements, centralBodyGravitationalParameter ),
                avoidSingularityAtPiInclination );
}

//! Convert Modified Equinoctial Elements to Cartesian Elements.
basic_mathematics::Vector6d convertModifiedEquinoctialToCartesianElements(
        const basic_mathematics::Vector6d& modifiedEquinoctialElements,
        const double centralBodyGravitationalParameter,
        const bool avoidSingularityAtPiInclination )
// Using unnamed pdf and code archive Bart Rˆmgens.
{
    // Creating output vector.
    basic_mathematics::Vector6d convertedCartesianElements = basic_mathematics::
            Vector6d::Zero( 6 );

    // If the prograde equations are to be used.
    if ( !avoidSingularityAtPiInclination )
    {
        // Local storage of elements for ease of access.
        double semiLatusRectum = modifiedEquinoctialElements( semiLatusRectumIndex );
        double fElement = modifiedEquinoctialElements( fElementIndex );
        double gElement = modifiedEquinoctialElements( gElementIndex );
        double hElement = modifiedEquinoctialElements( hElementIndex );
        double kElement = modifiedEquinoctialElements( kElementIndex );
        double trueLongitude = modifiedEquinoctialElements( trueLongitudeIndex );

        // Computing helper parameters.

        // Square-root
        double squareRootOfGravitationalParameterOverSemiLatusRectum =
                std::sqrt( centralBodyGravitationalParameter / semiLatusRectum );

        // Cosines and sine.
        double cosineTrueLongitude = std::cos( trueLongitude );
        double sineTrueLongitude = std::sin( trueLongitude );

        // s≤, a≤, w, r from references.
        double sSquaredParameter = 1.0 + hElement * hElement + kElement * kElement;
        double aSquaredParameter = hElement * hElement - kElement*kElement;
        double wParameter = 1.0 + fElement * cosineTrueLongitude
                + gElement * sineTrueLongitude;
        double radius = semiLatusRectum / wParameter;

        // Computing position and storing (using code archive Bart Rˆmgens and unnamed pdf).
        convertedCartesianElements( xCartesianPositionIndex )
                = radius / sSquaredParameter * ( cosineTrueLongitude + aSquaredParameter
                                                 * cosineTrueLongitude + 2.0 * hElement * kElement
                                                 * sineTrueLongitude );
        convertedCartesianElements( yCartesianPositionIndex )
                = radius / sSquaredParameter * ( sineTrueLongitude - aSquaredParameter
                                                 * sineTrueLongitude + 2.0 * hElement * kElement
                                                 * cosineTrueLongitude );
        convertedCartesianElements( zCartesianPositionIndex ) = 2.0 * radius / sSquaredParameter
                * ( hElement * sineTrueLongitude - kElement * cosineTrueLongitude );

        // Computing velocities (these can probably use more optimal computation).
        convertedCartesianElements( xCartesianVelocityIndex ) = -1.0 / sSquaredParameter
                * squareRootOfGravitationalParameterOverSemiLatusRectum
                * ( sineTrueLongitude + aSquaredParameter * sineTrueLongitude
                    - 2.0 * hElement * kElement * cosineTrueLongitude + gElement
                    - 2.0 * fElement * hElement * kElement + aSquaredParameter * gElement );
        convertedCartesianElements( yCartesianVelocityIndex ) = -1.0 / sSquaredParameter
                * squareRootOfGravitationalParameterOverSemiLatusRectum
                * ( -cosineTrueLongitude + aSquaredParameter * cosineTrueLongitude
                    + 2.0 * hElement * kElement * sineTrueLongitude - fElement
                    + 2.0 * gElement * hElement * kElement + aSquaredParameter * fElement );
        convertedCartesianElements( zCartesianVelocityIndex ) = 2.0 / sSquaredParameter
                * squareRootOfGravitationalParameterOverSemiLatusRectum
                * ( hElement * cosineTrueLongitude + kElement * sineTrueLongitude
                    + fElement * hElement + gElement * kElement );
    }
    else // use the indirect transformation via an intermediate Kepler state.
    {
        // Use the Keplerian state as an intermediary step, as there is no direct transformation
        // available in literature (personal derivation forgone for now due to time constraints).
        convertedCartesianElements =
                convertKeplerianToCartesianElements(
                    convertModifiedEquinoctialToKeplerianElements(
                        modifiedEquinoctialElements, avoidSingularityAtPiInclination ),
                    centralBodyGravitationalParameter );
    }

    // Return converted set of elements.
    return convertedCartesianElements;
}

} // namespace orbital_element_conversions

} // namespace tudat


//######################################################################
//#   SDF_Fusion Module
//#
//#   Copyright (C) 2020 Siemens AG
//#   SPDX-License-Identifier: MIT
//#   Author 2020: This module has been developed by
//#                or under supervision of Slobodan Ilic
//#######################################################################

#include <iostream>
#include <Eigen/Core>
#include "utils.hpp"
#include "io_utils.hpp"

#include "reconstructor_3d.hpp"
#include "marching_cubes.hpp"
#include "rgbd_types.hpp"

using namespace std;
using namespace cv;
using namespace Eigen;

sdf initialize_sdf(const Configuration &configuration, const vector<float> &volume)
{
	float voxel_size = configuration.get_voxel_size();

	int voxel_count_x = static_cast<int>(std::ceil((volume[1] - volume[0]) / voxel_size));
	int voxel_count_y = static_cast<int>(std::ceil((volume[3] - volume[2]) / voxel_size));
	int voxel_count_z = static_cast<int>(std::ceil((volume[5] - volume[4]) / voxel_size));

	sdf model(voxel_count_x, voxel_count_y, voxel_count_z, voxel_size, 0.f, 1.f);

	return model;
}

template <typename T>
Transform<T, 3, Isometry> to_isometry(const Matrix<T, 4, 4> &pose_matrix)
{
	Transform<T, 3, Isometry> pose;
	pose.setIdentity();

	pose.linear() = pose_matrix.block<3, 3>(0, 0);
	pose.translation() = pose_matrix.block<3, 1>(0, 3);

	return pose;
}

sdf generate_tsdf(const Mat& depth_map, const Mat& color_map, const Isometry3f &transformation_matrix, const Matrix3f &intrinsics, const vector<float>& dimensions,
	const Configuration& configuration) {

	sdf s = initialize_sdf(configuration, dimensions);
	const float voxel_size = configuration.get_voxel_size();

	float delta = voxel_size;
	float eta = 3 * voxel_size;

	Vector3f lower_left(dimensions[0], dimensions[2], dimensions[4]);

#pragma omp parallel for
	for (int z = 0; z < s.size_z; ++z)
		for (int y = 0; y < s.size_y; ++y)
			for (int x = 0; x < s.size_x; ++x) {
				Vector3f rp = lower_left + voxel_size * Vector3f(x + 0.5f, y + 0.5f, z + 0.5f);
				Vector3f transformed_point = transformation_matrix * rp;

				Vector2f point2d = project_point(transformed_point, intrinsics);

				int imgx = static_cast<int>(ceil(point2d(0)));
				int imgy = static_cast<int>(ceil(point2d(1)));

				if (imgx >= 0 && imgy >= 0 && imgx < depth_map.cols && imgy < depth_map.rows) {

					float depth = depth_map.at<float>(imgy, imgx);
					if (isfinite(depth) && depth > .0f) {
						const float signed_distance = depth - transformed_point(2);
						s.weights.at(x, y, z) = signed_distance > -eta;
						s.distance_field.at(x, y, z) = min(1.f, max(-1.f, signed_distance / delta));

						cv::Vec3b color = color_map.at<cv::Vec3b>(imgy, imgx);
						s.color_field.red.at(x, y, z) = color[2] / 255.f;
						s.color_field.green.at(x, y, z) = color[1] / 255.f;
						s.color_field.blue.at(x, y, z) = color[0] / 255.f;
					} // otherwise no need to change values (works only when they are initialized)
				}
			}

	return s;
}

void integrate_into_weighted_average(sdf& wa, const sdf& s) {
	color_cube<float> & wa_colors = wa.color_field;
	const color_cube<float> & s_colors = s.color_field;

#pragma omp parallel for
	for (int z = 0; z < wa.size_z; ++z)
		for (int y = 0; y < wa.size_y; ++y)
			for (int x = 0; x < wa.size_x; ++x) {
				float old_weight = wa.weights.at(x, y, z);
				float added_weight = s.weights.at(x, y, z);
				float new_weight = old_weight + added_weight;

				if (new_weight > 0.f) {
					//wa.distance_field.at(x, y, z) /= wa.weights.at(x, y, z);
					wa.distance_field.at(x, y, z) = (wa.distance_field.at(x, y, z) * old_weight + s.distance_field.at(x, y, z) * added_weight) / new_weight;

					//if (fabs(wa.distance_field.at(x, y, z)) < 1.f) { // assign color only at near-surface voxels
					wa_colors.red.at(x, y, z) = (wa_colors.red.at(x, y, z)   * old_weight + s_colors.red.at(x, y, z)   * added_weight) / new_weight;
					wa_colors.green.at(x, y, z) = (wa_colors.green.at(x, y, z) * old_weight + s_colors.green.at(x, y, z) * added_weight) / new_weight;
					wa_colors.blue.at(x, y, z) = (wa_colors.blue.at(x, y, z)  * old_weight + s_colors.blue.at(x, y, z)  * added_weight) / new_weight;
					//}
				}
				else {
					wa.distance_field.at(x, y, z) = -1.f;
					// and do not change color
				}
				wa.weights.at(x, y, z) = new_weight;
			}
}

sdf reconstruct_sdf(const Configuration& configuration, const vector<float>& volume, const vector<RgbdFile> &input_images, const vector<Matrix4f> &poses)
{

	sdf model = initialize_sdf(configuration, volume);
	const int number_of_frames = static_cast<int>(input_images.size());

	int64 elapsed_time = 0;

	for (int i = 0; i < number_of_frames; ++i)
	{
		RgbdFrame rgbd_frame = get_rgbd_frame(input_images[i]);
		Isometry3f pose = to_isometry(poses[i]);

		auto start_time = getTickCount();
		sdf current_sdf = generate_tsdf(rgbd_frame.second, rgbd_frame.first, pose.inverse(), configuration.get_intrinsics(),
		                                   volume, configuration);
		integrate_into_weighted_average(model, current_sdf);
		elapsed_time += getTickCount() - start_time;
		cout << "Integrated " << i + 1 << " out of " << number_of_frames << " frames into reconstruction" << endl;
	}

	std::cout << "sdf fusion elapsed time: " << elapsed_time / getTickFrequency() << " sec" << std::endl;

	return model;
}

bool run_reconstruction(const Configuration &configuration, const vector<float> &volume)
{
	vector<RgbdFile> input_images = get_input_images(configuration.get_out_rgb_images_dir(), configuration.get_out_depth_images_dir());
	vector<Matrix4f> poses = read_scene_poses<float>(configuration.get_poses_file());

	if (input_images.size() != poses.size())
	{
		cerr << "Number of frames (" << input_images.size() << ") and number of poses (" << poses.size() << ") don't match" << endl;
		return false;
	}

	sdf model = reconstruct_sdf(configuration, volume, input_images, poses);
	vector<marching_cubes::triangle> model_mesh = mesh_valid_only(model);

	save_mesh_ply(model_mesh, configuration.get_model_file(), true);
	return true;
}