////////////////////////////////////////////////////////////////
// Orkid Media Engine
// Copyright 1996-2020, Michael T. Mayers.
// Distributed under the Boost Software License - Version 1.0 - August 17, 2003
// see http://www.boost.org/LICENSE_1_0.txt
////////////////////////////////////////////////////////////////

#include <ork/lev2/config.h>
#if defined(ENABLE_IGL)

#include <ork/kernel/orklut.hpp>
#include <ork/math/plane.h>
#include <ork/lev2/gfx/meshutil/submesh.h>
#include <ork/lev2/gfx/meshutil/igl.h>
#include <iostream>

#include <Eigen/Core>
#include <igl/gaussian_curvature.h>
#include <igl/principal_curvature.h>
#include <igl/cotmatrix.h>
#include <igl/massmatrix.h>
#include <igl/invert_diag.h>

namespace ork::meshutil {
iglprinciplecurvature_ptr_t IglMesh::computePrincipleCurvature() const {
  auto rval = std::make_shared<IglPrincipleCurvature>();
  // Alternative discrete mean curvature
  Eigen::MatrixXd HN;
  Eigen::SparseMatrix<double> L, M, Minv;
  igl::cotmatrix(_verts, _faces, L);
  igl::massmatrix(_verts, _faces, igl::MASSMATRIX_TYPE_VORONOI, M);
  igl::invert_diag(M, Minv);
  // Laplace-Beltrami of position
  HN = -Minv * (L * _verts);
  // Extract magnitude as mean curvature
  rval->H = HN.rowwise().norm();

  // Compute curvature directions via quadric fitting
  igl::principal_curvature(_verts, _faces, rval->PD1, rval->PD2, rval->PV1, rval->PV2);
  // mean curvature
  rval->H = 0.5 * (rval->PV1 + rval->PV2);
  return rval;
}

Eigen::VectorXd IglMesh::computeGaussianCurvature() const {
  Eigen::VectorXd rval;
  igl::gaussian_curvature(_verts, _faces, rval);
  // Compute mass matrix
  Eigen::SparseMatrix<double> M, Minv;
  igl::massmatrix(_verts, _faces, igl::MASSMATRIX_TYPE_DEFAULT, M);
  igl::invert_diag(M, Minv);
  // Divide by area to get integral average
  rval = (Minv * rval).eval();
  return rval;
}

} // namespace ork::meshutil

#endif


#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include <OpenMesh/Core/IO/MeshIO.hh>
#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
#include <string>
#include <iostream>
#include <vector>
//#include "include/pybind11/pybind11.h"
//#include "include/pybind11/numpy.h"
//#include "include/pybind11/stl.h"
#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>
#include <fstream>

struct TriTraits : public OpenMesh::DefaultTraits
{
  /// Use double precision points
  typedef OpenMesh::Vec3d Point;
  /// Use double precision Normals
  typedef OpenMesh::Vec3d Normal;
  /// Use double precision TexCood2D
  typedef OpenMesh::Vec2d TexCoord2D;
};

/// Simple Name for Mesh
typedef OpenMesh::TriMesh_ArrayKernelT<TriTraits>  TriMesh;

namespace py=pybind11;

// compute matrix exp
Eigen::Matrix3d exp(Eigen::Matrix3d angle_cross_axis){
        Eigen::Matrix3d test=angle_cross_axis+angle_cross_axis.transpose();
        if(test.norm()>1e-6)
        {
            std::cout<<"rotation_log_exp::exp3::input matrix isn't skew-matrix!!!"<<std::endl;
            return Eigen::Matrix3d::Identity();
        }

        Eigen::Vector3d angle_axis;
        angle_axis(0)=angle_cross_axis(2,1);
        angle_axis(1)=angle_cross_axis(0,2);
        angle_axis(2)=angle_cross_axis(1,0);
        double angle=sqrt(angle_axis(0)*angle_axis(0)+angle_axis(1)*angle_axis(1)+angle_axis(2)*angle_axis(2));
        if(angle<1e-6)
            return Eigen::Matrix3d::Identity();
        Eigen::Matrix3d cross_axis=angle_cross_axis/angle;

        test=cross_axis+cross_axis.transpose();
        if(test.norm()>1e-6)
        {
            std::cout<<"rotation_log_exp::exp2::input matrix isn't skew-matrix!!!"<<std::endl;
            return Eigen::Matrix3d::Identity();
        }

        return Eigen::Matrix3d::Identity()+sin(angle)*cross_axis+(1.0-cos(angle))*cross_axis*cross_axis;
}

py::array_t<double> get_mesh(std::string file, py::array_t<double> feature_input){
    TriMesh ref_mesh_;
    if(!OpenMesh::IO::read_mesh(ref_mesh_, file)){
        std::cerr<<"Read Mesh error"<<std::endl;
    }
    int nver=ref_mesh_.n_vertices();
    OpenMesh::EPropHandleT<double> LB_weights;
    ref_mesh_.add_property(LB_weights);

    // compute LB weight
    TriMesh::EdgeIter e_it, e_end(ref_mesh_.edges_end());
    TriMesh::HalfedgeHandle    h0, h1, h2;
    TriMesh::VertexHandle      v0, v1;
    TriMesh::Point             p0, p1, p2, d0, d1;
    TriMesh::Scalar w;
    for (e_it=ref_mesh_.edges_begin(); e_it!=e_end; e_it++)
    {
        w  = 0.0;
        if(ref_mesh_.is_boundary(*e_it))
        {
            h0 = ref_mesh_.halfedge_handle(e_it.handle(),0);
            if(ref_mesh_.is_boundary(h0))
                h0 = ref_mesh_.opposite_halfedge_handle(h0);

            v0 = ref_mesh_.to_vertex_handle(h0);
            v1 = ref_mesh_.from_vertex_handle(h0);
            p0 = ref_mesh_.point(v0);
            p1 = ref_mesh_.point(v1);
            h1 = ref_mesh_.next_halfedge_handle(h0);
            p2 = ref_mesh_.point(ref_mesh_.to_vertex_handle(h1));
            d0 = (p0-p2).normalize();
            d1 = (p1-p2).normalize();
            w += 2.0 / tan(acos(std::min(0.99, std::max(-0.99, (d0|d1)))));
            if(std::isnan(w))
                std::cout<<"Some weight NAN"<<std::endl;
            ref_mesh_.property(LB_weights,e_it) = w;
            continue;
        }
        h0 = ref_mesh_.halfedge_handle(e_it.handle(),0);
        v0 = ref_mesh_.to_vertex_handle(h0);
        p0 = ref_mesh_.point(v0);

        h1 = ref_mesh_.opposite_halfedge_handle(h0);
        v1 = ref_mesh_.to_vertex_handle(h1);
        p1 = ref_mesh_.point(v1);

        h2 = ref_mesh_.next_halfedge_handle(h0);
        p2 = ref_mesh_.point(ref_mesh_.to_vertex_handle(h2));
        d0 = (p0 - p2).normalize();
        d1 = (p1 - p2).normalize();
        w += 1.0/ tan(acos(std::max(-1.0, std::min(1.0, dot(d1,d0) ))));

        h2 = ref_mesh_.next_halfedge_handle(h1);
        p2 = ref_mesh_.point(ref_mesh_.to_vertex_handle(h2));
        d0 = (p0 - p2).normalize();
        d1 = (p1 - p2).normalize();
        w += 1.0 / tan(acos(std::max(-1.0, std::min(1.0, dot(d1,d0)))));

        if(std::isnan(w))
            std::cout<<"Some weight is NAN"<<std::endl;
        ref_mesh_.property(LB_weights,e_it) = w;
    }

    // prepare Sparse A_ matrix
    Eigen::SparseMatrix<double> A_;
    A_.resize(3*nver,3*nver);
    std::vector<Eigen::Triplet<double> > tripletlist;
    TriMesh::VertexIter v_it = ref_mesh_.vertices_begin();
    for(;v_it!=ref_mesh_.vertices_end();v_it++)
    {
        // fix one point
        TriMesh::VertexEdgeIter ve_iter = ref_mesh_.ve_iter(*v_it);
        int center_id = (*v_it).idx();
        double center_val[3]={0.0,0.0,0.0};
        for(;ve_iter.is_valid();ve_iter++)
        {
            double w = ref_mesh_.property(LB_weights,*ve_iter);
            TriMesh::VertexHandle to_v = ref_mesh_.to_vertex_handle(ref_mesh_.halfedge_handle(*ve_iter,0));
            if(to_v.idx() == center_id)
                to_v = ref_mesh_.from_vertex_handle(ref_mesh_.halfedge_handle(*ve_iter,0));
            for(int i =0;i<3;i++)
            {
                center_val[i]+=w;
                tripletlist.push_back(Eigen::Triplet<double>(3*center_id+i,3*to_v.idx()+i,-w));
//                outfile << 3*center_id + i << " " << 3*to_v.idx()+i << " "<< -w << '\n';
            }
        }
        for(int i =0;i<3;i++)
        {
            tripletlist.push_back(Eigen::Triplet<double>(3*center_id+i,3*center_id+i,center_val[i]));
        }
    }
    A_.setFromTriplets(tripletlist.begin(),tripletlist.end());
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > A_solver_;
    A_solver_.compute(A_);
    // prepare T_matrix
    py::buffer_info feature_array = feature_input.request();
    std::vector<Eigen::Matrix3d> T_array;
    for(int i =0; i<ref_mesh_.n_vertices(); i++)
    {
        Eigen::Matrix3d logR;
        logR<<0,((double*)feature_array.ptr)[9*i+6],((double*)feature_array.ptr)[9*i+7],-((double*)feature_array.ptr)[9*i+6],0,((double*)feature_array.ptr)[9*i+8],-((double*)feature_array.ptr)[9*i+7],-((double*)feature_array.ptr)[9*i+8],0;
        Eigen::Matrix3d S;
        S<<((double*)feature_array.ptr)[9*i+0],((double*)feature_array.ptr)[9*i+1],((double*)feature_array.ptr)[9*i+2],((double*)feature_array.ptr)[9*i+1],((double*)feature_array.ptr)[9*i+3],((double*)feature_array.ptr)[9*i+4],((double*)feature_array.ptr)[9*i+2],((double*)feature_array.ptr)[9*i+4],((double*)feature_array.ptr)[9*i+5];
        T_array.push_back(exp(logR)*S);
    }

    // compute b_
    Eigen::VectorXd b_;
    b_.resize(3*nver);
    b_.setZero();
    v_it = ref_mesh_.vertices_begin();
    for(;v_it!=ref_mesh_.vertices_end();v_it++)
    {
        TriMesh::VertexEdgeIter ve_iter = ref_mesh_.ve_iter(*v_it);
        Eigen::Vector3d temp(0.0,0.0,0.0);
        for(;ve_iter.is_valid();ve_iter++)
        {
            TriMesh::VertexHandle v0 = ref_mesh_.to_vertex_handle(ref_mesh_.halfedge_handle(*ve_iter,0));
            if(v0.idx() == (*v_it).idx())
                v0 = ref_mesh_.from_vertex_handle(ref_mesh_.halfedge_handle(*ve_iter,0));
            OpenMesh::Vec3d Pj,Pk;
            Pj = ref_mesh_.point(*v_it);
            Pk = ref_mesh_.point(v0);
            Eigen::Vector3d e1jk(Pj[0]-Pk[0],Pj[1]-Pk[1],Pj[2]-Pk[2]);
            double cjk = ref_mesh_.property(LB_weights,*ve_iter);

            temp+= cjk*( T_array[v0.idx()] + T_array[(*v_it).idx()] )*e1jk;
        }
        temp*=0.5;
        b_.block<3,1>(3*(*v_it).idx(),0) = temp;
    }

    // solve
    Eigen::VectorXd out;
    out.resize(3*nver);
    out = A_solver_.solve(b_);

    // std::vector<double> P_;
        // std::cout<<"dahsdas"<<std::endl;

    // std::memcpy(P_.data(), out.data(), out.size()*sizeof(double));
    // Eigen::VectorXd::Map(&P_[0], out.size()) = out;
    std::vector<double> P_(out.data(), out.data()+out.size());
    auto result = py::array_t<double>(P_.size());
    auto result_buffer = result.request();
    double *result_ptr = (double *)result_buffer.ptr;

    std::memcpy(result_ptr, P_.data(), P_.size()*sizeof(double));

    // std::cout<<result[0]<<std::endl;
    return result;

    // for(int i =0; i< P_.rows(); i++)
    // std::cout<<P_[i]<<std::endl;
    // std::vector<size_t> strides = {sizeof(double)};
    // std::vector<size_t> shape = {feature_array.shape[0]/3};
    // size_t ndim = 1;
    // return py::array(py::buffer_info(P_.data(), sizeof(double), py::format_descriptor<double>::value, ndim, feature_array.shape, strides));
    // return py::array(nver, P_.data());
}
 PYBIND11_MODULE(get_mesh, m){
    m.doc() = "get mesh";
    m.def("get_mesh", &get_mesh, "get_mesh");
 }


