#pragma once

#include <Eigen/Core>

#include <ipc/distance/point_point.hpp>

namespace ipc {

/// @brief Compute the distance between a point and line in 2D or 3D.
/// @note The distance is actually squared distance.
/// @param p The point.
/// @param e0 The first vertex of the edge defining the line.
/// @param e1 The second vertex of the edge defining the line.
/// @return The distance between the point and line.
template <typename DerivedP, typename DerivedE0, typename DerivedE1>
auto point_line_distance(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1)
{
    assert(p.size() == 2 || p.size() == 3);
    assert(e0.size() == 2 || e0.size() == 3);
    assert(e1.size() == 2 || e1.size() == 3);

    if (p.size() == 2) {
        auto e = e1 - e0;
        auto numerator =
            (e[1] * p[0] - e[0] * p[1] + e1[0] * e0[1] - e1[1] * e0[0]);
        return numerator * numerator / e.squaredNorm();
    } else {
        return cross(e0 - p, e1 - p).squaredNorm() / (e1 - e0).squaredNorm();
    }
}

// Symbolically generated derivatives;
namespace autogen {
    void point_line_distance_gradient_2D(
        double v01,
        double v02,
        double v11,
        double v12,
        double v21,
        double v22,
        double g[6]);

    void point_line_distance_gradient_3D(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double g[9]);

    void point_line_distance_hessian_2D(
        double v01,
        double v02,
        double v11,
        double v12,
        double v21,
        double v22,
        double H[36]);

    void point_line_distance_hessian_3D(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double H[81]);
} // namespace autogen

/// @brief Compute the gradient of the distance between a point and line.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] e0 The first vertex of the edge defining the line.
/// @param[in] e1 The second vertex of the edge defining the line.
/// @param[out] grad The gradient of the distance wrt p, e0, and e1.
template <
    typename DerivedP,
    typename DerivedE0,
    typename DerivedE1,
    typename DerivedGrad>
void point_line_distance_gradient(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    assert(p.size() == 2 || p.size() == 3);
    assert(e0.size() == 2 || e0.size() == 3);
    assert(e1.size() == 2 || e1.size() == 3);

    grad.resize(p.size() + e0.size() + e1.size());
    if (p.size() == 2) {
        autogen::point_line_distance_gradient_2D(
            p[0], p[1], e0[0], e0[1], e1[0], e1[1], grad.data());
    } else {
        autogen::point_line_distance_gradient_3D(
            p[0], p[1], p[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2],
            grad.data());
    }
}

/// @brief Compute the hessian of the distance between a point and line.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] e0 The first vertex of the edge defining the line.
/// @param[in] e1 The second vertex of the edge defining the line.
/// @param[out] hess The hessian of the distance wrt p, e0, and e1.
template <
    typename DerivedP,
    typename DerivedE0,
    typename DerivedE1,
    typename DerivedHess>
void point_line_distance_hessian(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    assert(p.size() == 2 || p.size() == 3);
    assert(e0.size() == 2 || e0.size() == 3);
    assert(e1.size() == 2 || e1.size() == 3);

    hess.resize(
        p.size() + e0.size() + e1.size(), p.size() + e0.size() + e1.size());
    if (p.size() == 2) {
        autogen::point_line_distance_hessian_2D(
            p[0], p[1], e0[0], e0[1], e1[0], e1[1], hess.data());
    } else {
        autogen::point_line_distance_hessian_3D(
            p[0], p[1], p[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2],
            hess.data());
    }
}

} // namespace ipc


// restored from rdiff-backup: 2016-09-19T17:24:46+02:00

#include <omp.h>
#include <Eigen/Sparse>
#include <algorithm>
#include <boost/math/constants/constants.hpp>
#include <boost/program_options.hpp>
#include <fstream>
#include <iostream>
#include <numeric>
#include <string>

#include <base/eigen2hdf.hpp>
#include <fft/ft_grid_helpers.hpp>
#include <matrices/matrix_entries.hpp>
#include <ridgelet/construction/ft.hpp>
#include <ridgelet/construction/translation_grid.hpp>
#include <ridgelet/lambda.hpp>
#include <ridgelet/ridgelet_frame.hpp>
#include "base/init.hpp"

using namespace std;

const double Lx = 1.0;
const double Ly = 1.0;
const double tol = 1e-10;

const double PI = boost::math::constants::pi<double>();

int main(int argc, char* argv[])
{
  int J, rho;
  SOURCE_INFO();

  namespace po = boost::program_options;

  po::options_description options("options");
  options.add_options()("help", "produce help message")
      ("Jxy,J", po::value<int>(&J)->default_value(3), "J")
      ("rho, r", po::value<int>(&rho)->default_value(1), "rho_x")
      ("transport", "compute transport part too");
  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, options), vm);
    po::notify(vm);
    if (vm.count("help")) {
      std::cout << options << "\n";
      return 0;
    }
  } catch (std::exception& e) {
    std::cout << e.what() << "\n";
  }

  std::cout << "J:   " << J << "\n"
            << "rho: " << rho << "\n";

  RidgeletFrame frame(J, J, rho, rho);

  std::string fname = std::string("mass_matrix_entries") + std::to_string(J) + ".dat";
  // compute the translation grid sizes for reach lambda
  // and
  cout << "Translation grid..."
       << "\n";
  std::vector<unsigned int> tgrid_size(frame.size());
  std::vector<unsigned int> num_elements(frame.size() + 1, 0);
  auto& lambdas = frame.lambdas();
  for (unsigned int i = 0; i < frame.size(); ++i) {
    auto T = tgrid_dim(lambdas[i], frame);
    tgrid_size[i] = std::get<0>(T) * std::get<1>(T);
  }
  std::partial_sum(tgrid_size.begin(), tgrid_size.end(), num_elements.begin() + 1);
  {
    std::ofstream fout(std::string("num_elems") + std::to_string(J) + ".dat");
    std::for_each(
        num_elements.begin(), num_elements.end(), [&fout](unsigned int i) { fout << i << "\n"; });
    fout.close();
  }

  // Normalization ------------------------------
  cout << "Normalization..."
       << "\n";
  std::vector<double> ft_coeff_norms(frame.size());
#pragma omp parallel for
  for (unsigned int i = 0; i < frame.size(); ++i) {
    auto lambda = frame.lambdas()[i];
    if (lambda.t == rt_type::S) {
      ft_coeff_norms[i] = std::sqrt(frame.get_dense(lambda).cwiseAbs2().sum());
    } else {
      ft_coeff_norms[i] = std::sqrt(frame.get_sparse(lambda).cwiseAbs2().sum());
    }
  }

  /*
   *  key = (l_i, l_j, t_d)
   *  l_i, l_j, integer:
   *  ------------------
   *
   *  hint: lambda_i = lambdas[l_i]
   *
   *  difference translation grid t_d:
   *  -----------------
   *  tuple t_d = {tx_i - tx_j, ty_i - ty_j}
   */
  typedef std::tuple<int, int> tgrid_diff_key; /*  type of t_d */
  typedef std::tuple<int, int, tgrid_diff_key> key_type;
  std::unordered_map<key_type, double> mass_matrix_entries;
  std::unordered_map<key_type, double> transport_matrix_entries;

  cout << "Computing integrals..."
       << "\n";
#pragma omp parallel for schedule(dynamic, 1)
  for (unsigned int i = 0; i < frame.size(); ++i) {
    for (unsigned int j = 0; j <= i; ++j) {
      // skip pairs that are known to evaluate to zero
      if (std::abs(lambdas[i].j - lambdas[j].j) > 1) continue;
      if ((lambdas[i].t == rt_type::X && lambdas[j].t == rt_type::Y) ||
          (lambdas[i].t == rt_type::Y && lambdas[j].t == rt_type::X))
        continue;
      if ((lambdas[i].t == lambdas[j].t) && (lambdas[i].j == lambdas[j].j) &&
          (std::abs(lambdas[i].k - lambdas[j].k) > 1))
        continue;

      auto& Psih_i = frame.get_sparse(lambdas[i]);
      // Psih_i, Psih_j sparse
      auto& Psih_j = frame.get_sparse(lambdas[j]);

      auto T1 = tgrid_dim(lambdas[i], frame);
      auto T2 = tgrid_dim(lambdas[j], frame);

      int tx = std::max(std::get<0>(T1), std::get<0>(T2));
      int ty = std::max(std::get<1>(T1), std::get<1>(T2));

      // translation grid
      auto vtx = Eigen::ArrayXd::LinSpaced(tx, 0, (1 - 1 / double(tx)) * Lx);
      auto vty = Eigen::ArrayXd::LinSpaced(ty, 0, (1 - 1 / double(ty)) * Ly);

      double fnorm = 1 / (ft_coeff_norms[i] * ft_coeff_norms[j]);
      for (int idx_tx = 0; idx_tx < tx; ++idx_tx) {
        for (int idx_ty = 0; idx_ty < ty; ++idx_ty) {
          auto k = std::make_tuple(idx_tx, idx_ty);
          auto key = std::make_tuple(i, j, k);
          double v = compute_mass_entry(Psih_i, Psih_j, vtx[idx_tx], vty[idx_ty]);
          v *= fnorm;
          if (std::abs(v) > tol) {
#pragma omp critical
            mass_matrix_entries[key] = v;
          }

          if (vm.count("transport")) {
            double vxy = compute_tentry(
                Psih_i, Psih_j, vtx[idx_tx], vty[idx_ty], derivative_t::dX, derivative_t::dY);
            vxy *= fnorm;
            double vxx = compute_tentry(
                Psih_i, Psih_j, vtx[idx_tx], vty[idx_ty], derivative_t::dX, derivative_t::dX);
            vxx *= fnorm;
            double vyy = compute_tentry(
                Psih_i, Psih_j, vtx[idx_tx], vty[idx_ty], derivative_t::dY, derivative_t::dY);
            vyy *= fnorm;

            double vt = 2 * vxy + vxx + vyy;

            if (std::abs(vt) > tol) {
#pragma omp critical
              transport_matrix_entries[key] = vt;
            }
          }
        }
      }
    }  // end for inner lambda
  }    // end for outer lambda

  std::cout << "mass_matrix_entries.size: " << mass_matrix_entries.size() << "\n";
  std::cout << "writing results to " << fname << "\n";
  // write to disk
  std::ofstream fout(fname);
  for (auto& elem : mass_matrix_entries) {
    auto& key = elem.first;
    auto val = elem.second;
    int i = std::get<0>(key);
    int j = std::get<1>(key);
    int tx, ty;
    std::tie(tx, ty) = std::get<2>(key);
    fout << i << "\t" << j << "\t" << tx << "\t" << ty << "\t" << val << "\n";
  }
  fout.close();
  cout << "\tdone"
       << "\n";

  if (vm.count("transport")) {
    std::string fname = std::string("transport_matrix_" + std::to_string(J) + ".dat");
    std::ofstream fout(fname);
    for (auto& elem : transport_matrix_entries) {
      auto& key = elem.first;
      auto val = elem.second;
      int i = std::get<0>(key);
      int j = std::get<1>(key);
      int tx, ty;
      std::tie(tx, ty) = std::get<2>(key);
      fout << i << "\t" << j << "\t" << tx << "\t" << ty << "\t" << val << "\n";
    }
    fout.close();
  }

  return 0;
}


