//
// Created by green on 04/12/17.
//
#include <iostream>     // std::cout, std::fixed
#include <iomanip>      // std::setprecision

#include "inc/Modules/LocationModule.h"
#include <boost/lexical_cast.hpp>
#include <lapacke.h>
#include "inc/Exceptions/UnsupportedOperationException.h"

using boost::lexical_cast;

// I think nodes should be the scanned nodes m_ScannedNodes
LocationModule::LocationModule(std::shared_ptr<node::NodeContainer>& nodes) : m_Nodes(nodes),
                                                                              m_dgelsLoc(std::make_shared<Location>()),
                                                                              m_dgetrsLoc(std::make_shared<Location>()),
                                                                              m_dgesvLoc(std::make_shared<Location>())
{

}

void LocationModule::CalculateLocations()
{
    boost::mutex::scoped_lock lock(g_i_mutex);
    try
    {
      // I think m_Nodes should be the nodes read from Settings.json by the FileNodeReaderModule
      calculateDgels(GetNodeTargets(m_Nodes)); // m_Nodes is a container of AP nodes, with known (x,y,z) coordinates

      calculateDgesvDgetrs(GetNodeTargets(m_Nodes));
      std::cerr << std::endl; // skip a line
    }
    catch (std::exception e)
    {
      //std::cout << "Not Enough Nodes in Range." << std::endl;
      // UnsupportedOperationException ex;
      // throw(ex);
    }

}

std::shared_ptr<Location> LocationModule::GetLocation()
{
    CalculateLocations();
    return m_dgelsLoc;
}

std::shared_ptr<node::NodeContainer> LocationModule::GetNodeTargets(std::shared_ptr<node::NodeContainer> tempNodes)
{
  // I think tempNodes should be the nodes read from Settings.json by the FileNodeReaderModule

    if (tempNodes == NULL)
    {
        UnsupportedOperationException ex;
        throw (ex);
    }
    std::shared_ptr<node::NodeContainer> targetNodes = std::make_shared<node::NodeContainer>();

    std::sort(tempNodes->GetNodes().begin(), tempNodes->GetNodes().end(),
              [](std::shared_ptr<INode> &lhs, std::shared_ptr<INode> &rhs)
              {
                  return lhs->getRSSI() > rhs->getRSSI();
              });
    for (auto &i : tempNodes->GetNodes())
    {
      // std::cerr << "LocationModule::GetNodeTargets: tempNode: " << i->getSSID() << std::endl;

      if (targetNodes->GetNodes().size() < 4 && i->getRSSI() != 0 && i->getRecentlyUpdated())
        {
	  // add node to use in LSQ location calculation
	  targetNodes->AddNode(i);
        }
    }
    for (auto &i : targetNodes->GetNodes())
    {
      // std::cout << "RSSI ORDERING (SSID,RSSI,MAC): " << i->getSSID() << ", " << i->getRSSI() << ", " << i->getMAC() << std::endl;
    }
    if (targetNodes->GetNodes().size() < 4)
    {
        std::exception e;
        throw (e);
    }
    return targetNodes;
}

web::json::value LocationModule::GetJson()
{
    CalculateLocations();
    web::json::value response;
    response["DGESV"] = m_dgesvLoc->ToJson();
    response["DGETRS"] = m_dgetrsLoc->ToJson();
    response["DGELS"] = m_dgelsLoc->ToJson();
    response["TargetNodes"] = GetNodeTargets(m_Nodes)->ToJson();
    return response;
}

web::json::value LocationModule::BasicJson()
{
    CalculateLocations();
    web::json::value response;
    response["DGESV"] = m_dgesvLoc->ToJson();
    response["DGETRS"] = m_dgetrsLoc->ToJson();
    response["DGELS"] = m_dgelsLoc->ToJson();
    response = m_dgelsLoc->ToJson();
    return response;
}

void LocationModule::calculateDgesvDgetrs(std::shared_ptr<node::NodeContainer> nodes)
{
    if (nodes->GetNodes().size() != 4)
    {
        throw ("Error::Too few Nodes!");
    }

    std::vector<std::shared_ptr<TargetNode>> Nodes;

    for (std::shared_ptr<INode> a: nodes->GetNodes())
    {
        Nodes.push_back(std::static_pointer_cast<TargetNode>(a));
    }

    double A[3][3] = {2 * (Nodes[1]->getXCoord() - Nodes[0]->getXCoord()),
                      2 * (Nodes[1]->getYCoord() - Nodes[0]->getYCoord()),
                      2 * (Nodes[1]->getZCoord() - Nodes[0]->getZCoord()),
                      2 * (Nodes[2]->getXCoord() - Nodes[0]->getXCoord()),
                      2 * (Nodes[2]->getYCoord() - Nodes[0]->getYCoord()),
                      2 * (Nodes[2]->getZCoord() - Nodes[0]->getZCoord()),
                      2 * (Nodes[3]->getXCoord() - Nodes[0]->getXCoord()),
                      2 * (Nodes[3]->getYCoord() - Nodes[0]->getYCoord()),
                      2 * (Nodes[3]->getZCoord() - Nodes[0]->getZCoord())};

    double B[3][1] = {(pow(Nodes[0]->GetDistance(), 2.0)) - (pow(Nodes[1]->GetDistance(), 2.0)) -
                      (pow(Nodes[0]->getXCoord(), 2.0)) + (pow(Nodes[1]->getXCoord(), 2.0)) -
                      (pow(Nodes[0]->getYCoord(), 2.0)) + (pow(Nodes[1]->getYCoord(), 2.0)) -
                      (pow(Nodes[0]->getZCoord(), 2.0)) + (pow(Nodes[1]->getZCoord(), 2.0)),
                      (pow(Nodes[0]->GetDistance(), 2.0)) - (pow(Nodes[2]->GetDistance(), 2.0)) -
                      (pow(Nodes[0]->getXCoord(), 2.0)) + (pow(Nodes[2]->getXCoord(), 2.0)) -
                      (pow(Nodes[0]->getYCoord(), 2.0)) + (pow(Nodes[2]->getYCoord(), 2.0)) -
                      (pow(Nodes[0]->getZCoord(), 2.0)) + (pow(Nodes[2]->getZCoord(), 2.0)),
                      (pow(Nodes[0]->GetDistance(), 2.0)) - (pow(Nodes[3]->GetDistance(), 2.0)) -
                      (pow(Nodes[0]->getXCoord(), 2.0)) + (pow(Nodes[3]->getXCoord(), 2.0)) -
                      (pow(Nodes[0]->getYCoord(), 2.0)) + (pow(Nodes[3]->getYCoord(), 2.0)) -
                      (pow(Nodes[0]->getZCoord(), 2.0)) + (pow(Nodes[3]->getZCoord(), 2.0))};

    lapack_int info, m, n, lda, ldb, nrhs;
    lapack_int *ipiv;
    m = 3;
    n = 3;
    nrhs = 1;
    lda = 3;
    ldb = 1;
    ipiv = (lapack_int *) malloc(n * sizeof(lapack_int));
    boost::timer::auto_cpu_timer t;
    info = LAPACKE_dgesv(LAPACK_ROW_MAJOR, n, nrhs, *A, lda, ipiv, *B, ldb);
    t.stop();

    if (info > 0)
    {
        printf("The diagonal element %i of the triangular factor ", info);
        printf("of A is zero, so that A does not have full rank;\n");
        printf("the least squares solution could not be computed.\n");
        return;
    }

    // printf("Calcuated values.");
    // fflush(stdout);
    m_dgesvLoc->updateCoords(B[0][0], B[1][0], B[2][0]);
    /*m_dgesvLoc->xCoord=(B[0][0]);
    m_dgesvLoc->yCoord=(B[1][0]);
    m_dgesvLoc->zCoord=(B[2][0]);*/
    m_dgesvLoc->m_calculationTime = t.elapsed().wall;

    std::cerr << "Calculated Location (dgesv): ";
    std::cerr << std::fixed << std::setprecision(4);
    std::cerr <<  "X:" << lexical_cast<std::string>(B[0][0]) << " Y:" << lexical_cast<std::string>(B[1][0]) << " Z:"
              << lexical_cast<std::string>(B[2][0]) << " Time:"
              << lexical_cast<std::string>(m_dgesvLoc->m_calculationTime) << std::endl;

    double B2[3][1] = {(pow(Nodes[0]->GetDistance(), 2.0)) - (pow(Nodes[1]->GetDistance(), 2.0)) -
                       (pow(Nodes[0]->getXCoord(), 2.0)) + (pow(Nodes[1]->getXCoord(), 2.0)) -
                       (pow(Nodes[0]->getYCoord(), 2.0)) + (pow(Nodes[1]->getYCoord(), 2.0)) -
                       (pow(Nodes[0]->getZCoord(), 2.0)) + (pow(Nodes[1]->getZCoord(), 2.0)),
                       (pow(Nodes[0]->GetDistance(), 2.0)) - (pow(Nodes[2]->GetDistance(), 2.0)) -
                       (pow(Nodes[0]->getXCoord(), 2.0)) + (pow(Nodes[2]->getXCoord(), 2.0)) -
                       (pow(Nodes[0]->getYCoord(), 2.0)) + (pow(Nodes[2]->getYCoord(), 2.0)) -
                       (pow(Nodes[0]->getZCoord(), 2.0)) + (pow(Nodes[2]->getZCoord(), 2.0)),
                       (pow(Nodes[0]->GetDistance(), 2.0)) - (pow(Nodes[3]->GetDistance(), 2.0)) -
                       (pow(Nodes[0]->getXCoord(), 2.0)) + (pow(Nodes[3]->getXCoord(), 2.0)) -
                       (pow(Nodes[0]->getYCoord(), 2.0)) + (pow(Nodes[3]->getYCoord(), 2.0)) -
                       (pow(Nodes[0]->getZCoord(), 2.0)) + (pow(Nodes[3]->getZCoord(), 2.0))};

    t.resume();
    LAPACKE_dgetrs(LAPACK_ROW_MAJOR, 'N', n, nrhs, *A, lda, ipiv, *B2, ldb);

    if (info > 0)
    {
        printf("The diagonal element %i of the triangular factor ", info);
        printf("of A is zero, so that A does not have full rank;\n");
        printf("the least squares solution could not be computed.\n");
        return;
    }

    t.stop();
    m_dgetrsLoc->updateCoords(B2[0][0], B2[1][0], B2[2][0]);
    /*m_dgetrsLoc->xCoord=(B2[0][0]);
    m_dgetrsLoc->yCoord=(B2[1][0]);
    m_dgetrsLoc->zCoord=(B2[2][0]);*/

    m_dgetrsLoc->m_calculationTime = t.elapsed().wall - m_dgesvLoc->m_calculationTime;

    std::cerr << "Calculated Location (dgetrs): ";
    std::cerr << std::setprecision(4) << "X:" << lexical_cast<std::string>(B2[0][0]) << " Y:" << lexical_cast<std::string>(B2[1][0]) << " Z:"
              << lexical_cast<std::string>(B2[2][0]) << " Time:"
              << lexical_cast<std::string>(m_dgetrsLoc->m_calculationTime) << std::endl;

    return;
}

/*
 * LocationModule::calculateDgels
 *
 * Input: nodes is a container of access point nodes, with known (x,y,z) coordinates
 *
 */
void LocationModule::calculateDgels(std::shared_ptr<node::NodeContainer> nodes)
{
    if (nodes->GetNodes().size() != 4)
    {
        throw ("Error::Too few Nodes!");
    }

    std::vector<std::shared_ptr<TargetNode>> Nodes; // TargetNodes with known (x,y,z) coordinates

    for (std::shared_ptr<INode> a: nodes->GetNodes())
    {
	// I think we need to get the corresponding node from the TargetNodes container
        Nodes.push_back(std::static_pointer_cast<TargetNode>(a));
    }

    double A[3][3] = {2 * (Nodes[1]->getXCoord() - Nodes[0]->getXCoord()),
                      2 * (Nodes[1]->getYCoord() - Nodes[0]->getYCoord()),
                      2 * (Nodes[1]->getZCoord() - Nodes[0]->getZCoord()),
                      2 * (Nodes[2]->getXCoord() - Nodes[0]->getXCoord()),
                      2 * (Nodes[2]->getYCoord() - Nodes[0]->getYCoord()),
                      2 * (Nodes[2]->getZCoord() - Nodes[0]->getZCoord()),
                      2 * (Nodes[3]->getXCoord() - Nodes[0]->getXCoord()),
                      2 * (Nodes[3]->getYCoord() - Nodes[0]->getYCoord()),
                      2 * (Nodes[3]->getZCoord() - Nodes[0]->getZCoord())};

    double B[3][1] = {(pow(Nodes[0]->GetDistance(), 2.0)) - (pow(Nodes[1]->GetDistance(), 2.0)) -
                      (pow(Nodes[0]->getXCoord(), 2.0)) + (pow(Nodes[1]->getXCoord(), 2.0)) -
                      (pow(Nodes[0]->getYCoord(), 2.0)) + (pow(Nodes[1]->getYCoord(), 2.0)) -
                      (pow(Nodes[0]->getZCoord(), 2.0)) + (pow(Nodes[1]->getZCoord(), 2.0)),
                      (pow(Nodes[0]->GetDistance(), 2.0)) - (pow(Nodes[2]->GetDistance(), 2.0)) -
                      (pow(Nodes[0]->getXCoord(), 2.0)) + (pow(Nodes[2]->getXCoord(), 2.0)) -
                      (pow(Nodes[0]->getYCoord(), 2.0)) + (pow(Nodes[2]->getYCoord(), 2.0)) -
                      (pow(Nodes[0]->getZCoord(), 2.0)) + (pow(Nodes[2]->getZCoord(), 2.0)),
                      (pow(Nodes[0]->GetDistance(), 2.0)) - (pow(Nodes[3]->GetDistance(), 2.0)) -
                      (pow(Nodes[0]->getXCoord(), 2.0)) + (pow(Nodes[3]->getXCoord(), 2.0)) -
                      (pow(Nodes[0]->getYCoord(), 2.0)) + (pow(Nodes[3]->getYCoord(), 2.0)) -
                      (pow(Nodes[0]->getZCoord(), 2.0)) + (pow(Nodes[3]->getZCoord(), 2.0))};

    lapack_int info, m, n, lda, ldb, nrhs;

    m = 3;
    n = 3;
    nrhs = 1;
    lda = 3;
    ldb = 1;
    boost::timer::auto_cpu_timer t;
    info = LAPACKE_dgels(LAPACK_ROW_MAJOR, 'N', m, n, nrhs, *A, lda, *B, ldb);
    t.stop();
    if (info > 0)
    {
        printf("The diagonal element %i of the triangular factor ", info);
        printf("of A is zero, so that A does not have full rank;\n");
        printf("the least squares solution could not be computed.\n");
        return;
    }

    m_dgelsLoc->updateCoords(B[0][0], B[1][0], B[2][0]);
    m_dgelsLoc->m_calculationTime = t.elapsed().wall;

    std::cerr << "Calculated Location (dgels): ";
    std::cerr << std::setprecision(5) << "X:" << lexical_cast<std::string>(B[0][0]) << " Y:" << lexical_cast<std::string>(B[1][0]) << " Z:"
              << lexical_cast<std::string>(B[2][0]) << " Time:"
              << lexical_cast<std::string>(m_dgelsLoc->m_calculationTime) << std::endl;

    return;

}