#include <iostream>
#include <boost/filesystem.hpp>
#include <boost/preprocessor/repetition/enum.hpp>

#include "shannon.hpp"
#include "output.hpp"
#include "graph.hpp"

#define SHANNON_IDENT(z, n, value) value

template <uint8_t Base>
constexpr double log(double value) {
    return log2(value) / log2(Base);
}

namespace fs = boost::filesystem;

using counter_t = std::array<uint64_t, 256>;
using allowed_t = std::array<bool, 256>;
constexpr auto DEFAULT_BLOCK_SIZE = 16 * 1024;

constexpr allowed_t ALLOWED_DATA{BOOST_PP_ENUM(256, SHANNON_IDENT, true)};
constexpr allowed_t ALLOWED_TEXT{BOOST_PP_ENUM(128, SHANNON_IDENT, true)};
constexpr allowed_t ALLOWED_BASE64{
    BOOST_PP_ENUM(43, SHANNON_IDENT, false),
    true, // '+'
    BOOST_PP_ENUM(3, SHANNON_IDENT, false),
    BOOST_PP_ENUM(11, SHANNON_IDENT, true), // '/' and 0-9
    BOOST_PP_ENUM(7, SHANNON_IDENT, false),
    BOOST_PP_ENUM(26, SHANNON_IDENT, true), // A-Z
    BOOST_PP_ENUM(6, SHANNON_IDENT, false),
    BOOST_PP_ENUM(26, SHANNON_IDENT, true), // a-z
};

double shannon_score(const counter_t& counts, std::size_t total_size,
                     DataFormat format) {
    const allowed_t* allowed = nullptr;
    switch (format) {
    case DataFormat::DATA:
        allowed = &ALLOWED_DATA;
        break;
    case DataFormat::TEXT:
        allowed = &ALLOWED_TEXT;
        break;
    case DataFormat::BASE64:
        allowed = &ALLOWED_BASE64;
        break;
    }

    double score = 0;
    for (std::size_t index = 0; index < counts.size(); ++index) {
        auto count = counts[index];
        if (count == 0 || !(*allowed)[index])
            continue;
        double p_i = count / static_cast<double>(total_size);
        score += p_i * log2(p_i);
    }

    switch (format) {
    case DataFormat::DATA:
        return std::abs(score) / 8.0;
    case DataFormat::TEXT:
        return std::abs(score) / 7.0;
    case DataFormat::BASE64:
        return std::abs(score) / 6.0;
    }
    return 0.0;
}

template <typename Iter>
void shannon_digest(Iter begin, Iter end, counter_t& counts) {
    for (auto iter = begin; iter != end; ++iter) {
        counts[*iter] += 1;
    }
}

class shannon_iterator
    : public boost::iterator_facade<shannon_iterator, double,
                                    boost::forward_traversal_tag, double> {
    std::istream* m_stream = nullptr;
    DataFormat m_format;
    uint64_t m_block_size;
    std::vector<uint8_t> m_block;
    bool m_clear_stats;
    counter_t m_counter;

public:
    shannon_iterator() = default;
    shannon_iterator(std::istream& stream, uint64_t block_size,
                     DataFormat format, bool clear_stats = true)
        : m_stream{&stream},
          m_format{format},
          m_block_size{block_size},
          m_block(block_size),
          m_clear_stats{clear_stats},
          m_counter{} {
        this->increment(); // Get meaningful data in the buffer
    }
    void increment() {
        assert(m_block.capacity() >= m_block_size);

        if (m_stream != nullptr) {
            m_stream->read(reinterpret_cast<char*>(&m_block.front()),
                           m_block_size);
            std::streamsize bytes_read = m_stream->gcount();
            m_block.resize(bytes_read);

            if (m_clear_stats) {
                m_counter.fill(0);
            }
            shannon_digest(m_block.begin(), m_block.end(), m_counter);
        }
    }

    bool equal(shannon_iterator const& other) const {
        return m_stream == other.m_stream ||
               (!other.m_stream && m_stream && !*m_stream);
    }

    double dereference() const {
        assert(m_stream != nullptr);

        if (m_clear_stats) {
            return shannon_score(m_counter, m_block_size, m_format);
        } else {
            // If we have reached eof, seek to end and clear so we can
            // use tellg to get the total bytes read
            if (m_stream->fail()) {
                m_stream->seekg(0, std::ios_base::end);
                m_stream->clear();
            }
            auto bytes_read = m_stream->tellg();
            return shannon_score(m_counter, bytes_read, m_format);
        }
    }

    const std::vector<uint8_t>& block() const { return m_block; }
    std::streampos position() {
        return m_stream->tellg() - std::streamoff(m_block_size);
    }
};

void shannon_file(const std::string& path, uint64_t block_size,
                  const PrintingPolicy& policy, DataFormat format,
                  EntropyGraph& graph) {
    std::ifstream file_stream{path, std::ifstream::binary};

    if (!block_size) {
        shannon_iterator iter{file_stream, DEFAULT_BLOCK_SIZE, format, false};
        shannon_iterator end{};
        for (; iter != end; ++iter) {
        }
        auto score = *iter;

        if (score < policy.bounds.first || score > policy.bounds.second) {
            return;
        }

        print_score(path, score, policy);
    } else {
        shannon_iterator iter{file_stream, block_size, format};
        shannon_iterator end{};
        auto file_size = fs::file_size(path);
        auto addr_width = address_width(policy.addr_format, file_size);

        for (; iter != end; ++iter) {
            auto score = *iter;

            if (score < policy.bounds.first || score > policy.bounds.second) {
                continue;
            }

            if (policy.print_graph) {
                graph.insert(path, iter.position(), score);
                continue;
            }
            print_score(path, iter.position(), addr_width, score, policy);
            if (policy.print_blocks) {
                print_block_bytes(iter.block().begin(), iter.block().end(),
                                  iter.position(), addr_width, policy);
            }
        }
    }
}


#include <iostream>
#include <mtl/strided1D.h>
#include <mtl/light1D.h>
#include <mtl/mtl.h>

int
main()
{
  using namespace mtl;

  double dx[20], dy[20];
  for (int i = 0; i < 20; ++i) {
    if (i % 2 == 0) {
      dx[i] = i;
      dy[i] = 2*i;
    } else {
      dx[i] = 0;
      dy[i] = 0;
    }
  }
  light1D<double> x(dx, 20);
  strided1D< light1D<double> > sx(x, -2);

  light1D<double> y(dy, 20);
  strided1D< light1D<double> > sy(y, -2);

  if (dot(sx, sy) == dot(x, y))
    std::cout << "success" << std::endl;
  else
    std::cout << "failure" << std::endl;

  return 0;
}


/*
    Copyright 2005-2007 Adobe Systems Incorporated
    Distributed under the MIT License (see accompanying file LICENSE_1_0_0.txt
    or a copy at http://opensource.adobe.com/licenses.html)
*/

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

#ifndef GIL_CHANNEL_NUMERIC_OPERATIONS_HPP
#define GIL_CHANNEL_NUMERIC_OPERATIONS_HPP

/*!
/// \file
/// \brief Structures for channel-wise numeric operations
/// \author Hailin Jin and Lubomir Bourdev \n
///         Adobe Systems Incorporated
/// \date   2005-2007 \n Last updated on September 30, 2006
/// Currently defined structures:
///    channel_plus_t (+), channel_minus_t (-),
///    channel_multiplies_t (*), channel_divides_t (/),
///    channel_plus_scalar_t (+s), channel_minus_scalar_t (-s),
///    channel_multiplies_scalar_t (*s), channel_divides_scalar_t (/s),
///    channel_halves_t (/=2), channel_zeros_t (=0), channel_assigns_t (=)
*/

#include <functional>
#include <boost/gil/gil_config.hpp>
#include <boost/gil/channel.hpp>

namespace boost { namespace gil {

/// \ingroup ChannelNumericOperations
/// structure for adding one channel to another
/// this is a generic implementation; user should specialize it for better performance
template <typename Channel1,typename Channel2,typename ChannelR>
struct channel_plus_t : public std::binary_function<Channel1,Channel2,ChannelR> {
    ChannelR operator()(typename channel_traits<Channel1>::const_reference ch1,
                        typename channel_traits<Channel2>::const_reference ch2) const {
        return ChannelR(ch1)+ChannelR(ch2);
    }
};

/// \ingroup ChannelNumericOperations
/// structure for subtracting one channel from another
/// this is a generic implementation; user should specialize it for better performance
template <typename Channel1,typename Channel2,typename ChannelR>
struct channel_minus_t : public std::binary_function<Channel1,Channel2,ChannelR> {
    ChannelR operator()(typename channel_traits<Channel1>::const_reference ch1,
                        typename channel_traits<Channel2>::const_reference ch2) const {
        return ChannelR(ch1)-ChannelR(ch2);
    }
};

/// \ingroup ChannelNumericOperations
/// structure for multiplying one channel to another
/// this is a generic implementation; user should specialize it for better performance
template <typename Channel1,typename Channel2,typename ChannelR>
struct channel_multiplies_t : public std::binary_function<Channel1,Channel2,ChannelR> {
    ChannelR operator()(typename channel_traits<Channel1>::const_reference ch1,
                        typename channel_traits<Channel2>::const_reference ch2) const {
        return ChannelR(ch1)*ChannelR(ch2);
    }
};

/// \ingroup ChannelNumericOperations
/// structure for dividing channels
/// this is a generic implementation; user should specialize it for better performance
template <typename Channel1,typename Channel2,typename ChannelR>
struct channel_divides_t : public std::binary_function<Channel1,Channel2,ChannelR> {
    ChannelR operator()(typename channel_traits<Channel1>::const_reference ch1,
                        typename channel_traits<Channel2>::const_reference ch2) const {
        return ChannelR(ch1)/ChannelR(ch2);
    }
};

/// \ingroup ChannelNumericOperations
/// structure for adding a scalar to a channel
/// this is a generic implementation; user should specialize it for better performance
template <typename Channel,typename Scalar,typename ChannelR>
struct channel_plus_scalar_t : public std::binary_function<Channel,Scalar,ChannelR> {
    ChannelR operator()(typename channel_traits<Channel>::const_reference ch,
                        const Scalar& s) const {
        return ChannelR(ch)+ChannelR(s);
    }
};

/// \ingroup ChannelNumericOperations
/// structure for subtracting a scalar from a channel
/// this is a generic implementation; user should specialize it for better performance
template <typename Channel,typename Scalar,typename ChannelR>
struct channel_minus_scalar_t : public std::binary_function<Channel,Scalar,ChannelR> {
    ChannelR operator()(typename channel_traits<Channel>::const_reference ch,
                        const Scalar& s) const {
        return ChannelR(ch-s);
    }
};

/// \ingroup ChannelNumericOperations
/// structure for multiplying a scalar to one channel
/// this is a generic implementation; user should specialize it for better performance
template <typename Channel,typename Scalar,typename ChannelR>
struct channel_multiplies_scalar_t : public std::binary_function<Channel,Scalar,ChannelR> {
    ChannelR operator()(typename channel_traits<Channel>::const_reference ch,
                        const Scalar& s) const {
        return ChannelR(ch)*ChannelR(s);
    }
};

/// \ingroup ChannelNumericOperations
/// structure for dividing a channel by a scalar
/// this is a generic implementation; user should specialize it for better performance
template <typename Channel,typename Scalar,typename ChannelR>
struct channel_divides_scalar_t : public std::binary_function<Channel,Scalar,ChannelR> {
    ChannelR operator()(typename channel_traits<Channel>::const_reference ch,
                        const Scalar& s) const {
        return ChannelR(ch)/ChannelR(s);
    }
};

/// \ingroup ChannelNumericOperations
/// structure for halving a channel
/// this is a generic implementation; user should specialize it for better performance
template <typename Channel>
struct channel_halves_t : public std::unary_function<Channel,Channel> {
    typename channel_traits<Channel>::reference
    operator()(typename channel_traits<Channel>::reference ch) const {
        return ch/=2.0;
    }
};

/// \ingroup ChannelNumericOperations
/// structure for setting a channel to zero
/// this is a generic implementation; user should specialize it for better performance
template <typename Channel>
struct channel_zeros_t : public std::unary_function<Channel,Channel> {
    typename channel_traits<Channel>::reference
    operator()(typename channel_traits<Channel>::reference ch) const {
        return ch=Channel(0);
    }
};

/// \ingroup ChannelNumericOperations
/// structure for assigning one channel to another
/// this is a generic implementation; user should specialize it for better performance
template <typename Channel1,typename Channel2>
struct channel_assigns_t : public std::binary_function<Channel1,Channel2,Channel2> {
    typename channel_traits<Channel2>::reference
    operator()(typename channel_traits<Channel1>::const_reference ch1,
               typename channel_traits<Channel2>::reference ch2) const {
        return ch2=Channel2(ch1);
    }
};

} }  // namespace boost::gil