// Copyright 2021 Apex.AI, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

#ifndef HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_
#define HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_

#include <Eigen/Cholesky>

namespace autoware
{
namespace common
{
namespace helper_functions
{
/// \brief Calculate square of mahalanobis distance
/// \tparam T Type of elements in the matrix
/// \tparam kNumOfStates Number of states
/// \param sample Single column matrix containing sample whose distance needs to be computed
/// \param mean Single column matrix containing mean of samples received so far
/// \param covariance_factor Covariance matrix
/// \return Square of mahalanobis distance
template<typename T, std::int32_t kNumOfStates>
types::float32_t calculate_squared_mahalanobis_distance(
  const Eigen::Matrix<T, kNumOfStates, 1> & sample,
  const Eigen::Matrix<T, kNumOfStates, 1> & mean,
  const Eigen::Matrix<T, kNumOfStates, kNumOfStates> & covariance_factor)
{
  using Vector = Eigen::Matrix<T, kNumOfStates, 1>;
  // This is equivalent to the squared Mahalanobis distance of the form: diff.T * C.inv() * diff
  // Instead of the covariance matrix C we have its lower-triangular factor L, such that C = L * L.T
  // squared_mahalanobis_distance = diff.T * C.inv() * diff
  // = diff.T * (L * L.T).inv() * diff
  // = diff.T * L.T.inv() * L.inv() * diff
  // = (L.inv() * diff).T * (L.inv() * diff)
  // this allows us to efficiently find the squared Mahalanobis distance using (L.inv() * diff),
  // which can be found as a solution to: L * x = diff.
  const Vector diff = sample - mean;
  const Vector x = covariance_factor.ldlt().solve(diff);
  return x.transpose() * x;
}

/// \brief Calculate mahalanobis distance
/// \tparam T Type of elements in the matrix
/// \tparam kNumOfStates Number of states
/// \param sample Single column matrix containing sample whose distance needs to be computed
/// \param mean Single column matrix containing mean of samples received so far
/// \param covariance_factor Covariance matrix
/// \return Mahalanobis distance
template<typename T, std::int32_t kNumOfStates>
types::float32_t calculate_mahalanobis_distance(
  const Eigen::Matrix<T, kNumOfStates, 1> & sample,
  const Eigen::Matrix<T, kNumOfStates, 1> & mean,
  const Eigen::Matrix<T, kNumOfStates, kNumOfStates> & covariance_factor
)
{
  return sqrtf(calculate_squared_mahalanobis_distance(sample, mean, covariance_factor));
}
}  // namespace helper_functions
}  // namespace common
}  // namespace autoware

#endif  // HELPER_FUNCTIONS__MAHALANOBIS_DISTANCE_HPP_


// Boost.Geometry

// Copyright (c) 2016-2017 Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_FORMULAS_MAXIMUM_LONGITUDE_HPP
#define BOOST_GEOMETRY_FORMULAS_MAXIMUM_LONGITUDE_HPP

#include <boost/geometry/formulas/spherical.hpp>
#include <boost/geometry/formulas/flattening.hpp>

#include <boost/mpl/assert.hpp>

#include <boost/math/special_functions/hypot.hpp>

namespace boost { namespace geometry { namespace formula
{

/*!
\brief Algorithm to compute the vertex longitude of a geodesic segment. Vertex is
a point on the geodesic that maximizes (or minimizes) the latitude. The algorithm
is given the vertex latitude.
*/

//Classes for spesific CS

template <typename CT>
class vertex_longitude_on_sphere
{

public:

    template <typename T>
    static inline CT apply(T const& lat1, //segment point 1
                           T const& lat2, //segment point 2
                           T const& lat3, //vertex latitude
                           T const& sin_l12,
                           T const& cos_l12) //lon1 -lon2
    {
        //https://en.wikipedia.org/wiki/Great-circle_navigation#Finding_way-points
        CT const A = sin(lat1) * cos(lat2) * cos(lat3) * sin_l12;
        CT const B = sin(lat1) * cos(lat2) * cos(lat3) * cos_l12
                - cos(lat1) * sin(lat2) * cos(lat3);
        CT lon = atan2(B, A);
        return lon + math::pi<CT>();
    }
};

template <typename CT>
class vertex_longitude_on_spheroid
{
    template<typename T>
    static inline void normalize(T& x, T& y)
    {
        T h = boost::math::hypot(x, y);
        x /= h;
        y /= h;
    }

public:

    template <typename T, typename Spheroid>
    static inline CT apply(T const& lat1, //segment point 1
                           T const& lat2, //segment point 2
                           T const& lat3, //vertex latitude
                           T& alp1,
                           Spheroid const& spheroid)
    {
        // We assume that segment points lay on different side w.r.t.
        // the vertex

        // Constants
        CT const c0 = 0;
        CT const c2 = 2;
        CT const half_pi = math::pi<CT>() / c2;
        if (math::equals(lat1, half_pi)
                || math::equals(lat2, half_pi)
                || math::equals(lat1, -half_pi)
                || math::equals(lat2, -half_pi))
        {
            // one segment point is the pole
            return c0;
        }

        // More constants
        CT const f = flattening<CT>(spheroid);
        CT const pi = math::pi<CT>();
        CT const c1 = 1;
        CT const cminus1 = -1;

        // First, compute longitude on auxiliary sphere

        CT const one_minus_f = c1 - f;
        CT const bet1 = atan(one_minus_f * tan(lat1));
        CT const bet2 = atan(one_minus_f * tan(lat2));
        CT const bet3 = atan(one_minus_f * tan(lat3));

        CT cos_bet1 = cos(bet1);
        CT cos_bet2 = cos(bet2);
        CT const sin_bet1 = sin(bet1);
        CT const sin_bet2 = sin(bet2);
        CT const sin_bet3 = sin(bet3);

        CT omg12 = 0;

        if (bet1 < c0)
        {
            cos_bet1 *= cminus1;
            omg12 += pi;
        }
        if (bet2 < c0)
        {
            cos_bet2 *= cminus1;
            omg12 += pi;
        }

        CT const sin_alp1 = sin(alp1);
        CT const cos_alp1 = math::sqrt(c1 - math::sqr(sin_alp1));

        CT const norm = math::sqrt(math::sqr(cos_alp1) + math::sqr(sin_alp1 * sin_bet1));
        CT const sin_alp0 = sin(atan2(sin_alp1 * cos_bet1, norm));

        BOOST_ASSERT(cos_bet2 != c0);
        CT const sin_alp2 = sin_alp1 * cos_bet1 / cos_bet2;

        CT const cos_alp0 = math::sqrt(c1 - math::sqr(sin_alp0));
        CT const cos_alp2 = math::sqrt(c1 - math::sqr(sin_alp2));

        CT const sig1 = atan2(sin_bet1, cos_alp1 * cos_bet1);
        CT const sig2 = atan2(sin_bet2, -cos_alp2 * cos_bet2); //lat3 is a vertex

        CT const cos_sig1 = cos(sig1);
        CT const sin_sig1 = math::sqrt(c1 - math::sqr(cos_sig1));

        CT const cos_sig2 = cos(sig2);
        CT const sin_sig2 = math::sqrt(c1 - math::sqr(cos_sig2));

        CT const omg1 = atan2(sin_alp0 * sin_sig1, cos_sig1);
        CT const omg2 = atan2(sin_alp0 * sin_sig2, cos_sig2);

        omg12 += omg1 - omg2;

        CT const sin_omg12 = sin(omg12);
        CT const cos_omg12 = cos(omg12);

        CT omg13 = geometry::formula::vertex_longitude_on_sphere<CT>
                ::apply(bet1, bet2, bet3, sin_omg12, cos_omg12);

        if (lat1 * lat2 < c0)//different hemispheres
        {
            if ((lat2 - lat1) * lat3  > c0)// ascending segment
            {
                omg13 = pi - omg13;
            }
        }

        // Second, compute the ellipsoidal longitude

        CT const e2 = f * (c2 - f);
        CT const ep = math::sqrt(e2 / (c1 - e2));
        CT const k2 = math::sqr(ep * cos_alp0);
        CT const sqrt_k2_plus_one = math::sqrt(c1 + k2);
        CT const eps = (sqrt_k2_plus_one - c1) / (sqrt_k2_plus_one + c1);
        CT const eps2 = eps * eps;
        CT const n = f / (c2 - f);

        // sig3 is the length from equator to the vertex
        CT sig3;
        if(sin_bet3 > c0)
        {
            sig3 = half_pi;
        } else {
            sig3 = -half_pi;
        }
        CT const cos_sig3 = 0;
        CT const sin_sig3 = 1;

        CT sig13 = sig3 - sig1;
        if (sig13 > pi)
        {
            sig13 -= 2 * pi;
        }

        // Order 2 approximation
        CT const c1over2 = 0.5;
        CT const c1over4 = 0.25;
        CT const c1over8 = 0.125;
        CT const c1over16 = 0.0625;
        CT const c4 = 4;
        CT const c8 = 8;

        CT const A3 = 1 - (c1over2 - c1over2 * n) * eps - c1over4 * eps2;
        CT const C31 = (c1over4 - c1over4 * n) * eps + c1over8 * eps2;
        CT const C32 = c1over16 * eps2;

        CT const sin2_sig3 = c2 * cos_sig3 * sin_sig3;
        CT const sin4_sig3 = sin_sig3 * (-c4 * cos_sig3
                                         + c8 * cos_sig3 * cos_sig3 * cos_sig3);
        CT const sin2_sig1 = c2 * cos_sig1 * sin_sig1;
        CT const sin4_sig1 = sin_sig1 * (-c4 * cos_sig1
                                         + c8 * cos_sig1 * cos_sig1 * cos_sig1);
        CT const I3 = A3 * (sig13
                            + C31 * (sin2_sig3 - sin2_sig1)
                            + C32 * (sin4_sig3 - sin4_sig1));

        CT const sign = bet3 >= c0
                      ? c1
                      : cminus1;

        CT const dlon_max = omg13 - sign * f * sin_alp0 * I3;

        return dlon_max;
    }
};

//CS_tag dispatching

template <typename CT, typename CS_Tag>
struct compute_vertex_lon
{
    BOOST_MPL_ASSERT_MSG
    (
        false, NOT_IMPLEMENTED_FOR_THIS_COORDINATE_SYSTEM, (types<CS_Tag>)
    );

};

template <typename CT>
struct compute_vertex_lon<CT, spherical_equatorial_tag>
{
    template <typename Strategy>
    static inline CT apply(CT const& lat1,
                           CT const& lat2,
                           CT const& vertex_lat,
                           CT const& sin_l12,
                           CT const& cos_l12,
                           CT,
                           Strategy)
    {
        return vertex_longitude_on_sphere<CT>
                ::apply(lat1,
                        lat2,
                        vertex_lat,
                        sin_l12,
                        cos_l12);
    }
};

template <typename CT>
struct compute_vertex_lon<CT, geographic_tag>
{
    template <typename Strategy>
    static inline CT apply(CT const& lat1,
                           CT const& lat2,
                           CT const& vertex_lat,
                           CT,
                           CT,
                           CT& alp1,
                           Strategy const& azimuth_strategy)
    {
        return vertex_longitude_on_spheroid<CT>
                ::apply(lat1,
                        lat2,
                        vertex_lat,
                        alp1,
                        azimuth_strategy.model());
    }
};

// Vertex longitude interface
// Assume that lon1 < lon2 and vertex_lat is the latitude of the vertex

template <typename CT, typename CS_Tag>
class vertex_longitude
{
public :
    template <typename Strategy>
    static inline CT apply(CT& lon1,
                           CT& lat1,
                           CT& lon2,
                           CT& lat2,
                           CT const& vertex_lat,
                           CT& alp1,
                           Strategy const& azimuth_strategy)
    {
        CT const c0 = 0;
        CT pi = math::pi<CT>();

        //Vertex is a segment's point
        if (math::equals(vertex_lat, lat1))
        {
            return lon1;
        }
        if (math::equals(vertex_lat, lat2))
        {
            return lon2;
        }

        //Segment lay on meridian
        if (math::equals(lon1, lon2))
        {
            return (std::max)(lat1, lat2);
        }
        BOOST_ASSERT(lon1 < lon2);

        CT dlon = compute_vertex_lon<CT, CS_Tag>::apply(lat1, lat2,
                                                        vertex_lat,
                                                        sin(lon1 - lon2),
                                                        cos(lon1 - lon2),
                                                        alp1,
                                                        azimuth_strategy);

        CT vertex_lon = std::fmod(lon1 + dlon, 2 * pi);

        if (vertex_lat < c0)
        {
            vertex_lon -= pi;
        }

        if (std::abs(lon1 - lon2) > pi)
        {
            vertex_lon -= pi;
        }

        return vertex_lon;
    }
};

}}} // namespace boost::geometry::formula
#endif // BOOST_GEOMETRY_FORMULAS_MAXIMUM_LONGITUDE_HPP