void LocationModule::initialize()
{
    m_isRunning = true;
}

void LocationModule::deInitialize()
{
    m_isRunning = false;
}

bool LocationModule::isRunning()
{
    return m_isRunning;
}


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


#include "mesh/mechanics/beam/submesh.hpp"

#include "geometry/profile_factory.hpp"
#include "math/jacobian_determinant.hpp"
#include "mesh/dof_allocator.hpp"
#include "interpolations/interpolation_factory.hpp"
#include "numeric/float_compare.hpp"
#include "io/json.hpp"

#include <tbb/parallel_for.h>
#include <Eigen/Geometry>

#include <iostream>

namespace neon::mechanics::beam
{
submesh::submesh(json const& material_data,
                 json const& simulation_data,
                 json const& section_data,
                 std::shared_ptr<material_coordinates>& coordinates,
                 basic_submesh const& submesh)
    : basic_submesh(submesh),
      sf(make_line_interpolation(topology(), simulation_data)),
      coordinates(coordinates),
      view(sf->quadrature().points()),
      variables(std::make_shared<internal_variable_type>(elements() * sf->quadrature().points())),
      cm(std::make_unique<isotropic_linear>(variables, material_data))
{
    allocate_normal_and_tangent(section_data);

    dof_allocator(node_indices, dof_indices, traits::dofs_per_node);

    variables->add(variable::second::cauchy_stress,
                   variable::scalar::shear_area_1,
                   variable::scalar::shear_area_2,
                   variable::scalar::cross_sectional_area,
                   variable::scalar::second_moment_area_1,
                   variable::scalar::second_moment_area_2);

    profile = geometry::make_profile(
        json{{"name", "rect"}, {"type", "rectangle"}, {"width", 1.0}, {"height", 1.0}});
}

void submesh::update_internal_variables(double)
{
    auto& A = variables->get(variable::scalar::cross_sectional_area);
    auto& As1 = variables->get(variable::scalar::shear_area_1);
    auto& As2 = variables->get(variable::scalar::shear_area_2);

    auto& area_moment_1 = variables->get(variable::scalar::second_moment_area_1);
    auto& area_moment_2 = variables->get(variable::scalar::second_moment_area_2);

    // Loop over all elements and quadrature points to compute the profile
    // properties for each quadrature point in the beam
    tbb::parallel_for(std::int64_t{0}, elements(), [&, this](auto const element) {
        sf->quadrature().for_each([&, this](auto const&, auto const l) {
            A.at(view(element, l)) = profile->area();

            auto const [shear_area_1, shear_area_2] = profile->shear_area();
            As1.at(view(element, l)) = shear_area_1;
            As2.at(view(element, l)) = shear_area_2;

            auto const [I1, I2] = profile->second_moment_area();
            area_moment_1.at(view(element, l)) = I1;
            area_moment_2.at(view(element, l)) = I2;
        });
    });
    cm->update_internal_variables();
}

auto submesh::tangent_stiffness(std::int32_t const element) const -> matrix const&
{
    static thread_local matrix ke(12, 12);

    auto const& configuration = coordinates->initial_configuration(local_node_view(element));

    ke = rotation_matrix
         * (bending_stiffness(configuration, element) + shear_stiffness(configuration, element)
            + axial_stiffness(configuration, element) + torsional_stiffness(configuration, element))
         * rotation_matrix.transpose();

    return ke;
}

matrix const& submesh::bending_stiffness(matrix3x const& configuration, std::int32_t const element) const
{
    static thread_local matrix2x B_bending(2, 6 * sf->number_of_nodes());
    static thread_local matrix k_bending(6 * sf->number_of_nodes(), 6 * sf->number_of_nodes());

    B_bending.setZero();
    k_bending.setZero();

    auto const& D_bending = variables->get(variable::second::bending_stiffness);

    sf->quadrature().integrate_inplace(k_bending, [&, this](auto const& femval, auto const l) {
        auto const& [N, dN] = femval;

        double const j = jacobian_determinant(configuration * dN);

        for (int i = 0; i < sf->number_of_nodes(); ++i)
        {
            auto const offset = i * 6;

            B_bending(0, 3 + offset) = B_bending(1, 4 + offset) = dN(i, l) / j;
        }

        return B_bending.transpose() * D_bending.at(view(element, l)) * B_bending * j;
    });
    return k_bending;
}

matrix const& submesh::shear_stiffness(matrix3x const& configuration, std::int32_t const element) const
{
    static thread_local matrix2x B_shear(2, 6 * sf->number_of_nodes());
    static thread_local matrix k_shear(6 * sf->number_of_nodes(), 6 * sf->number_of_nodes());

    B_shear.setZero();
    k_shear.setZero();

    auto const& D_shear = variables->get(variable::second::shear_stiffness);

    sf->quadrature().integrate_inplace(k_shear, [&, this](auto const& femval, auto const l) {
        auto const& [N, dN] = femval;

        auto const j = jacobian_determinant(configuration * dN);

        for (int i = 0; i < sf->number_of_nodes(); ++i)
        {
            auto const offset = i * 6;

            B_shear(0, 0 + offset) = B_shear(1, 1 + offset) = dN(i, l) / j;

            B_shear(0, 4 + offset) = -N(i, l);
            B_shear(1, 3 + offset) = N(i, l);
        }
        return B_shear.transpose() * D_shear.at(view(element, l)) * B_shear * j;
    });
    return k_shear;
}

matrix const& submesh::axial_stiffness(matrix3x const& configuration, std::int32_t const element) const
{
    static thread_local vector B_axial(6 * sf->number_of_nodes());
    static thread_local matrix k_axial(6 * sf->number_of_nodes(), 6 * sf->number_of_nodes());

    B_axial.setZero();
    k_axial.setZero();

    auto const D_axial = variables->get(variable::scalar::axial_stiffness);

    sf->quadrature().integrate_inplace(k_axial, [&, this](auto const& femval, auto const l) {
        auto const& [N, dN] = femval;

        auto const j = jacobian_determinant(configuration * dN);

        for (int i = 0; i < sf->number_of_nodes(); ++i)
        {
            auto const offset = i * 6;

            B_axial(2 + offset) = dN(i, l) / j;
        }

        return B_axial * D_axial.at(view(element, l)) * B_axial.transpose() * j;
    });
    return k_axial;
}

matrix const& submesh::torsional_stiffness(matrix3x const& configuration,
                                           std::int32_t const element) const
{
    static thread_local vector B_torsion(6 * sf->number_of_nodes());
    static thread_local matrix k_torsion(6 * sf->number_of_nodes(), 6 * sf->number_of_nodes());

    B_torsion.setZero();
    k_torsion.setZero();

    auto const D_torsion = variables->get(variable::scalar::torsional_stiffness);

    sf->quadrature().integrate_inplace(k_torsion, [&, this](auto const& femval, auto const l) {
        auto const& [N, dN] = femval;

        auto const j = jacobian_determinant(configuration * dN);

        for (int i = 0; i < sf->number_of_nodes(); ++i)
        {
            auto const offset = i * 6;

            B_torsion(5 + offset) = dN(i, l) / j;
        }

        return B_torsion * D_torsion.at(view(element, l)) * B_torsion.transpose() * j;
    });
    return k_torsion;
}

void submesh::allocate_normal_and_tangent(json const& section_data)
{
    if (section_data.find("tangent") == section_data.end())
    {
        throw std::domain_error("A \"tangent\" vector must be specified in the \"section\"");
    }
    if (section_data.find("normal") == section_data.end())
    {
        throw std::domain_error("A \"normal\" vector must be specified in the \"section\"");
    }

    auto const& tangent_vector = section_data["tangent"];

    if (!tangent_vector.is_array() || tangent_vector.size() != 3)
    {
        throw std::domain_error("A \"tangent\" vector must be specified using three (3) "
                                "coordinates [x, y, z]");
    }

    auto const& normal_vector = section_data["normal"];

    if (!normal_vector.is_array() || normal_vector.size() != 3)
    {
        throw std::domain_error("A \"normal\" vector must be specified using three (3) coordinates "
                                "[x, y, z]");
    }

    tangent(0) = tangent_vector[0];
    tangent(1) = tangent_vector[1];
    tangent(2) = tangent_vector[2];

    normal(0) = normal_vector[0];
    normal(1) = normal_vector[1];
    normal(2) = normal_vector[2];

    if (!is_approx(normal.dot(tangent), 0.0))
    {
        throw std::domain_error("normal and tangent vectors must be orthogonal");
    }

    matrix3 rotation;

    // Local coordinate (x1)
    rotation.col(0) = normal;
    // Local coordinate (x2)
    rotation.col(1) = normal.cross(tangent);
    // Local coordinate (x3)
    rotation.col(2) = tangent;

    rotation_matrix = matrix12::Zero();

    rotation_matrix.block<3, 3>(0, 0) = rotation;
    rotation_matrix.block<3, 3>(3, 3) = rotation;
    rotation_matrix.block<3, 3>(6, 6) = rotation;
    rotation_matrix.block<3, 3>(9, 9) = rotation;
}
}


#pragma once
#include <type_traits>
#include <vector>
#include <string>
#include <array>
#include <exception>
#include <boost/multiprecision/cpp_int.hpp>
#include "keys.hpp"
#include "stream_handler.hpp"

namespace mp = boost::multiprecision;

namespace rsa {

    template<size_t E, size_t D>
    struct cipher {

        template<
                typename Handler,
                typename = std::enable_if_t<has_operation_v<Handler, read_number_padding_operation> && has_operation_v<Handler, eof_operation>>>
        static auto encrypt(Handler& handler, const keys<D * 8>& keys, unsigned int& n) {
            std::vector<typename num_utils<D * 8>::number> result;
            while (!handler.eof())
                result.push_back(mp::powm(
                        static_cast<typename num_utils<D * 8>::number>(handler.template read_number_padding<E * 8>(n)),
                        keys.get_e(), keys.get_n()));
            return result;
        }

        template<
                typename Handler,
                typename = std::enable_if_t<has_operation_v<Handler, read_number_operation> && has_operation_v<Handler, eof_operation>>>
        static auto decrypt(Handler& handler, const keys<D * 8>& keys) {
            std::vector<typename num_utils<E * 8>::number> result;
            while (!handler.eof())
                result.push_back(static_cast<typename num_utils<E * 8>::number>(mp::powm(
                        static_cast<typename num_utils<D * 8>::number>(handler.template read_number<D * 8>()),
                        keys.get_d(), keys.get_n())));
            return result;
        }

    };

}// namespace rsa