#endif


/*
 * gpcxx/app/benchmark_problems/keijzer.hpp
 * Date: 2015-06-24
 * Author: Karsten Ahnert (karsten.ahnert@gmx.de)
 * Copyright: Karsten Ahnert
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef GPCXX_APP_BENCHMARK_PROBLEMS_KEIJZER_HPP_INCLUDED
#define GPCXX_APP_BENCHMARK_PROBLEMS_KEIJZER_HPP_INCLUDED

#include <gpcxx/app/generate_evenly_spaced_test_data.hpp>
#include <gpcxx/app/generate_uniform_distributed_test_data.hpp>
#include <gpcxx/util/assert.hpp>

#include <boost/math/constants/constants.hpp>
#include <cmath>

namespace gpcxx {

double keijzer_func4( double x ) { return 0.3 * x * sin( 2.0 * boost::math::double_constants::pi * x ); }

double keijzer_func5( double x ) {
    double sx = sin( x );
    double cx = cos( x );
    return x * x * x * exp( - x ) * cx * sx * ( sx * sx * cx - 1.0 );
}

double keijzer_func6( double x , double y , double z ) { return 30.0 * x * z / ( x - 10.0 ) / y / y; }

double keijzer_func7( double x ) {
    GPCXX_ASSERT( x >= 1.0 );
    double n = static_cast< double >( static_cast< int >( x ) );
    return n * ( n - 1 ) / 2.0;
}

double keijzer_func8( double x ) { return std::log( x ); }

double keijzer_func9( double x ) { return std::sqrt( x ); }

double keijzer_func10( double x ) { return std::asinh( x ); }

double keijzer_func11( double x , double y ) { return std::pow( x , y ); }

double keijzer_func12( double x , double y ) { return x * y + sin( ( x -1.0 ) * ( y-1.0 ) ); }

double keijzer_func13( double x , double y ) { return x * x * x * x - x * x * x + 0.5 * y * y - y; }

double keijzer_func14( double x , double y ) { return 6.0 * sin( x ) * cos( x ); }

double keijzer_func15( double x , double y ) { return 8.0 / ( 2.0 + x * x + y * y ); }

double keijzer_func16( double x , double y ) { return x * x * x / 3.0 + y * y * y / 2.0 - y - x; }

auto generate_keijzer1( void ) {
    return gpcxx::generate_evenly_spaced_test_data< 1 >( -1.0 , 1.0 , 0.1 , keijzer_func4 );
}

auto generate_keijzer2( void ) {
    return gpcxx::generate_evenly_spaced_test_data< 1 >( -2.0 , 2.0 , 0.1 , keijzer_func4 );
}

auto generate_keijzer3( void ) {
    return gpcxx::generate_evenly_spaced_test_data< 1 >( -3.0 , 3.0 , 0.1 , keijzer_func4 );
}

auto generate_keijzer4( void ) {
    return gpcxx::generate_evenly_spaced_test_data< 1 >( 0.0 , 10.0 , 0.05  , keijzer_func5 );
}

template< typename Rng >
auto generate_keijzer5( Rng& rng ) {
    return gpcxx::generate_uniform_distributed_test_data< 3 >( rng , 1000 ,
        std::array< std::pair< double , double > , 3 >{{ std::make_pair( -1.0 , 1.0 ) , std::make_pair( 1.0 , 2.0 ) , std::make_pair( -1.0 , 1.0 )  }} ,
        keijzer_func6 );
}

auto generate_keijzer6( void ) {
    return gpcxx::generate_evenly_spaced_test_data< 1 >( 1.0 , 50.0 , 1.0 , keijzer_func7 );
}

auto generate_keijzer7( void ) {
    return gpcxx::generate_evenly_spaced_test_data< 1 >( 1.0 , 100.0 , 1.0 , keijzer_func8 );
}

auto generate_keijzer8( void ) {
    return gpcxx::generate_evenly_spaced_test_data< 1 >( 0.0 , 100.0 , 1.0 , keijzer_func9 );
}

auto generate_keijzer9( void ) {
    return gpcxx::generate_evenly_spaced_test_data< 1 >( 0.0 , 100.0 , 1.0 , keijzer_func10 );
}

template< typename Rng >
auto generate_keijzer10( Rng& rng ) {
    return gpcxx::generate_uniform_distributed_test_data< 2 >( rng , 100 , 0.0 , 1.0 , keijzer_func11 );
}

template< typename Rng >
auto generate_keijzer11( Rng& rng ) {
    return gpcxx::generate_uniform_distributed_test_data< 2 >( rng , 20 , -3.0 , 3.0 , keijzer_func12 );
}

template< typename Rng >
auto generate_keijzer12( Rng& rng ) {
    return gpcxx::generate_uniform_distributed_test_data< 2 >( rng , 20 , -3.0 , 3.0 , keijzer_func13 );
}

template< typename Rng >
auto generate_keijzer13( Rng& rng ) {
    return gpcxx::generate_uniform_distributed_test_data< 2 >( rng , 20 , -3.0 , 3.0 , keijzer_func14 );
}

template< typename Rng >
auto generate_keijzer14( Rng& rng ) {
    return gpcxx::generate_uniform_distributed_test_data< 2 >( rng , 20 , -3.0 , 3.0 , keijzer_func15 );
}

template< typename Rng >
auto generate_keijzer15( Rng& rng ) {
    return gpcxx::generate_uniform_distributed_test_data< 2 >( rng , 20 , -3.0 , 3.0 , keijzer_func16 );
}

} // namespace gpcxx

#endif // GPCXX_APP_BENCHMARK_PROBLEMS_KEIJZER_HPP_INCLUDED


#pragma once

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

#include <boost/random/mersenne_twister.hpp>
#include <boost/shared_ptr.hpp>

#include "dpmeans.hpp"

using namespace Eigen;
using std::cout;
using std::endl;

template<class T>
class DPvMFMeans : public DPMeans<T>
{
public:
  DPvMFMeans(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx, uint32_t K0,
      double lambda, boost::mt19937* pRndGen);
  virtual ~DPvMFMeans();

//  virtual void updateLabels();
//  virtual void updateCenters();

  virtual T dist(const Matrix<T,Dynamic,1>& a, const Matrix<T,Dynamic,1>& b);
  virtual bool closer(T a, T b);
  virtual uint32_t indOfClosestCluster(int32_t i);
  virtual Matrix<T,Dynamic,1> computeCenter(uint32_t k);

//protected:
//  double lambda_;
};
// --------------------------- impl -------------------------------------------

template<class T>
DPvMFMeans<T>::DPvMFMeans(
    const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx, uint32_t K0, double lambda,
    boost::mt19937* pRndGen)
  : DPMeans<T>(spx,K0, lambda, pRndGen)
{
  assert(-2.0 < this->lambda_ && this->lambda_ < 0.0);
}

template<class T>
DPvMFMeans<T>::~DPvMFMeans()
{}

template<class T>
T DPvMFMeans<T>::dist(const Matrix<T,Dynamic,1>& a, const Matrix<T,Dynamic,1>& b)
{
//  return acos(min(1.0,max(-1.0,(a.transpose()*b)(0)))); // angular similarity
  return a.transpose()*b; // cosine similarity
};

template<class T>
bool DPvMFMeans<T>::closer(T a, T b)
{
//  return a<b; // if dist a is greater than dist b a is closer than b (angular dist)
  return a>b; // if dist a is greater than dist b a is closer than b (cosine dist)
};

template<class T>
Matrix<T,Dynamic,1> DPvMFMeans<T>::computeCenter(uint32_t k)
{
  this->Ns_(k) = 0.0;
  Matrix<T,Dynamic,1> mean_k(this->D_);
  mean_k.setZero(this->D_);
  for(uint32_t i=0; i<this->N_; ++i)
    if(this->z_(i) == k)
    {
      mean_k += this->spx_->col(i);
      this->Ns_(k) ++;
    }
  if(this->Ns_(k) > 0)
    return mean_k/mean_k.norm();
  else
    return mean_k;
}

template<class T>
uint32_t DPvMFMeans<T>::indOfClosestCluster(int32_t i)
{
  // use cosine similarity because it is faster since acos is not computed
  int z_i = this->K_;
  T sim_closest = this->lambda_ +1.;// new formulation -2<lambda<0
  for(uint32_t k=0; k<this->K_; ++k)
  {
    T sim_k = this->ps_.col(k).transpose()* this->spx_->col(i);
    if( sim_k > sim_closest) // because of cosine distance
    {
      sim_closest = sim_k;
      z_i = k;
    }
  }
  return z_i;
};

//template<class T>
//void DPvMFMeans<T>::updateLabels()
//{
////#pragma omp parallel for
//// TODO not sure how to parallelize
//  for(uint32_t i=0; i<this->N_; ++i)
//  {
//
////    Matrix<T,Dynamic,1> sim(this->K_+1);
////    sim(this->K_) = lambda_;
////    for(uint32_t k=0; k<this->K_; ++k)
////      sim(k) = this->ps_.col(k).transpose()*this->spx_->col(i);
////    int z_i,dummy;
//////    cout<<sim.transpose()<<endl;
////    sim.maxCoeff(&z_i,&dummy);
//
//    int z_i = this->K_;
//    T sim_max = lambda_;
//    for (uint32_t k=0; k<this->K_; ++k)
//    {
//      T sim_k = this->ps_.col(k).transpose()*this->spx_->col(i);
//      if(sim_k > sim_max)
//      {
//        sim_max = sim_k;
//        z_i = k;
//      }
//    }
//
//    if(z_i == this->K_)
//    {
//      MatrixXd psNew(this->D_,this->K_+1);
//      psNew.leftCols(this->K_) = this->ps_;
//      psNew.col(this->K_) = this->spx_->col(i);
//      this->ps_ = psNew;
//      this->K_ ++;
//    }
//    this->z_(i) = z_i;
//  }
//}
//
//template<class T>
//void DPvMFMeans<T>::updateCenters()
//{
//  vector<bool> toDelete(this->K_,false);
//#pragma omp parallel for
//  for(uint32_t k=0; k<this->K_; ++k)
//  {
//    T N_k=0;
//    Matrix<T,Dynamic,1> mean_k(this->D_);
//    mean_k.setZero(this->D_);
//    for(uint32_t i=0; i<this->N_; ++i)
//      if(this->z_(i) == k)
//      {
//        mean_k += this->spx_->col(i);
//        N_k ++;
//      }
//    if (N_k > 0)
//      this->ps_.col(k) = mean_k/mean_k.norm();
//    else
//      toDelete[k] = true;
//  }
//
//  uint32_t kNew = this->K_;
//  for(int32_t k=this->K_-1; k>-1; --k)
//    if(toDelete[k])
//    {
//      cout<<"cluster k "<<k<<" empty"<<endl;
//#pragma omp parallel for
//      for(uint32_t i=0; i<this->N_; ++i)
//      {
//        if(this->z_(i) >= k) this->z_(i)--;
//      }
//      kNew --;
//    }
//
//  MatrixXd psNew(this->D_,kNew);
//  int32_t offset = 0;
//  for(uint32_t k=0; k<this->K_; ++k)
//    if(toDelete[k])
//    {
//      offset ++;
//    }else{
//      psNew.col(k-offset) = this->ps_.col(k);
//    }
//  this->ps_ = psNew;
//  this->K_ = kNew;
//
////  cout<<"centers="<<endl<<this->ps_<<endl;
//}
//


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_FNMA_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FNMA_HPP_INCLUDED

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

 /*!

    @ingroup group-arithmetic
    Function object implementing fnma capabilities

    Computes the fused negated add-multiply of three values.

    @par semantic:
    For any given value @c x,  @c y,  @c z of type @c T:

    @code
    T r = fnma(x, y, z);
    @endcode

    The code is similar to:

    @code
    T r = -(x+y*z);
    @endcode

    @par Note:

    fnma can be called with the same syntax as @ref fma

    @see fma, fms, fnms, correct_fma

  **/
  const boost::dispatch::functor<tag::fnma_> fnma = {};
} }
#endif

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