//==================================================================================================
/*!
  @file
  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ERFC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ERFC_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!
  @ingroup group-euler
    Function object implementing erfc capabilities

  Computes the complementary error function
   \f$\displaystyle \frac{2}{\sqrt\pi}\int_{x}^{\infty} e^{-t^2}\mbox{d}t\f$

  @par Semantic:

  For every parameter of floating type T

  @code
  T r = erfc(x);
  @endcode

  is similar to:

  @code
  T0 r = oneminus(erf(x));
  @endcode

  @par Decorators

  std_ for floating entries provides access to @c std::erfc

  @see erf, erfcx, oneminus

  **/
  Value erfc(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/erfc.hpp>
#include <boost/simd/function/simd/erfc.hpp>

#endif


#include "distances.h"
#include "EMD_wrapper.h"

#define EIGEN_NO_DEBUG
#define EIGEN_DONT_PARALLELIZE
#include <Eigen/Dense>

#include <cmath> // for std::isnormal

DistanceIndexPairs lp_distances(int Mp_i, const Options& opts, const Manifold& M, const Manifold& Mp,
                                std::vector<int> inpInds)
{
  std::vector<int> inds;
  std::vector<double> dists;

  // Compare every observation in the M manifold to the
  // Mp_i'th observation in the Mp manifold.
  for (int i : inpInds) {
    // Calculate the distance between M[i] and Mp[Mp_i]
    double dist_i = 0.0;

    // If we have panel data and the M[i] / Mp[Mp_j] observations come from different panels
    // then add the user-supplied penalty/distance for the mismatch.
    if (opts.panelMode && opts.idw > 0) {
      dist_i += opts.idw * (M.panel(i) != Mp.panel(Mp_i));
    }

    for (int j = 0; j < M.E_actual(); j++) {
      // Get the sub-distance between M[i,j] and Mp[Mp_i, j]
      double dist_ij;

      // If either of these values is missing, the distance from
      // M[i,j] to Mp[Mp_i, j] is opts.missingdistance.
      // However, if the user doesn't specify this, then the entire
      // M[i] to Mp[Mp_i] distance is set as missing.
      if ((M(i, j) == MISSING_SENTINEL) || (Mp(Mp_i, j) == MISSING_SENTINEL)) {
        if (opts.missingdistance == 0) {
          dist_i = MISSING_SENTINEL;
          break;
        } else {
          dist_ij = opts.missingdistance;
        }
      } else { // Neither M[i,j] nor Mp[Mp_i, j] is missing.
        // How do we compare them? Do we treat them like continuous values and subtract them,
        // or treat them like unordered categorical variables and just check if they're the same?
        if (opts.metrics[j] == Metric::Diff) {
          dist_ij = M(i, j) - Mp(Mp_i, j);
        } else { // Metric::CheckSame
          dist_ij = (M(i, j) != Mp(Mp_i, j));
        }
      }

      if (opts.distance == Distance::MeanAbsoluteError) {
        dist_i += abs(dist_ij) / M.E_actual();
      } else { // Distance::Euclidean
        dist_i += dist_ij * dist_ij;
      }
    }

    if (dist_i != 0 && dist_i != MISSING_SENTINEL) {
      if (opts.distance == Distance::MeanAbsoluteError) {
        dists.push_back(dist_i);
      } else { // Distance::Euclidean
        dists.push_back(sqrt(dist_i));
      }
      inds.push_back(i);
    }
  }

  return { inds, dists };
}

// This function compares the M(i,.) multivariate time series to the Mp(j,.) multivariate time series.
// The M(i,.) observation has data for E consecutive time points (e.g. time(i), time(i+1), ..., time(i+E-1)) and
// the Mp(j,.) observation corresponds to E consecutive time points (e.g. time(j), time(j+1), ..., time(j+E-1)).
// At each time instant we observe n >= 1 pieces of data.
// These may either be continuous data or unordered categorical data.
//
// The Wasserstein distance (using the 'curve-matching' strategy) is equivalent to the (minimum) cost of turning
// the first time series into the second time series. In a simple example, say E = 2 and n = 1, and
//         M(i,.) = [ 1, 2 ] and Mp(j,.) = [ 2, 2 ].
// To turn M(i,.) into Mp(j,.) the first element needs to be increased by 1, so the overall cost is
//         Wasserstein( M(i,.), Mp(j,.) ) = 1.
// The distance can also reorder the points, so for example say
//         M(i,.) = [ 1, 100 ] and Mp(j,.) = [ 100, 1 ].
// If we just change the 1 to 100 and the 100 to 1 then the cost of each is 99 + 99 = 198.
// However, Wasserstein can instead reorder these points at a cost of
//         Wasserstein( M(i,.), Mp(j,.) ) = 2 * gamma * (time(1)-time(2))
// so if the observations occur on a regular grid so time(i) = i then the distance will just be 2 * gamma.
//
// The return value of this function is a matrix which shows the pairwise costs associated to each
// potential Wasserstein solution. E.g. the (n,m) element of the returned matrix shows the cost
// of turning the individual point M(i, n) into Mp(j, m).
//
// When there are missing values in one or other observation, we can either ignore this time period
// and compute the Wasserstein for the mismatched regime where M(i,.) is of size len_i and Mp(j,.) is
// of size len_j, where len_i != len_j is possible. Alternatively, we can fill in the affected elements
// of the cost matrix with some user-supplied 'missingDistance' value and then len_i == len_j is upheld.
std::unique_ptr<double[]> wasserstein_cost_matrix(const Manifold& M, const Manifold& Mp, int i, int j,
                                                  const Options& opts, int& len_i, int& len_j)
{
  // The M(i,.) observation will be stored as one flat vector of length M.E_actual():
  // - the first M.E() observations will the lagged version of the main time series
  // - the next M.E() observations will be the lagged 'dt' time series (if it is included, i.e., if M.E_dt() > 0)
  // - the next n * M.E() observations will be the n lagged extra variables,
  //   so in total that is M.E_lagged_extras() = n * M.E() observations
  // - the remaining M.E_actual() - M.E() - M.E_dt() - M.E_lagged_extras() are the unlagged extras and the distance
  //   between those two vectors forms a kind of minimum distance which is added to the time-series curve matching
  //   Wasserstein distance.

  bool skipMissing = (opts.missingdistance == 0);

  auto M_i = M.laggedObsMap(i);
  auto Mp_j = Mp.laggedObsMap(j);

  auto M_i_missing = (M_i.array() == M.missing()).colwise().any();
  auto Mp_j_missing = (Mp_j.array() == Mp.missing()).colwise().any();

  if (skipMissing) {
    // N.B. Can't .sum() a vector of bools to count them in Eigen.
    len_i = M.E() - M_i_missing.count();
    len_j = Mp.E() - Mp_j_missing.count();
  } else {
    len_i = M.E();
    len_j = Mp.E();
  }

  double gamma = 1.0;
  if (M.E_dt() > 0) {
    // Imagine the M_i time series as a plot, and calculate the
    // aspect ratio of this plot, so we can rescale the time variable
    // to get the user-supplied aspect ratio.
    double minData = std::numeric_limits<double>::max();
    double maxData = std::numeric_limits<double>::min();
    double maxTime = 0.0;
    for (int t = 0; t < M_i.cols(); t++) {
      if (M_i(0, t) != MISSING_SENTINEL) {
        if (M_i(0, t) < minData) {
          minData = M_i(0, t);
        }
        if (M_i(0, t) > maxData) {
          maxData = M_i(0, t);
        }
      }
      if (M_i(1, t) != MISSING_SENTINEL && M_i(1, t) > maxTime) {
        maxTime = M_i(1, t);
      }
    }

    double epsilon = 1e-6; // Some small number in case the following ratio gets wildly large/small
    gamma = opts.aspectRatio * (maxData - minData + epsilon) / (maxTime + epsilon);
  }

  int timeSeriesDim = M_i.rows();

  double unlaggedDist = 0.0;
  int numUnlaggedExtras = M.E_extras() - M.E_lagged_extras();
  for (int e = 0; e < numUnlaggedExtras; e++) {
    double x = M.unlagged_extras(i, e), y = Mp.unlagged_extras(j, e);
    bool eitherMissing = (x == M.missing()) || (y == M.missing());

    if (eitherMissing) {
      unlaggedDist += opts.missingdistance;
    } else {
      if (opts.metrics[timeSeriesDim + e] == Metric::Diff) {
        unlaggedDist += abs(x - y);
      } else {
        unlaggedDist += x != y;
      }
    }
  }

  // If we have panel data and the M[i] / Mp[j] observations come from different panels
  // then add the user-supplied penalty/distance for the mismatch.
  if (opts.panelMode && opts.idw > 0) {
    unlaggedDist += opts.idw * (M.panel(i) != Mp.panel(j));
  }

  auto flatCostMatrix = std::make_unique<double[]>(len_i * len_j);
  std::fill_n(flatCostMatrix.get(), len_i * len_j, unlaggedDist);
  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> costMatrix(flatCostMatrix.get(),
                                                                                len_i, len_j);
  for (int k = 0; k < timeSeriesDim; k++) {
    int n = 0;
    for (int nn = 0; nn < M_i.cols(); nn++) {
      if (skipMissing && M_i_missing[nn]) {
        continue;
      }

      int m = 0;

      for (int mm = 0; mm < Mp_j.cols(); mm++) {
        if (skipMissing && Mp_j_missing[mm]) {
          continue;
        }
        double dist;
        bool eitherMissing = M_i_missing[nn] || Mp_j_missing[mm];

        if (eitherMissing) {
          dist = opts.missingdistance;
        } else {
          if (opts.metrics[k] == Metric::Diff) {
            dist = abs(M_i(k, nn) - Mp_j(k, mm));
          } else {
            dist = M_i(k, nn) != Mp_j(k, mm);
          }
        }

        // For the time data, we add in the 'gamma' scaling factor calculated earlier
        if ((M.E_dt() > 0) && (k == 1)) {
          dist *= gamma;
        }

        costMatrix(n, m) += dist;

        m += 1;
      }

      n += 1;
    }
  }
  return flatCostMatrix;
}

// TODO: Subtract the D(x,x) and D(y,y) parts from this.
double approx_wasserstein(double* C, int len_i, int len_j, double eps, double stopErr)
{
  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> costMatrix(C, len_i, len_j);

  double r = 1.0 / len_i;
  double c = 1.0 / len_j;

  Eigen::MatrixXd K = Eigen::exp(-costMatrix.array() / eps);
  Eigen::MatrixXd Kp = len_i * K.array();

  Eigen::VectorXd u = Eigen::VectorXd::Ones(len_i) / len_i;
  Eigen::VectorXd v = Eigen::VectorXd::Ones(len_j) / len_j;

  int maxIter = 10000;
  for (int iter = 0; iter < maxIter; iter++) {

    v = c / (K.transpose() * u).array();
    u = 1.0 / (Kp * v).array();

    if (iter % 10 == 0) {
      // Compute right marginal (diag(u) K diag(v))^T1
      Eigen::VectorXd tempColSums = (u.asDiagonal() * K * v.asDiagonal()).colwise().sum();
      double LInfErr = (tempColSums.array() - c).abs().maxCoeff();
      if (LInfErr < stopErr) {
        break;
      }
    }
  }

  Eigen::MatrixXd transportPlan = u.asDiagonal() * K * v.asDiagonal();
  double dist = (transportPlan.array() * costMatrix.array()).sum();
  return dist;
}

double wasserstein(double* C, int len_i, int len_j)
{
  // Create vectors which are just 1/len_i and 1/len_j of length len_i and len_j.
  auto w_1 = std::make_unique<double[]>(len_i);
  std::fill_n(w_1.get(), len_i, 1.0 / len_i);
  auto w_2 = std::make_unique<double[]>(len_j);
  std::fill_n(w_2.get(), len_j, 1.0 / len_j);

  int maxIter = 10000;
  double cost;
  EMD_wrap(len_i, len_j, w_1.get(), w_2.get(), C, &cost, maxIter);
  return cost;
}

DistanceIndexPairs wasserstein_distances(int Mp_i, const Options& opts, const Manifold& M, const Manifold& Mp,
                                         std::vector<int> inpInds)
{
  std::vector<int> inds;
  std::vector<double> dists;

  // Compare every observation in the M manifold to the
  // Mp_i'th observation in the Mp manifold.
  for (int i : inpInds) {
    int len_i, len_j;
    auto C = wasserstein_cost_matrix(M, Mp, i, Mp_i, opts, len_i, len_j);

    if (len_i > 0 && len_j > 0) {
      double dist_i = wasserstein(C.get(), len_i, len_j);

      // Alternatively, the approximate version based on Sinkhorn's algorithm can be called with something like:
      // double dist_i = approx_wasserstein(C.get(), len_i, len_j, 0.1, 0.1)
      // In that case, the "std::isnormal" is really needed on the next line, as some
      // instability gives us some 'nan' distances using that method.

      if (dist_i != 0 && std::isnormal(dist_i)) {
        dists.push_back(dist_i);
        inds.push_back(i);
      }
    }
  }

  return { inds, dists };
}


/* Enlarges source image trying to estimate missing pixels - Quadratic Model
   (c) 2016, Eduardo Valle, blog.eduardovalle.com/
*/
#include <cassert>
#include <iostream>
#include <map>

// #define SOLVER_SPARSE
#include <Eigen/Core>
#ifdef SOLVER_SPARSE
    #include <Eigen/Sparse>
    typedef Eigen::Triplet<double> Triplet;
#else
    #include <Eigen/Dense>
#endif

#include <gd.h>

using namespace std;

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

    // Check command-line and open source image
    const int nParamsMin = 2;
    const int nParamsMax = 8;
    if ( argc <= nParamsMin || argc>(nParamsMax+1) ) {
        cerr << "Usage: " << argv[0]
             << " <in.png> <out.png> [<dom>] [<asp>] [H up] [V up] [gweight] [toler]" << endl
             << endl
             << "   in.png    Input image in PNG format (mandatory)" << endl
             << "   out.png   Output image in PNG format (mandatory)" << endl
             << "   dom       Pixel connectedness domain: 4 or 8 (def: 4)" << endl
             << "   asp       Pixel aspect ratio width/height (def: 1.0)" << endl
             << "   H up      How much to upscale the image horizontally (def: 3)" << endl
             << "   V up      How much to upscale the image vertically (def: 3)" << endl
             << "   gweight   Relative importance of respecting data (def: 1.0 = neutral)" << endl
             << "   toler     Graylevel difference for  equal pixels (def: 0, max: 255)" << endl
             << endl
             << "   Optional parameters must be entered in the right sequence" << endl
             << endl
             #ifdef SOLVER_SPARSE
                 << "   Solver: Eigen::SparseQR" << endl
             #else
                 << "   Solver: Eigen::HouseholderQR" << endl
             #endif
             << endl;
        return 1;
    }
    FILE *infile = fopen(argv[1], "r");
    if (infile == NULL) {
        cerr << "Error opening:" << argv[1] << endl;
        return 1;
    }
    gdImagePtr simage = gdImageCreateFromPng(infile);
    fclose(infile);
    if (simage == NULL) {
        cerr << "Error reading:" << argv[1] << endl;
        return 1;
    }
    const int w = simage->sx;
    const int h = simage->sy;

    // Are pixel and sample neighborhoods 4-connected or 8-connected ?
    const int connectedness = (argc <= 3) ? 4 : atoi(argv[3]);
    if (connectedness!=4 && connectedness!=8) {
        cerr << "Connectedness domain must be 4 or 8: " << argv[3] << endl;
        return 1;
    }
    const bool  four = (connectedness == 4);

    // What is the relative importance of neighbors
    const float aspect     = (argc <= 4) ? 1.0 : atof(argv[4]);
    if (aspect <= 0.0f) {
        cerr << "Aspect ratio must be >= 0.0: " << argv[4] << endl;
        return 1;
    }
    const float weightV    = 1.0;
    const float weightH    = pow(1.0/aspect, 2.0);
    const float weightD    = 1.0/(1.0 + aspect*aspect);

    // Horizontal, vertical, and surface ampliation
    const int   upX        = (argc <= 5) ? 3 : atoi(argv[5]);
    const int   upY        = (argc <= 6) ? 3 : atoi(argv[6]);
    if (upX < 1) {
        cerr << "Horizontal upscaling must be 1 or greater: " << argv[5] << endl;
        return 1;
    }
    if (upY < 1) {
        cerr << "Vertical upscaling must be 1 or greater: " << argv[6] << endl;
        return 1;
    }
    const int   up2D       = upX*upY;

    // Relative importance of attachment to graylevels
    const float grayWeight = up2D * ((argc <= 7) ? 1.0 : atof(argv[7]));
    if (grayWeight <= 0.0) {
        cerr << "Grayscale attachment weight must be > 0.0: " << argv[7] << endl;
        return 1;
    }

    // Tolerance for pixels be considered equal
    const float tolerance = (argc <= 8) ? 1.0 : atof(argv[8]);
    if (tolerance < 0.0) {
        cerr << "Pixel difference tolerance must be >= 0.0: " << argv[8] << endl;
        return 1;
    }

    // First pass : reads pixels matrix and count the number of needed variables
    const int ws = w+2;
    const int hs = h+2;
    float samples[hs][ws],
          lp = 0.0f;
    int   hotEstimate = 0;
    for (int y=0, ys=1; y<h; y++, ys++) {
        for (int x=0, xs=1; x<w; x++, xs++) {
            const int rgb = gdImageGetTrueColorPixel(simage, x, y);
            const float r = rgb >> 16 & 0xff;
            const float g = rgb >> 8  & 0xff;
            const float b = rgb       & 0xff;
            // Luminance coefficient for each color
            const float rl = 0.21f;
            const float gl = 0.72f;
            const float bl = 0.07f;
            const float l = r*rl + g*gl + b*bl;
            // Destination pixels
            samples[ys][xs] = l;
            hotEstimate += (abs(l-lp)>tolerance) ? 1 : 0;
        }
    }
    // ...correct borders (tries to create uniform border) to ease processing
    for (int ys=0; ys<hs; ys++) {
        samples[ys][0   ] = samples[   1][   1];
        samples[ys][ws-1] = samples[hs-2][ws-2];
    }
    for (int xs=0; xs<ws; xs++) {
        samples[   0][xs] = samples[   1][   1];
        samples[hs-1][xs] = samples[hs-2][ws-2];
    }

    gdImageDestroy(simage);

    cerr << "Hot pixels estimate: " << hotEstimate << endl;

    // Second pass : assembles list of the hot pixels :
    // those which are not equal to their neighbors
    map<pair<int, int>, int> hotMap;
    int hotCount = 0;
    for (int ys=1; ys<hs-1; ys++) {
        for (int xs=1; xs<ws-1; xs++) {
            if ((abs(samples[ys][xs] - samples[ys+1][xs+1]) > tolerance) ||
                (abs(samples[ys][xs] - samples[ys+1][xs  ]) > tolerance) ||
                (abs(samples[ys][xs] - samples[ys+1][xs-1]) > tolerance) ||
                (abs(samples[ys][xs] - samples[ys  ][xs+1]) > tolerance) ||
                (abs(samples[ys][xs] - samples[ys  ][xs-1]) > tolerance) ||
                (abs(samples[ys][xs] - samples[ys-1][xs+1]) > tolerance) ||
                (abs(samples[ys][xs] - samples[ys-1][xs  ]) > tolerance) ||
                (abs(samples[ys][xs] - samples[ys-1][xs-1]) > tolerance)) {
                hotMap.insert(make_pair(make_pair(xs, ys), hotCount));
                hotCount++;
            }
        }
    }

    cerr << "Hot pixels count: " << hotCount << endl;

    // Assembles optimization matrix...

    // The optimization we'll attempt tries to minimize the quadradic
    // difference between neighboring subsamples, while constraining all
    // subsamples inside a pixel to average to the given pixel. I.e.:
    // If subsamples si,1 to si,n belong to pi, we are trying to minimize
    // Sum_{for all neighboring subsample pairs s_1 s_2 } (s_1 - s_2)^2
    // constrained to : For all pixels p_i :
    // Avg_{for all subsamples s_i,j of pixel pi} si,j = pi

    // The optimization above leads to an overdetermined linear system:
    // 1) One equation per pixel for the averaging constraint
    // 2) One equation per subsample for the quadratic minimization that
    //    attains its mininimum when each subsample is the average of its
    //    neighbors

    const int vars    =  hotCount*up2D;
    const int eqns    =  hotCount*up2D                 + hotCount;
    const int nonzero = (hotCount*up2D)*(four ? 4 : 8) + (hotCount*up2D);
    cerr << "Optimizing : " << vars
             << " variables and " << eqns
             << " equations. " << nonzero
             << " non-zero entries / " << ( ((float)vars)*eqns )
             << " total (" << ( ((float) nonzero)/vars/eqns*100.0 )
             << "%)"  << endl;

    #ifdef SOLVER_SPARSE
        cerr << "Using sparse matrices" << endl;
        std::vector<Triplet> triplets(nonzero);
        Eigen::SparseMatrix<float> optimization(eqns, vars);
    #else
        cerr << "Using dense matrices" << endl;
        Eigen::MatrixXf optimization(eqns, vars);
        optimization.setZero();
    #endif
    Eigen::VectorXf targets(eqns);

    // ...variable index for subsample at position (u,v) of the pixel (x,y)
    auto varindex = [&](int i, int u, int v) -> int  {
        assert(i>=0 && i<hotCount && u>=0 && u<upX && v>=0 && v<upY);
        return i*up2D + v*upX+u;
    };
    auto findpix = [&](int x, int y) -> int {
        assert(x>=0 && x<ws && y>=0 && y<hs);
        try {
            return hotMap.at(make_pair(x, y));
        }
        catch(out_of_range e) {
            return -1;
        }
    };

    // ...assemble the graylevel constraints
    cerr << "Assembling graylevel constraints... " << endl;
    for (auto pixel : hotMap) {
        const int x = pixel.first.first;
        const int y = pixel.first.second;
        const int i = pixel.second;
        // The constraint is for the average of the subsamples to
        // be the graylevel of the pixel
        for (int v=0; v<upY; v++) {
            for (int u=0; u<upX; u++) {
                const int var   = varindex(i, u, v);
                const float val = (1.0f / up2D) * grayWeight;
                #ifdef SOLVER_SPARSE
                    triplets.push_back(Triplet(i, var, val));
                #else
                    optimization(i, var) = val;
                #endif
            }
        }
        targets(i) = samples[y][x] * grayWeight;
    }
    int eqn = hotCount;

    // ...assemble the quadratic minimizations
    cerr << "Assembling quadratic minimizers... " << endl;
    for (auto pixel : hotMap) {
        const int x = pixel.first.first;
        const int y = pixel.first.second;
        const int i = pixel.second;

        // Finds neighboring pixels
        const int iLU = four ? -1 : findpix(x-1, y-1);
        const int iL  =             findpix(x-1, y  );
        const int iLD = four ? -1 : findpix(x-1, y+1);
        const int iU  =             findpix(x  , y-1);
        const int iD  =             findpix(x  , y+1);
        const int iRU = four ? -1 : findpix(x+1, y-1);
        const int iR  =             findpix(x+1, y  );
        const int iRD = four ? -1 : findpix(x+1, y+1);

        // Finds variable corresponding to subsample at u+m,v+n
        // n, m = {-1, 0, 1}
        auto neighbor = [&](int u, int v, int m, int n) -> int {
            assert(u>=0 && u<upX && v>=0 && v<upY && m>=-1 && m<=1 && n>=-1 && n<=1);
            int nu = u+m, nv = v+n;
            // Safe cases : neighbor is in same pixel
            if (nu>=0 && nu<upX && nv>=0 && nv<upY) {
                return varindex(i, nu, nv);
            }
            // Problematic cases : neighbor is accross pixels
            int ni = -1;
            if (nu < 0) {
                if (nv <    0) { ni = iLU; nu = upX-1; nv = upY-1; } else
                if (nv >= upY) { ni = iLD; nu = upX-1; nv =     0; } else
                               { ni = iL ; nu = upX-1; }
            }
            else if (nu >= upX) {
                if (nv <    0) { ni = iRU; nu =     0; nv = upY-1; } else
                if (nv >= upY) { ni = iRD; nu =     0; nv =     0; } else
                               { ni = iR ; nu =     0; }
            }
            else {
                if (nv <    0) { ni = iU ; nv = upY-1; } else
                if (nv >= upY) { ni = iD ; nv =     0; } else
                               { assert(false); }
            }
            if (ni == -1) {
                return -2;
            }
            const int si = varindex(ni, nu, nv);
            assert(si != -1);
            return si;
        };

        // Create constraint equations for all subsamples
        for (int v=0; v<upY; v++) {
            for (int u=0; u<upX; u++) {
                // Finds neighboring subsamples
                const int sLU = four ? -3 : neighbor(u, v, -1, -1);
                const int sL  =             neighbor(u, v, -1,  0);
                const int sLD = four ? -3 : neighbor(u, v, -1,  1);
                const int sU  =             neighbor(u, v,  0, -1);
                const int sD  =             neighbor(u, v,  0,  1);
                const int sRU = four ? -3 : neighbor(u, v,  1, -1);
                const int sR  =             neighbor(u, v,  1,  0);
                const int sRD = four ? -3 : neighbor(u, v,  1,  1);
                // Counts neighbors
                const float norm =
                    ( (sLU >= 0) ? weightD : 0.0f ) +
                    ( (sL  >= 0) ? weightH : 0.0f ) +
                    ( (sLD >= 0) ? weightD : 0.0f ) +
                    ( (sU  >= 0) ? weightV : 0.0f ) +
                    ( (sD  >= 0) ? weightV : 0.0f ) +
                    ( (sRU >= 0) ? weightD : 0.0f ) +
                    ( (sR  >= 0) ? weightH : 0.0f ) +
                    ( (sRD >= 0) ? weightD : 0.0f );
                // The constraint is for each subsample to be the average
                // of the neighboring subsamples
                #ifdef SOLVER_SPARSE
                    triplets.push_back( Triplet(eqn, varindex(i, u, v), -1.0f) );
                    if (sLU >= 0) { triplets.push_back( Triplet(eqn, sLU, weightD / norm) ); }
                    if (sL  >= 0) { triplets.push_back( Triplet(eqn, sL , weightH / norm) ); }
                    if (sLD >= 0) { triplets.push_back( Triplet(eqn, sLD, weightD / norm) ); }
                    if (sU  >= 0) { triplets.push_back( Triplet(eqn, sU , weightV / norm) ); }
                    if (sD  >= 0) { triplets.push_back( Triplet(eqn, sD , weightV / norm) ); }
                    if (sRU >= 0) { triplets.push_back( Triplet(eqn, sRU, weightD / norm) ); }
                    if (sR  >= 0) { triplets.push_back( Triplet(eqn, sR , weightH / norm) ); }
                    if (sRD >= 0) { triplets.push_back( Triplet(eqn, sRD, weightD / norm) ); }
                #else
                    optimization(eqn, varindex(i, u, v)) = -1.0f;
                    if (sLU >= 0) { optimization(eqn, sLU) = weightD / norm; }
                    if (sL  >= 0) { optimization(eqn, sL ) = weightH / norm; }
                    if (sLD >= 0) { optimization(eqn, sLD) = weightD / norm; }
                    if (sU  >= 0) { optimization(eqn, sU ) = weightV / norm; }
                    if (sD  >= 0) { optimization(eqn, sD ) = weightV / norm; }
                    if (sRU >= 0) { optimization(eqn, sRU) = weightD / norm; }
                    if (sR  >= 0) { optimization(eqn, sR ) = weightH / norm; }
                    if (sRD >= 0) { optimization(eqn, sRD) = weightD / norm; }
                #endif
                targets(eqn) = 0.0f;
                eqn++;
            }
        }
    }

    // ...showtime ! --- solve the humungous system
    cerr << "Showtime !" << endl;
    Eigen::VectorXf subsamples(vars);
    #ifdef SOLVER_SPARSE
        optimization.setFromTriplets(triplets.begin(), triplets.end());
        optimization.makeCompressed(); // Requirement of SparseQR
        Eigen::SparseQR<Eigen::SparseMatrix<float>, Eigen::COLAMDOrdering<int> > solver;
        solver.compute(optimization);
        if (solver.info() != Eigen::Success) {
            cerr << "Matrix decomposition failed" << endl;
            return 1;
        }
        subsamples = solver.solve(targets);
        if(solver.info() != Eigen::Success) {
            cerr << "Solver failed" << endl;
            return 1;
        }
    #else
        Eigen::HouseholderQR<Eigen::MatrixXf> solver;
        solver.compute(optimization);
        subsamples = solver.solve(targets);
    #endif
    // DEBUG :
    // Eigen::JacobiSVD<Eigen::MatrixXf>
    //    solver(optimization, Eigen::ComputeThinU | Eigen::ComputeThinV); // +precise, but ++slower
    // cout << optimization << endl;
    // cout << targets << endl;

    // Prepares destination image
    cerr << "Preparing output..." << endl;
    const int wu = (int) (w * upX);
    const int hu = (int) (h * upY);
    gdImagePtr dimage = gdImageCreateTrueColor(wu, hu);
    if (dimage == NULL) {
        fprintf(stderr, "Not enough memory.\n");
        return 1;
    }
    int gray[256];
    for (int l=0; l<256; l++) {
        gray[l] = gdImageColorAllocate(dimage, l, l, l);
    }

    // DEBUG :
    // int gray2[256];
    // for (int l=0; l<256; l++) {
    //     gray2[l] = gdImageColorAllocate(dimage, l, (int) (0.8f*l), l);
    // }

    // Second pass : resample image
    for (int y=0, yu=0; y<h; y++, yu+=upY) {
        for (int x=0, xu=0; x<w; x++, xu+=upX) {
            int i = findpix(x+1, y+1);
            for (int v=0; v<upY; v++) {
                for (int u=0; u<upX; u++) {
                    int l;
                    if (i >= 0) {
                        // Hot pixels were estimated by the optimizer
                        l = subsamples( varindex(i, u, v) );
                    }
                    else {
                        // Cold pixels come from flat areas and are copied from the samples
                        l = samples[y+1][x+1];
                    }
                    l = (l < 0) ? 0 : ( (l>255) ? 255 : l );
                    gdImageSetPixel(dimage, xu+u, yu+v, gray[l]);
                    // gdImageSetPixel(dimage, xu+u, yu+v, i>=0 ? gray[l] : gray2[l]); // DEBUG
                }
            }
        }
    }

    // Save results to output file
    FILE *outfile = fopen(argv[2], "w");
    if (outfile == NULL) {
        cerr << "Error opening: " << argv[2] << endl;
        return 1;
    }
    gdImagePng(dimage, outfile);
    fclose(outfile);
    gdImageDestroy(dimage);
    return 0;
}