/*
 *
 * Copyright (c) Toon Knapen, Karl Meerbergen & Kresimir Fresl 2003
 *
 * Permission to copy, modify, use and distribute this software
 * for any non-commercial or commercial purpose is granted provided
 * that this license appear on all copies of the software source code.
 *
 * Authors assume no responsibility whatsoever for its use and makes
 * no guarantees about its quality, correctness or reliability.
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_HEEV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_HEEV_HPP

#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/type.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>
// #include <boost/numeric/bindings/traits/std_vector.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    //
    // Eigendecomposition of a complex Hermitian matrix A = Q * D * Q'
    //
    ///////////////////////////////////////////////////////////////////

    /*
     * heev() computes the eigendecomposition of a N x N matrix
     * A = Q * D * Q',  where Q is a N x N unitary matrix and
     * D is a diagonal matrix. The diagonal element D(i,i) is an
     * eigenvalue of A and Q(:,i) is a corresponding eigenvector.
     * The eigenvalues are stored in ascending order.
     *
     * On return of heev, A is overwritten by Q and w contains the main
     * diagonal of D.
     *
     * int heev (char jobz, char uplo, A& a, W& w, minimal_workspace ) ;
     *    jobz : 'V' : compute eigenvectors
     *           'N' : do not compute eigenvectors
     *    uplo : 'U' : only the upper triangular part of A is used on input.
     *           'L' : only the lower triangular part of A is used on input.
     */

    namespace detail {

      inline
      void heev (char const jobz, char const uplo, int const n,
		 traits::complex_f* a, int const lda,
                 float* w, traits::complex_f* work, int const lwork,
                 float* rwork, int& info)
      {
        LAPACK_CHEEV (&jobz, &uplo, &n,
		      reinterpret_cast<fcomplex_t*>(a), &lda, w,
		      reinterpret_cast<fcomplex_t*>(work), &lwork,
		      rwork, &info);
      }

      inline
      void heev (char const jobz, char const uplo, int const n,
		 traits::complex_d* a, int const lda,
                 double* w, traits::complex_d* work, int const lwork,
                 double* rwork, int& info)
      {
        LAPACK_ZHEEV (&jobz, &uplo, &n,
		      reinterpret_cast<dcomplex_t*>(a), &lda, w,
		      reinterpret_cast<dcomplex_t*>(work), &lwork,
		      rwork, &info);
      }

      template <typename A, typename W, typename Work, typename RWork>
      inline
      int heev (char jobz, char uplo, A& a, W& w, Work& work, RWork& rwork) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<A>::matrix_structure,
          traits::general_t
        >::value));
#endif

        int const n = traits::matrix_size1 (a);
        assert (traits::matrix_size2 (a)==n);
        assert (traits::vector_size (w)==n);
        assert (2*n-1 <= traits::vector_size (work));
        assert (3*n-2 <= traits::vector_size (rwork));
        assert ( uplo=='U' || uplo=='L' );
        assert ( jobz=='N' || jobz=='V' );

        int info;
        detail::heev (jobz, uplo, n,
                     traits::matrix_storage (a),
                     traits::leading_dimension (a),
                     traits::vector_storage (w),
                     traits::vector_storage (work),
                     traits::vector_size (work),
                     traits::vector_storage (rwork),
                     info);
        return info;
      }
    } // namespace detail

    // Function that allocates temporary arrays
    template <typename A, typename W>
    int heev (char jobz, char uplo, A& a, W& w, minimal_workspace ) {
       typedef typename A::value_type                              value_type ;
       typedef typename traits::type_traits<value_type>::real_type real_type ;

       int const n = traits::matrix_size1 (a);

       traits::detail::array<value_type> work( std::max(1,2*n-1) );
       traits::detail::array<real_type> rwork( std::max(3*n-1,1) );

       return detail::heev( jobz, uplo, a, w, work, rwork );
    }

    // Function that allocates temporary arrays
    template <typename A, typename W>
    int heev (char jobz, char uplo, A& a, W& w, optimal_workspace ) {
       typedef typename A::value_type                              value_type ;
       typedef typename traits::type_traits<value_type>::real_type real_type ;

       int const n = traits::matrix_size1 (a);

       traits::detail::array<value_type> work( std::max(1,33*n) );
       traits::detail::array<real_type> rwork( std::max(3*n-1,1) );

       return detail::heev( jobz, uplo, a, w, work, rwork );
    }

    // Function that uses given workarrays
    template <typename A, typename W, typename WC, typename WR>
    int heev (char jobz, char uplo, A& a, W& w, detail::workspace2<WC,WR> workspace ) {
       typedef typename A::value_type                              value_type ;
       typedef typename traits::type_traits<value_type>::real_type real_type ;

       return detail::heev( jobz, uplo, a, w, workspace.w_, workspace.wr_ );
    }

  }

}}}

#endif


#define BOOST_TEST_MODULE arithmetic
#include <boost/test/included/unit_test.hpp>
#include "exprtest.hpp"

EXPRTEST(basicop1, " 2 +\t3\n",  5)       // Whitespace ignored
EXPRTEST(basicop2, " 2 -\t3\n", -1)
EXPRTEST(basicop3, " 2 *\t3\n",  6)
EXPRTEST(basicop4, " 2 /\t3\n",  2./3.)   // Double division
EXPRTEST(basicop5, " 2 ** 3\n",  8)


#include <boost/simd/function/load.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/sum.hpp>
#include <boost/simd/pack.hpp>

// [scalar-dot-simd]
Value simddot(Value* first1, Value* last1, Value* first2)
{
  namespace bs = boost::simd;
  using pack_t = bs::pack<Value>;

  pack_t tmp{0};
  int card = pack_t::static_size;

  for (; first1 + card <= last1; first1 += card, first2 += card) {
    // Load current values from the datasets
    pack_t x1 = bs::load<pack_t>(first1);
    pack_t x2 = bs::load<pack_t>(first2);
    // Computation
    tmp = tmp + x1 * x2;
  }

  Value dot_product = bs::sum(tmp); // horizontal SIMD vector summation
  for (; first1 < last1; ++first1, ++first2) {
    dot_product += (*first1) * (*first2);
  }

  return dot_product;
}
//! [scalar-dot-simd]


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_FUNCTIONS_SCALAR_IMPL_LOGS_D_LOG_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_SCALAR_IMPL_LOGS_D_LOG_HPP_INCLUDED

#include <nt2/include/functions/scalar/sqr.hpp>
#include <nt2/include/functions/scalar/tofloat.hpp>
#include <nt2/include/functions/scalar/is_ltz.hpp>
#include <nt2/include/functions/scalar/is_eqz.hpp>
#include <nt2/include/functions/scalar/is_greater.hpp>
#include <nt2/include/functions/scalar/multiplies.hpp>
#include <nt2/include/functions/scalar/plus.hpp>
#include <nt2/include/functions/scalar/fast_frexp.hpp>
#include <nt2/include/functions/scalar/genmask.hpp>
#include <nt2/include/functions/scalar/minusone.hpp>
#include <nt2/include/functions/scalar/fma.hpp>
#include <nt2/include/functions/scalar/bitwise_and.hpp>
#include <nt2/polynomials/functions/scalar/impl/horner.hpp>
#include <nt2/include/constants/real_splat.hpp>
#include <nt2/include/constants/invlog_2.hpp>
#include <nt2/include/constants/invlog_10.hpp>
#include <nt2/include/constants/log_2olog_10.hpp>
#include <nt2/include/constants/sqrt_2o_2.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/nan.hpp>
#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_NANS
#include <nt2/include/functions/scalar/is_nan.hpp>
#endif
#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/constants/inf.hpp>
#endif

namespace nt2 { namespace details
{
  //////////////////////////////////////////////////////////////////////////////
  // math log functions
  //////////////////////////////////////////////////////////////////////////////

  template < class A0 >
  struct logarithm< A0, tag::not_simd_type, double>
  {
    static inline void kernel_log(const A0& a0,
                                  A0& dk,
                                  A0& hfsq,
                                  A0& s,
                                  A0& R,
                                  A0& f)
    {
      typedef typename meta::as_integer<A0, signed>::type int_type;
      typedef typename meta::scalar_of<A0>::type               sA0;
      A0 x;
      int_type k;
      nt2::fast_frexp(a0, x, k);
      const int_type x_lt_sqrthf = nt2::is_greater(nt2::Sqrt_2o_2<A0>(), x)?nt2::Mone<int_type>():nt2::Zero<int_type>();
      k += x_lt_sqrthf;
      f = nt2::minusone(x+nt2::b_and(x, nt2::genmask(x_lt_sqrthf)));
      dk = nt2::tofloat(k);
      s = f/nt2::add(nt2::Two<A0>(),f);
      A0 z = nt2::sqr(s);
      A0 w = nt2::sqr(z);
      A0 t1= w*nt2::horner<NT2_HORNER_COEFF_T(sA0, 3,
                                              (0x3fc39a09d078c69fll,
                                               0x3fcc71c51d8e78afll,
                                               0x3fd999999997fa04ll)
                                             )> (w);
      A0 t2= z*horner<NT2_HORNER_COEFF_T(sA0, 4,
                                         (0x3fc2f112df3e5244ll,
                                          0x3fc7466496cb03dell,
                                          0x3fd2492494229359ll,
                                          0x3fe5555555555593ll)
                                        )> (w);
      R = t2+t1;
      hfsq = nt2::mul(Half<A0>(), nt2::sqr(f));
    }

    static inline A0 log(const A0& a0)
    {
      // ln(2)hi  =  6.93147180369123816490e-01  or  0x3fe62e42fee00000
      // ln(2)lo  =  1.90821492927058770002e-10  or  0x3dea39ef35793c76
#ifndef BOOST_SIMD_NO_INFINITIES
      if (a0 == Inf<A0>()) return a0;
#endif
      if (is_eqz(a0)) return nt2::Minf<A0>();
#ifdef BOOST_SIMD_NO_NANS
      if (nt2::is_ltz(a0)) return nt2::Nan<A0>();
#else
      if (nt2::is_nan(a0)||nt2::is_ltz(a0)) return nt2::Nan<A0>();
#endif
      A0 dk, hfsq, s, R, f;
      kernel_log(a0, dk, hfsq, s, R, f);
      return  nt2::mul(dk, double_constant<A0, 0x3fe62e42fee00000ll>())-
        ((hfsq-(s*(hfsq+R)+nt2::mul(dk,double_constant<A0, 0x3dea39ef35793c76ll>())))-f);
    }

    static inline A0 log2(const A0& a0)
    {
#ifndef BOOST_SIMD_NO_INFINITIES
      if (a0 == nt2::Inf<A0>()) return a0;
#endif
      if (nt2::is_eqz(a0)) return nt2::Minf<A0>();
#ifdef BOOST_SIMD_NO_NANS
      if (nt2::is_ltz(a0)) return nt2::Nan<A0>();
#else
      if (nt2::is_nan(a0)||nt2::is_ltz(a0)) return nt2::Nan<A0>();
#endif
      A0 dk, hfsq, s, R, f;
      kernel_log(a0, dk, hfsq, s, R, f);
      return -(hfsq-(s*(hfsq+R))-f)*Invlog_2<A0>()+dk;
    }

    static inline A0 log10(const A0& a0)
    {
#ifndef BOOST_SIMD_NO_INFINITIES
      if (a0 == nt2::Inf<A0>()) return a0;
#endif
      if (nt2::is_eqz(a0)) return nt2::Minf<A0>();
#ifdef BOOST_SIMD_NO_NANS
      if (nt2::is_ltz(a0)) return nt2::Nan<A0>();
#else
      if (nt2::is_nan(a0)||nt2::is_ltz(a0)) return nt2::Nan<A0>();
#endif
      A0 dk, hfsq, s, R, f;
      kernel_log(a0, dk, hfsq, s, R, f);
      return -(hfsq-(s*(hfsq+R))-f)*nt2::Invlog_10<A0>()+dk*nt2::Log_2olog_10<A0>();
    }
  };
} }

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <cmath>

// #include <boost/test/minimal.hpp>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

struct Random
{
  std::complex<double> operator()() const
  {
    return std::complex<double>(
        (2.*(static_cast<double>(rand())/RAND_MAX - 0.5)),
        (2.*(static_cast<double>(rand())/RAND_MAX - 0.5))
        );
  }
};

template <typename Matrix>
void test1(Matrix& m, double tau)
{
  Random m_rand;
  mtl::mat::inserter<Matrix> ins(m);
  size_t nrows=num_rows(m);
  std::complex<double> val;
  for (size_t r=0;r<nrows;++r)
  {
    for (size_t c=0;c<nrows;++c)
    {
      if(r==c)
        ins(r,c) << 1.;
      else
      {
        val=m_rand();
        if (abs(val)<tau)
          ins(r,c) << val;
      }
    }
  }
}

int main(int, char**)
{
  const int size= 10, N = size * size; // Original from Jan had 2000
  const int Niter = 3*N;

  typedef mtl::compressed2D<double> matrix_type;
  //typedef compressed2D<std::complex<double> ,mat::parameters<tag::col_major> > matrix_type;
  matrix_type                   A(N, N);
  laplacian_setup(A, size, size);
  mtl::dense_vector<double> b(N), x(N, 1.0);
  b= A*x;

  itl::pc::identity<matrix_type>     Ident(A);

  x= 0.0;
  itl::cyclic_iteration<double> iter_1(b, Niter, 1.e-8, 0.0, 5);
  idr_s(A, x, b, Ident, Ident, iter_1, 4);
#if 0
  std::cout << "Non-preconditioned bicgstab(2)" << std::endl;
  x= 0.5;
  itl::cyclic_iteration<double> iter_2b(b, Niter, 1.e-8, 0.0, 5);
  idr_s(A, x, b, Ident, Ident, iter_2b,2);

  std::cout << "Non-preconditioned bicgstab(4)" << std::endl;
  x= 0.5;
  itl::cyclic_iteration<double> iter_4b(b, Niter, 1.e-8, 0.0, 5);
  idr_s(A, x, b, Ident, Ident, iter_4b,4);

  std::cout << "Non-preconditioned bicgstab(8)" << std::endl;
  x= 0.5;
  itl::cyclic_iteration<double> iter_8b(b, Niter, 1.e-8, 0.0, 5);
  idr_s(A, x, b, Ident, Ident, iter_8b,8);

  itl::pc::ilu_0<matrix_type>        P(A);

  std::cout << "Right ilu(0) preconditioned bicgstab(1)" << std::endl;
  x= 0.5;
  itl::cyclic_iteration<double> iter_1r(b, Niter, 1.e-8, 0.0, 5);
  idr_s(A, x, b, Ident, P, iter_1r,1);

  std::cout << "Right ilu(0) preconditioned bicgstab(2)" << std::endl;
  x= 0.5;
  itl::cyclic_iteration<double> iter_2r(b, Niter, 1.e-8, 0.0, 5);
  idr_s(A, x, b, Ident, P, iter_2r,2);

  std::cout << "Left ilu(0) preconditioned bicgstab(4)" << std::endl;
  x= 0.5;
  itl::cyclic_iteration<double> iter_4l(b, Niter, 1.e-8, 0.0, 5);
  idr_s(A, x, b, P, Ident, iter_4l,4);

  std::cout << "Right ilu(0) preconditioned bicgstab(4)" << std::endl;
  x= 0.5;
  itl::cyclic_iteration<double> iter_4r(b, Niter, 1.e-8, 0.0, 5);
  idr_s(A, x, b, Ident, P, iter_4r,4);

  std::cout << "Right ilu(0) preconditioned bicgstab(8)" << std::endl;
  x= 0.5;
  itl::cyclic_iteration<double> iter_8r(b, Niter, 1.e-8, 0.0, 5);
  idr_s(A, x, b, Ident, P, iter_8r,8);
#endif
  return 0;
}