#endif


/**
 * \file dcs/math/stats/distribution/mmpp.hpp
 *
 * \brief Markov-modulated Poisson Process (MMPP).
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * 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.
 */

#ifndef DCS_MATH_STATS_DISTRIBUTION_MMPP_HPP
#define DCS_MATH_STATS_DISTRIBUTION_MMPP_HPP

#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/operation/diag.hpp>
#include <cstddef>
#include <dcs/math/policies/policy.hpp>
#include <dcs/math/stats/distribution/base_distribution.hpp>
#include <dcs/math/stats/distribution/map.hpp>
#include <dcs/math/stats/function/rand.hpp>
#include <iostream>
#include <vector>

namespace dcs { namespace math { namespace stats {

template <
	typename RealT = double,
	typename PolicyT = ::dcs::math::policies::policy<>
>
class mmpp_distribution//: public base_distribution<RealT>
{
	public: typedef RealT value_type;
	public: typedef value_type support_type;
	public: typedef ::boost::numeric::ublas::matrix<support_type> matrix_type;
	public: typedef ::boost::numeric::ublas::vector<support_type> vector_type;
	public: typedef ::std::size_t size_type;
	private: typedef map_distribution<RealT,PolicyT> map_distribution_type;

	public: template <typename VectorExprT, typename MatrixExprT>
		mmpp_distribution(::boost::numeric::ublas::vector_expression<VectorExprT> const& lambda,
						 ::boost::numeric::ublas::matrix_expression<MatrixExprT> const& Q)
	: //base_type(),
	  map_(Q-::boost::numeric::ublasx::diag(lambda), ::boost::numeric::ublasx::diag(lambda))
	{
	}

	// Compiler-generated copy-constructor, copy-assignment, and destructor
	// are fine.

	public: vector_type lambda() const
	{
		return ::boost::numeric::ublasx::diag(map_.D1());
	}

	public: matrix_type Q() const
	{
		return map_.D0()+map_.D1();
	}

/*
	private: value_type do_rand(::dcs::math::random::any_generator<value_type>& rng) const
	{
		return rand_sample(rng);
	}

	private: value_type do_rand(::dcs::math::random::base_generator<value_type>& rng) const
	{
		return rand_sample(rng);
	}
*/

	public: template <typename URNG>
		value_type rand(URNG& rng) const
	{
		return map_.rand(rng);
	}

	/**
	 * The following algorithm is an adaptation of the one found in the MMPP-QN
	 * toolbox (by G. Casale et al, http://www.cs.wm.edu/MMPPQN/).
	 */
	public: template <typename URNG>
		::std::vector<value_type> rand(URNG& rng, size_type n) const
	{
		return map_.rand(rng, n);
	}

	private: map_distribution_type map_;
};

template <
	typename CharT,
	typename CharTraitsT,
	typename RealT,
	typename PolicyT
>
::std::basic_ostream<CharT,CharTraitsT>& operator<<(::std::basic_ostream<CharT,CharTraitsT>& os, mmpp_distribution<RealT,PolicyT> const& dist)
{
	return os << "MMPP("
			  << "lambda=" <<  dist.lambda()
			  << ",Q=" <<  dist.Q()
			  << ")";
}

}}} // Namespace dcs:math::stats

#endif // DCS_MATH_STATS_DISTRIBUTION_MMPP_HPP


#include "vtkbackbone.h"
#include <Eigen/Core>
#include <vtkLineSource.h>
#include <vtkArcSource.h>

VtkBackbone::VtkBackbone(continuum_robot::ConfigSingleSeg config,
                         continuum_robot::BackboneNum backboneNum,
                         continuum_robot::MbcrParameter mbcrParameter): mbcrParameter_(mbcrParameter),
    backboneNum_(backboneNum)
{

    // generate mapper
    mapperPtr_ = vtkSmartPointer<vtkPolyDataMapper>::New();
    lineSourcePtr_ = vtkSmartPointer<vtkLineSource>::New();
    arcSourcePtr_ = vtkSmartPointer<vtkArcSource>::New();
    genShapeMapper(config); //generate backbone shape

    // set mapper
    actorPtr_ = vtkSmartPointer<vtkActor>::New();
    actorPtr_->SetMapper(mapperPtr_);

    // set transformations
    transformPtr_ = vtkSmartPointer<vtkTransform>::New();
    transformPtr_->Identity();
    actorPtr_->SetUserTransform(transformPtr_);

    // initialize xformVtkMatPtr_
    xformVtkMatPtr_ = vtkSmartPointer<vtkMatrix4x4>::New();
    xformVtkMatPtr_->Identity();

    //
    //pArcSource->SetOutputPointsPrecision(vtkAlgorithm::SINGLE_PRECISION);

    //    pArcSource->SetNormal(0,0,1); //plane normal
    //    pArcSource->SetPolarVector(1,90,0); // polar coordinate (r,theta,z) of starting point
    //    pArcSource->SetAngle(180); //CCW angle of arc
    //    pArcSource->Update();
}

vtkSmartPointer<vtkActor> VtkBackbone::getActor()
{
    return actorPtr_;
}

void VtkBackbone::setTransform(const Eigen::Matrix4d &xform)
{
    // update hom transform of actor
    // note position must be in mm
    double homXform[16] = {xform(0,0), xform(0,1), xform(0,2), xform(0,3),
                           xform(1,0), xform(1,1), xform(1,2), xform(1,3),
                           xform(2,0), xform(2,1), xform(2,2), xform(2,3),
                           xform(3,0), xform(3,1), xform(3,2), xform(3,3)};
    xformVtkMatPtr_->DeepCopy(homXform);
    transformPtr_->SetMatrix(xformVtkMatPtr_);
    transformPtr_->Update();
}

void VtkBackbone::genShapeMapper(continuum_robot::ConfigSingleSeg config)
{
    //config = flipConfig(config);
    Eigen::Vector3d pointStart;
    switch (backboneNum_)
    {
    case continuum_robot::BACKBONE1:
        pointStart << mbcrParameter_.kin_Radius,
                0,
                0;
        break;
    case continuum_robot::BACKBONE2:
        pointStart << -mbcrParameter_.kin_Radius*(1.0/2.0),
                mbcrParameter_.kin_Radius*(std::sqrt(3.0)/2.0),
                0;
        break;
    case continuum_robot::BACKBONE3:
        pointStart << -mbcrParameter_.kin_Radius*(1.0/2.0),
                -mbcrParameter_.kin_Radius*(std::sqrt(3.0)/2.0),
                0;
        break;
    }

    Eigen::Matrix4d gripperToBaseXform = continuum_robot::directKinSingleSeg(config,
                                                                             mbcrParameter_.length);
    Eigen::Vector3d pointEnd = gripperToBaseXform.block(0,0,3,3)*pointStart + gripperToBaseXform.block(0,3,3,1);

    if (abs(config.theta-::PI/2.0) <= 0.5*(::PI/180.0))
    {
        lineSourcePtr_->SetPoint1(pointStart(0),pointStart(1),pointStart(2));
        lineSourcePtr_->SetPoint2(pointEnd(0),pointEnd(1),pointEnd(2));
        lineSourcePtr_->Update();
        mapperPtr_->SetInputConnection(lineSourcePtr_->GetOutputPort());
    }
    else
    {
        Eigen::Vector3d pointCenter = continuum_robot::getBackboneCurveCenterSingleSeg(config,
                                                                                mbcrParameter_,
                                                                                backboneNum_);
        arcSourcePtr_->SetResolution(50);
        arcSourcePtr_->SetPoint1(pointStart(0),pointStart(1),pointStart(2));
        arcSourcePtr_->SetPoint2(pointEnd(0),pointEnd(1),pointEnd(2));
        arcSourcePtr_->SetCenter(pointCenter(0),pointCenter(1),pointCenter(2));
        arcSourcePtr_->Update();
        mapperPtr_->SetInputConnection(arcSourcePtr_->GetOutputPort());
    }
}

void VtkBackbone::setPrimBackboneLength(double length)
{
    mbcrParameter_.length = length;
}

void VtkBackbone::setBackboneKinRadius(double kinRadius)
{
    mbcrParameter_.kin_Radius = kinRadius;
}

void VtkBackbone::setBackboneNumber(continuum_robot::BackboneNum backboneNum)
{
    backboneNum_ = backboneNum;
}

double VtkBackbone::getPrimBackboneLength()
{
    return mbcrParameter_.length;
}

double VtkBackbone::getBackboneKinRadius()
{
    return mbcrParameter_.kin_Radius;
}

continuum_robot::BackboneNum VtkBackbone::getBackboneNumber()
{
    return backboneNum_;
}