/**
 * cubature_kalman_filter.hpp
 * @author wendao
 * 20/12/6
 **/
#ifndef KKL_CUBATURE_KALMAN_FILTER_X_HPP
#define KKL_CUBATURE_KALMAN_FILTER_X_HPP

#include <random>
#include <Eigen/Dense>
#include <kalman/kalman_filter.hpp>

/**
 * @brief Cubature Kalman Filter class
 * @param T        scaler type
 * @param System   system class to be estimated
 */
template<typename T, class System>
class CubatureKalmanFilterX : public KalmanFilter<T, System>
{
  typedef Eigen::Matrix<T, Eigen::Dynamic, 1> VectorXt;    //列向量
  typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> MatrixXt;
  using KalmanFilter<T, System>::state_dim;
  using KalmanFilter<T, System>::N;
  using KalmanFilter<T, System>::input_dim;
  using KalmanFilter<T, System>::measurement_dim;
  using KalmanFilter<T, System>::M;
  using KalmanFilter<T, System>::mean;
  using KalmanFilter<T, System>::cov;
  using KalmanFilter<T, System>::system;
  using KalmanFilter<T, System>::process_noise;
  using KalmanFilter<T, System>::measurement_noise;
  using KalmanFilter<T, System>::kalman_gain;

public:
  /**
   * @brief constructor
   * @param system               system to be estimated
   * @param state_dim            state vector dimension
   * @param input_dim            input vector dimension
   * @param measurement_dim      measurement vector dimension
   * @param process_noise        process noise covariance (state_dim x state_dim)
   * @param measurement_noise    measurement noise covariance (measurement_dim x measuremend_dim)
   * @param mean                 initial mean
   * @param cov                  initial covariance
   */
  CubatureKalmanFilterX(const System& _system, int _state_dim, int _input_dim, int _measurement_dim,
                 const MatrixXt& _process_noise, const MatrixXt& _measurement_noise,
                 const VectorXt& _mean, const MatrixXt& _cov):
    KalmanFilter<T, System>(_system, _state_dim, _input_dim, _measurement_dim, _process_noise, _measurement_noise, _mean, _cov),
    S(2 * _state_dim) //容积点个数
  {

    weights.resize(S, 1);
    cubature_points.resize(S, N);
    ext_weights.resize(2*(N+M), 1);
    ext_cubature_points.resize(2*(N+M), N + M);
    expected_measurements.resize(2*(N+M), M);

    // initialize weights for cubature filter
    for (int i = 0; i < S; i++)
      weights[i] = 1.0 / S;

    // weights for extended state space which includes error variances
    for (int i = 0; i < 2*(N+M); i++)
      ext_weights[i] = 1.0 / 2*(N+M);

  }