#include <iostream>
#include <cmath>
#include <algorithm>

#include <Eigen/Dense>

#include <TriMesh.h>

#include <GL/glut.h>
#include <gl/glu.h>

#include "Mesh.h"

using namespace std;
using namespace Eigen;
using namespace trimesh;
using namespace sen;

sen::Mesh mesh;
MatrixXf scaleFreeMatrix;
MatrixXf Hp, D;
int selected_id = 0;

std::vector<std::array<Eigen::Vector3f, 3>> temp_triangles_21;

void ScreenToOpenGL(const Vector2f &screen_coord, Vector3f &opengl_coord)
{
	GLint viewPort[4];
	GLdouble modelView[16];
	GLdouble projection[16];
	GLfloat winX, winY, winZ;
	GLdouble posX, posY, posZ;
	glPushMatrix();
	glLoadIdentity();
	glScalef(0.01f, 0.01f, 1.0f);
	glGetIntegerv(GL_VIEWPORT, viewPort);
	glGetDoublev(GL_MODELVIEW_MATRIX, modelView);
	glGetDoublev(GL_PROJECTION_MATRIX, projection);
	glPopMatrix();

	winX = screen_coord(0);
	winY = viewPort[3] - screen_coord[1];
	glReadPixels((int)winX, (int)winY, 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &winZ);
	gluUnProject(winX, winY, 0, modelView, projection, viewPort, &posX, &posY, &posZ);
	opengl_coord[0] = (float)posX;
	opengl_coord[1] = (float)posY;
	opengl_coord[2] = 0.0f;
}

void OpenGLToScreen(const Vector3f &opengl_coord, Vector2f &screen_coord)
{
	GLint viewPort[4];
	GLdouble modelView[16];
	GLdouble projection[16];
	GLdouble winX, winY, winZ;
	GLfloat posX, posY, posZ;
	glPushMatrix();
	glGetIntegerv(GL_VIEWPORT, viewPort);
	glGetDoublev(GL_MODELVIEW_MATRIX, modelView);
	glGetDoublev(GL_PROJECTION_MATRIX, projection);
	glPopMatrix();

	posX = opengl_coord[0];
	posY = opengl_coord[1];
	posZ = opengl_coord[2];

	gluProject(posX, posY, posZ, modelView, projection, viewPort, &winX, &winY, &winZ);
	screen_coord[0] = (float)winX;
	screen_coord[1] = (float)viewPort[3] - (float)winY;
}

int FindHitVertex(float x, float y)
{
	Vector2f screen_coord(x, y);
	Vector3f opengl_coord;
	for (int i = 0; i < static_cast<int>(mesh.vertices.size()); ++i)
	{
		Vector2f coord_2d;
		opengl_coord = mesh.vertices[i].coord;
		OpenGLToScreen(opengl_coord, coord_2d);
		float length = sqrt((screen_coord - coord_2d).dot(screen_coord - coord_2d));
		if (length < 5)
		{
			return mesh.vertices[i].id;
		}
	}
	return -1;
}

void OnMouseClick(int button, int state, int x, int y)
{
	if (button == GLUT_LEFT_BUTTON)
	{
		if (state == GLUT_DOWN)
		{
			int origial_id = FindHitVertex((float)x, (float)y);
			if (-1 == origial_id)
				return;
			selected_id = find(mesh.vertices.begin(), mesh.vertices.end(), Vertex(origial_id)) - mesh.vertices.begin();
		}
		else
			selected_id = -1;
		glutPostRedisplay();
	}
	else if (button == GLUT_RIGHT_BUTTON && state == GLUT_UP)
	{
		int HitPointId = FindHitVertex((float)x, (float)y);
		std::cout << "HitPointId: " << HitPointId << std::endl;
		if (-1 != HitPointId)
		{
			if (mesh.find(HitPointId))
			{
				mesh.resetVerticesTo_uq();
			}
			std::cout << "computing" << std::endl;
			scaleFreeMatrix = mesh.PreComputeScaleFreeMatrix();
			mesh.PreComputeScaleAdjustmentMatrixHf(Hp, D);
			glutPostRedisplay();
		}
	}
}

void OnMouseMove(int x, int y)
{
	if (selected_id != 0 && mesh.q_size > 0)
	{
		Vector2f screen_coord((float)x, (float)y);
		Vector3f opengl_coord;
		ScreenToOpenGL(screen_coord, opengl_coord);
		opengl_coord *= 0.4f;// 0.4 for mesh, 1 for square mesh
		mesh.vertices[selected_id].coord = opengl_coord;
		glutPostRedisplay();
	}
}

void Deform()
{
	if (mesh.q_size > 1 && mesh.q_size < static_cast<int>(mesh.vertices.size()))
	{
		VectorXf q(2 * mesh.q_size);
		for (int i = static_cast<int>(mesh.vertices.size()) - mesh.q_size, j = 0; i < static_cast<int>(mesh.vertices.size()), j < mesh.q_size; ++i, ++j)
		{
			Vector3f v = mesh.vertices[i].coord;
			q(2 * j) = v(0);
			q(2 * j + 1) = v(1);
		}

		VectorXf u = scaleFreeMatrix * q;
		for (int i = 0; i < static_cast<int>(mesh.vertices.size()) - mesh.q_size; ++i)
		{
			mesh.vertices[i].coord(0) = u(2 * i);
			mesh.vertices[i].coord(1) = u(2 * i + 1);
		}

		VectorXf f = VectorXf::Zero(2 * mesh.vertices.size());
		for (int i = 0; i < static_cast<int>(mesh.triangles.size()); ++i)
		{
			vector<Vector2f> current_triangle_points;
			mesh.AdjustScaleToTriangle(i, current_triangle_points);
			for (int j = 0; j < 3; j++)
			{
				Vector2f v0f, v1f, v2f;
				v0f = current_triangle_points[j];
				v1f = current_triangle_points[(j + 1) % 3];

				int v0f_id, v1f_id;
				v0f_id = find(mesh.vertices.begin(), mesh.vertices.end(), Vertex(mesh.triangles[i].vertex_id[j])) - mesh.vertices.begin();
				v1f_id = find(mesh.vertices.begin(), mesh.vertices.end(), Vertex(mesh.triangles[i].vertex_id[(j + 1) % 3])) - mesh.vertices.begin();

				int v0p_id = v0f_id;
				int v1p_id = v1f_id;
				f(2 * v0p_id) += -2 * v0f(0) + 2 * v1f(0);
				f(2 * v0p_id + 1) += -2 * v0f(1) + 2 * v1f(1);
				f(2 * v1p_id) += 2 * v0f(0) - 2 * v1f(0);
				f(2 * v1p_id + 1) += 2 * v0f(1) - 2 * v1f(1);
			}
		}
		int u_size = mesh.vertices.size() - mesh.q_size;
		VectorXf f0 = VectorXf::Zero(2 * u_size);
		for (int i = 0; i < 2 * u_size; i++)
		{
			f0[i] = f[i];
		}
		VectorXf b = VectorXf::Zero(2 * u_size);
		for (int i = static_cast<int>(mesh.vertices.size()) - mesh.q_size, j = 0; i < static_cast<int>(mesh.vertices.size()), j < mesh.q_size; ++i, ++j)
		{
			Vector3f v = mesh.vertices[i].coord;
			q(2 * j) = v(0);
			q(2 * j + 1) = v(1);
		}
		b = -(D*q + f0);
		u = Hp.llt().solve(b);
		for (int i = 0; i < static_cast<int>(mesh.vertices.size()) - mesh.q_size; ++i)
		{
			mesh.vertices[i].coord(0) = u(2 * i);
			mesh.vertices[i].coord(1) = u(2 * i + 1);
		}
	}
}

void display()
{
	Deform();
	glClearColor(1, 1, 1, 1);
	glClear(GL_COLOR_BUFFER_BIT);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	gluLookAt(0, 0, 4, 0, 0, 0, 0, 1, 0);//4 for mesh, 10 for square mesh
	glLineWidth(2);
	glColor3f(0, 0, 0);
	for (unsigned int i = 0; i < mesh.triangles.size(); i++)
	{
		int v0_id = find(mesh.vertices.begin(), mesh.vertices.end(), Vertex(mesh.triangles[i].vertex_id[0])) - mesh.vertices.begin();
		int v1_id = find(mesh.vertices.begin(), mesh.vertices.end(), Vertex(mesh.triangles[i].vertex_id[1])) - mesh.vertices.begin();
		int v2_id = find(mesh.vertices.begin(), mesh.vertices.end(), Vertex(mesh.triangles[i].vertex_id[2])) - mesh.vertices.begin();

		GLfloat *v0 = mesh.vertices[v0_id].coord.data();
		GLfloat *v1 = mesh.vertices[v1_id].coord.data();
		GLfloat *v2 = mesh.vertices[v2_id].coord.data();

		glBegin(GL_LINE_LOOP);
		glVertex3fv(v0);
		glVertex3fv(v1);
		glVertex3fv(v2);
		glEnd();
	}

	glPointSize(10.0f);
	glBegin(GL_POINTS);
	glColor3f(1, 0, 0);
	for (int i = static_cast<int>(mesh.vertices.size()) - mesh.q_size; i < static_cast<int>(mesh.vertices.size()); ++i)
	{
		GLfloat *v = mesh.vertices[i].coord.data();
		glVertex3fv(v);
	}
	glEnd();
	glFlush();
}

void reshape(int w, int h)
{
	glViewport(0, 0, w, h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60, 1, 0.1, 10);
}

int main(int argc, char **argv)
{
	mesh.Read("man.obj");
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_SINGLE | GLUT_RGB);
	glutInitWindowSize(500, 500);
	glutInitWindowPosition(100, 100);
	glutCreateWindow("test");
	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	glutMouseFunc(OnMouseClick);
	glutMotionFunc(OnMouseMove);
	glutMainLoop();
	return 0;
}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/module/bessel.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/bessel_prime.hpp>

//==================================================================================================
//== Types tests
//==================================================================================================
EVE_TEST_TYPES( "Check return types of cyl_bessel_jn"
              , eve::test::simd::ieee_reals
              )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  using i_t = eve::as_integer_t<v_t>;
  using I_t = eve::wide<i_t, eve::cardinal_t<T>>;
  TTS_EXPR_IS( eve::cyl_bessel_jn(T(), T())  ,  T);
  TTS_EXPR_IS( eve::cyl_bessel_jn(v_t(),v_t()), v_t);
  TTS_EXPR_IS( eve::cyl_bessel_jn(i_t(),T()),   T);
  TTS_EXPR_IS( eve::cyl_bessel_jn(I_t(),T()),   T);
  TTS_EXPR_IS( eve::cyl_bessel_jn(i_t(),v_t()), v_t);
  TTS_EXPR_IS( eve::cyl_bessel_jn(I_t(),v_t()), T);
};