/*=========================================================================
 *
 *  Copyright 2011-2013 The University of North Carolina at Chapel Hill
 *  All rights reserved.
 *
 *  Licensed under the MADAI Software License. You may obtain a copy of
 *  this license at
 *
 *         https://madai-public.cs.unc.edu/visualization/software-license/
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/

#include <iostream>
#include <fstream>
#include <cmath>
#include <vector>

#include <Eigen/Dense>
#include "GaussianProcessEmulatorTestGenerator.h"
#include "GaussianProcessEmulator.h"
#include "GaussianProcessEmulatorDirectoryFormatIO.h"
#include "Paths.h"

const char DEFAULT_MODEL_OUTPUT_DIRECTORY[] = "model_output";
const char DEFAULT_EXPERIMENTAL_RESULTS_FILE[] = "experimental_results.dat";

inline double LogisticFunction(double x) {
  return 1.0 / (1.0 + std::exp(-x));
}
inline double sinc(double x) {
  if (x==0) return 1.0;
  x *= 3.141592653589793;
  return std::sin(x) / x;
}
void model(const std::vector< double > & params, std::vector< double > & out) {
  const double & x = params[0];
  const double & y = params[1];
  out.at(0) = sinc(std::sqrt((x * x) + (y * y) + (0.5 * x * y)));
  out.at(1) = LogisticFunction(x - 0.25 * y);
}

int main( int, char *[] ) {
  static const int N = 100;

  madai::Parameter param0( "param_0", -1, 1 );
  madai::Parameter param1( "param_1", -1, 1 );
  std::vector< madai::Parameter > parameters;
  parameters.push_back( param0 );
  parameters.push_back( param1 );

  GaussianProcessEmulatorTestGenerator generator( &model,2,2,N, parameters);

  std::string TempDirectory = "../Testing/Temporary/GaussianProcessEmulatorTest";
  if ( !generator.WriteDirectoryStructure(TempDirectory) ) {
    std::cerr << "Error writing directory structure\n";
    return EXIT_FAILURE;
  }

  std::string MOD = TempDirectory + madai::Paths::SEPARATOR +
    DEFAULT_MODEL_OUTPUT_DIRECTORY;
  std::string ERF = TempDirectory + madai::Paths::SEPARATOR +
    DEFAULT_EXPERIMENTAL_RESULTS_FILE;

  madai::GaussianProcessEmulator gpe;
  madai::GaussianProcessEmulatorDirectoryFormatIO directoryReader;
  if ( !directoryReader.LoadTrainingData( &gpe, MOD, TempDirectory, ERF ) ) {
    std::cerr << "Error loading from created directory structure\n";
    return EXIT_FAILURE;
  }

  if ( !gpe.PrincipalComponentDecompose() ) {
    std::cerr << "Error decomposing model data.\n";
    return EXIT_FAILURE;
  }

  std::string PCAFileName = TempDirectory + madai::Paths::SEPARATOR
      + madai::Paths::PCA_DECOMPOSITION_FILE;
  std::ofstream PCAFile( PCAFileName.c_str() );
  if ( !PCAFile ) {
    std::cerr << "Could not open file '" << PCAFileName << "'\n";
    return EXIT_FAILURE;
  }

  madai::GaussianProcessEmulatorDirectoryFormatIO directoryFormatIO;
  directoryFormatIO.WritePCA( &gpe, PCAFile );
  PCAFile.close();

  double fractionResolvingPower = 0.999;
  madai::GaussianProcessEmulator::CovarianceFunctionType covarianceFunction
      = madai::GaussianProcessEmulator::SQUARE_EXPONENTIAL_FUNCTION;
  int regressionOrder = 1;
  double defaultNugget = 1e-3;
  double amplitude = 1.0;
  double scale = 1e-2;

  if (! gpe.RetainPrincipalComponents( fractionResolvingPower ) )
    return EXIT_FAILURE;

  if (! gpe.BasicTraining(
          covarianceFunction,
          regressionOrder,
          defaultNugget,
          amplitude,
          scale))
    return EXIT_FAILURE;

  std::string EmulatorStateFileName = TempDirectory + madai::Paths::SEPARATOR
      + madai::Paths::EMULATOR_STATE_FILE;
  std::ofstream EmulatorStateFile( EmulatorStateFileName.c_str() );
  if ( !EmulatorStateFile ) {
    std::cerr << "Could not open file '" << EmulatorStateFileName << "'\n";
    return EXIT_FAILURE;
  }

  directoryFormatIO.Write( &gpe, EmulatorStateFile );
  EmulatorStateFile.close();

  if (! gpe.MakeCache()) {
    std::cerr << "Error while makeing cache.\n";
    return false;
  }

  std::cout.precision(17);
  double error = 0.0;

  std::vector< double > x(2,0.0);
  std::vector< double > y(2,0.0);
  for (int i = 0; i < N; ++i) {
    x[0] = generator.m_X(i,0);
    x[1] = generator.m_X(i,1);
    if (! gpe.GetEmulatorOutputs(x, y))
      return EXIT_FAILURE;
    error += std::abs(y[0] - generator.m_Y(i,0));
    error += std::abs(y[1] - generator.m_Y(i,1));
  }
  std::cout << "Sum of errors at training points: " << error << '\n';

  error = 0.0;
  std::vector< double > y2(2,0.0);
  double range_over_N = 2.0 / static_cast< double >(N);
  double half_range_over_N = 0.5 * range_over_N;
  for (int i = 0; i < N; ++i) {
    x[0] = range_over_N * i + half_range_over_N;
    for (int j = 0; j < N; ++j) {
      x[1] = range_over_N * j + half_range_over_N;
      gpe.GetEmulatorOutputs(x, y);
      model(x, y2);
      error = std::max(error, std::abs(y[0] - y2[0]));
      error = std::max(error, std::abs(y[1] - y2[1]));
    }
  }
  std::cout << "Maximum error over all space: " << error << '\n';

  std::string ThetaFileName = TempDirectory + madai::Paths::SEPARATOR + "thetas.dat";
  std::ofstream ThetaFile( ThetaFileName.c_str() );
  if(! directoryFormatIO.PrintThetas(&gpe,ThetaFile)) {
    std::cerr << "Error printing Thetas.\n";
    return EXIT_FAILURE;
  }
  ThetaFile.close();

  return EXIT_SUCCESS;
}


//! \file
//! \brief defines the built-in capabilities of the drone language
//!
//! Each built-in capability in the drone language is implemented
//! as an object.  This object contains a method called execute
//! which acts upon the data stack or does work through some
//! other mechanism.

#ifndef AUTONOMY_SCRIPT_BUILTINS_HPP
#define AUTONOMY_SCRIPT_BUILTINS_HPP
#include <boost/lexical_cast.hpp>

#include <autonomy/script_instruction.hpp>
#include <autonomy/entity/scripted_drone.hpp>

namespace autonomy { namespace script_builtins{
    //! \brief duplicate the top data element
    //!
    //! push a duplicate of the top stack element onto the stack
    struct dup : public script_instruction_base<dup>
    {
        friend class boost::serialization::access;
        static std::string name() { return "dup"; }
        virtual unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<dup>);
            }

    };

    //! \brief pop the top data element from the stack
    struct pop : public script_instruction_base<pop>
    {
        friend class boost::serialization::access;
        static std::string name() { return "pop"; }
        virtual unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<pop>);
            }

    };

    //! \brief add two data elements
    //!
    //! pop the top two elements from the stack and push their sum back
    //! onto the stack
    struct add : public script_instruction_base<add>
    {
        friend class boost::serialization::access;
        static std::string name() { return "add"; }
        virtual unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<add>);
            }
    };

    //! \brief subtract one data element from another
    //!
    //! pop the top two elements from the stack. subtract the top element
    //! from the next element and push the difference back onto the stack
    struct sub : public script_instruction_base<sub>
    {
        friend class boost::serialization::access;
        static std::string name() { return "sub"; }
        virtual unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<sub>);
            }
    };

    //! \brief multiply two data elements
    //!
    //! pop the top two elements from the stack and push their product
    //! back onto the stack
    struct multiply : public script_instruction_base<multiply>
    {
        friend class boost::serialization::access;
        static std::string name() { return "multiply"; }
        virtual unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<multiply>);
            }
    };

    //! \brief divide a data element by another
    //!
    //! pop the top two elements from the stack. divide the top element
    //! from the next element and push the difference back onto the stack
    struct divide : public script_instruction_base<divide>
    {
        friend class boost::serialization::access;
        static std::string name() { return "divide"; }
        virtual unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<divide>);
            }
    };

    //! \brief raise a data element to the power of another
    //!
    //! pop the first element from the stack, raise it to the power of the
    //! second item popped and push the result back onto the stack
    struct power : public script_instruction_base<power>
    {
        friend class boost::serialization::access;
        static std::string name() { return "power"; }
        virtual unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<power>);
            }
    };

    //! \brief jump to a given instruction based on an absolute location
    struct jump : public script_instruction_base<jump>
    {
        friend class boost::serialization::access;
        static std::string name() { return "jump"; }
        virtual unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<jump>);
            }
    };

    //! \brief jump to a given instruction based on a relative location
    struct rel_jump : public script_instruction_base<rel_jump>
    {
        friend class boost::serialization::access;
        static std::string name() { return "rel_jump"; }
        virtual unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<rel_jump>);
            }
    };

    //! \brief jump to a given instruction based on an abosolute location
    //! if a condition evaulates as true
    struct cond_jump : public script_instruction_base<cond_jump>
    {
        friend class boost::serialization::access;
        static std::string name() { return "cond_jump"; }
        virtual unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<cond_jump>);
            }
    };

    //! \brief jump to a given instruction based on a relative location
    //! if a condition evaulates as true
    struct cond_rel_jump : public script_instruction_base<cond_rel_jump>
    {
        friend class boost::serialization::access;
        static std::string name() { return "cond_rel_jump"; }
        virtual unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<cond_rel_jump>);
            }
    };

    //! \brief determine if two data elements are equal
    //!
    //! pop the top two elements off the stack and push 1 if equal else
    //! push 0 back onto the stack
    struct equals : public script_instruction_base<equals>
    {
        friend class boost::serialization::access;
        static std::string name() { return "equals"; }
        virtual unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<equals>);
            }
    };

    //! \brief determine if one data element is less than another
    //!
    //! pop the top two elements off the stack, if the second element
    //! is less than the first element, push 1, else pus 0 back onto the
    //! stack
    struct less_than : public script_instruction_base<less_than>
    {
        friend class boost::serialization::access;
        static std::string name() { return "less_than"; }
        virtual unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<less_than>);
            }
    };

    //! \brief logical AND comparison of two data elements
    struct logical_and : public script_instruction_base<logical_and>
    {
        friend class boost::serialization::access;
        static std::string name() { return "and"; }
        virtual unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<logical_and>);
            }
    };

    //! \brief logical OR comparison of two data elements
    struct logical_or : public script_instruction_base<logical_or>
    {
        friend class boost::serialization::access;
        static std::string name() { return "or"; }
        virtual unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<logical_or>);
            }
    };

    //! \brief logical NOT of a data element
    struct logical_not : public script_instruction_base<logical_not>
    {
        friend class boost::serialization::access;
        static std::string name() { return "not"; }
        virtual unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<logical_not>);
            }
    };

    //! \brief a literal value (e.g. 5, 31, 12)
    class literal : public script_instruction_base<literal>
    {
        friend class boost::serialization::access;
        public:
        literal(int val = 0)
            : _val(val)
        {}
        std::string name() const { return "literal " + boost::lexical_cast<std::string>(_val); }

        virtual unsigned int execute(size_t which_queue, entity::scripted_drone & drone);

        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<literal>);
                ar & BOOST_SERIALIZATION_NVP(_val);
            }
        int _val;
    };

    //! \brief load an instruction
    struct load : public script_instruction_base<load>
    {
        friend class boost::serialization::access;
        static std::string name() { return "load"; }
        virtual unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<load>);
            }
    };

    //! \brief store an instruction
    struct store : public script_instruction_base<store>
    {
        friend class boost::serialization::access;
        static std::string name() { return "store"; }
        virtual unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<store>);
            }
    };

}}
#endif


// Boost.Geometry

// Copyright (c) 2018 Adeel Ahmad, Islamabad, Pakistan.

// Contributed and/or modified by Adeel Ahmad, as part of Google Summer of Code 2018 program.

// This file was modified by Oracle on 2019.
// Modifications copyright (c) 2019 Oracle and/or its affiliates.

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

// This file is converted from GeographicLib, https://geographiclib.sourceforge.io
// GeographicLib is originally written by Charles Karney.

// Author: Charles Karney (2008-2017)

// Last updated version of GeographicLib: 1.49

// Original copyright notice:

// Copyright (c) Charles Karney (2008-2017) <charles@karney.com> and licensed
// under the MIT/X11 License. For more information, see
// https://geographiclib.sourceforge.io

#ifndef BOOST_GEOMETRY_FORMULAS_KARNEY_INVERSE_HPP
#define BOOST_GEOMETRY_FORMULAS_KARNEY_INVERSE_HPP

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

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/series_expansion.hpp>
#include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>

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

namespace boost { namespace geometry { namespace math {

// TODO: Moved temporarily because of C++11 is used

/*!
\brief The exact difference of two angles reduced to (-180deg, 180deg].
*/
template<typename T>
inline T difference_angle(T const& x, T const& y, T& e)
{
    T t, d = math::sum_error(std::remainder(-x, T(360)), std::remainder(y, T(360)), t);

    normalize_azimuth<degree, T>(d);

    // Here y - x = d + t (mod 360), exactly, where d is in (-180,180] and
    // abs(t) <= eps (eps = 2^-45 for doubles).  The only case where the
    // addition of t takes the result outside the range (-180,180] is d = 180
    // and t > 0.  The case, d = -180 + eps, t = -eps, can't happen, since
    // sum_error would have returned the exact result in such a case (i.e., given t = 0).
    return math::sum_error(d == 180 && t > 0 ? -180 : d, t, e);
}

}}} // namespace boost::geometry::math