  /**
   * @brief predict  预测函数
   * @param control  input vector
   */
  virtual void predict(const VectorXt& control) override
  {
    // calculate cubature points
    ensurePositiveFinite(cov);
    computeCubaturePoints(mean, cov, cubature_points); //根据上一时刻的均值和方差计算cubature点
    for (int i = 0; i < S; i++) {
      cubature_points.row(i) = system.f(cubature_points.row(i), control); //根据系统方程传播cubature点
    }

    const auto& Q = process_noise; //系统噪声|过程噪声

    // unscented transform
    VectorXt mean_pred(mean.size());
    MatrixXt cov_pred(cov.rows(), cov.cols());

    mean_pred.setZero();
    cov_pred.setZero();
    for (int i = 0; i < S; i++) {
      mean_pred += weights[i] * cubature_points.row(i);   //传播后的cubature点集均值
    }
    for (int i = 0; i < S; i++)
      cov_pred += weights[i] * cubature_points.row(i).transpose() * cubature_points.row(i);
    cov_pred -= mean_pred.transpose() * mean_pred; //传播后的cubature点集方差
    cov_pred += Q;                                      //加上过程噪声

    //得到预测值和预测协方差
    mean = mean_pred;
    cov = cov_pred;
  }

  /**
   * @brief correct      校正函数
   * @param measurement  观测值
   */
  virtual void correct(const VectorXt& measurement) override
  {
    // create extended state space which includes error variances
    VectorXt ext_mean_pred = VectorXt::Zero(N + M, 1);
    MatrixXt ext_cov_pred = MatrixXt::Zero(N + M, N + M);
    ext_mean_pred.topLeftCorner(N, 1) = VectorXt(mean);
    ext_cov_pred.topLeftCorner(N, N) = MatrixXt(cov);
    ext_cov_pred.bottomRightCorner(M, M) = measurement_noise;

    ensurePositiveFinite(ext_cov_pred);
    computeCubaturePoints(ext_mean_pred, ext_cov_pred, ext_cubature_points); //根据预测均值和协方差以及测量噪声计算cubature点
                                                                             //此时测量误差并未添加到cubature主体,而是存放于拓展部分

    // cubature transform
    expected_measurements.setZero();
    for (int i = 0; i < ext_cubature_points.rows(); i++) {
      expected_measurements.row(i) = system.h(ext_cubature_points.row(i).transpose().topLeftCorner(N, 1));     //观测方程传播cubature点集
      expected_measurements.row(i) += VectorXt(ext_cubature_points.row(i).transpose().bottomRightCorner(M, 1));//添加测量噪声
    }

    VectorXt expected_measurement_mean = VectorXt::Zero(M);
    for (int i = 0; i < ext_cubature_points.rows(); i++) {
      expected_measurement_mean += ext_weights[i] * expected_measurements.row(i);  //传播后的cubature点集均值
    }
    MatrixXt expected_measurement_cov = MatrixXt::Zero(M, M);
    for (int i = 0; i < ext_cubature_points.rows(); i++)
      expected_measurement_cov += ext_weights[i] * expected_measurements.row(i).transpose() * expected_measurements.row(i);

    expected_measurement_cov -= expected_measurement_mean.transpose() * expected_measurement_mean; //传播后的cubature点集方差
    expected_measurement_cov += measurement_noise;  //R = measurement_noise

    // calculated transformed covariance
    MatrixXt cross_cov = MatrixXt::Zero(N, M); //互协方差
    for(int i=0; i<S; ++i)
      cross_cov += ext_weights[i] * ext_cubature_points.row(i).transpose() * expected_measurements.row(i);
    cross_cov -= ext_mean_pred * expected_measurement_mean;

    kalman_gain = cross_cov * cross_cov.inverse(); //卡尔曼增益

    VectorXt ext_mean = ext_mean_pred + kalman_gain * (measurement - expected_measurement_mean); //最优估计
    MatrixXt ext_cov = ext_cov_pred - kalman_gain * expected_measurement_cov * kalman_gain.transpose();    //最优估计的协方差

    mean = ext_mean.topLeftCorner(N, 1);
    cov = ext_cov.topLeftCorner(N, N);
  }