//==================================================================================================
//== integral orders
//==================================================================================================
EVE_TEST( "Check behavior of cyl_bessel_jn on wide with integral order"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::ramp(0), eve::test::randoms(0.0, 20000.0))
        )
  <typename T>(T n, T a0)
{
  using v_t = eve::element_type_t<T>;
  auto eve__cyl_bessel_jn =  [](auto n, auto x) { return eve::cyl_bessel_jn(n, x); };
  auto std__cyl_bessel_jn =  [](auto n, auto x)->v_t { return boost::math::cyl_bessel_j(double(n), double(x)); };
  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(0, eve::minf(eve::as<v_t>())), eve::zero(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(2, eve::inf(eve::as<v_t>())), v_t(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(3, eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
  }
  //scalar large x
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(3, v_t(1500)), std__cyl_bessel_jn(3, v_t(1500)), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(2, v_t(500)), std__cyl_bessel_jn(2, v_t(500)), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(-3, v_t(1500)), std__cyl_bessel_jn(-3, v_t(1500)), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(-2, v_t(500)), std__cyl_bessel_jn(-2, v_t(500)), 2.0);
  //scalar forward
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(2, v_t(10)), std__cyl_bessel_jn(2, v_t(10))  , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(3, v_t(5)),  std__cyl_bessel_jn(3, v_t(5))   , 2.0);
  //scalar serie
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(2, v_t(0.1)), std__cyl_bessel_jn(2, v_t(0.1))  , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(3, v_t(0.2)),  std__cyl_bessel_jn(3, v_t(0.2))   , 2.0);
  //scalar besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(10, v_t(8)), std__cyl_bessel_jn(10, v_t(8))  , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(10, v_t(8)),  std__cyl_bessel_jn(10, v_t(8))   , 2.0);

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(0, eve::minf(eve::as<T>())), eve::zero(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(2, eve::inf(eve::as<T>())), T(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(3, eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }
  //scalar large x
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(3, T(1500)),  T(std__cyl_bessel_jn(3, v_t(1500))),  2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(2, T(500)),   T(std__cyl_bessel_jn(2, v_t(500))),   2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(-3, T(1500)), T(std__cyl_bessel_jn(-3, v_t(1500))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(-2, T(500)),  T(std__cyl_bessel_jn(-2, v_t(500))),  2.0);
  //scalar forward
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(2, T(10)),    T(std__cyl_bessel_jn(2, v_t(10)))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(3, T(5)),     T(std__cyl_bessel_jn(3, v_t(5)))    , 2.0);
  //scalar serie
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(2, T(0.1)),   T(std__cyl_bessel_jn(2, v_t(0.1)))  , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(3, T(0.2)),   T(std__cyl_bessel_jn(3, v_t(0.2)))  , 2.0);
  //scalar besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(10, T(8)),   T(std__cyl_bessel_jn(10, v_t(8)))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(10, T(8)),   T(std__cyl_bessel_jn(10, v_t(8)))   , 2.0);

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(0), eve::minf(eve::as<T>())), eve::zero(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(2), eve::inf(eve::as<T>())), T(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(3), eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }
  // large x
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(3), T(1500)),  T(std__cyl_bessel_jn(3, v_t(1500))),  2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(2), T(500)),   T(std__cyl_bessel_jn(2, v_t(500))),   2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(-3), T(1500)), T(std__cyl_bessel_jn(-3, v_t(1500))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(-2), T(500)),  T(std__cyl_bessel_jn(-2, v_t(500))),  2.0);
  // forward
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(2), T(10)),    T(std__cyl_bessel_jn(2, v_t(10)))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(3), T(5)),     T(std__cyl_bessel_jn(3, v_t(5)))    , 2.0);
  // serie
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(2), T(0.1)),   T(std__cyl_bessel_jn(2, v_t(0.1)))  , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(3), T(0.2)),   T(std__cyl_bessel_jn(3, v_t(0.2)))  , 2.0);
  // besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(10), T(8)),   T(std__cyl_bessel_jn(10, v_t(8)))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(10), T(8)),   T(std__cyl_bessel_jn(10, v_t(8)))   , 2.0);

  using i_t = eve::as_integer_t<v_t>;
  using I_t = eve::wide<i_t, eve::cardinal_t<T>>;

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(I_t(0), eve::minf(eve::as<T>())), eve::zero(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(I_t(2), eve::inf(eve::as<T>())), T(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(I_t(3), eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }
  // large x
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(I_t(3), T(1500)),  T(std__cyl_bessel_jn(3, v_t(1500))),  2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(I_t(2), T(500)),   T(std__cyl_bessel_jn(2, v_t(500))),   2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(I_t(-3), T(1500)), T(std__cyl_bessel_jn(-3, v_t(1500))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(I_t(-2), T(500)),  T(std__cyl_bessel_jn(-2, v_t(500))),  2.0);
  // forward
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(I_t(2), T(10)),    T(std__cyl_bessel_jn(2, v_t(10)))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(I_t(3), T(5)),     T(std__cyl_bessel_jn(3, v_t(5)))    , 2.0);
  // serie
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(I_t(2), T(0.1)),   T(std__cyl_bessel_jn(2, v_t(0.1)))  , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(I_t(3), T(0.2)),   T(std__cyl_bessel_jn(3, v_t(0.2)))  , 2.0);
  // besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(I_t(10), T(8)),   T(std__cyl_bessel_jn(10, v_t(8)))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(I_t(10), T(8)),   T(std__cyl_bessel_jn(10, v_t(8)))   , 2.0);

  TTS_RELATIVE_EQUAL(eve__cyl_bessel_jn(n, a0),   map(std__cyl_bessel_jn, n, a0)   , 0.0015);
  TTS_RELATIVE_EQUAL(eve__cyl_bessel_jn(n, -a0),   map(std__cyl_bessel_jn, n, -a0)   , 0.0015);
  TTS_RELATIVE_EQUAL(map(eve__cyl_bessel_jn, n, -a0),   map(std__cyl_bessel_jn, n, -a0)   , 0.0015);
};

//==================================================================================================
//== non integral orders
//==================================================================================================
EVE_TEST( "Check behavior of cyl_bessel_jn on wide with non integral order"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 10.0)
                             , eve::test::randoms(0.0, 2000.0))
        )
  <typename T>(T n, T a0)
{
  using v_t = eve::element_type_t<T>;

  auto eve__cyl_bessel_jn =  [](auto n, auto x) { return eve::cyl_bessel_jn(n, x); };
  auto std__cyl_bessel_jn =  [](auto n, auto x)->v_t { return boost::math::cyl_bessel_j(double(n), double(x)); };
  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(0.5), eve::minf(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(2.5), eve::inf(eve::as<v_t>())), v_t(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(3.5), eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
  }
  //scalar large x
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(3.5), v_t(1500)), std__cyl_bessel_jn(v_t(3.5), v_t(1500)), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(2.5), v_t( 500)),  std__cyl_bessel_jn(v_t(2.5), v_t( 500)), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(-3.5), v_t(1500)), std__cyl_bessel_jn(v_t(-3.5), v_t(1500)), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(-2.5), v_t( 500)), std__cyl_bessel_jn(v_t(-2.5), v_t(500)), 2.0);
  //scalar forward
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(2.5), v_t(10)), std__cyl_bessel_jn(v_t(2.5), v_t(10))  , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(3.5), v_t(5)),  std__cyl_bessel_jn(v_t(3.5), v_t(5))   , 2.0);
  //scalar serie
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(2.5), v_t(0.1)), std__cyl_bessel_jn(v_t(2.5), v_t(0.1))  , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(3.5), v_t(0.2)),  std__cyl_bessel_jn(v_t(3.5), v_t(0.2))   , 2.0);
  //scalar besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(10.5), v_t(8)), std__cyl_bessel_jn(v_t(10.5), v_t(8))  , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(10.5), v_t(8)),  std__cyl_bessel_jn(v_t(10.5), v_t(8))   , 2.0);

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(0.5), eve::minf(eve::as<T>())), eve::nan(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(2.5), eve::inf(eve::as<T>())), T(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(3.5), eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }
  //scalar large x
   TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(3.5), T(1500)),  T(std__cyl_bessel_jn(v_t(3.5), v_t(1500))),  2.0);
   TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(2.5), T(500)),   T(std__cyl_bessel_jn(v_t(2.5), v_t(500))),   2.0);
  //scalar forward
   TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(2.5), T(10)),    T(std__cyl_bessel_jn(v_t(2.5), v_t(10)))   , 5.0);
   TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(3.5), T(5)),     T(std__cyl_bessel_jn(v_t(3.5), v_t(5)))    , 2.0);
  //scalar serie
   TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(2.5), T(0.1)),   T(std__cyl_bessel_jn(v_t(2.5), v_t(0.1)))  , 2.0);
   TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(3.5), T(0.2)),   T(std__cyl_bessel_jn(v_t(3.5), v_t(0.2)))  , 2.5);
  //scalar besseljy
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(10.5), T(8)),   T(std__cyl_bessel_jn(v_t(10.5), v_t(8)))   , 2.0);
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(10.5), T(8)),   T(std__cyl_bessel_jn(v_t(10.5), v_t(8)))   , 2.0);

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(0.5), eve::minf(eve::as<T>())), eve::nan(eve::as<T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(2.5), eve::inf(eve::as<T>())), T(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(3.5), eve::nan(eve::as<T>())), eve::nan(eve::as<T>()), 0);
  }
  // large x
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(3.5), T(1500)),  T(std__cyl_bessel_jn(v_t(3.5), v_t(1500))),  2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(2.5), T(500)),   T(std__cyl_bessel_jn(v_t(2.5), v_t(500))),   2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(-3.5), T(1500)), T(std__cyl_bessel_jn(v_t(-3.5), v_t(1500))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(-2.5), T(500)),  T(std__cyl_bessel_jn(v_t(-2.5), v_t( 500))), 2.0);
  // forward
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(2.5), T(10)),    T(std__cyl_bessel_jn(v_t(2.5), v_t(10)))   , 5.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(3.5), T(5)),     T(std__cyl_bessel_jn(v_t(3.5), v_t(5)))    , 2.0);
  // serie
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(2.5), T(0.1)),   T(std__cyl_bessel_jn(v_t(2.5), v_t(0.1)))  , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(3.5), T(0.2)),   T(std__cyl_bessel_jn(v_t(3.5), v_t(0.2)))  , 2.5);
  // besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(10.5), T(8)),   T(std__cyl_bessel_jn(v_t(10.5), v_t(8)))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(10.5), T(8)),   T(std__cyl_bessel_jn(v_t(10.5), v_t(8)))   , 2.0);

  TTS_RELATIVE_EQUAL(eve__cyl_bessel_jn(n, a0),   map(std__cyl_bessel_jn, n, a0)   , 0.001);

    //scalar large x
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(-3.5), v_t(1500)), std__cyl_bessel_jn(v_t(-3.5), v_t(1500)), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(-2.5), v_t(500)),  std__cyl_bessel_jn(v_t(-2.5), v_t(500)), 2.0);

  //scalar forward
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(-2.5), v_t(10)), std__cyl_bessel_jn(v_t(-2.5), v_t(10))  , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(-3.5), v_t(5)),  std__cyl_bessel_jn(v_t(-3.5), v_t(5))   , 35.0);
  //scalar serie
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(-2.5), v_t(0.1)), std__cyl_bessel_jn(v_t(-2.5), v_t(0.1))  , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(-3.5), v_t(0.2)),  std__cyl_bessel_jn(v_t(-3.5), v_t(0.2))   , 2.0);
  //scalar besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(-10.5), v_t(8)), std__cyl_bessel_jn(v_t(-10.5), v_t(8))  , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(v_t(-10.5), v_t(8)),  std__cyl_bessel_jn(v_t(-10.5), v_t(8))   , 2.0);

   // large x
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(-3.5), T(1500)),  T(std__cyl_bessel_jn(v_t(-3.5), v_t(1500))),  2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(-2.5), T(500)),   T(std__cyl_bessel_jn(v_t(-2.5), v_t(500))),   2.0);

  // forward
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(-2.5), T(10)),    T(std__cyl_bessel_jn(v_t(-2.5), v_t(10)))   , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(-3.5), T(5)),     T(std__cyl_bessel_jn(v_t(-3.5), v_t(5)))    , 35.0);
  // serie
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(-2.5), T(0.1)),   T(std__cyl_bessel_jn(v_t(-2.5), v_t(0.1)))  , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(-3.5), T(0.2)),   T(std__cyl_bessel_jn(v_t(-3.5), v_t(0.2)))  , 2.5);
  // besseljy
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(-10.5), T(8)),   T(std__cyl_bessel_jn(v_t(-10.5), v_t(8)))   , 2.5);
  TTS_ULP_EQUAL(eve__cyl_bessel_jn(T(-10.5), T(8)),   T(std__cyl_bessel_jn(v_t(-10.5), v_t(8)))   , 2.5);

  TTS_RELATIVE_EQUAL(eve__cyl_bessel_jn(-n, a0),   map(std__cyl_bessel_jn, -n, a0)   , 0.001);
};

EVE_TEST( "Check behavior of diff(cyl_bessel_jn) on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate( eve::test::randoms(0.0, 10.0)
                             , eve::test::randoms(0.0, 60.0))
        )
  <typename T>(T n, T a0 )
{
  using v_t =  eve::element_type_t<T>;
  auto eve__diff_bessel_jn =  [](auto n, auto x) { return eve::diff(eve::cyl_bessel_jn)(n, x); };
  auto std__diff_bessel_jn =  [](auto n, auto x)->v_t { return boost::math::cyl_bessel_j_prime(double(n), double(x)); };
  TTS_RELATIVE_EQUAL(eve__diff_bessel_jn(n, a0),   map(std__diff_bessel_jn, n, a0), 1.0e-3);
  auto nn = eve::trunc(n);
  TTS_RELATIVE_EQUAL(eve__diff_bessel_jn(nn, a0),   map(std__diff_bessel_jn, nn, a0), 2.0e-3);

};


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT license.
//
// Generalized Pose-and-Scale Estimation using 4-Point Congruence Constraints
//
// Victor Fragoso and Sudipta Sinha.
// In Proc. of the IEEE International Conf. on 3D Vision (3DV) 2020.
//
// Please contact the author of this library if you have any questions.
// Author: Victor Fragoso (victor.fragoso@microsoft.com)

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <glog/logging.h>
#include <gp4pc/gp4pc.h>
#include <math/solve_gp4pc_polynomial.h>
#include <third_party/align_point_clouds.h>
#include <algorithm>
#include <utility>
#include <vector>