namespace boost { namespace geometry { namespace formula
{

namespace se = series_expansion;

/*!
\brief The solution of the inverse problem of geodesics on latlong coordinates,
       after Karney (2011).
\author See
- Charles F.F Karney, Algorithms for geodesics, 2011
https://arxiv.org/pdf/1109.4448.pdf
*/
template <
    typename CT,
    bool EnableDistance,
    bool EnableAzimuth,
    bool EnableReverseAzimuth = false,
    bool EnableReducedLength = false,
    bool EnableGeodesicScale = false,
    size_t SeriesOrder = 8
>
class karney_inverse
{
    static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale;
    static const bool CalcAzimuths = EnableAzimuth || EnableReverseAzimuth || CalcQuantities;
    static const bool CalcFwdAzimuth = EnableAzimuth || CalcQuantities;
    static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcQuantities;

public:
    typedef result_inverse<CT> result_type;

    template <typename T1, typename T2, typename Spheroid>
    static inline result_type apply(T1 const& lo1,
                                    T1 const& la1,
                                    T2 const& lo2,
                                    T2 const& la2,
                                    Spheroid const& spheroid)
    {
        static CT const c0 = 0;
        static CT const c0_001 = 0.001;
        static CT const c0_1 = 0.1;
        static CT const c1 = 1;
        static CT const c2 = 2;
        static CT const c3 = 3;
        static CT const c8 = 8;
        static CT const c16 = 16;
        static CT const c90 = 90;
        static CT const c180 = 180;
        static CT const c200 = 200;
        static CT const pi = math::pi<CT>();
        static CT const d2r = math::d2r<CT>();
        static CT const r2d = math::r2d<CT>();

        result_type result;

        CT lat1 = la1;
        CT lat2 = la2;

        CT lon1 = lo1;
        CT lon2 = lo2;

        CT const a = CT(get_radius<0>(spheroid));
        CT const b = CT(get_radius<2>(spheroid));
        CT const f = formula::flattening<CT>(spheroid);
        CT const one_minus_f = c1 - f;
        CT const two_minus_f = c2 - f;

        CT const tol0 = std::numeric_limits<CT>::epsilon();
        CT const tol1 = c200 * tol0;
        CT const tol2 = sqrt(tol0);

        // Check on bisection interval.
        CT const tol_bisection = tol0 * tol2;

        CT const etol2 = c0_1 * tol2 /
            sqrt((std::max)(c0_001, std::abs(f)) * (std::min)(c1, c1 - f / c2) / c2);

        CT tiny = std::sqrt((std::numeric_limits<CT>::min)());

        CT const n = f / two_minus_f;
        CT const e2 = f * two_minus_f;
        CT const ep2 = e2 / math::sqr(one_minus_f);

        // Compute the longitudinal difference.
        CT lon12_error;
        CT lon12 = math::difference_angle(lon1, lon2, lon12_error);

        int lon12_sign = lon12 >= 0 ? 1 : -1;

        // Make points close to the meridian to lie on it.
        lon12 = lon12_sign * lon12;
        lon12_error = (c180 - lon12) - lon12_sign * lon12_error;

        // Convert to radians.
        CT lam12 = lon12 * d2r;
        CT sin_lam12;
        CT cos_lam12;

        if (lon12 > c90)
        {
            math::sin_cos_degrees(lon12_error, sin_lam12, cos_lam12);
            cos_lam12 *= -c1;
        }
        else
        {
            math::sin_cos_degrees(lon12, sin_lam12, cos_lam12);
        }

        // Make points close to the equator to lie on it.
        lat1 = math::round_angle(std::abs(lat1) > c90 ? c90 : lat1);
        lat2 = math::round_angle(std::abs(lat2) > c90 ? c90 : lat2);

        // Arrange points in a canonical form, as explained in
        // paper, Algorithms for geodesics, Eq. (44):
        //
        //     0 <= lon12 <= 180
        //     -90 <= lat1 <= 0
        //     lat1 <= lat2 <= -lat1
        int swap_point = std::abs(lat1) < std::abs(lat2) ? -1 : 1;

        if (swap_point < 0)
        {
            lon12_sign *= -1;
            swap(lat1, lat2);
        }

        // Enforce lat1 to be <= 0.
        int lat_sign = lat1 < 0 ? 1 : -1;
        lat1 *= lat_sign;
        lat2 *= lat_sign;

        CT sin_beta1, cos_beta1;
        math::sin_cos_degrees(lat1, sin_beta1, cos_beta1);
        sin_beta1 *= one_minus_f;

        math::normalize_unit_vector<CT>(sin_beta1, cos_beta1);
        cos_beta1 = (std::max)(tiny, cos_beta1);

        CT sin_beta2, cos_beta2;
        math::sin_cos_degrees(lat2, sin_beta2, cos_beta2);
        sin_beta2 *= one_minus_f;

        math::normalize_unit_vector<CT>(sin_beta2, cos_beta2);
        cos_beta2 = (std::max)(tiny, cos_beta2);

        // If cos_beta1 < -sin_beta1, then cos_beta2 - cos_beta1 is a
        // sensitive measure of the |beta1| - |beta2|. Alternatively,
        // (cos_beta1 >= -sin_beta1), abs(sin_beta2) + sin_beta1 is
        // a better measure.
        // Sometimes these quantities vanish and in that case we
        // force beta2 = +/- bet1a exactly.
        if (cos_beta1 < -sin_beta1)
        {
            if (cos_beta1 == cos_beta2)
            {
                sin_beta2 = sin_beta2 < 0 ? sin_beta1 : -sin_beta1;
            }
        }
        else
        {
            if (std::abs(sin_beta2) == -sin_beta1)
            {
                cos_beta2 = cos_beta1;
            }
        }

        CT const dn1 = sqrt(c1 + ep2 * math::sqr(sin_beta1));
        CT const dn2 = sqrt(c1 + ep2 * math::sqr(sin_beta2));

        CT sigma12;
        CT m12x, s12x, M21;

        // Index zero element of coeffs_C1 is unused.
        se::coeffs_C1<SeriesOrder, CT> const coeffs_C1(n);

        bool meridian = lat1 == -90 || sin_lam12 == 0;

        CT cos_alpha1, sin_alpha1;
        CT cos_alpha2, sin_alpha2;

        if (meridian)
        {
            // Endpoints lie on a single full meridian.

            // Point to the target latitude.
            cos_alpha1 = cos_lam12;
            sin_alpha1 = sin_lam12;

            // Heading north at the target.
            cos_alpha2 = c1;
            sin_alpha2 = c0;

            CT sin_sigma1 = sin_beta1;
            CT cos_sigma1 = cos_alpha1 * cos_beta1;

            CT sin_sigma2 = sin_beta2;
            CT cos_sigma2 = cos_alpha2 * cos_beta2;

            CT sigma12 = std::atan2((std::max)(c0, cos_sigma1 * sin_sigma2 - sin_sigma1 * cos_sigma2),
                                                   cos_sigma1 * cos_sigma2 + sin_sigma1 * sin_sigma2);

            CT dummy;
            meridian_length(n, ep2, sigma12, sin_sigma1, cos_sigma1, dn1,
                                             sin_sigma2, cos_sigma2, dn2,
                                             cos_beta1, cos_beta2, s12x,
                                             m12x, dummy, result.geodesic_scale,
                                             M21, coeffs_C1);

            if (sigma12 < c1 || m12x >= c0)
            {
                if (sigma12 < c3 * tiny)
                {
                    sigma12  = m12x = s12x = c0;
                }

                m12x *= b;
                s12x *= b;
            }
            else
            {
                // m12 < 0, i.e., prolate and too close to anti-podal.
                meridian = false;
            }
        }

        CT omega12;

        if (!meridian && sin_beta1 == c0 &&
            (f <= c0 || lon12_error >= f * c180))
        {
            // Points lie on the equator.
            cos_alpha1 = cos_alpha2 = c0;
            sin_alpha1 = sin_alpha2 = c1;

            s12x = a * lam12;
            sigma12 = omega12 = lam12 / one_minus_f;
            m12x = b * sin(sigma12);

            if (BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
            {
                result.geodesic_scale = cos(sigma12);
            }
        }
        else if (!meridian)
        {
            // If point1 and point2 belong within a hemisphere bounded by a
            // meridian and geodesic is neither meridional nor equatorial.

            // Find the starting point for Newton's method.
            CT dnm;
            sigma12 = newton_start(sin_beta1, cos_beta1, dn1,
                                   sin_beta2, cos_beta2, dn2,
                                   lam12, sin_lam12, cos_lam12,
                                   sin_alpha1, cos_alpha1,
                                   sin_alpha2, cos_alpha2,
                                   dnm, coeffs_C1, ep2,
                                   tol1, tol2, etol2,
                                   n, f);

            if (sigma12 >= c0)
            {
                // Short lines case (newton_start sets sin_alpha2, cos_alpha2, dnm).
                s12x = sigma12 * b * dnm;
                m12x = math::sqr(dnm) * b * sin(sigma12 / dnm);
                if (BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
                {
                    result.geodesic_scale = cos(sigma12 / dnm);
                }

                // Convert to radians.
                omega12 = lam12 / (one_minus_f * dnm);
            }
            else
            {
                // Apply the Newton's method.
                CT sin_sigma1 = c0, cos_sigma1 = c0;
                CT sin_sigma2 = c0, cos_sigma2 = c0;
                CT eps = c0, diff_omega12 = c0;

                // Bracketing range.
                CT sin_alpha1a = tiny, cos_alpha1a = c1;
                CT sin_alpha1b = tiny, cos_alpha1b = -c1;

                size_t iteration = 0;
                size_t max_iterations = 20 + std::numeric_limits<size_t>::digits + 10;

                for (bool tripn = false, tripb = false;
                     iteration < max_iterations;
                     ++iteration)
                {
                    CT dv;
                    CT v = lambda12(sin_beta1, cos_beta1, dn1,
                                    sin_beta2, cos_beta2, dn2,
                                    sin_alpha1, cos_alpha1,
                                    sin_lam12, cos_lam12,
                                    sin_alpha2, cos_alpha2,
                                    sigma12,
                                    sin_sigma1, cos_sigma1,
                                    sin_sigma2, cos_sigma2,
                                    eps, diff_omega12,
                                    iteration < max_iterations,
                                    dv, f, n, ep2, tiny, coeffs_C1);

                    // Reversed test to allow escape with NaNs.
                    if (tripb || !(std::abs(v) >= (tripn ? c8 : c1) * tol0))
                        break;

                    // Update bracketing values.
                    if (v > c0 && (iteration > max_iterations ||
                        cos_alpha1 / sin_alpha1 > cos_alpha1b / sin_alpha1b))
                    {
                        sin_alpha1b = sin_alpha1;
                        cos_alpha1b = cos_alpha1;
                    }
                    else if (v < c0 && (iteration > max_iterations ||
                             cos_alpha1 / sin_alpha1 < cos_alpha1a / sin_alpha1a))
                    {
                        sin_alpha1a = sin_alpha1;
                        cos_alpha1a = cos_alpha1;
                    }

                    if (iteration < max_iterations && dv > c0)
                    {
                        CT diff_alpha1 = -v / dv;

                        CT sin_diff_alpha1 = sin(diff_alpha1);
                        CT cos_diff_alpha1 = cos(diff_alpha1);

                        CT nsin_alpha1 = sin_alpha1 * cos_diff_alpha1 +
                            cos_alpha1 * sin_diff_alpha1;

                        if (nsin_alpha1 > c0 && std::abs(diff_alpha1) < pi)
                        {
                            cos_alpha1 = cos_alpha1 * cos_diff_alpha1 - sin_alpha1 * sin_diff_alpha1;
                            sin_alpha1 = nsin_alpha1;
                            math::normalize_unit_vector<CT>(sin_alpha1, cos_alpha1);

                            // In some regimes we don't get quadratic convergence because
                            // slope -> 0. So use convergence conditions based on epsilon
                            // instead of sqrt(epsilon).
                            tripn = std::abs(v) <= c16 * tol0;
                            continue;
                        }
                    }

                    // Either dv was not positive or updated value was outside legal
                    // range. Use the midpoint of the bracket as the next estimate.
                    // This mechanism is not needed for the WGS84 ellipsoid, but it does
                    // catch problems with more eeccentric ellipsoids. Its efficacy is
                    // such for the WGS84 test set with the starting guess set to alp1 =
                    // 90deg:
                    // the WGS84 test set: mean = 5.21, sd = 3.93, max = 24
                    // WGS84 and random input: mean = 4.74, sd = 0.99
                    sin_alpha1 = (sin_alpha1a + sin_alpha1b) / c2;
                    cos_alpha1 = (cos_alpha1a + cos_alpha1b) / c2;
                    math::normalize_unit_vector<CT>(sin_alpha1, cos_alpha1);
                    tripn = false;
                    tripb = (std::abs(sin_alpha1a - sin_alpha1) + (cos_alpha1a - cos_alpha1) < tol_bisection ||
                             std::abs(sin_alpha1 - sin_alpha1b) + (cos_alpha1 - cos_alpha1b) < tol_bisection);
                }

                CT dummy;
                se::coeffs_C1<SeriesOrder, CT> const coeffs_C1_eps(eps);
                // Ensure that the reduced length and geodesic scale are computed in
                // a "canonical" way, with the I2 integral.
                meridian_length(eps, ep2, sigma12, sin_sigma1, cos_sigma1, dn1,
                                                   sin_sigma2, cos_sigma2, dn2,
                                                   cos_beta1, cos_beta2, s12x,
                                                   m12x, dummy, result.geodesic_scale,
                                                   M21, coeffs_C1_eps);

                m12x *= b;
                s12x *= b;
            }
        }

        if (swap_point < 0)
        {
            swap(sin_alpha1, sin_alpha2);
            swap(cos_alpha1, cos_alpha2);
            swap(result.geodesic_scale, M21);
        }

        sin_alpha1 *= swap_point * lon12_sign;
        cos_alpha1 *= swap_point * lat_sign;

        sin_alpha2 *= swap_point * lon12_sign;
        cos_alpha2 *= swap_point * lat_sign;

        if (BOOST_GEOMETRY_CONDITION(EnableReducedLength))
        {
            result.reduced_length = m12x;
        }

        if (BOOST_GEOMETRY_CONDITION(CalcAzimuths))
        {
            if (BOOST_GEOMETRY_CONDITION(CalcFwdAzimuth))
            {
                result.azimuth = atan2(sin_alpha1, cos_alpha1) * r2d;
            }

            if (BOOST_GEOMETRY_CONDITION(CalcRevAzimuth))
            {
                result.reverse_azimuth = atan2(sin_alpha2, cos_alpha2) * r2d;
            }
        }

        if (BOOST_GEOMETRY_CONDITION(EnableDistance))
        {
            result.distance = s12x;
        }

        return result;
    }

    template <typename CoeffsC1>
    static inline void meridian_length(CT const& epsilon, CT const& ep2, CT const& sigma12,
                                       CT const& sin_sigma1, CT const& cos_sigma1, CT const& dn1,
                                       CT const& sin_sigma2, CT const& cos_sigma2, CT const& dn2,
                                       CT const& cos_beta1, CT const& cos_beta2,
                                       CT& s12x, CT& m12x, CT& m0,
                                       CT& M12, CT& M21,
                                       CoeffsC1 const& coeffs_C1)
    {
        static CT const c1 = 1;

        CT A12x = 0, J12 = 0;
        CT expansion_A1, expansion_A2;

        // Evaluate the coefficients for C2.
        se::coeffs_C2<SeriesOrder, CT> coeffs_C2(epsilon);

        if (BOOST_GEOMETRY_CONDITION(EnableDistance) ||
            BOOST_GEOMETRY_CONDITION(EnableReducedLength) ||
            BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
        {
            // Find the coefficients for A1 by computing the
            // series expansion using Horner scehme.
            expansion_A1 = se::evaluate_A1<SeriesOrder>(epsilon);

            if (BOOST_GEOMETRY_CONDITION(EnableReducedLength) ||
                BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
            {
                // Find the coefficients for A2 by computing the
                // series expansion using Horner scehme.
                expansion_A2 = se::evaluate_A2<SeriesOrder>(epsilon);

                A12x = expansion_A1 - expansion_A2;
                expansion_A2 += c1;
            }
            expansion_A1 += c1;
        }

        if (BOOST_GEOMETRY_CONDITION(EnableDistance))
        {
            CT B1 = se::sin_cos_series(sin_sigma2, cos_sigma2, coeffs_C1)
                  - se::sin_cos_series(sin_sigma1, cos_sigma1, coeffs_C1);

            s12x = expansion_A1 * (sigma12 + B1);

            if (BOOST_GEOMETRY_CONDITION(EnableReducedLength) ||
                BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
            {
                CT B2 = se::sin_cos_series(sin_sigma2, cos_sigma2, coeffs_C2)
                      - se::sin_cos_series(sin_sigma1, cos_sigma1, coeffs_C2);

                J12 = A12x * sigma12 + (expansion_A1 * B1 - expansion_A2 * B2);
            }
        }
        else if (BOOST_GEOMETRY_CONDITION(EnableReducedLength) ||
                 BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
        {
            for (size_t i = 1; i <= SeriesOrder; ++i)
            {
                coeffs_C2[i] = expansion_A1 * coeffs_C1[i] -
                               expansion_A2 * coeffs_C2[i];
            }

            J12 = A12x * sigma12 +
                   (se::sin_cos_series(sin_sigma2, cos_sigma2, coeffs_C2)
                  - se::sin_cos_series(sin_sigma1, cos_sigma1, coeffs_C2));
        }

        if (BOOST_GEOMETRY_CONDITION(EnableReducedLength))
        {
            m0 = A12x;

            m12x = dn2 * (cos_sigma1 * sin_sigma2) -
                   dn1 * (sin_sigma1 * cos_sigma2) -
                   cos_sigma1 * cos_sigma2 * J12;
        }

        if (BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
        {
            CT cos_sigma12 = cos_sigma1 * cos_sigma2 + sin_sigma1 * sin_sigma2;
            CT t = ep2 * (cos_beta1 - cos_beta2) *
                         (cos_beta1 + cos_beta2) / (dn1 + dn2);

            M12 = cos_sigma12 + (t * sin_sigma2 - cos_sigma2 * J12) * sin_sigma1 / dn1;
            M21 = cos_sigma12 - (t * sin_sigma1 - cos_sigma1 * J12) * sin_sigma2 / dn2;
        }
    }

    /*
     Return a starting point for Newton's method in sin_alpha1 and
     cos_alpha1 (function value is -1). If Newton's method
     doesn't need to be used, return also sin_alpha2 and
     cos_alpha2 and function value is sig12.
    */
    template <typename CoeffsC1>
    static inline CT newton_start(CT const& sin_beta1, CT const& cos_beta1, CT const& dn1,
                                  CT const& sin_beta2, CT const& cos_beta2, CT dn2,
                                  CT const& lam12, CT const& sin_lam12, CT const& cos_lam12,
                                  CT& sin_alpha1, CT& cos_alpha1,
                                  CT& sin_alpha2, CT& cos_alpha2,
                                  CT& dnm, CoeffsC1 const& coeffs_C1, CT const& ep2,
                                  CT const& tol1, CT const& tol2, CT const& etol2, CT const& n, CT const& f)
    {
        static CT const c0 = 0;
        static CT const c0_01 = 0.01;
        static CT const c0_1 = 0.1;
        static CT const c0_5 = 0.5;
        static CT const c1 = 1;
        static CT const c2 = 2;
        static CT const c6 = 6;
        static CT const c1000 = 1000;
        static CT const pi = math::pi<CT>();

        CT const one_minus_f = c1 - f;
        CT const x_thresh = c1000 * tol2;

        // Return a starting point for Newton's method in sin_alpha1
        // and cos_alpha1 (function value is -1). If Newton's method
        // doesn't need to be used, return also sin_alpha2 and
        // cos_alpha2 and function value is sig12.
        CT sig12 = -c1;

        // bet12 = bet2 - bet1 in [0, pi); beta12a = bet2 + bet1 in (-pi, 0]
        CT sin_beta12 = sin_beta2 * cos_beta1 - cos_beta2 * sin_beta1;
        CT cos_beta12 = cos_beta2 * cos_beta1 + sin_beta2 * sin_beta1;

        CT sin_beta12a = sin_beta2 * cos_beta1 + cos_beta2 * sin_beta1;

        bool shortline = cos_beta12 >= c0 && sin_beta12 < c0_5 &&
            cos_beta2 * lam12 < c0_5;

        CT sin_omega12, cos_omega12;

        if (shortline)
        {
            CT sin_beta_m2 = math::sqr(sin_beta1 + sin_beta2);

            sin_beta_m2 /= sin_beta_m2 + math::sqr(cos_beta1 + cos_beta2);
            dnm = math::sqrt(c1 + ep2 * sin_beta_m2);

            CT omega12 = lam12 / (one_minus_f * dnm);

            sin_omega12 = sin(omega12);
            cos_omega12 = cos(omega12);
        }
        else
        {
            sin_omega12 = sin_lam12;
            cos_omega12 = cos_lam12;
        }

        sin_alpha1 = cos_beta2 * sin_omega12;
        cos_alpha1 = cos_omega12 >= c0 ?
            sin_beta12 + cos_beta2 * sin_beta1 * math::sqr(sin_omega12) / (c1 + cos_omega12) :
            sin_beta12a - cos_beta2 * sin_beta1 * math::sqr(sin_omega12) / (c1 - cos_omega12);

        CT sin_sigma12 = boost::math::hypot(sin_alpha1, cos_alpha1);
        CT cos_sigma12 = sin_beta1 * sin_beta2 + cos_beta1 * cos_beta2 * cos_omega12;

        if (shortline && sin_sigma12 < etol2)
        {
            sin_alpha2 = cos_beta1 * sin_omega12;
            cos_alpha2 = sin_beta12 - cos_beta1 * sin_beta2 *
                (cos_omega12 >= c0 ? math::sqr(sin_omega12) /
                (c1 + cos_omega12) : c1 - cos_omega12);

            math::normalize_unit_vector<CT>(sin_alpha2, cos_alpha2);
            // Set return value.
            sig12 = atan2(sin_sigma12, cos_sigma12);
        }
        // Skip astroid calculation if too eccentric.
        else if (std::abs(n) > c0_1 ||
                 cos_sigma12 >= c0 ||
                 sin_sigma12 >= c6 * std::abs(n) * pi *
                 math::sqr(cos_beta1))
        {
            // Nothing to do, zeroth order spherical approximation will do.
        }
        else
        {
            // Scale lam12 and bet2 to x, y coordinate system where antipodal
            // point is at origin and singular point is at y = 0, x = -1.
            CT lambda_scale, beta_scale;

            CT y;
            volatile CT x;

            CT lam12x = atan2(-sin_lam12, -cos_lam12);
            if (f >= c0)
            {
                CT k2 = math::sqr(sin_beta1) * ep2;
                CT eps = k2 / (c2 * (c1 + sqrt(c1 + k2)) + k2);

                se::coeffs_A3<SeriesOrder, CT> const coeffs_A3(n);

                CT const A3 = math::horner_evaluate(eps, coeffs_A3.begin(), coeffs_A3.end());

                lambda_scale = f * cos_beta1 * A3 * pi;
                beta_scale = lambda_scale * cos_beta1;

                x = lam12x / lambda_scale;
                y = sin_beta12a / beta_scale;
            }
            else
            {
                CT cos_beta12a = cos_beta2 * cos_beta1 - sin_beta2 * sin_beta1;
                CT beta12a = atan2(sin_beta12a, cos_beta12a);

                CT m12b, m0, dummy;
                meridian_length(n, ep2, pi + beta12a,
                                sin_beta1, -cos_beta1, dn1,
                                sin_beta2, cos_beta2, dn2,
                                cos_beta1, cos_beta2, dummy,
                                m12b, m0, dummy, dummy, coeffs_C1);

                x = -c1 + m12b / (cos_beta1 * cos_beta2 * m0 * pi);
                beta_scale = x < -c0_01
                           ? sin_beta12a / x
                           : -f * math::sqr(cos_beta1) * pi;
                lambda_scale = beta_scale / cos_beta1;

                y = lam12x / lambda_scale;
            }

            if (y > -tol1 && x > -c1 - x_thresh)
            {
                // Strip near cut.
                if (f >= c0)
                {
                    sin_alpha1 = (std::min)(c1, -CT(x));
                    cos_alpha1 = - math::sqrt(c1 - math::sqr(sin_alpha1));
                }
                else
                {
                    cos_alpha1 = (std::max)(CT(x > -tol1 ? c0 : -c1), CT(x));
                    sin_alpha1 = math::sqrt(c1 - math::sqr(cos_alpha1));
                }
            }
            else
            {
                // Solve the astroid problem.
                CT k = astroid(CT(x), y);

                CT omega12a = lambda_scale * (f >= c0 ? -x * k /
                    (c1 + k) : -y * (c1 + k) / k);

                sin_omega12 = sin(omega12a);
                cos_omega12 = -cos(omega12a);

                // Update spherical estimate of alpha1 using omgega12 instead of lam12.
                sin_alpha1 = cos_beta2 * sin_omega12;
                cos_alpha1 = sin_beta12a - cos_beta2 * sin_beta1 *
                    math::sqr(sin_omega12) / (c1 - cos_omega12);
            }
        }

        // Sanity check on starting guess. Backwards check allows NaN through.
        if (!(sin_alpha1 <= c0))
        {
            math::normalize_unit_vector<CT>(sin_alpha1, cos_alpha1);
        }
        else
        {
            sin_alpha1 = c1;
            cos_alpha1 = c0;
        }

        return sig12;
    }

    /*
     Solve the astroid problem using the equation:
     κ4 + 2κ3 + (1 − x2 − y 2 )κ2 − 2y 2 κ − y 2 = 0.

     For details, please refer to Eq. (65) in,
     Geodesics on an ellipsoid of revolution, Charles F.F Karney,
     https://arxiv.org/abs/1102.1215
    */
    static inline CT astroid(CT const& x, CT const& y)
    {
        static CT const c0 = 0;
        static CT const c1 = 1;
        static CT const c2 = 2;
        static CT const c3 = 3;
        static CT const c4 = 4;
        static CT const c6 = 6;

        CT k;

        CT p = math::sqr(x);
        CT q = math::sqr(y);
        CT r = (p + q - c1) / c6;

        if (!(q == c0 && r <= c0))
        {
            // Avoid possible division by zero when r = 0 by multiplying
            // equations for s and t by r^3 and r, respectively.
            CT S = p * q / c4;
            CT r2 = math::sqr(r);
            CT r3 = r * r2;

            // The discriminant of the quadratic equation for T3. This is
            // zero on the evolute curve p^(1/3)+q^(1/3) = 1.
            CT discriminant = S * (S + c2 * r3);

            CT u = r;

            if (discriminant >= c0)
            {
                CT T3 = S + r3;

                // Pick the sign on the sqrt to maximize abs(T3). This minimizes
                // loss of precision due to cancellation. The result is unchanged
                // because of the way the T is used in definition of u.
                T3 += T3 < c0 ? -std::sqrt(discriminant) : std::sqrt(discriminant);

                CT T = std::cbrt(T3);

                // T can be zero; but then r2 / T -> 0.
                u += T + (T != c0 ? r2 / T : c0);
            }
            else
            {
                CT ang = std::atan2(std::sqrt(-discriminant), -(S + r3));

                // There are three possible cube roots. We choose the root which avoids
                // cancellation. Note that discriminant < 0 implies that r < 0.
                u += c2 * r * cos(ang / c3);
            }

            CT v = std::sqrt(math::sqr(u) + q);

            // Avoid loss of accuracy when u < 0.
            CT uv = u < c0 ? q / (v - u) : u + v;
            CT w = (uv - q) / (c2 * v);

            // Rearrange expression for k to avoid loss of accuracy due to
            // subtraction. Division by 0 not possible because uv > 0, w >= 0.
            k = uv / (std::sqrt(uv + math::sqr(w)) + w);
        }
        else // q == 0 && r <= 0
        {
            // y = 0 with |x| <= 1. Handle this case directly.
            // For y small, positive root is k = abs(y)/sqrt(1-x^2).
            k = c0;
        }
        return k;
    }

    template <typename CoeffsC1>
    static inline CT lambda12(CT const& sin_beta1, CT const& cos_beta1, CT const& dn1,
                              CT const& sin_beta2, CT const& cos_beta2, CT const& dn2,
                              CT const& sin_alpha1, CT cos_alpha1,
                              CT const& sin_lam120, CT const& cos_lam120,
                              CT& sin_alpha2, CT& cos_alpha2,
                              CT& sigma12,
                              CT& sin_sigma1, CT& cos_sigma1,
                              CT& sin_sigma2, CT& cos_sigma2,
                              CT& eps, CT& diff_omega12,
                              bool diffp, CT& diff_lam12,
                              CT const& f, CT const& n, CT const& ep2, CT const& tiny,
                              CoeffsC1 const& coeffs_C1)
    {
        static CT const c0 = 0;
        static CT const c1 = 1;
        static CT const c2 = 2;

        CT const one_minus_f = c1 - f;

        if (sin_beta1 == c0 && cos_alpha1 == c0)
        {
            // Break degeneracy of equatorial line.
            cos_alpha1 = -tiny;
        }

        CT sin_alpha0 = sin_alpha1 * cos_beta1;
        CT cos_alpha0 = boost::math::hypot(cos_alpha1, sin_alpha1 * sin_beta1);

        CT sin_omega1, cos_omega1;
        CT sin_omega2, cos_omega2;
        CT sin_omega12, cos_omega12;

        CT lam12;

        sin_sigma1 = sin_beta1;
        sin_omega1 = sin_alpha0 * sin_beta1;

        cos_sigma1 = cos_omega1 = cos_alpha1 * cos_beta1;

        math::normalize_unit_vector<CT>(sin_sigma1, cos_sigma1);

        // Enforce symmetries in the case abs(beta2) = -beta1.
        // Otherwise, this can yield singularities in the Newton iteration.

        // sin(alpha2) * cos(beta2) = sin(alpha0).
        sin_alpha2 = cos_beta2 != cos_beta1 ?
            sin_alpha0 / cos_beta2 : sin_alpha1;

        cos_alpha2 = cos_beta2 != cos_beta1 || std::abs(sin_beta2) != -sin_beta1 ?
            sqrt(math::sqr(cos_alpha1 * cos_beta1) +
                (cos_beta1 < -sin_beta1 ?
                    (cos_beta2 - cos_beta1) * (cos_beta1 + cos_beta2) :
                    (sin_beta1 - sin_beta2) * (sin_beta1 + sin_beta2))) / cos_beta2 :
            std::abs(cos_alpha1);

        sin_sigma2 = sin_beta2;
        sin_omega2 = sin_alpha0 * sin_beta2;

        cos_sigma2 = cos_omega2 =
            (cos_alpha2 * cos_beta2);

        // Break degeneracy of equatorial line.
        math::normalize_unit_vector<CT>(sin_sigma2, cos_sigma2);

        // sig12 = sig2 - sig1, limit to [0, pi].
        sigma12 = atan2((std::max)(c0, cos_sigma1 * sin_sigma2 - sin_sigma1 * cos_sigma2),
                                          cos_sigma1 * cos_sigma2 + sin_sigma1 * sin_sigma2);

        // omg12 = omg2 - omg1, limit to [0, pi].
        sin_omega12 = (std::max)(c0, cos_omega1 * sin_omega2 - sin_omega1 * cos_omega2);
        cos_omega12 = cos_omega1 * cos_omega2 + sin_omega1 * sin_omega2;

        // eta = omg12 - lam120.
        CT eta = atan2(sin_omega12 * cos_lam120 - cos_omega12 * sin_lam120,
                       cos_omega12 * cos_lam120 + sin_omega12 * sin_lam120);

        CT B312;
        CT k2 = math::sqr(cos_alpha0) * ep2;

        eps = k2 / (c2 * (c1 + std::sqrt(c1 + k2)) + k2);

        se::coeffs_C3<SeriesOrder, CT> const coeffs_C3(n, eps);

        B312 = se::sin_cos_series(sin_sigma2, cos_sigma2, coeffs_C3)
             - se::sin_cos_series(sin_sigma1, cos_sigma1, coeffs_C3);

        se::coeffs_A3<SeriesOrder, CT> const coeffs_A3(n);

        CT const A3 = math::horner_evaluate(eps, coeffs_A3.begin(), coeffs_A3.end());

        diff_omega12 = -f * A3 * sin_alpha0 * (sigma12 + B312);
        lam12 = eta + diff_omega12;

        if (diffp)
        {
            if (cos_alpha2 == c0)
            {
                diff_lam12 = - c2 * one_minus_f * dn1 / sin_beta1;
            }
            else
            {
                CT dummy;
                meridian_length(eps, ep2, sigma12, sin_sigma1, cos_sigma1, dn1,
                                                   sin_sigma2, cos_sigma2, dn2,
                                                   cos_beta1, cos_beta2, dummy,
                                                   diff_lam12, dummy, dummy,
                                                   dummy, coeffs_C1);

                diff_lam12 *= one_minus_f / (cos_alpha2 * cos_beta2);
            }
        }
        return lam12;
    }

};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_KARNEY_INVERSE_HPP


#include "random.hpp"

#include <boost/predef/other/endian.h>

#if defined (BOOST_ENDIAN_LITTLE_BYTE_AVAILABLE)
#   define PCG_LITTLE_ENDIAN 1
#endif

#include <pcg_random.hpp>
#include <pcg_extras.hpp>

#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_smallint.hpp>
#include <boost/random/normal_distribution.hpp>

#include <random>

namespace boken {

random_state::~random_state() = default;
uint32_t random_state::min() noexcept { return pcg32::min(); }
uint32_t random_state::max() noexcept { return pcg32::max(); }

class random_state_impl final : public random_state {
public:
    random_state_impl() = default;

    result_type generate() noexcept final override;

    boost::random::uniform_smallint<int32_t>         dist_coin    {0, 1};
    boost::random::uniform_int_distribution<int32_t> dist_uniform {};
    boost::random::normal_distribution<>             dist_normal  {};

    pcg32 state {};
};

random_state::result_type random_state_impl::generate() noexcept {
    return state();
}

std::unique_ptr<random_state> make_random_state() {
    return std::make_unique<random_state_impl>();
}

bool random_coin_flip(random_state& rng) noexcept {
    auto& r = reinterpret_cast<random_state_impl&>(rng);
    return !!r.dist_coin(r.state);
}

int32_t random_uniform_int(random_state& rng, int32_t const lo, int32_t const hi) noexcept {
    auto& r = reinterpret_cast<random_state_impl&>(rng);

    using param_t = decltype(r.dist_uniform)::param_type;

    r.dist_uniform.param(param_t {lo, hi});

    return r.dist_uniform(r.state);
}

bool random_chance_in_x(random_state& rng, int32_t const num, int32_t const den) noexcept {
    return random_uniform_int(rng, 0, den - 1) < num;
}

double random_normal(random_state& rs, double const m, double const v) noexcept {
    auto& r = reinterpret_cast<random_state_impl&>(rs);

    using param_t = decltype(r.dist_normal)::param_type;

    r.dist_normal.param(param_t {m, v});

    return r.dist_normal(r.state);
}

uint32_t random_color(random_state& rng) noexcept {
    auto const random_color_comp = [&]() noexcept {
        return static_cast<uint32_t>(random_uniform_int(rng, 0, 255));
    };

    return 0xFFu               << 24
         | random_color_comp() << 16
         | random_color_comp() << 8
         | random_color_comp() << 0;
}

} //namespace boken


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