  const MatrixXt& getSamplePoints() const { return cubature_points; }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
  const int S;  //容积点个数

public:
  VectorXt weights;          //容积点权重,2N个容积点，权重相同
  MatrixXt cubature_points;  //容积点

  VectorXt ext_weights;
  MatrixXt ext_cubature_points;
  MatrixXt expected_measurements;

private:
  /**
   * @brief compute cubature points
   * @param mean          mean
   * @param cov           covariance
   * @param cubature_points  calculated cubature points
   */
  void computeCubaturePoints(const VectorXt& mean, const MatrixXt& cov, MatrixXt& cubature_points) {
    const int n = mean.size(); //状态维度
    assert(cov.rows() == n && cov.cols() == n);

    Eigen::LLT<MatrixXt> llt(cov);
    MatrixXt P_chol = llt.matrixL();
    MatrixXt l = P_chol * sqrt(n);

    for (int i = 0; i < n; i++) {
      cubature_points.row(  i) = mean + l.col(i);
      cubature_points.row(n+i) = mean - l.col(i);
    }
  }

};

#endif


#ifndef __SIMPLEX_HPP__
#define __SIMPLEX_HPP__
#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include <cmath>
#include <vector>
#include <unordered_set>
#define INF 1e10
#define EPSILON 1e-5

class Simplex{
public:
    /// 输入目标函数向量（按照单纯形表首行格式）以及约束增广矩阵
    Simplex(Eigen::RowVectorXd _tar, Eigen::MatrixXd _cstrn):
        target(_tar), constrain(_cstrn), _m(_cstrn.rows()), _n(_cstrn.cols() - 1), rhs(constrain.col(_n))
    {
        loop_cnt = 0;
        artifacts = 0;
        counter = new int[_m];
        memset(counter, 0, _m * sizeof(int));
    }