namespace msft {
namespace {

// Default coplanar threshold.
const double kDefaultCoplanarThresh = 1e-3;
const double kDefaultColinearThresh = 1e-2;
const double kRealNumberThreshold = 0.0;
const double kDuplicateSolnErrorThreshold = 1e-6;
const double kDuplicateCameraCenterErrorThreshold = 1e-6;
// Dimensions of the coefficient matrix: kNumConstraints x kNumMonomials.
const int kNumConstraints = 4;
const int kNumMonomials = 15;
// Max. num. of depth solutions.
const int kMaxNumDepthSolutions = 16;
// Size of the minimal sample.
const int kSizeOfMinimalSample = 4;

inline Eigen::Vector3d ComputeCameraPoint(const Eigen::Vector3d& camera_center,
                                          const Eigen::Vector3d& ray_direction,
                                          const double depth) {
  return camera_center + depth * ray_direction;
}

bool IsInputValid(const Gp4pc::Input& input) {
  // Check that the camera centers are different.
  int num_different_origins = 0;
  for (int i = 0; i < input.ray_origins.size(); ++i) {
    for (int j = i + 1; j < input.ray_origins.size(); ++j) {
      const double distance =
          (input.ray_origins[i] - input.ray_origins[j]).squaredNorm();
      if (distance > kDuplicateCameraCenterErrorThreshold) {
        num_different_origins += 1;
      }
    }
  }
  const bool has_different_origins = num_different_origins > 0;
  return has_different_origins;
}

// Build least-squares system Ax = b, where
// b = [p_1 p_2 p_3 p_4]^T
// and
//     | L(w_1)  I |
// A = | L(w_2)  I |
//     | L(w_3)  I |
//     | L(w_4)  I |.
void BuildLinearSystem(const std::vector<Eigen::Vector3d>& world_points,
                       const std::vector<Eigen::Vector3d>& camera_points,
                       Eigen::VectorXd* camera_points_vec_ptr,
                       Eigen::MatrixXd* world_points_mat_ptr) {
  // Dimensionality of the points.
  static const int kPointDimension = 3;

  const int num_points = world_points.size();
  Eigen::VectorXd& camera_points_vec = *camera_points_vec_ptr;
  Eigen::MatrixXd& world_points_mat = *world_points_mat_ptr;
  world_points_mat.setZero();

  int row_index = 0;
  for (int i = 0; i < num_points; ++i) {
    // Fill in camera points.
    camera_points_vec.segment(i * kPointDimension, kPointDimension) =
        camera_points[i];

    // Fill in world-point matrices.
    row_index = i * kPointDimension;
    world_points_mat.block(row_index, 0, 1, 3) = world_points[i].transpose();
    world_points_mat.block(row_index + 1, 3, 1, 3) =
        world_points[i].transpose();
    world_points_mat.block(row_index + 2, 6, 1, 3) =
        world_points[i].transpose();
    world_points_mat.block(row_index, 9, 3, 3) = Eigen::Matrix3d::Identity();
  }
}

bool IsInputDegenerateOrPlanar(const Gp4pc::Input& input,
                               const double coplanar_threshold,
                               const double colinear_threshold,
                               bool* is_planar) {
  *CHECK_NOTNULL(is_planar) = false;
  // Compute the SVD of the input 3D points.
  const Eigen::Vector3d mean_direction =
      (input.world_points[0] + input.world_points[1] + input.world_points[2] +
       input.world_points[3]) /
      4;
  Eigen::Matrix<double, 4, 3> normalized_points;
  normalized_points.row(0) = input.world_points[0] - mean_direction;
  normalized_points.row(1) = input.world_points[1] - mean_direction;
  normalized_points.row(2) = input.world_points[2] - mean_direction;
  normalized_points.row(3) = input.world_points[3] - mean_direction;
  const Eigen::Matrix3d covariance =
      normalized_points.transpose() * normalized_points;
  const Eigen::JacobiSVD<Eigen::Matrix3d> svd(
      covariance, Eigen::ComputeThinU | Eigen::ComputeThinV);
  const Eigen::Vector3d singular_values = svd.singularValues();

  // Is input degenerate, i.e., a line?
  double singular_value_ratio = singular_values[1] / singular_values[0];
  if (singular_value_ratio < colinear_threshold) {
    VLOG(2) << "Singular value ratio (colinear test): " << singular_value_ratio
            << " threshold: " << colinear_threshold;
    return true;
  }

  // Is the input planar?
  singular_value_ratio = singular_values[2] / singular_values[0];
  *is_planar = singular_value_ratio < coplanar_threshold;
  VLOG_IF(4, *is_planar) << "Singular value ratio (coplanar test): "
                         << singular_value_ratio
                         << " threshold: " << coplanar_threshold;

  return false;
}

// This function computes the values of t1 and t3 given the four points:
// (x1, x2, x3, x4).
//
// The point (x1 + t1 * v1) + t3 * v3 must be equal to the point (x3 + t2 * v2).
// This gives a linear system in t1, t2, t3. Solve it. This gives the following
// closest points on the lines.
//    x1 + t1 * v1;
//    x3 + t2 * v2;
Eigen::Vector2d Findt1t3(const Eigen::Vector3d& x1,
                         const Eigen::Vector3d& x2,
                         const Eigen::Vector3d& x3,
                         const Eigen::Vector3d& x4) {
  const Eigen::Vector3d v1 = x2 - x1;
  const Eigen::Vector3d v2 = x4 - x3;
  const Eigen::Vector3d v3 = v1.cross(v2);
  const Eigen::Vector3d b = x3 - x1;

  // A = [v1 -v2 v3]; (MATLAB code).
  Eigen::MatrixXd A(3, 3);
  A.col(0) = v1;
  A.col(1) = -v2;
  A.col(2) = v3;

  // t = A \ b; (MATLAB code).
  const Eigen::VectorXd x = A.lu().solve(b);

  // Solution
  return x;
}

// This function calculates (f1, f2, f3, f4) and (K1, K2) from the four points
// (x1, x2, x3, x4).
struct DerivedConstants {
  double f1;
  double f2;
  double f3;
  double f4;
  double k1;
  double k2;
};

DerivedConstants CalculateRatiosFromFourPoints(
    const Eigen::Vector3d& x1,
    const Eigen::Vector3d& x2,
    const Eigen::Vector3d& x3,
    const Eigen::Vector3d& x4) {
  DerivedConstants constants;
  double& f1 = constants.f1;
  double& f2 = constants.f2;
  double& f3 = constants.f3;
  double& f4 = constants.f4;
  double& K1 = constants.k1;
  double& K2 = constants.k2;

  // x1--x2 is the first line segment and x3--x4 is the second line segment.
  // z1 is on x1--x2, z3 is on x3--x4 and (z1--z3) is also perpendicular
  // to x1--x2 and x3--x4
  //
  // t1 is equal to dist(x1,z1) / dist(x1,x2)
  // t2 is equal to dist(x3,z3) / dist(x3,x4)
  //
  // [t1, t3, x1, x2, x3, x4] = findClosestPoints(X,1,2,3,4); (MATLAB code)

  const Eigen::Vector2d t_vec = Findt1t3(x1, x2, x3, x4);

  const double len12 = (x1 - x2).norm();
  const double len34 = (x3 - x4).norm();
  const double len13 = (x1 - x3).norm();

  const double s12 = len12 * len12;
  const double s34 = len34 * len34;
  const double s13 = len13 * len13;

  K1 = s12 / s34;
  K2 = s12 / s13;

  f1 = 1 - t_vec(0);
  f2 = t_vec(0);
  f3 = 1 - t_vec(1);
  f4 = t_vec(1);

  return constants;
}

Eigen::MatrixXd SolveForDepthsFromAPlanarInput(const Gp4pc::Input& input) {
  // Useful aliases.
  // Camera centers.
  const std::vector<Eigen::Vector3d>& ray_origins = input.ray_origins;
  // Unit rays originating from ray_origins.
  const std::vector<Eigen::Vector3d>& ray_directions = input.ray_directions;
  // World points.
  const std::vector<Eigen::Vector3d>& world_points = input.world_points;

  // Verify the input is consistent.
  CHECK_EQ(ray_origins.size(), ray_directions.size());
  CHECK_EQ(ray_origins.size(), world_points.size());

  // The mathematical relationship is the following:
  //
  // ray_origins[i] + depth[i] * ray_directions[i] =
  //         s * R * world_points[i] + t,
  //
  // where s is scale, R is a rotation matrix, and t is translation.

  // Input Constants.
  const Eigen::Vector3d& p1 = ray_origins[0];
  const Eigen::Vector3d& p2 = ray_origins[1];
  const Eigen::Vector3d& p3 = ray_origins[2];
  const Eigen::Vector3d& p4 = ray_origins[3];

  // Input Constants.
  const Eigen::Vector3d& u1 = ray_directions[0];
  const Eigen::Vector3d& u2 = ray_directions[1];
  const Eigen::Vector3d& u3 = ray_directions[2];
  const Eigen::Vector3d& u4 = ray_directions[3];

  // Input Constants.
  const Eigen::Vector3d& x1 = world_points[0];
  const Eigen::Vector3d& x2 = world_points[1];
  const Eigen::Vector3d& x3 = world_points[2];
  const Eigen::Vector3d& x4 = world_points[3];

  // Derived Constants.
  const DerivedConstants constants =
      CalculateRatiosFromFourPoints(x1, x2, x3, x4);

  // Derived Constants.
  const Eigen::Vector3d p5 =
      constants.f1 * p1 + constants.f2 * p2 -
      constants.f3 * p3 - constants.f4 * p4;
  const Eigen::Vector3d p6 = p1 - p2;
  const Eigen::Vector3d p8 = p1 - p3;

  // Derived Constants.
  const Eigen::Vector3d v1 = constants.f1 * u1;
  const Eigen::Vector3d v2 = constants.f2 * u2;
  const Eigen::Vector3d v3 = -constants.f3 * u3;
  const Eigen::Vector3d v4 = -constants.f4 * u4;

  // Think of the 3 x 3 system in s1, s2, s3
  // [a1 b1 c1] = [d1]
  // [a2 b2 c2] = [d2]
  // [a3 b3 c3] = [d3].
  const double& a1 = v1(0);
  const double& b1 = v2(0);
  const double& c1 = v3(0);
  const double& a2 = v1(1);
  const double& b2 = v2(1);
  const double& c2 = v3(1);
  const double& a3 = v1(2);
  const double& b3 = v2(2);
  const double& c3 = v3(2);

  const double denom =
      a1 * b2 * c3 + b1 * c2 * a3 + c1 * a2 * b3 -
      a3 * b2 * c1 - b3 * c2 * a1 - c3 * a2 * b1;

  const double coeff11 = b2 * c3 - b3 * c2;
  const double coeff12 = c1 * b3 - c3 * b1;
  const double coeff13 = b1 * c2 - b2 * c1;

  const double G1 =
      (-v4(0) * coeff11 - v4(1) * coeff12 - v4(2) * coeff13) / denom;
  const double H1 =
      (-p5(0) * coeff11 - p5(1) * coeff12 - p5(2) * coeff13) / denom;

  const double coeff21 = c2 * a3 - c3 * a2;
  const double coeff22 = a1 * c3 - a3 * c1;
  const double coeff23 = c1 * a2 - c2 * a1;

  const double G2 =
      (-v4(0) * coeff21 - v4(1) * coeff22 - v4(2) * coeff23) / denom;
  const double H2 =
      (-p5(0) * coeff21 - p5(1) * coeff22 - p5(2) * coeff23) / denom;

  const double coeff31 = b3 * a2 - b2 * a3;
  const double coeff32 = a3 * b1 - a1 * b3;
  const double coeff33 = b2 * a1 - b1 * a2;

  const double G3 =
      (-v4(0) * coeff31 - v4(1) * coeff32 - v4(2) * coeff33) / denom;
  const double H3 =
      (-p5(0) * coeff31 - p5(1) * coeff32 - p5(2) * coeff33) / denom;

  const double C1 = 1 - constants.k2;
  const double C2 = 1;
  const double C3 = -constants.k2;
  const double C4 = -2.0 * u1.dot(u2);
  const double C5 = 2.0 * constants.k2 * u1.dot(u3);
  const double C6 = 2.0 * (u1.dot(p6) - constants.k2 * u1.dot(p8));
  const double C7 = -2.0 * u2.dot(p6);
  const double C8 = 2.0 * constants.k2 * u3.dot(p8);
  const double C9 = p6.dot(p6) - constants.k2 * p8.dot(p8);

  // Derive the coefficients of the quadratic.
  const double A =
      C1 * G1 * G1 + C2 * G2 * G2 + C3 * G3 * G3 + C4 * G1 * G2 + C5 * G1 * G3;
  const double B =
      C1 * 2 * G1 * H1 + C2 * 2 * G2 * H2 + C3 * 2 * G3 * H3 +
      C4 * (G1 * H2 + G2 * H1) + C5 * (G1 * H3 + G3 * H1) + C6 * G1 +
      C7 * G2 + C8 * G3;
  const double C =
      C1 * H1 * H1 + C2 * H2 * H2 + C3 * H3 * H3 + C4 * H1 * H2 +
      C5 * H1 * H3 + C6 * H1 + C7 * H2 + C8 * H3 + C9;
  const double disc = (B * B - 4 * A * C);

  Eigen::MatrixXd depths;
  depths.resize(4, 2);
  depths.setZero();

  if (disc >= 0) {
    const double sqrtD = sqrt(disc);
    const double s4a = (-B - sqrtD) / (2 * A);
    const double s4b = (-B + sqrtD) / (2 * A);

    if (s4a > 0) {
      depths(0, 0) = G1 * s4a + H1;
      depths(1, 0) = G2 * s4a + H2;
      depths(2, 0) = G3 * s4a + H3;
      depths(3, 0) = s4a;
    }

    if (s4b > 0) {
      depths(0, 1) = G1 * s4b + H1;
      depths(1, 1) = G2 * s4b + H2;
      depths(2, 1) = G3 * s4b + H3;
      depths(3, 1) = s4b;
    }
  }

  return depths;
}

Eigen::MatrixXd BuildCoefficientMatrix(const Gp4pc::Input& input) {
  Eigen::MatrixXd coeff_mat(kNumConstraints, kNumMonomials);
  coeff_mat.setZero();

  // Useful aliases.
  // Camera centers.
  const std::vector<Eigen::Vector3d>& ray_origins = input.ray_origins;
  // Unit rays originating from ray_origins.
  const std::vector<Eigen::Vector3d>& ray_directions = input.ray_directions;
  // World points.
  const std::vector<Eigen::Vector3d>& world_points = input.world_points;

  // Verify the input is consistent.
  CHECK_EQ(ray_origins.size(), ray_directions.size());
  CHECK_EQ(ray_origins.size(), world_points.size());

  // The mathematical relationship is the following:
  //
  // ray_origins[i] + depth[i] * ray_directions[i] =
  //       s * R * world_points[i] + t,
  //
  // where s is scale, R is a rotation matrix, and t is translation.

  // Input Constants
  const Eigen::Vector3d& p1 = ray_origins[0];
  const Eigen::Vector3d& p2 = ray_origins[1];
  const Eigen::Vector3d& p3 = ray_origins[2];
  const Eigen::Vector3d& p4 = ray_origins[3];

  // Input Constants
  const Eigen::Vector3d& u1 = ray_directions[0];
  const Eigen::Vector3d& u2 = ray_directions[1];
  const Eigen::Vector3d& u3 = ray_directions[2];
  const Eigen::Vector3d& u4 = ray_directions[3];

  // Input Constants
  const Eigen::Vector3d& x1 = world_points[0];
  const Eigen::Vector3d& x2 = world_points[1];
  const Eigen::Vector3d& x3 = world_points[2];
  const Eigen::Vector3d& x4 = world_points[3];

  // Derived Constants
  const DerivedConstants constants =
      CalculateRatiosFromFourPoints(x1, x2, x3, x4);

  // Derived Constants
  const Eigen::Vector3d p5  =
      constants.f1 * p1 + constants.f2 * p2 -
      constants.f3 * p3 - constants.f4 * p4;
  const Eigen::Vector3d p6 = p1 - p2;
  const Eigen::Vector3d p7 = p3 - p4;
  const Eigen::Vector3d p8 = p1 - p3;
  const Eigen::Vector3d p9 = p2 - p4;
  const Eigen::Vector3d p10 = p1 - p4;
  const Eigen::Vector3d p11 = p2 - p3;

  // Derived Constants
  const Eigen::Vector3d v1 = constants.f1 * u1;
  const Eigen::Vector3d v2 = constants.f2 * u2;
  const Eigen::Vector3d v3 = -constants.f3 * u3;
  const Eigen::Vector3d v4 = -constants.f4 * u4;

  // Equation derived from orthogonality constraint 1.
  coeff_mat(0, 0) = v1.dot(u1);
  coeff_mat(0, 1) = -u2.dot(v2);
  coeff_mat(0, 2) = 0;
  coeff_mat(0, 3) = 0;
  coeff_mat(0, 4) = u1.dot(v2) - u2.dot(v1);
  coeff_mat(0, 5) = u1.dot(v3);
  coeff_mat(0, 6) = u1.dot(v4);
  coeff_mat(0, 7) = -u2.dot(v3);
  coeff_mat(0, 8) = -u2.dot(v4);
  coeff_mat(0, 9) = 0;
  coeff_mat(0, 10) = p6.dot(v1) + u1.dot(p5);
  coeff_mat(0, 11) = p6.dot(v2) - u2.dot(p5);
  coeff_mat(0, 12) = p6.dot(v3);
  coeff_mat(0, 13) = p6.dot(v4);
  coeff_mat(0, 14) = p6.dot(p5);

  // Equation derived from orthogonality constraint 2.
  coeff_mat(1, 0) =  0;
  coeff_mat(1, 1) =  0;
  coeff_mat(1, 2) =  u3.dot(v3);
  coeff_mat(1, 3) = -u4.dot(v4);
  coeff_mat(1, 4) =  0;
  coeff_mat(1, 5) =  u3.dot(v1);
  coeff_mat(1, 6) = -u4.dot(v1);
  coeff_mat(1, 7) =  u3.dot(v2);
  coeff_mat(1, 8) = -u4.dot(v2);
  coeff_mat(1, 9) =  u3.dot(v4) - u4.dot(v3);
  coeff_mat(1, 10) =  p7.dot(v1);
  coeff_mat(1, 11) =  p7.dot(v2);
  coeff_mat(1, 12) =  p7.dot(v3) + u3.dot(p5);
  coeff_mat(1, 13) =  p7.dot(v4) - u4.dot(p5);
  coeff_mat(1, 14) =  p7.dot(p5);

  // Coefficients of quadratic polynomial that corresponds to the squared
  // length for edges 1--2, 1--3 and 3--4.
  Eigen::VectorXd d12(kNumMonomials);
  Eigen::VectorXd d13(kNumMonomials);
  Eigen::VectorXd d34(kNumMonomials);
  d12.setZero();
  d13.setZero();
  d34.setZero();

  d12(0) = 1;
  d12(1) = 1;
  d12(4) = -2.0 * u1.dot(u2);
  d12(10) = 2.0 * u1.dot(p6);
  d12(11) = -2.0 * u2.dot(p6);
  d12(14) = p6.dot(p6);

  d13(0) = 1;
  d13(2) = 1;
  d13(5) = -2.0 * u1.dot(u3);
  d13(10) = 2.0 * u1.dot(p8);
  d13(12) = -2.0 * u3.dot(p8);
  d13(14) = p8.dot(p8);

  d34(2) = 1;
  d34(3) = 1;
  d34(9) = -2.0 * u3.dot(u4);
  d34(12) = 2.0 * u3.dot(p7);
  d34(13) = -2.0 * u4.dot(p7);
  d34(14) = p7.dot(p7);

  // Two equations derived from distance ratio constraints.
  coeff_mat.row(2) = d12 - constants.k1 * d34;
  coeff_mat.row(3) = d12 - constants.k2 * d13;

  return coeff_mat;
}

bool IsSimilarityTransformValid(const Eigen::Matrix3d& rotation,
                                const Eigen::Vector3d& translation,
                                const double scale) {
  // Make sure rotation is finite.
  const double* rotation_vals = rotation.data();
  bool is_rotation_finite = true;
  for (int i = 0; i < 9; ++i) {
    is_rotation_finite = is_rotation_finite && std::isfinite(rotation_vals[i]);
  }
  // Make sure translation is finite.
  const bool is_translation_finite =
      std::isfinite(translation(0)) &&
      std::isfinite(translation(1)) &&
      std::isfinite(translation(2));
  // Make scale is finite.
  const bool is_scale_valid = scale > 0.0;
  if (is_rotation_finite && is_translation_finite && is_scale_valid) {
    return true;
  }

  return false;
}

void SolveForRotationAndTranslation(
    const Gp4pc::Input& input,
    const std::vector<Eigen::Vector4d>& plausible_depths,
    Gp4pc::Solution* solution) {
  solution->rotations.reserve(plausible_depths.size());
  solution->translations.reserve(plausible_depths.size());
  solution->scales.reserve(plausible_depths.size());
  solution->depths.reserve(plausible_depths.size());

  // Solve for similarity transform using Umeyama's method. See
  // theia/sfm/transformation/align_point_clouds.h for more information.
  std::vector<Eigen::Vector3d> camera_points(input.world_points.size());
  Eigen::Matrix3d rotation;
  Eigen::Vector3d translation;
  double scale;
  for (const Eigen::Vector4d& depth : plausible_depths) {
    // Compute the camera points.
    camera_points[0] = ComputeCameraPoint(
        input.ray_origins[0], input.ray_directions[0], depth[0]);
    camera_points[1] = ComputeCameraPoint(
        input.ray_origins[1], input.ray_directions[1], depth[1]);
    camera_points[2] = ComputeCameraPoint(
        input.ray_origins[2], input.ray_directions[2], depth[2]);
    camera_points[3] = ComputeCameraPoint(
        input.ray_origins[3], input.ray_directions[3], depth[3]);
    theia::AlignPointCloudsUmeyama(
        input.world_points, camera_points, &rotation, &translation, &scale);

    if (IsSimilarityTransformValid(rotation, translation, scale)) {
      solution->rotations.emplace_back(rotation);
      solution->translations.emplace_back(translation);
      solution->scales.emplace_back(scale);
      solution->depths.emplace_back(depth);
    }
  }
}

}  // namespace

Gp4pc::Gp4pc(const Params& params) : params_(params) {}

std::vector<Eigen::Vector4d>
Gp4pc::KeepPlausibleSolutions(const Eigen::MatrixXcd& solutions) {
  std::vector<Eigen::Vector4d> plausible_solutions;
  plausible_solutions.reserve(kMaxNumDepthSolutions);

  // Iterate through the possible solutions.
  Eigen::Vector4d estimated_solution;
  double discriminant = 0.0;
  double max_imag_entry = 0.0;
  Eigen::Vector4d prev_solution;
  double min_depth = 0.0;
  prev_solution.setZero();
  for (int i = 0; i < solutions.cols(); ++i) {
    max_imag_entry = std::max(std::abs(solutions(0, i).imag()),
                              std::abs(solutions(1, i).imag()));
    max_imag_entry = std::max(max_imag_entry, std::abs(solutions(2, i).imag()));
    max_imag_entry = std::max(max_imag_entry, std::abs(solutions(3, i).imag()));
    // TODO(vfragoso): Threshold solutions with large imaginary parts. Set a
    // a good threshold for getting good solutions with small imaginary parts.
    const bool real_solution = (max_imag_entry <= kRealNumberThreshold);
    VLOG(4) << "Max imag entry: " << max_imag_entry
            << " => real_solution: " << real_solution;

    // Keep the solutions that correspond to positive depths.
    estimated_solution[0] = solutions(0, i).real();
    estimated_solution[1] = solutions(1, i).real();
    estimated_solution[2] = solutions(2, i).real();
    estimated_solution[3] = solutions(3, i).real();

    min_depth = estimated_solution.minCoeff();
    const bool all_positive_depths = (min_depth >= 0.0);
    VLOG(4) << "Min. depth: " << min_depth
            << " => " << all_positive_depths
            << " " << estimated_solution.transpose();

    // Avoid duplicated solutions coming from those with small imaginary parts.
    // Remove duplicate solutions which are contigous.
    const double error = (prev_solution - estimated_solution).squaredNorm();
    const bool is_unique_soln = (error >= kDuplicateSolnErrorThreshold);

    // Save previous solution, regardless.
    VLOG(4) << "Unique: " << is_unique_soln << " "
            << prev_solution.transpose() << " <-> "
            << estimated_solution.transpose();
    prev_solution = estimated_solution;
    if (real_solution && all_positive_depths && is_unique_soln) {
      // Save solution.
      plausible_solutions.emplace_back(std::move(estimated_solution));
    }
  }

  VLOG(4) << "Plausible depths: " << plausible_solutions.size()
          << " out of " << solutions.cols();
  return plausible_solutions;
}

std::vector<Eigen::Vector4d>
Gp4pc::SolveForPoseViaPlanarSolver(const Gp4pc::Input& input) {
  VLOG(4) << "Solving pose using planar solver...";
  // Solve for depths via the planar solver.
  const Eigen::MatrixXcd depths = SolveForDepthsFromAPlanarInput(input);

  // Discard bad solutions.
  const std::vector<Eigen::Vector4d> final_depths =
      KeepPlausibleSolutions(depths);

  return final_depths;
}

std::vector<Eigen::Vector4d>
Gp4pc::SolveForPoseViaGeneralSolver(const Input& input) {
  VLOG(4) << "Solving for pose!";
  // Build coefficient matrix.
  const Eigen::MatrixXd coefficients_matrix = BuildCoefficientMatrix(input);
  VLOG(4) << "Coeff matrix: \n" << coefficients_matrix;

  // Solve for depths.
  const Eigen::MatrixXcd depths = SolveGp4pcPolynomial(coefficients_matrix);

  // Discard bad solutions.
  const std::vector<Eigen::Vector4d> final_depths =
      KeepPlausibleSolutions(depths);

  return final_depths;
}

bool Gp4pc::EstimateSimilarityTransformation(const Input& input,
                                             Solution* solution) {
  using Eigen::MatrixXcd;
  using Eigen::Vector4d;
  CHECK_EQ(input.ray_origins.size(), kSizeOfMinimalSample);
  CHECK_EQ(input.ray_origins.size(), input.ray_directions.size());
  CHECK_EQ(input.ray_origins.size(), input.world_points.size());
  CHECK_NOTNULL(solution)->rotations.clear();
  solution->translations.clear();
  solution->scales.clear();
  solution->depths.clear();

  // Validate input.
  if (!IsInputValid(input)) {
    return false;
  }

  // Each entry is the depth wrt to a camera center.
  MatrixXcd depths;
  std::vector<Vector4d> plausible_depths;
  plausible_depths.reserve(kMaxNumDepthSolutions);

  // Check whether the input is planar.
  bool is_planar = false;
  const bool use_planar_solver = !params_.use_general_solver;
  if (IsInputDegenerateOrPlanar(input,
                                params_.coplanar_threshold,
                                params_.colinear_threshold,
                                &is_planar)) {
    VLOG(4) << "Degenerate case. Could not compute pose.";
    return false;
  } else if (use_planar_solver && is_planar) {
    VLOG(4) << "Coplanar case.";
    // Solve for the planar case.
    plausible_depths = SolveForPoseViaPlanarSolver(input);
  } else {
    VLOG(4) << "Using general solver.";
    plausible_depths = SolveForPoseViaGeneralSolver(input);
  }

  if (plausible_depths.empty()) {
    return false;
  }
  VLOG(4) << "Number of plausible depths: " << plausible_depths.size();

  // Solve for rotation and translation.
  SolveForRotationAndTranslation(input, plausible_depths, solution);
  VLOG(4) << "Number of final solutions: " << solution->rotations.size();

  return !solution->rotations.empty();
}

}  // namespace msft


/*
 * PoseOptimizationSQP.cpp
 *
 *  Created on: Jun 10, 2015
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich
 */

#include "free_gait_core/pose_optimization/PoseOptimizationSQP.hpp"
#include "free_gait_core/pose_optimization/PoseParameterization.hpp"
#include "free_gait_core/pose_optimization/PoseOptimizationProblem.hpp"

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <kindr/Core>
#include <message_logger/message_logger.hpp>
#include <numopt_quadprog/ActiveSetFunctionMinimizer.hpp>
#include <numopt_sqp/SQPFunctionMinimizer.hpp>

#include <functional>

namespace free_gait {

PoseOptimizationSQP::PoseOptimizationSQP(const AdapterBase& adapter)
    : PoseOptimizationBase(adapter),
      timer_("PoseOptimizationSQP"),
      durationInCallback_(0.0),
      nIterations_(0)
{
  objective_.reset(new PoseOptimizationObjectiveFunction());

  constraints_.reset(new PoseOptimizationFunctionConstraints());
  PoseOptimizationFunctionConstraints::LegPositions positionsBaseToHipInBaseFrame;
  for (const auto& limb : adapter_.getLimbs()) {
    positionsBaseToHipInBaseFrame[limb] = adapter_.getPositionBaseToHipInBaseFrame(limb);
  }
  constraints_->setPositionsBaseToHip(positionsBaseToHipInBaseFrame);

  timer_.setAlpha(1.0);
}

PoseOptimizationSQP::~PoseOptimizationSQP()
{
}

void PoseOptimizationSQP::setCurrentState(const State& state)
{
  originalState_ = state;
}

void PoseOptimizationSQP::registerOptimizationStepCallback(OptimizationStepCallbackFunction callback)
{
  optimizationStepCallback_ = callback;
}

bool PoseOptimizationSQP::optimize(Pose& pose)
{
  timer_.pinTime("total");
  durationInCallback_ = 0.0;
  state_ = originalState_;
  checkSupportRegion();

  objective_->setInitialPose(pose);
  objective_->setStance(stance_);
  objective_->setNominalStance(nominalStanceInBaseFrame_);
  objective_->setSupportRegion(supportRegion_);

  constraints_->setStance(stance_);
  constraints_->setSupportRegion(supportRegion_);
  constraints_->setLimbLengthConstraints(minLimbLenghts_, maxLimbLenghts_);

  state_.setPoseBaseToWorld(pose);
  adapter_.setInternalDataFromState(state_, false, true, false, false); // To guide IK.
  updateJointPositionsInState(state_); // For CoM calculation.
  adapter_.setInternalDataFromState(state_, false, true, false, false);
  const Position centerOfMassInBaseFrame(adapter_.transformPosition(adapter_.getWorldFrameId(), adapter_.getBaseFrameId(),
                                 adapter_.getCenterOfMassInWorldFrame()));
  objective_->setCenterOfMass(centerOfMassInBaseFrame);
  constraints_->setCenterOfMass(centerOfMassInBaseFrame);
  callExternalOptimizationStepCallback(0);

  // Optimize.
  PoseOptimizationProblem problem(objective_, constraints_);
  std::shared_ptr<numopt_common::QuadraticProblemSolver> qpSolver(
      new numopt_quadprog::ActiveSetFunctionMinimizer);
  numopt_sqp::SQPFunctionMinimizer solver(qpSolver, 30, 0.01, 3, -DBL_MAX);
  solver.registerOptimizationStepCallback(
      std::bind(&PoseOptimizationSQP::optimizationStepCallback, this, std::placeholders::_1, std::placeholders::_2,
                std::placeholders::_3, std::placeholders::_4));
  solver.setCheckConstraints(false);
  solver.setPrintOutput(false);
  PoseParameterization params;
  params.setPose(pose);
  double functionValue;
  if (!solver.minimize(&problem, params, functionValue)) return false;
  pose = params.getPose();
  // TODO Fix unit quaternion?

  timer_.splitTime("total");
  return true;
}

void PoseOptimizationSQP::optimizationStepCallback(const size_t iterationStep,
                                                   const numopt_common::Parameterization& parameters,
                                                   const double functionValue,
                                                   const bool finalIteration)
{
  nIterations_ = iterationStep;
  auto& poseParameterization = dynamic_cast<const PoseParameterization&>(parameters);

  // Update center of mass. // TODO Make optional.
//  state_.setPoseBaseToWorld(poseParameterization.getPose());
//  state_.setAllJointPositions(originalState_.getJointPositions());
//  adapter_.setInternalDataFromState(state_, false, true, false, false);
//  updateJointPositionsInState(state_);
//  adapter_.setInternalDataFromState(state_, false, true, false, false); // TODO Improve efficiency.
//  const Position centerOfMassInBaseFrame(adapter_.transformPosition(adapter_.getWorldFrameId(), adapter_.getBaseFrameId(),
//                                 adapter_.getCenterOfMassInWorldFrame()));
//  objective_->setCenterOfMass(centerOfMassInBaseFrame);
//  constraints_->setCenterOfMass(centerOfMassInBaseFrame);

  if (optimizationStepCallback_) {
    timer_.pinTime("callback");
    auto& poseParameterization = dynamic_cast<const PoseParameterization&>(parameters);
    state_.setPoseBaseToWorld(poseParameterization.getPose());
    timer_.splitTime("callback");
    durationInCallback_ += timer_.getAverageElapsedTimeUSec("callback");
  }

  callExternalOptimizationStepCallback(iterationStep + 1, functionValue, finalIteration);
}

void PoseOptimizationSQP::callExternalOptimizationStepCallbackWithPose(const Pose& pose, const size_t iterationStep,
                                                                       const double functionValue,
                                                                       const bool finalIteration)
{
  state_ = originalState_;
  state_.setPoseBaseToWorld(pose);
  callExternalOptimizationStepCallback(iterationStep, functionValue, finalIteration);
}

double PoseOptimizationSQP::getOptimizationDuration() const
{
  return timer_.getAverageElapsedTimeUSec("total") - durationInCallback_;
}

size_t PoseOptimizationSQP::getNumberOfIterations() const
{
  return nIterations_;
}

void PoseOptimizationSQP::callExternalOptimizationStepCallback(const size_t iterationStep, const double functionValue,
                                                               const bool finalIteration)
{
  if (optimizationStepCallback_) {
    timer_.pinTime("callback");
    adapter_.setInternalDataFromState(state_, false, true, false, false);
    State previewState(state_);
    updateJointPositionsInState(previewState);
    optimizationStepCallback_(iterationStep, previewState, functionValue, finalIteration);
    timer_.splitTime("callback");
    durationInCallback_ += timer_.getAverageElapsedTimeUSec("callback");
  }
}

} /* namespace */