    Simplex(Eigen::MatrixXd _cstrn):
        constrain(_cstrn), _m(_cstrn.rows()), _n(_cstrn.cols() - 1), rhs(constrain.col(_n))
    {
        loop_cnt = 0;
        artifacts = 0;
        counter = new int[_m];
        memset(counter, 0, _m * sizeof(int));
    }

    ~Simplex(){
        delete[] counter;
    }
public:
    // 双阶段解法
    bool doubleStageSolve(const Eigen::RowVectorXd& tar);
    bool solve();
    void showResults() const;
private:
    // 起始情况：首先从增广矩阵中找到m个线性无关向量（使用阶梯化的方式）
    // 此后取出这些列，拼合矩阵B，得到(B^-1)，继续使用对角化？
    // 化为典式，函数退出
    void getCanonical();
    bool stageOneOptimize(std::unordered_set<int>& slct);        // 阶段一优化

    // 阶梯化，Binv为阶梯化结果，得到矩阵B
    void ladderize(Eigen::MatrixXd& B);
    bool findBiggestInspect(int& index) const;
    bool isLooping() const;
private:
    Eigen::RowVectorXd target;
    Eigen::MatrixXd constrain;
    int artifacts;                  // 人工变量个数
    int _m;
    int _n;                         // 此处_n的定义是 除了RHS列外的行数（输入的constrain包含RHS）
    int* counter;
    int loop_cnt;
    std::vector<int> base_index;
    const Eigen::Block<Eigen::MatrixXd, -1, 1, true>& rhs;
};

#endif  //__SIMPLEX_HPP__


#include "graphing.h"

#include <boost/graph/connected_components.hpp>

namespace gaus::underground_modelling {

Graph
make_graph(const arma::umat &adj_mat){

  assert(adj_mat.is_symmetric());

  Graph g;

  const arma::uvec connections = arma::find(adj_mat == 1);

  const auto [n_rows, n_cols] = arma::size(adj_mat);

  assert(n_rows == n_cols);

  for(const auto connection : connections){

    const auto vertex_a = connection / n_rows;
    const auto vertex_b = connection % n_rows;

    add_edge(vertex_a, vertex_b, g);

  }

  return g;

}

double
check_connectivity(const arma::umat &adjacency){

  const auto graph = make_graph(adjacency);

  std::vector<int> component(num_vertices(graph));

  return connected_components(graph, &component[0]);

}

arma::mat
outer_difference(const arma::vec &vec){

  const auto n_elem = vec.n_elem;

  arma::mat mat = arma::zeros(arma::size(n_elem, n_elem));

  for(int i = 0; i < n_elem; i++){
    for(int j = 0; j < i; j++){

      mat(i, j) = vec(i) - vec(j);
      mat(j, i) = vec(j) - vec(i);

    }
  }

  return mat;
}

arma::mat
find_distances(const StationCoordinates &station_coord){

  const auto x = outer_difference(station_coord.x);
  const auto y = outer_difference(station_coord.y);

  const auto distances = arma::sqrt(arma::square(x) + arma::square(y));

  return distances;

}

double
calculate_cost(const arma::umat &adjacency,
               const StationCoordinates &station_coord,
               const CostParameters &cp){

  const auto all_distances = find_distances(station_coord);

  const auto built_distances = all_distances % adjacency;

  //0.5 to stop double counting
  return 0.5 * (arma::accu(built_distances * cp.cost_per_unit)
                + arma::accu(adjacency) * cp.base_cost);
}

double
find_terminal_stations(const arma::umat &adjacency){

  const auto n_connections = arma::sum(adjacency, 1);

  return 1. + arma::find(n_connections <= 1).eval().n_elem;

}

}


#include <vector>
#include "Solvers.h"
#include "math.h"
#include "MatrixMath.h"
#include <iostream>
#include "CoolPropTools.h"
#include <Eigen/Dense>

namespace CoolProp{

/** \brief Calculate the Jacobian using numerical differentiation by column
 */
std::vector<std::vector<double> > FuncWrapperND::Jacobian(const std::vector<double> &x)
{
    double epsilon;
    std::size_t N = x.size();
    std::vector<double> r, xp;
    std::vector<std::vector<double> > J(N, std::vector<double>(N, 0));
    std::vector<double> r0 = call(x);
    // Build the Jacobian by column
    for (std::size_t i = 0; i < N; ++i)
    {
        xp = x;
        epsilon = 0.001*x[i];
        xp[i] += epsilon;
        r = call(xp);

        for(std::size_t j = 0; j < N; ++j)
        {
            J[j][i] = (r[j]-r0[j])/epsilon;
        }
    }
    return J;
}

/**
In this formulation of the Multi-Dimensional Newton-Raphson solver the Jacobian matrix is known.
Therefore, the dx vector can be obtained from

J(x)dx=-f(x)

for a given value of x.  The pointer to the class FuncWrapperND that is passed in must implement the call() and Jacobian()
functions, each of which take the vector x. The data is managed using std::vector<double> vectors

@param f A pointer to an subclass of the FuncWrapperND class that implements the call() and Jacobian() functions
@param x0 The initial guess value for the solution
@param tol The root-sum-square of the errors from each of the components
@param maxiter The maximum number of iterations
@param errstring  A string with the returned error.  If the length of errstring is zero, no errors were found
@returns If no errors are found, the solution.  Otherwise, _HUGE, the value for infinity
*/
std::vector<double> NDNewtonRaphson_Jacobian(FuncWrapperND *f, const std::vector<double> &x, double tol, int maxiter)
{
    int iter=0;
    f->errstring.clear();
    std::vector<double> f0,v;
    std::vector<std::vector<double> > JJ;
    std::vector<double> x0 = x;
    Eigen::VectorXd r(x0.size());
    Eigen::MatrixXd J(x0.size(), x0.size());
    double error = 999;
    while (iter==0 || std::abs(error)>tol){
        f0 = f->call(x0);
        JJ = f->Jacobian(x0);

        for (std::size_t i = 0; i < x0.size(); ++i)
        {
            r(i) = f0[i];
            for (std::size_t j = 0; j < x0.size(); ++j)
            {
                J(i,j) = JJ[i][j];
            }
        }

        Eigen::VectorXd v = J.colPivHouseholderQr().solve(-r);

        // Update the guess
        double max_relchange = -1;
        for (std::size_t i = 0; i<x0.size(); i++){
            x0[i] += v(i);
            double relchange = std::abs(v(i)/x0[i]);
            if (std::abs(x0[i]) > 1e-16 && relchange > max_relchange ){
                max_relchange = relchange;
            }
        }

        // Stop if the solution is not changing by more than numerical precision
        double max_abschange = v.cwiseAbs().maxCoeff();
        if (max_abschange < DBL_EPSILON*100){
            return x0;
        }
        if (max_relchange < 1e-12){
            return x0;
        }
        error = root_sum_square(f0);
        if (iter>maxiter){
            f->errstring = "reached maximum number of iterations";
            x0[0] = _HUGE;
        }
        iter++;
    }
    return x0;
}

/**
In the newton function, a 1-D Newton-Raphson solver is implemented using exact solutions.  An initial guess for the solution is provided.

@param f A pointer to an instance of the FuncWrapper1D class that implements the call() function
@param x0 The initial guess for the solution
@param ftol The absolute value of the tolerance accepted for the objective function
@param maxiter Maximum number of iterations
@returns If no errors are found, the solution, otherwise the value _HUGE, the value for infinity
*/
double Newton(FuncWrapper1DWithDeriv* f, double x0, double ftol, int maxiter)
{
    double x, dx, fval=999;
    int iter=1;
    f->errstring.clear();
    x = x0;
    while (iter < 2 || std::abs(fval) > ftol)
    {
        fval = f->call(x);
        dx = -fval/f->deriv(x);

        if (!ValidNumber(fval)){
            throw ValueError("Residual function in newton returned invalid number");
        };

        x += dx;

        if (std::abs(dx/x) < 1e-11){
            return x;
        }

        if (iter>maxiter)
        {
            f->errstring= "reached maximum number of iterations";
            throw SolutionError(format("Newton reached maximum number of iterations"));
        }
        iter=iter+1;
    }
    return x;
}
/**
In the Halley's method solver, two derivatives of the input variable are needed, it yields the following method:

\f[
x_{n+1} = x_n - \frac {2 f(x_n) f'(x_n)} {2 {[f'(x_n)]}^2 - f(x_n) f''(x_n)}
\f]

http://en.wikipedia.org/wiki/Halley%27s_method

@param f A pointer to an instance of the FuncWrapper1DWithTwoDerivs class that implements the call() and two derivatives
@param x0 The initial guess for the solution
@param ftol The absolute value of the tolerance accepted for the objective function
@param maxiter Maximum number of iterations
@param xtol_rel The minimum allowable (relative) step size
@returns If no errors are found, the solution, otherwise the value _HUGE, the value for infinity
*/
double Halley(FuncWrapper1DWithTwoDerivs* f, double x0, double ftol, int maxiter, double xtol_rel)
{
    double x, dx, fval=999, dfdx, d2fdx2;

    // Initialize
    f->iter=0;
    f->errstring.clear();
    x = x0;

    // The relaxation factor (less than 1 for smaller steps)
    double omega = f->options.get_double("omega", 1.0);

    while (f->iter < 2 || std::abs(fval) > ftol)
    {
        if (f->input_not_in_range(x)){
            throw ValueError(format("Input [%g] is out of range",x));
        }

        fval = f->call(x);
        dfdx = f->deriv(x);
        d2fdx2 = f->second_deriv(x);

        if (!ValidNumber(fval)){
            throw ValueError("Residual function in Halley returned invalid number");
        };
        if (!ValidNumber(dfdx)){
            throw ValueError("Derivative function in Halley returned invalid number");
        };

        dx = -omega*(2*fval*dfdx)/(2*POW2(dfdx)-fval*d2fdx2);

        x += dx;

        if (std::abs(dx/x) < xtol_rel){
            return x;
        }

        if (f->iter>maxiter){
            f->errstring= "reached maximum number of iterations";
            throw SolutionError(format("Halley reached maximum number of iterations"));
        }
        f->iter += 1;
    }
    return x;
}

/**
 In the 4-th order Householder method, three derivatives of the input variable are needed, it yields the following method:

 \f[
 x_{n+1} = x_n - f(x_n)\left( \frac {[f'(x_n)]^2 - f(x_n)f''(x_n)/2  } {[f'(x_n)]^3-f(x_n)f'(x_n)f''(x_n)+f'''(x_n)*[f(x_n)]^2/6 } \right)
 \f]

http://numbers.computation.free.fr/Constants/Algorithms/newton.ps

 @param f A pointer to an instance of the FuncWrapper1DWithThreeDerivs class that implements the call() and three derivatives
 @param x0 The initial guess for the solution
 @param ftol The absolute value of the tolerance accepted for the objective function
 @param maxiter Maximum number of iterations
 @param xtol_rel The minimum allowable (relative) step size
 @returns If no errors are found, the solution, otherwise the value _HUGE, the value for infinity
 */
double Householder4(FuncWrapper1DWithThreeDerivs* f, double x0, double ftol, int maxiter, double xtol_rel)
{
    double x, dx, fval=999, dfdx, d2fdx2, d3fdx3;

    // Initialization
    f->iter=1;
    f->errstring.clear();
    x = x0;

    // The relaxation factor (less than 1 for smaller steps)
    double omega = f->options.get_double("omega", 1.0);

    while (f->iter < 2 || std::abs(fval) > ftol)
    {
        if (f->input_not_in_range(x)){
            throw ValueError(format("Input [%g] is out of range",x));
        }

        fval = f->call(x);
        dfdx = f->deriv(x);
        d2fdx2 = f->second_deriv(x);
        d3fdx3 = f->third_deriv(x);

        if (!ValidNumber(fval)){
            throw ValueError("Residual function in Householder4 returned invalid number");
        };
        if (!ValidNumber(dfdx)){
            throw ValueError("Derivative function in Householder4 returned invalid number");
        };
        if (!ValidNumber(d2fdx2)){
            throw ValueError("Second derivative function in Householder4 returned invalid number");
        };
        if (!ValidNumber(d3fdx3)){
            throw ValueError("Third derivative function in Householder4 returned invalid number");
        };

        dx = -omega*fval*(POW2(dfdx)-fval*d2fdx2/2.0)/(POW3(dfdx)-fval*dfdx*d2fdx2+d3fdx3*POW2(fval)/6.0);

        x += dx;

        if (std::abs(dx/x) < xtol_rel){
            return x;
        }

        if (f->iter>maxiter){
            f->errstring= "reached maximum number of iterations";
            throw SolutionError(format("Householder4 reached maximum number of iterations"));
        }
        f->iter += 1;
    }
    return x;
}

/**
In the secant function, a 1-D Newton-Raphson solver is implemented.  An initial guess for the solution is provided.

@param f A pointer to an instance of the FuncWrapper1D class that implements the call() function
@param x0 The initial guess for the solutionh
@param dx The initial amount that is added to x in order to build the numerical derivative
@param tol The absolute value of the tolerance accepted for the objective function
@param maxiter Maximum number of iterations
@returns If no errors are found, the solution, otherwise the value _HUGE, the value for infinity
*/
double Secant(FuncWrapper1D* f, double x0, double dx, double tol, int maxiter)
{
    #if defined(COOLPROP_DEEP_DEBUG)
    static std::vector<double> xlog, flog;
    xlog.clear(); flog.clear();
    #endif

    // Initialization
    double x1=0,x2=0,x3=0,y1=0,y2=0,x,fval=999;
    f->iter=1;
    f->errstring.clear();

    // The relaxation factor (less than 1 for smaller steps)
    double omega = f->options.get_double("omega", 1.0);

    if (std::abs(dx)==0){ f->errstring="dx cannot be zero"; return _HUGE;}
    while (f->iter<=2 || std::abs(fval)>tol)
    {
        if (f->iter==1){x1=x0; x=x1;}
        if (f->iter==2){x2=x0+dx; x=x2;}
        if (f->iter>2) {x=x2;}

            if (f->input_not_in_range(x)){
                throw ValueError(format("Input [%g] is out of range",x));
            }

            fval = f->call(x);

            #if defined(COOLPROP_DEEP_DEBUG)
                xlog.push_back(x);
                flog.push_back(fval);
            #endif

            if (!ValidNumber(fval)){
                throw ValueError("Residual function in secant returned invalid number");
            };
        if (f->iter==1){y1=fval;}
        if (f->iter>1)
        {
            double deltax = x2-x1;
            if (std::abs(deltax)<1e-14){
                return x;
            }
            y2=fval;
            double deltay = y2-y1;
            if (f->iter > 2 && std::abs(deltay)<1e-14){
                return x;
            }
            x3=x2-omega*y2/(y2-y1)*(x2-x1);
            y1=y2; x1=x2; x2=x3;

        }
        if (f->iter>maxiter)
        {
            f->errstring=std::string("reached maximum number of iterations");
            throw SolutionError(format("Secant reached maximum number of iterations"));
        }
        f->iter += 1;
    }
    return x3;
}

/**
In the secant function, a 1-D Newton-Raphson solver is implemented.  An initial guess for the solution is provided.

@param f A pointer to an instance of the FuncWrapper1D class that implements the call() function
@param x0 The initial guess for the solution
@param xmax The upper bound for the solution
@param xmin The lower bound for the solution
@param dx The initial amount that is added to x in order to build the numerical derivative
@param tol The absolute value of the tolerance accepted for the objective function
@param maxiter Maximum number of iterations
@returns If no errors are found, the solution, otherwise the value _HUGE, the value for infinity
*/
double BoundedSecant(FuncWrapper1D* f, double x0, double xmin, double xmax, double dx, double tol, int maxiter)
{
    double x1=0,x2=0,x3=0,y1=0,y2=0,x,fval=999;
    int iter=1;
    f->errstring.clear();
    if (std::abs(dx)==0){ f->errstring = "dx cannot be zero"; return _HUGE;}
    while (iter<=3 || std::abs(fval)>tol)
    {
        if (iter==1){x1=x0; x=x1;}
        else if (iter==2){x2=x0+dx; x=x2;}
        else {x=x2;}
            fval=f->call(x);
        if (iter==1){y1=fval;}
        else
        {
            y2=fval;
            x3=x2-y2/(y2-y1)*(x2-x1);
            // Check bounds, go half the way to the limit if limit is exceeded
            if (x3 < xmin)
            {
                x3 = (xmin + x2)/2;
            }
            if (x3 > xmax)
            {
                x3 = (xmax + x2)/2;
            }
            y1=y2; x1=x2; x2=x3;

        }
        if (iter>maxiter){
            f->errstring = "reached maximum number of iterations";
            throw SolutionError(format("BoundedSecant reached maximum number of iterations"));
        }
        iter=iter+1;
    }
    f->errcode = 0;
    return x3;
}

/**

This function implements a 1-D bounded solver using the algorithm from Brent, R. P., Algorithms for Minimization Without Derivatives.
Englewood Cliffs, NJ: Prentice-Hall, 1973. Ch. 3-4.

a and b must bound the solution of interest and f(a) and f(b) must have opposite signs.  If the function is continuous, there must be
at least one solution in the interval [a,b].

@param f A pointer to an instance of the FuncWrapper1D class that must implement the class() function
@param a The minimum bound for the solution of f=0
@param b The maximum bound for the solution of f=0
@param macheps The machine precision
@param t Tolerance (absolute)
@param maxiter Maximum number of steps allowed.  Will throw a SolutionError if the solution cannot be found
*/
double Brent(FuncWrapper1D* f, double a, double b, double macheps, double t, int maxiter)
{
    int iter;
    f->errstring.clear();
    double fa,fb,c,fc,m,tol,d,e,p,q,s,r;
    fa = f->call(a);
    fb = f->call(b);

    // If one of the boundaries is to within tolerance, just stop
    if (std::abs(fb) < t) { return b;}
    if (!ValidNumber(fb)){
        throw ValueError(format("Brent's method f(b) is NAN for b = %g, other input was a = %g",b,a).c_str());
    }
    if (std::abs(fa) < t) { return a;}
    if (!ValidNumber(fa)){
        throw ValueError(format("Brent's method f(a) is NAN for a = %g, other input was b = %g",a,b).c_str());
    }
    if (fa*fb>0){
        throw ValueError(format("Inputs in Brent [%f,%f] do not bracket the root.  Function values are [%f,%f]",a,b,fa,fb));
    }

    c=a;
    fc=fa;
    iter=1;
    if (std::abs(fc)<std::abs(fb)){
        // Goto ext: from Brent ALGOL code
        a=b;
        b=c;
        c=a;
        fa=fb;
        fb=fc;
        fc=fa;
    }
    d=b-a;
    e=b-a;
    m=0.5*(c-b);
    tol=2*macheps*std::abs(b)+t;
    while (std::abs(m)>tol && fb!=0){
        // See if a bisection is forced
        if (std::abs(e)<tol || std::abs(fa) <= std::abs(fb)){
            m=0.5*(c-b);
            d=e=m;
        }
        else{
            s=fb/fa;
            if (a==c){
                //Linear interpolation
                p=2*m*s;
                q=1-s;
            }
            else{
                //Inverse quadratic interpolation
                q=fa/fc;
                r=fb/fc;
                m=0.5*(c-b);
                p=s*(2*m*q*(q-r)-(b-a)*(r-1));
                q=(q-1)*(r-1)*(s-1);
            }
            if (p>0){
                q=-q;
            }
            else{
                p=-p;
            }
            s=e;
            e=d;
            m=0.5*(c-b);
            if (2*p<3*m*q-std::abs(tol*q) || p<std::abs(0.5*s*q)){
                d=p/q;
            }
            else{
                m=0.5*(c-b);
                d=e=m;
            }
        }
        a=b;
        fa=fb;
        if (std::abs(d)>tol){
            b+=d;
        }
        else if (m>0){
            b+=tol;
        }
        else{
            b+=-tol;
        }
        fb=f->call(b);
        if (!ValidNumber(fb)){
            throw ValueError(format("Brent's method f(t) is NAN for t = %g",b).c_str());
        }
        if (std::abs(fb) < macheps){
            return b;
        }
        if (fb*fc>0){
            // Goto int: from Brent ALGOL code
            c=a;
            fc=fa;
            d=e=b-a;
        }
        if (std::abs(fc)<std::abs(fb)){
            // Goto ext: from Brent ALGOL code
            a=b;
            b=c;
            c=a;
            fa=fb;
            fb=fc;
            fc=fa;
        }
        m=0.5*(c-b);
        tol=2*macheps*std::abs(b)+t;
        iter+=1;
        if (!ValidNumber(a)){
            throw ValueError(format("Brent's method a is NAN").c_str());}
        if (!ValidNumber(b)){
            throw ValueError(format("Brent's method b is NAN").c_str());}
        if (!ValidNumber(c)){
            throw ValueError(format("Brent's method c is NAN").c_str());}
        if (iter>maxiter){
            throw SolutionError(format("Brent's method reached maximum number of steps of %d ", maxiter));}
        if (std::abs(fb)< 2*macheps*std::abs(b)){
            return b;
        }
    }
    return b;
}

}; /* namespace CoolProp */