#ifndef STAN_MATH_PRIM_SCAL_PROB_GAMMA_LPDF_HPP
#define STAN_MATH_PRIM_SCAL_PROB_GAMMA_LPDF_HPP

#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/scal/meta/operands_and_partials.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_greater_or_equal.hpp>
#include <stan/math/prim/scal/err/check_less_or_equal.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/fun/gamma_p.hpp>
#include <stan/math/prim/scal/fun/digamma.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <stan/math/prim/scal/fun/grad_reg_inc_gamma.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>

namespace stan {
  namespace math {

    /**
     * The log of a gamma density for y with the specified
     * shape and inverse scale parameters.
     * Shape and inverse scale parameters must be greater than 0.
     * y must be greater than or equal to 0.
     *
     \f{eqnarray*}{
     y &\sim& \mbox{\sf{Gamma}}(\alpha, \beta) \\
     \log (p (y \, |\, \alpha, \beta) ) &=& \log \left( \frac{\beta^\alpha}{\Gamma(\alpha)} y^{\alpha - 1} \exp^{- \beta y} \right) \\
     &=& \alpha \log(\beta) - \log(\Gamma(\alpha)) + (\alpha - 1) \log(y) - \beta y\\
     & & \mathrm{where} \; y > 0
     \f}
     * @param y A scalar variable.
     * @param alpha Shape parameter.
     * @param beta Inverse scale parameter.
     * @throw std::domain_error if alpha is not greater than 0.
     * @throw std::domain_error if beta is not greater than 0.
     * @throw std::domain_error if y is not greater than or equal to 0.
     * @tparam T_y Type of scalar.
     * @tparam T_shape Type of shape.
     * @tparam T_inv_scale Type of inverse scale.
     */
    template <bool propto,
              typename T_y, typename T_shape, typename T_inv_scale>
    typename return_type<T_y, T_shape, T_inv_scale>::type
    gamma_lpdf(const T_y& y, const T_shape& alpha, const T_inv_scale& beta) {
      static const char* function("gamma_lpdf");
      typedef typename stan::partials_return_type<T_y, T_shape,
                                                  T_inv_scale>::type
        T_partials_return;

      using stan::is_constant_struct;

      if (!(stan::length(y) && stan::length(alpha) && stan::length(beta)))
        return 0.0;

      T_partials_return logp(0.0);

      check_not_nan(function, "Random variable", y);
      check_positive_finite(function, "Shape parameter", alpha);
      check_positive_finite(function, "Inverse scale parameter", beta);
      check_consistent_sizes(function,
                             "Random variable", y,
                             "Shape parameter", alpha,
                             "Inverse scale parameter", beta);

      if (!include_summand<propto, T_y, T_shape, T_inv_scale>::value)
        return 0.0;

      scalar_seq_view<T_y> y_vec(y);
      scalar_seq_view<T_shape> alpha_vec(alpha);
      scalar_seq_view<T_inv_scale> beta_vec(beta);

      for (size_t n = 0; n < length(y); n++) {
        const T_partials_return y_dbl = value_of(y_vec[n]);
        if (y_dbl < 0)
          return LOG_ZERO;
      }

      size_t N = max_size(y, alpha, beta);
      operands_and_partials<T_y, T_shape, T_inv_scale>
        ops_partials(y, alpha, beta);

      using boost::math::lgamma;
      using boost::math::digamma;
      using std::log;

      VectorBuilder<include_summand<propto, T_y, T_shape>::value,
                    T_partials_return, T_y> log_y(length(y));
      if (include_summand<propto, T_y, T_shape>::value) {
        for (size_t n = 0; n < length(y); n++) {
          if (value_of(y_vec[n]) > 0)
            log_y[n] = log(value_of(y_vec[n]));
        }
      }

      VectorBuilder<include_summand<propto, T_shape>::value,
                    T_partials_return, T_shape> lgamma_alpha(length(alpha));
      VectorBuilder<!is_constant_struct<T_shape>::value,
                    T_partials_return, T_shape> digamma_alpha(length(alpha));
      for (size_t n = 0; n < length(alpha); n++) {
        if (include_summand<propto, T_shape>::value)
          lgamma_alpha[n] = lgamma(value_of(alpha_vec[n]));
        if (!is_constant_struct<T_shape>::value)
          digamma_alpha[n] = digamma(value_of(alpha_vec[n]));
      }

      VectorBuilder<include_summand<propto, T_shape, T_inv_scale>::value,
                    T_partials_return, T_inv_scale> log_beta(length(beta));
      if (include_summand<propto, T_shape, T_inv_scale>::value) {
        for (size_t n = 0; n < length(beta); n++)
          log_beta[n] = log(value_of(beta_vec[n]));
      }

      for (size_t n = 0; n < N; n++) {
        const T_partials_return y_dbl = value_of(y_vec[n]);
        const T_partials_return alpha_dbl = value_of(alpha_vec[n]);
        const T_partials_return beta_dbl = value_of(beta_vec[n]);

        if (include_summand<propto, T_shape>::value)
          logp -= lgamma_alpha[n];
        if (include_summand<propto, T_shape, T_inv_scale>::value)
          logp += alpha_dbl * log_beta[n];
        if (include_summand<propto, T_y, T_shape>::value)
          logp += (alpha_dbl - 1.0) * log_y[n];
        if (include_summand<propto, T_y, T_inv_scale>::value)
          logp -= beta_dbl * y_dbl;

        if (!is_constant_struct<T_y>::value)
          ops_partials.edge1_.partials_[n] += (alpha_dbl - 1) / y_dbl
            - beta_dbl;
        if (!is_constant_struct<T_shape>::value)
          ops_partials.edge2_.partials_[n] += -digamma_alpha[n] + log_beta[n]
            + log_y[n];
        if (!is_constant_struct<T_inv_scale>::value)
          ops_partials.edge3_.partials_[n] += alpha_dbl / beta_dbl - y_dbl;
      }
      return ops_partials.build(logp);
    }

    template <typename T_y, typename T_shape, typename T_inv_scale>
    inline
    typename return_type<T_y, T_shape, T_inv_scale>::type
    gamma_lpdf(const T_y& y, const T_shape& alpha, const T_inv_scale& beta) {
      return gamma_lpdf<false>(y, alpha, beta);
    }

  }
}
#endif