#include <catch2/catch.hpp>

#include "constitutive/internal_variables.hpp"
#include "constitutive/mechanics/solid/affine_microsphere.hpp"
#include "constitutive/mechanics/solid/compressible_neohooke.hpp"
#include "constitutive/mechanics/plane/isotropic_linear_elasticity.hpp"
#include "constitutive/constitutive_model_factory.hpp"

#include "exceptions.hpp"
#include "io/json.hpp"

#include <Eigen/Eigenvalues>

#include <iostream>

std::string json_input_file()
{
    return "{\"name\": \"rubber\", \"elastic_modulus\": 2.0, \"poissons_ratio\": 0.45}";
}

constexpr auto internal_variable_size = 2;
constexpr auto ZERO_MARGIN = 1.0e-5;

using neon::json;
using namespace neon;

TEST_CASE("Neo-Hookean model")
{
    using namespace neon::mechanics::solid;

    auto variables = std::make_shared<internal_variables_t>(internal_variable_size);

    // Add the required variables for an updated Lagrangian formulation
    variables->add(variable::second::deformation_gradient, variable::second::cauchy_stress);
    variables->add(variable::scalar::DetF);

    auto neo_hooke = make_constitutive_model(variables,
                                             json::parse(json_input_file()),
                                             json::parse("{\"constitutive\" : {\"name\": "
                                                         "\"neohooke\"} }"));

    // Get the tensor variables
    auto [F_list, cauchy_stresses] = variables->get(variable::second::deformation_gradient,
                                                    variable::second::cauchy_stress);

    auto& J_list = variables->get(variable::scalar::DetF);

    // Fill with identity matrix
    for (auto& F : F_list) F = neon::matrix3::Identity();
    for (auto& J : J_list) J = 1.0;

    neo_hooke->update_internal_variables(1.0);

    Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver;

    SECTION("Sanity check")
    {
        // Ensure the internal variables are allocated correctly
        REQUIRE(F_list.size() == internal_variable_size);
        REQUIRE(cauchy_stresses.size() == internal_variable_size);
        REQUIRE(J_list.size() == internal_variable_size);

        REQUIRE(neo_hooke->is_finite_deformation());
        REQUIRE(neo_hooke->intrinsic_material().name() == "rubber");
    }
    SECTION("Update of internal variables")
    {
        for (auto& cauchy_stress : cauchy_stresses)
        {
            REQUIRE(cauchy_stress.norm() == Approx(0.0).margin(ZERO_MARGIN));
        }
    }
    SECTION("Check of material tangent")
    {
        // Get the matrix variable
        auto& material_tangents = variables->get(variable::fourth::tangent_operator);

        for (auto const& material_tangent : material_tangents)
        {
            // Check a few of the numerical values
            REQUIRE(material_tangent(0, 0) == Approx(material_tangent(1, 1)));
            REQUIRE(material_tangent(0, 1) == Approx(material_tangent(0, 2)));
            REQUIRE(material_tangent(1, 1) == Approx(material_tangent(2, 2)));
            REQUIRE(material_tangent(3, 3) == Approx(material_tangent(4, 4)));
            REQUIRE(material_tangent(5, 5) == Approx(material_tangent(4, 4)));

            // Ensure symmetry is correct
            REQUIRE((material_tangent - material_tangent.transpose()).norm()
                    == Approx(0.0).margin(ZERO_MARGIN));

            eigen_solver.compute(material_tangent);

            REQUIRE((eigen_solver.eigenvalues().real().array() > 0.0).all());
        }
    }
}
TEST_CASE("microsphere model error test")
{
    using namespace neon::mechanics::solid;

    auto variables = std::make_shared<internal_variables_t>(internal_variable_size);

    SECTION("type not specified")
    {
        REQUIRE_THROWS_AS(make_constitutive_model(variables,
                                                  json::parse("{}"),
                                                  json::parse("{\"constitutive\" : {\"name\": "
                                                              "\"microsphere\", \"Tyasdpe\" "
                                                              ": \"affine\"}}")),
                          std::domain_error);
    }
    SECTION("type not specified correctly")
    {
        // clang-format off
        auto const input{"{\"constitutive\":{\"name\":\"microsphere\",\"type\": \"Afwsfine\"}}"};
        // clang-format on

        REQUIRE_THROWS_AS(make_constitutive_model(variables, json::parse("{}"), json::parse(input)),
                          std::domain_error);
    }
    SECTION("Exception for quadrature scheme")
    {
        // clang-format off
        auto const input{"{\"constitutive\":{\"name\":\"microsphere\",\"type\": \"affine\",\"statistics\":\"gaussian\"}}"};
        // clang-format on

        REQUIRE_THROWS_AS(make_constitutive_model(variables, json::parse("{}"), json::parse(input)),
                          std::domain_error);
    }
}
TEST_CASE("Gaussian affine microsphere")
{
    using namespace neon::mechanics::solid;

    auto variables = std::make_shared<internal_variables_t>(internal_variable_size);

    // Add the required variables for an updated Lagrangian formulation
    variables->add(variable::second::deformation_gradient, variable::second::cauchy_stress);

    variables->add(variable::scalar::DetF);

    auto affine = make_constitutive_model(variables,
                                          json::parse("{\"name\" : \"rubber\", "
                                                      "\"elastic_modulus\" : 10.0e6, "
                                                      "\"poissons_ratio\" : 0.45, "
                                                      "\"segments_per_chain\" : 50}"),
                                          json::parse("{\"constitutive\" : {\"name\": "
                                                      "\"microsphere\", \"type\" "
                                                      ": \"affine\", \"statistics\":\"gaussian\",  "
                                                      "\"quadrature\" : \"BO21\"}}"));

    auto [F_list, cauchy_stresses] = variables->get(variable::second::deformation_gradient,
                                                    variable::second::cauchy_stress);

    auto& J_list = variables->get(variable::scalar::DetF);

    for (auto& J : J_list) J = 1.0;

    auto& material_tangents = variables->get(variable::fourth::tangent_operator);

    Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver;

    SECTION("Sanity checks")
    {
        REQUIRE(affine->is_symmetric());
        REQUIRE(affine->is_finite_deformation());
        REQUIRE(affine->intrinsic_material().name() == "rubber");
    }
    SECTION("affine model under no load")
    {
        // Fill with identity matrix
        for (auto& F : F_list) F = neon::matrix3::Identity();

        affine->update_internal_variables(1.0);

        for (auto const& material_tangent : material_tangents)
        {
            REQUIRE((material_tangent - material_tangent.transpose()).norm()
                    == Approx(0.0).margin(ZERO_MARGIN));

            eigen_solver.compute(material_tangent);
            REQUIRE((eigen_solver.eigenvalues().real().array() > 0.0).all());
        }

        for (auto& cauchy_stress : cauchy_stresses)
        {
            REQUIRE(cauchy_stress.norm() == Approx(0.0).margin(ZERO_MARGIN));
        }
    }
    SECTION("affine model under uniaxial load")
    {
        for (auto& F : F_list)
        {
            F(0, 0) = 1.1;
            F(1, 1) = 1.0 / std::sqrt(1.1);
            F(2, 2) = 1.0 / std::sqrt(1.1);
        }

        affine->update_internal_variables(1.0);

        for (auto const& material_tangent : material_tangents)
        {
            REQUIRE((material_tangent - material_tangent.transpose()).norm()
                    == Approx(0.0).margin(ZERO_MARGIN));

            eigen_solver.compute(material_tangent);
            REQUIRE((eigen_solver.eigenvalues().real().array() > 0.0).all());
        }

        for (auto& cauchy_stress : cauchy_stresses)
        {
            REQUIRE(cauchy_stress.norm() > 0.0);
        }
    }
}
TEST_CASE("Affine microsphere")
{
    using namespace neon::mechanics::solid;

    auto variables = std::make_shared<internal_variables_t>(internal_variable_size);

    // Add the required variables for an updated Lagrangian formulation
    variables->add(variable::second::deformation_gradient, variable::second::cauchy_stress);

    variables->add(variable::scalar::DetF);

    auto affine = make_constitutive_model(variables,
                                          json::parse("{\"name\" : \"rubber\", "
                                                      "\"elastic_modulus\" : 10.0e6, "
                                                      "\"poissons_ratio\" : 0.45, "
                                                      "\"segments_per_chain\" : 50}"),
                                          json::parse("{\"constitutive\" : {\"name\": "
                                                      "\"microsphere\", \"type\" "
                                                      ": \"affine\", \"statistics\":\"langevin\", "
                                                      "\"quadrature\" : \"BO21\"}}"));

    auto [F_list, cauchy_stresses] = variables->get(variable::second::deformation_gradient,
                                                    variable::second::cauchy_stress);

    auto& J_list = variables->get(variable::scalar::DetF);

    for (auto& J : J_list) J = 1.0;

    auto& material_tangents = variables->get(variable::fourth::tangent_operator);

    Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver;

    SECTION("Sanity checks")
    {
        REQUIRE(affine->is_symmetric());
        REQUIRE(affine->is_finite_deformation());
        REQUIRE(affine->intrinsic_material().name() == "rubber");
    }
    SECTION("affine model under no load")
    {
        // Fill with identity matrix
        for (auto& F : F_list) F = neon::matrix3::Identity();

        affine->update_internal_variables(1.0);

        for (auto const& material_tangent : material_tangents)
        {
            REQUIRE((material_tangent - material_tangent.transpose()).norm()
                    == Approx(0.0).margin(ZERO_MARGIN));

            eigen_solver.compute(material_tangent);
            REQUIRE((eigen_solver.eigenvalues().real().array() > 0.0).all());
        }

        for (auto& cauchy_stress : cauchy_stresses)
        {
            REQUIRE(cauchy_stress.norm() == Approx(0.0).margin(ZERO_MARGIN));
        }
    }
    SECTION("affine model under uniaxial load")
    {
        for (auto& F : F_list)
        {
            F(0, 0) = 1.1;
            F(1, 1) = 1.0 / std::sqrt(1.1);
            F(2, 2) = 1.0 / std::sqrt(1.1);
        }

        affine->update_internal_variables(1.0);

        for (auto const& material_tangent : material_tangents)
        {
            REQUIRE((material_tangent - material_tangent.transpose()).norm()
                    == Approx(0.0).margin(ZERO_MARGIN));

            eigen_solver.compute(material_tangent);
            REQUIRE((eigen_solver.eigenvalues().real().array() > 0.0).all());
        }

        for (auto& cauchy_stress : cauchy_stresses)
        {
            REQUIRE(cauchy_stress.norm() > 0.0);
        }
    }
}
TEST_CASE("Nonaffine microsphere")
{
    using namespace neon::mechanics::solid;

    auto variables = std::make_shared<internal_variables_t>(internal_variable_size);

    // Add the required variables for an updated Lagrangian formulation
    variables->add(variable::second::deformation_gradient, variable::second::cauchy_stress);
    variables->add(variable::scalar::DetF);

    auto affine = make_constitutive_model(variables,
                                          json::parse("{\"name\" : \"rubber\", "
                                                      "\"elastic_modulus\" : 10.0e6, "
                                                      "\"poissons_ratio\" : 0.45, "
                                                      "\"nonaffine_stretch_parameter\":1.0, "
                                                      "\"segments_per_chain\" : 50}"),
                                          json::parse("{\"constitutive\" : {\"name\": "
                                                      "\"microsphere\", \"type\" "
                                                      ": \"nonaffine\", \"quadrature\" : "
                                                      "\"BO21\"}}"));

    // Get the tensor variables
    auto [F_list, cauchy_stresses] = variables->get(variable::second::deformation_gradient,
                                                    variable::second::cauchy_stress);

    auto& J_list = variables->get(variable::scalar::DetF);

    for (auto& J : J_list) J = 1.0;

    auto& material_tangents = variables->get(variable::fourth::tangent_operator);

    Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver;

    SECTION("exception")
    {
        REQUIRE_THROWS_AS(make_constitutive_model(variables,
                                                  json::parse("{\"name\" : \"rubber\", "
                                                              "\"elastic_modulus\" : 10.0e6, "
                                                              "\"poissons_ratio\" : 0.45, "
                                                              "\"nonaffine_Strearameter\":1."
                                                              "0, "
                                                              "\"segments_per_chain\" : 50}"),
                                                  json::parse("{\"constitutive\" : "
                                                              "{\"name\": "
                                                              "\"microsphere\", \"type\" "
                                                              ": \"nonaffine\", \"quadrature\" "
                                                              ": "
                                                              "\"BO21\"}}")),
                          std::domain_error);
    }
    SECTION("Sanity test")
    {
        REQUIRE(affine->is_symmetric());
        REQUIRE(affine->is_finite_deformation());
        REQUIRE(affine->intrinsic_material().name() == "rubber");
    }
    SECTION("Nonaffine model under no load")
    {
        // Fill with identity matrix
        for (auto& F : F_list) F = neon::matrix3::Identity();

        affine->update_internal_variables(1.0);

        for (auto const& material_tangent : material_tangents)
        {
            // Ensure symmetry is correct
            REQUIRE((material_tangent - material_tangent.transpose()).norm()
                    == Approx(0.0).margin(ZERO_MARGIN));

            eigen_solver.compute(material_tangent);
            REQUIRE((eigen_solver.eigenvalues().real().array() > 0.0).all());
        }

        for (auto& cauchy_stress : cauchy_stresses)
        {
            REQUIRE(cauchy_stress.norm() == Approx(0.0).margin(ZERO_MARGIN));
        }
    }
    SECTION("Nonaffine model under uniaxial load")
    {
        for (auto& F : F_list)
        {
            F(0, 0) = 1.1;
            F(1, 1) = 1.0 / std::sqrt(1.1);
            F(2, 2) = 1.0 / std::sqrt(1.1);
        }

        affine->update_internal_variables(1.0);

        for (auto const& material_tangent : material_tangents)
        {
            // Ensure symmetry is correct
            REQUIRE((material_tangent - material_tangent.transpose()).norm()
                    == Approx(0.0).margin(ZERO_MARGIN));

            eigen_solver.compute(material_tangent);
            REQUIRE((eigen_solver.eigenvalues().real().array() > 0.0).all());
        }

        for (auto& cauchy_stress : cauchy_stresses)
        {
            REQUIRE(cauchy_stress.norm() > 0.0);
        }
    }
}


/********************************************************************************
*  This file is part of CinoLib                                                 *
*  Copyright(C) 2016: Marco Livesu                                              *
*                                                                               *
*  The MIT License                                                              *
*                                                                               *
*  Permission is hereby granted, free of charge, to any person obtaining a      *
*  copy of this software and associated documentation files (the "Software"),   *
*  to deal in the Software without restriction, including without limitation    *
*  the rights to use, copy, modify, merge, publish, distribute, sublicense,     *
*  and/or sell copies of the Software, and to permit persons to whom the        *
*  Software is furnished to do so, subject to the following conditions:         *
*                                                                               *
*  The above copyright notice and this permission notice shall be included in   *
*  all copies or substantial portions of the Software.                          *
*                                                                               *
*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR   *
*  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,     *
*  FITNESS FOR A PARTICULAR PURPOSE AND NON INFRINGEMENT. IN NO EVENT SHALL THE *
*  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER       *
*  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING      *
*  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS *
*  IN THE SOFTWARE.                                                             *
*                                                                               *
*  Author(s):                                                                   *
*                                                                               *
*     Marco Livesu (marco.livesu@gmail.com)                                     *
*     http://pers.ge.imati.cnr.it/livesu/                                       *
*                                                                               *
*     Italian National Research Council (CNR)                                   *
*     Institute for Applied Mathematics and Information Technologies (IMATI)    *
*     Via de Marini, 6                                                          *
*     16149 Genoa,                                                              *
*     Italy                                                                     *
*********************************************************************************/
#include <cinolib/laplacian.h>
#include <cinolib/symbols.h>
#include <Eigen/Sparse>

namespace cinolib
{

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<class M, class V, class E, class P>
CINO_INLINE
std::vector<Eigen::Triplet<double>> laplacian_matrix_entries(const AbstractMesh<M,V,E,P> & m,
                                                             const int mode,
                                                             const int n) // diagonally replicate n times
{
    std::vector<Entry> entries;

    unsigned int nv = m.num_verts();
    std::vector<unsigned int> base(n);
    for(int i=0; i<n; ++i) base[i] = nv*i;

    for(unsigned int vid=0; vid<m.num_verts(); ++vid)
    {
        std::vector<std::pair<unsigned int,double>> wgts;
        m.vert_weights(vid, mode, wgts);
        double sum = 0.0;
        for(auto item : wgts)
        {
            for(int i=0; i<n; ++i)
            {
                entries.push_back(Entry(base[i] + vid, base[i] + item.first, item.second));
            }
            sum -= item.second;
        }
        if(sum == 0.0)
        {
            std::cerr << "WARNING: null row in the matrix! (disconnected vertex? I put 1 in the diagonal)" << std::endl;
            sum = 1.0;
        }
        for(int i=0; i<n; ++i)
        {
            entries.push_back(Entry(base[i] + vid, base[i] + vid, sum));
        }
    }

    return entries;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<class M, class V, class E, class P>
CINO_INLINE
Eigen::SparseMatrix<double> laplacian(const AbstractMesh<M,V,E,P> & m, const int mode, const int n)
{
    std::vector<Entry> entries = laplacian_matrix_entries(m, mode, n);

    unsigned int nv = n*m.num_verts();
    Eigen::SparseMatrix<double> L(nv,nv);
    L.setFromTriplets(entries.begin(), entries.end());

    return L;
}

}


//
// Created by AIYOJ on 2019-03-04.
//

#include "calibration/hybrid.h"
#include "log/algocomp_log.h"
#include <exception>
#include <fstream>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <exception>

namespace algocomp {
    Hybrid::Hybrid() {
        _logger = spdlog::get(ACLog::_logger_name);
        _logger->info("Initialize Hybrid.");
    }

    Hybrid::~Hybrid() {
        _logger->info("Leave Hybrid.");
    }

    std::vector<float> Hybrid::calibrate(const std::vector<float> &scores) {
        std::vector<float> calibrated_scores;
        calibrated_scores.reserve(scores.size());
        std::for_each(scores.begin(), scores.end(), [&calibrated_scores, this](float score) {
            float calibrated_score = score / (score + (1 - score) * (1 / _neg_smp_r));
            calibrated_scores.push_back(calibrated_score);
        });

        std::for_each(calibrated_scores.begin(), calibrated_scores.end(), [this](float &score) {
            auto calibrated_score = get(score, _search_type);
            score = calibrated_score;
        });

        return calibrated_scores;
    }

    float Hybrid::calibrate(float score) {
        float score_ = score / (score + (1 - score) * (1 / _neg_smp_r));
        float calibrated_score = get(score_, _search_type);
        return calibrated_score;
    }

    std::unique_ptr<Hybrid> Hybrid::load(const std::string &path1, const std::string &path2) {
        auto logger = spdlog::get(ACLog::_logger_name);
        try {
            std::ifstream prior_correction_file;
            prior_correction_file.open(path1);

            std::ifstream isotonic_regression_file;
            isotonic_regression_file.open(path2);

            if (prior_correction_file.is_open() && isotonic_regression_file.is_open()) {
                std::unique_ptr<Hybrid> output_calibration = std::unique_ptr<Hybrid>(
                        new Hybrid());
                std::string line;
                int read = 0;
                while (!prior_correction_file.eof()) {
                    std::getline(prior_correction_file, line);
                    line = boost::algorithm::trim_copy(line);
                    if (line.length() == 0) {
                        continue;
                    }

                    float neg_smp_r = boost::lexical_cast<float>(line);
                    output_calibration->update(neg_smp_r);

                    read++;
                    if (read == 1) {
                        break;
                    }
                }

                float pre = 0;
                bool order = true;
                std::vector<std::pair<float, float >> tmp_bins;
                while (!isotonic_regression_file.eof()) {
                    std::getline(isotonic_regression_file, line);
                    line = boost::algorithm::trim_copy(line);
                    if (line.length() == 0) {
                        continue;
                    }

                    std::vector<std::string> splits;
                    boost::algorithm::split(splits, line, boost::algorithm::is_any_of(" "));

                    if (splits.size() != 2) {
                        logger->error("Invalid isotonic regression coef {}", line);
                        continue;
                    }

                    float bin = boost::lexical_cast<float>(splits[0]);
                    float value = boost::lexical_cast<float>(splits[1]);

                    if (bin > pre) {
                        order = true;
                        pre = bin;
                    } else if (bin < pre) {
                        order = false;
                        pre = bin;
                    } else {
                        logger->error("the same bin: {}", bin);
                        break;
                    }

                    tmp_bins.push_back(std::make_pair(bin, value));
                }

                if (order) {
                    output_calibration->update(tmp_bins);
                } else {
                    // sort
                    std::sort(tmp_bins.begin(), tmp_bins.end(),
                              [](const std::pair<float, float> &a, const std::pair<float, float> &b) {
                                  return a.first < b.first;
                              });
                    output_calibration->update(tmp_bins);
                }

                if (prior_correction_file.is_open()) {
                    prior_correction_file.close();
                }

                if (isotonic_regression_file.is_open()) {
                    isotonic_regression_file.close();
                }

                if (output_calibration) {

                    logger->info("Natively loaded hybrid from {} {}.", path1, path2);
                    return output_calibration;
                } else {
                    logger->error("Fail to read calibration file when natively loading hybrid from {} {}.",
                                  path1, path2);
                }
            } else {
                logger->error(
                        "Fail to open hybrid file when natively loading hybrid from {} {}.",
                        path1, path2);
            }
        } catch (std::exception &ex) {
            logger->error("Fail to natively load hybrid from {} {}, detail: {}.", path1, path2, ex.what());
        }
        return nullptr;
    }

    void Hybrid::update(const std::vector<std::pair<float, float>> &bins) {
        _bins.clear();
        _bins = bins;
    }

    void Hybrid::update(float neg_smp_r) {
        _neg_smp_r = neg_smp_r;
    }

    float Hybrid::get(float bin, const SearchType search_type) {
        // upper bound
        auto upper_bound = [this](float key) {
            int first = 0;
            int len = _bins.size() - 1;
            int half, middle;

            while (len > 0) {
                half = len >> 1;
                middle = first + half;
                // The median is greater than the key and is looked up in the left half of the sequence containing last.
                if (_bins[middle].first > key) {
                    len = half;
                } else {  // The median is less than or equal to the key and is looked up in the right half of the sequence.
                    first = middle + 1;
                    len = len - half - 1;
                }
            }
            return first;
        };

        // lower bound
        auto lower_bound = [this](float key) {
            int first = 0, middle;
            int half, len;
            len = _bins.size();

            while (len > 0) {
                half = len >> 1;
                middle = first + half;
                if (_bins[middle].first < key) {
                    first = middle + 1;
                    len = len - half - 1;
                } else {
                    len = half;
                }
            }
            return first;
        };

        // search type
        if (search_type == SearchType::UPPERBOUND) {
            auto index = upper_bound(bin);
            return _bins[index].second;
        } else if (search_type == SearchType::LOWERBOUND) {
            auto index = lower_bound(bin);
            return _bins[index].second;
        } else {
            _logger->error("Unknown Search Type.");
        }
    }
}


#include <stdio.h>
#include <math.h>

#include <QGraphicsView>
#include <QPen>

#include <Eigen/Dense>

#include <libbwcnc/bwcnc.h>
#include <libbwcnc/stdtforms.h>

#include "mainwindow.h"

//using namespace BWCNC;

static int cols = 40; //108; // 58;
static int rows = round(1.13 * cols / mainwindow::frame_aspect_ratio()); //1080 / 2048.0);
//static double s = 8 * 58.0 / cols;
static double s = 8 * 40.0 / cols;

static struct cmdline_params {
    int cols;
    int rows;
    int nested;
    double nested_spacing;

    double sidelength;
    double scale;
    double xshift;
    double yshift;
    double zshift;
    bool   center;

    bool suppress_grid;
    const char * moveto_clr;
    const char * lineto_clr;
    const char * backgd_clr;

    double tick_size;
    int ticks_start;

} parms = {
  //cols, rows, 1, .1,
  cols, rows, 1, .18,
  //cols, rows, 1, .25,
  //cols, rows, 1, .5,
  //cols, rows, 1, .6,
  //cols, rows, 1, .7,
  //cols, rows, 1, .8,
  //cols, rows, 1, .9,

  //20, 20, 1, .2,
  //1, 10, 0, 0,
    1,  s, s/7*100, s/7*50, 0, true,
  //1, 20, 0, 0,
    true,
    nullptr,     // don't show moveto lines
    "#009900",
    "#000000", // "#fe8736",
    .151515, -1600
};

#define DO2PARTCONTEXTS 0

void mainwindow::create_hexgrid_xhatchwaves()
{
    BWCNC::HexGrid grid(
            parms.cols, parms.rows, parms.sidelength,
            parms.nested, parms.nested_spacing, ! parms.suppress_grid,
            parms.lineto_clr, parms.moveto_clr, parms.backgd_clr );

    grid.fill_partctx_with_grid( kontext );
    printf( "passing %d to workers.start_threads()\n", threadcnt );
    workers.start_threads( threadcnt );
    kontext.setup_shareable_workers( &workers );
}

static void paint_z_blue( const char * label, BWCNC::PartContext & ktx );
//static void paint_z_red( BWCNC::PartContext & k );

void render_eye_perspective( BWCNC::PartContext & k, double scene_width, double scene_height, bool isleft )
{
    //                                                      x                y                 z
  //leftrighteye3D leftright_tform( (isleft ? -1 : 1) * 21.16,  scene_height/4,  2 * scene_width);
    leftrighteye3D leftright_tform( (isleft ? -1 : 1) * 21.16,  scene_height/8,  2 * scene_width);
#if 0
    leftright_tform.eye[1] = 0;
    leftright_tform.eye[0] = 21.16; // with 10' wide screen and 2048 pixels. average interpupillary distance is 2.47"
    if( isleft )                    // with bridge of nose at y == 0, (2.47/2) ... 1.24 * 2048/(10*12) =~ 21.16 pixels
        leftright_tform.eye[0] *= -1;
    leftright_tform.eye[2] = 2 * scene_width;

    // with bridge of nose at y == 0, (2.47/2) ... 1.24 * 2048/(10*12) =~ 21.16 pixels
    if( isleft )
        leftright_tform.eye[0] *= -1;

#endif

    //Eigen::Vector3d back2;
    //shift2( k, BWCNC::to_center, BWCNC::to_center, BWCNC::to_none, &back2 );
    k.translate( Eigen::Vector3d( -scene_width/2, -scene_height/2, 0 ) );
    k.position_dependent_transform( &leftright_tform );
    k.translate( Eigen::Vector3d( scene_width/2, scene_height/2, 0 ) );
    //undo_shift2( k, back2 );
}

#if 1
void mainwindow::refresh_hexgrid_xhatchwaves()
{
    if( ! l_bool && ! r_bool ) return;

    static bool kontext_is_ready = false;
    if( ! kontext_is_ready )
    {
        kontext_is_ready = true;
        create_hexgrid_xhatchwaves();
    }

    BWCNC::Boundingbox kontext_bbox = kontext.get_bbox();

    const double w = scene->sceneRect().width();
    const double h = scene->sceneRect().height();

    QImage img( w, h, QImage::Format_RGB32 );
    QPainter painter;

    BWCNC::PixmapRenderer renderer_l( &img, &painter );
    BWCNC::PixmapRenderer renderer_r( &img, &painter );

    renderer_l.render_positive_z = p_bool;
    renderer_l.render_negative_z = n_bool;
    renderer_l.set_backgd_color( parms.backgd_clr );

    renderer_r.render_positive_z = p_bool;
    renderer_r.render_negative_z = n_bool;
    renderer_r.set_backgd_color( l_bool ? nullptr : parms.backgd_clr );

    /////////////////////////////////////////////////////////////////////////////////////
    // changing the ticksize without any compensation will catapult the image forward or
    // backward through the sequence. what we want is for it to just speed up or slow
    // down, so when the tick-size is changed, compensate using a ticks-start offset.
    double new_tick_size = exp( log_tick_stepsize );
    parms.ticks_start += (1 - new_tick_size/parms.tick_size)*(ticks + parms.ticks_start);
    parms.tick_size = new_tick_size;
    /////////////////////////////////////////////////////////////////////////////////////

    BWCNC::PartContext kl;
    BWCNC::PartContext kr;

    crosshatchwaves chw_tform(
            (ticks + parms.ticks_start) * parms.tick_size, // ticks
            (a_value - 499)/(M_PI * 100),                  // omega
            (b_value - 499)/50.0,                          // scaler
            z_param );

    if( (ticks + parms.ticks_start) % 1000 == 0 )
        printf( "ticks == %d\n", ticks + parms.ticks_start );

    BWCNC::PartContext k;
    kontext.copy_into( k );

    // this transform acts periodically over a plane, so no shifting is needed.
    BWCNC::Boundingbox bbox = k.get_bbox();

    k.translate( Eigen::Vector3d( -bbox.width()/2, -bbox.height()/2, 0 ) );
    k.position_dependent_transform( &chw_tform );
    k.translate( Eigen::Vector3d(  bbox.width()/2,   bbox.height()/2, 0 ) );

  //k.translate( Eigen::Vector3d( 0, 0, parms.zshift ) );

#if 1
    double zoomscale =    (1 - ::exp( -(ticks * parms.tick_size / 10) ));
    parms.zshift = -w*(1 - zoomscale) + z_shift;
    parms.xshift =  w*(1 - zoomscale)/2 + 150*zoomscale;
    parms.yshift =  h*(1 - zoomscale)/2 + 100*zoomscale;
#else
    parms.zshift = -200;
  //parms.zshift = 1000*(sin((double)ticks/10.0) - 1);
  //printf( "\rzshift: 1000*(sin((double)%5d/10.0) - 1) == %14.6f", ticks, parms.zshift );
  //parms.zshift = (ticks % 2000) - 1500;
    parms.xshift = 150;
    parms.yshift = 100;
#endif

    parms.scale = .75 * w / bbox.width();
    k.scale( parms.scale );
    k.translate( Eigen::Vector3d( 0, 0, parms.zshift ) );

    painter.begin(&img);

    // make the hue variable
    paint_z_blue( "k", k );
    //if( r_bool ) paint_z_blue( "kr", kr );

#if 1
    if( l_bool ) k.copy_into( kl );
    if( r_bool ) k.copy_into( kr );

#if 0
    // shift the image/grid toward the center of the image
#if 1
    if( l_bool ) kl.translate( Eigen::Vector3d( 0, 0, parms.zshift ) );
    if( r_bool ) kr.translate( Eigen::Vector3d( 0, 0, parms.zshift ) );
#else
    bbox = kl.get_bbox();
    parms.xshift = ( w - bbox.width()  )/2;
    parms.yshift = ( h - bbox.height() )/2;
    parms.zshift = 0;
    kl.translate( Eigen::Vector3d( parms.xshift, parms.yshift, parms.zshift ) );
    kr.translate( Eigen::Vector3d( parms.xshift, parms.yshift, parms.zshift ) );
#endif
#endif

    if( ! f_bool )  // we desire flat presentation, without perspective shifts?
    {
        bbox = l_bool ? kl.get_bbox() : kr.get_bbox() ;
        if( l_bool ) render_eye_perspective( kl, bbox.width(), bbox.height(), true /* isleft */ );
        if( r_bool ) render_eye_perspective( kr, bbox.width(), bbox.height(), false );
    }

    if( l_bool ) kl.translate( Eigen::Vector3d( parms.xshift, parms.yshift, 0 ) );
    if( r_bool ) kr.translate( Eigen::Vector3d( parms.xshift, parms.yshift, 0 ) );

#if 0
    if( l_bool )
        bbox = k.get_bbox();
    else if( r_bool )
        bbox = k.get_bbox();
    if( l_bool || r_bool )
        printf( "z:[%.3f %.3f]\n", bbox.min[2], bbox.max[2] );
#endif

    // render the objects in z-ascending order
    if( l_bool ) renderer_l.render_all_z_order( kl );
    if( r_bool ) renderer_r.render_all_z_order( kr );

    painter.end();
#else
    BWCNC::PixmapRenderer renderer( &img );
    renderer.render_positive_z = p_bool;
    renderer.render_negative_z = n_bool;
    renderer.set_backgd_color( parms.backgd_clr );

    paint_z_blue( k );
  //bbox = k.get_bbox();
  //k.translate( Eigen::Vector3d( (w - bbox.width())/2, (h - bbox.height())/2, 0 ) );
    k.translate( Eigen::Vector3d( parms.xshift, parms.yshift, 0 ) );
    renderer.render_all( k );
#endif

    QPixmap pxmp;
    if( pxmp.convertFromImage( img ) )
        pixmap_item->setPixmap(pxmp);

#if 0
    BWCNC::Boundingbox bbox_posttform = kr.get_bbox();
    BWCNC::Boundingbox bbox_diff = bbox_posttform - kontext_bbox;
    std::cout <<  "bbox diff: " << bbox_diff << "\n";
#endif
}

#else

static int leftorright = 0;

void mainwindow::refresh_hexgrid_xhatchwaves()
{
    static bool kontext_is_ready = false;
    if( ! kontext_is_ready )
    {
        kontext_is_ready = true;
        create_hexgrid_xhatchwaves();
    }

    crosshatchwaves chw_tform;

    chw_tform.ticks       = ticks * parms.tick_size;
    chw_tform.shiftscale  = (b_value - 499)/50.0;
    chw_tform.w           = (a_value - 499)/(M_PI * 100);

    double scene_width  = scene->sceneRect().width();
    double scene_height = scene->sceneRect().height();

    BWCNC::PartContext k;
    kontext.copy_into( k );

    // this transform acts periodically over a plane, so no shifting is needed.
    BWCNC::Boundingbox bbox = k.get_bbox();

    k.translate( Eigen::Vector3d( -bbox.width()/2, -bbox.height()/2, 0 ) );
    k.position_dependent_transform( &chw_tform );
    k.translate( Eigen::Vector3d( bbox.width()/2,   bbox.height()/2, 0 ) );

    k.scale( parms.scale );

    QImage img( scene_width, scene_height, QImage::Format_RGB32 );

    bool doleft = ( leftorright++ % 2 == 0 );

    BWCNC::PixmapRenderer renderer_l( &img );
    renderer_l.render_positive_z = p_bool;
    renderer_l.render_negative_z = n_bool;
    renderer_l.set_backgd_color( parms.backgd_clr );

    BWCNC::PartContext kl;
    paint_z_blue( k );

    bbox = k.get_bbox();
    render_eye_perspective( k, bbox.width(), bbox.height(), doleft );
  //k.translate( Eigen::Vector3d( (scene_width - bbox.width())/2, (scene_height - bbox.height())/2, 0 ) );
    k.translate( Eigen::Vector3d( parms.xshift, parms.yshift, 0 ) );

    if( l_bool &&   doleft ) renderer_l.render_all( k );
    if( r_bool && ! doleft ) renderer_l.render_all( k );

    QPixmap pxmp;
    if( pxmp.convertFromImage( img ) )
        pixmap_item->setPixmap(pxmp);
}
#endif

static void paint_z_blue( const char * /*label*/, BWCNC::PartContext & ktx )
{
    BWCNC::Boundingbox bbox = ktx.get_bbox();
#if 1
    double range =  1000; // bbox.depth();
    double least =  -500; // bbox.min[2];
#else
    double range = bbox.depth(); //10.0; // 1000;
    double least = bbox.min[2]; // -500;
    double most  = bbox.max[2]; // -500;
#endif

    //double global_zmin = 1000;
    //double global_zmax = -1000;

//  double stats[10] = {0};
//  double cmd_stats[10] = {0};

    for( auto p : ktx.partlist )
    {
        if( p )
        {
            if( p->is_closed() )
            {
                const BWCNC::Boundingbox bbox = p->get_bbox();
              //double z_avg = bbox.min[2] + bbox.depth()/2;
                double avg_z = bbox.avg()[2];

#if 1
                double multiplier = (avg_z - least)/range;
                int R, G, B;
                G = R =   32 + 64 * multiplier; // =  0;
                B     =  255 * multiplier;
#else
                int R, G, B;
                if( range > 1e-9 )
                {
                    multiplier = (avg_z - least)/range;
                    G = R =   8 + 64*multiplier; // =  0;
                  //G = R =   32;
                    B     =  255*multiplier;
                }
                else
                {
                    G = R =   32;
                    B     =  128; // an average value
                }
#endif
#if 0
                if( avg_z < 0 )
                {
                    G = B =    8 + 64 * multiplier; // =  0;
                    R     =  255 * multiplier;
                }
#endif
                for( auto cmd : p->cmds ) if( cmd ) cmd->clr = BWCNC::Color( R, G, B );
            }
#if 0
            else
                for( auto cmd : p->cmds )
            {
                if( cmd )
                {
                    double z_avg = (cmd->begin[2] + cmd->end[2]) / 2;
                    //Eigen::Vector3d avg_v = (cmd->begin + cmd->end) / 2;
                    //multiplier = (avg_v[2] - least)/range;
                    multiplier = (z_avg - least)/range;
                    G = R =   32 + 64 * multiplier; // =  0;
                    B     =  255 * multiplier;
                    cmd->clr = BWCNC::Color( R, G, B );
                }
            }
#endif
        }
    }

#if 0
    if( cmd_stats[0] > 0 )
    {
        printf( "cmd_stats(%s): least:%f range:%f cnt:%.0f z_avg:%f begin:%f end:%f mult:%f\n",
                    label,
                    least,
                    range,
                    cmd_stats[0],
                    cmd_stats[1] / cmd_stats[0],
                    cmd_stats[2] / cmd_stats[0],
                    cmd_stats[3] / cmd_stats[0],
                    cmd_stats[4] / cmd_stats[0]
                );
#if 0
                    cmd_stats[0] += 1;
                    cmd_stats[1] += z_avg;
                    cmd_stats[2] += cmd->begin[2];
                    cmd_stats[3] += cmd->end[2];
                    cmd_stats[4] += multiplier;
#endif
    }

    if( stats[0] > 0 )
    {
        printf( "stats(%s): Glblzrange{%f,%f} least:%f range:%f cnt:%.0f mult:%f min:%f depth:%f z_avg:%f avg_z:%f (z_avg-avg_z):%f\n",
                label,
                global_zmin, global_zmax,

                least, range,
                stats[0],
                stats[1] / stats[0],
                stats[3] / stats[0],
                stats[4] / stats[0],
                stats[5] / stats[0],
                stats[6] / stats[0],
                stats[7] / stats[0] );
#if 0
                stats[0] += 1;
                stats[1] += multiplier;
                stats[3] += bbox.min[2];
                stats[4] += bbox.depth();
                stats[5] += z_avg;
                stats[6] += avg_z;
                stats[7] += (z_avg - avg_z);
#endif
    }
#endif
}

#if 0
static void paint_z_red( BWCNC::PartContext & k )
{
    BWCNC::Boundingbox bbox = k.get_bbox();
    double range =  bbox.depth();
    double least =  bbox.min[2];

    for( auto p : k.partlist )
    {
        if( p )
        {
            for( auto cmd : p->cmds )
            {
                if( cmd )
                {
                    Eigen::Vector3d avg_v = (cmd->begin + cmd->end) / 2;
                    double multiplier = (avg_v[2] - least)/range;
                    int R, G, B;
                    G = B =  32 + 96 * multiplier; // =  0;
                    R     =  255 * multiplier;
                    cmd->clr = BWCNC::Color( R, G, B );

                }
            }
        }
    }
}

void mainwindow::refresh_hexgrid_xhatchwaves()
{
    BWCNC::PartContext k;

    crosshatchwaves chw_tform;

    chw_tform.ticks       = ticks * parms.tick_size;
    chw_tform.shiftscale  = (b_value - 499)/50.0;
    chw_tform.w           = (a_value - 499)/(M_PI * 100);

#if 1
    BWCNC::HexGrid grid(
            parms.cols, parms.rows, parms.sidelength,
            parms.nested, parms.nested_spacing, ! parms.suppress_grid,
            parms.lineto_clr, parms.moveto_clr, parms.backgd_clr );
#else
    BWCNC::LizardGrid grid( parms.cols, parms.rows, parms.sidelength, parms.scale );
#endif

    grid.fill_partctx_with_grid( k );

    parms.scene_width  = scene->sceneRect().width();
    parms.scene_height = scene->sceneRect().height();

#if DO2PARTCONTEXTS
    BWCNC::PartContext k2;
    k.copy_into( k2 );
    crosshatchwaves chw_tform2;
    chw_tform2.ticks = (ticks + 10) * parms.tick_size;
    chw_tform2.shiftscale = chw_tform.shiftscale;
    chw_tform2.w          = chw_tform.w;
    k2.position_dependent_transform( &chw_tform2 );
    k2.scale( parms.scale );
#endif

    k.position_dependent_transform( &chw_tform );
    k.scale( parms.scale );

    QImage img( scene->sceneRect().width(),
                scene->sceneRect().height(),
                QImage::Format_RGB32 );

    BWCNC::PixmapRenderer renderer( &img );

    grid.set_renderer_colors( & renderer );
    renderer.render_all( k );

#if DO2PARTCONTEXTS
    renderer.set_backgd_color( nullptr );
    renderer.set_lineto_color( "#ff0000" );
    renderer.render_all( k2 );
#endif

    QPixmap pxmp;
    if( pxmp.convertFromImage( img ) )
        pixmap_item->setPixmap(pxmp);
}
#endif


// Copyright © 2016-2021 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#pragma once

#include <boost/numeric/odeint.hpp>
#include <functional>

namespace vinecopulib {

namespace tools_integration {

inline double
integrate_zero_to_one(std::function<double(double)> f)
{
  boost::numeric::odeint::runge_kutta_dopri5<double> stepper;
  double lb = 1e-12;
  double ub = 1.0 - lb;
  double x = 0.0;
  auto ifunc = [f](const double /* x */, double& dxdt, const double t) {
    dxdt = f(t);
  };
  integrate_adaptive(boost::numeric::odeint::make_controlled(lb, lb, stepper),
                     ifunc,
                     x,
                     lb,
                     ub,
                     lb);
  return x;
}
}
}


#if !defined(BALSA_GEOMETRY_TRIANGLE_MESH_EARCLIPPING_HPP)
#define BALSA_GEOMETRY_TRIANGLE_MESH_EARCLIPPING_HPP
#include <list>

#include <range/v3/view/subrange.hpp>
#include <range/v3/view/sliding.hpp>
#include <range/v3/algorithm/copy.hpp>
#include <range/v3/view/take_exactly.hpp>
#include <range/v3/view/cycle.hpp>
#include <range/v3/range/conversion.hpp>
#include "balsa/eigen/stl2eigen.hpp"
#include <Eigen/Core>
#include "balsa/geometry/winding_number.hpp"
#include "balsa/geometry/get_angle.hpp"
#include "balsa/eigen/types.hpp"
#include "balsa/eigen/shape_checks.hpp"
#include <numbers>
#include <utility>

namespace balsa::geometry::triangle_mesh {
template<eigen::concepts::Vec2Compatible VDerived, std::forward_iterator BeginIt, std::forward_iterator EndIt>
requires std::is_integral_v<std::decay_t<decltype(*std::declval<BeginIt>())>>
  std::vector<std::array<std::decay_t<decltype(*std::declval<BeginIt>())>, 3>> earclipping_stl(
    const VDerived &V,
    const BeginIt &beginit,
    const EndIt &endit) {
    using Index = std::decay_t<decltype(*std::declval<BeginIt>())>;
    using Face = std::array<int, 3>;
    std::vector<Face> stlF;

    auto range = ranges::subrange(beginit, endit);

    if constexpr (std::is_integral_v<std::decay_t<decltype(*beginit)>>) {

        size_t size = std::distance(beginit, endit);
        stlF.reserve(size - 2);
        double inner_ang_sum = 0;
        double outer_ang_sum = 0;
        auto triplets = range | ranges::views::cycle// pretend range is cyclic
                        | ranges::views::sliding(3)// take sequences of triplets
                        | ranges::views::take_exactly(std::distance(beginit, endit));// we only want to see each cycle once
        for (auto &&triplet : triplets) {
            Face f;
            ranges::copy(triplet, f.begin());
            const auto &[ai, bi, ci] = f;
            auto a = V.col(ai);
            auto b = V.col(bi);
            auto c = V.col(ci);
            double ang = get_positive_clamped_angle(get_angle(c - b, a - b));
            inner_ang_sum += ang;
            outer_ang_sum += 2 * std::numbers::pi_v<double> - ang;
        }

        bool reverse_orientation = outer_ang_sum < inner_ang_sum;
        if (reverse_orientation) {
        }

        std::list<Index> CL(beginit, endit);

        auto is_earclip = [&](const Face &f) -> bool {
            auto a = V.col(f[0]);
            auto b = V.col(f[1]);
            auto c = V.col(f[2]);
            auto cb = c - b;
            auto ab = a - b;
            // balsa::Vec2d n(-ac.y(),ac.x());
            /*
            if(cb.x() * ab.y() -  cb.y() * ab.x() < 1e-12) {
                return false;
            }
            */
            if (cb.x() * ab.y() - cb.y() * ab.x() < 1e-10) {
                return false;
            }
            // double ang = balsa::geometry::trigonometry::angle(cb,ab)(0);
            double ang = get_positive_clamped_angle(get_angle(c - b, a - b));
            if (ang > std::numbers::pi_v<double> || ang < 0) {
                return false;
            }

            for (auto &&i : range) {
                if (i == f[0] || i == f[1] || i == f[2]) {
                    continue;
                }
                auto v = V.col(i);
                if (interior_winding_number(V, f, v)) {
                    return false;
                }
            }
            return true;
        };

        while (CL.size() > 3) {
            bool earclipped = false;

            for (auto it = CL.begin(); it != CL.end(); ++it) {
                auto it1 = it;
                it1++;
                if (it1 == CL.end()) {
                    it1 = CL.begin();
                }
                auto it2 = it1;
                it2++;
                if (it2 == CL.end()) {
                    it2 = CL.begin();
                }
                Face f{ { *it, *it1, *it2 } };
                if (reverse_orientation) {
                    std::swap(f[0], f[2]);
                }
                if (is_earclip(f)) {
                    stlF.push_back(f);
                    CL.erase(it1);
                    earclipped = true;
                    break;
                }
            }
            if (!earclipped) {
                // logging::warn() << "Earclipping failed!";
                auto it = CL.begin();
                auto it1 = it;
                it1++;
                auto it2 = it1;
                it2++;
                Face f{ { *it, *it1, *it2 } };

                stlF.push_back(f);
                CL.erase(it1);
            }
        }

        Face f;
        std::copy(CL.begin(), CL.end(), f.begin());
        auto a = V.col(f[0]);
        auto b = V.col(f[1]);
        auto c = V.col(f[2]);
        auto cb = c - b;
        auto ab = a - b;
        // auto ac = a - c;
        if (cb.x() * ab.y() - cb.y() * ab.x() < 1e-10) {
            std::swap(f[0], f[1]);
        }
        stlF.push_back(f);
        //} else {
        //    size_t size = 0;
        //    for (auto &&c : ranges::subrange(beginit, endit)) {
        //        size += c.size() - 2;
        //    }
        //    stlF.reserve(size);

        //    for (auto &&c : ranges::subrange(beginit, endit)) {
        //        if (c.size() >= 3) {
        //            auto F = earclipping_stl(V, c.begin(), c.end());
        //            stlF.insert(stlF.end(), F.begin(), F.end());
        //        }
        //    }
    }
    return stlF;
}
// both return balsa::eigen::ColVectors<integral_type, 3>
template<eigen::concepts::ColVecs2Compatible VDerived, typename Container>

auto earclipping(const VDerived &V,
                 const Container &C) {
    return eigen::stl2eigen(earclipping_stl(V, C.begin(), C.end()));
}
template<eigen::concepts::ColVecs2Compatible VDerived, typename T>
auto earclipping(const VDerived &V,
                 const std::initializer_list<T> &C) {
    return eigen::stl2eigen(earclipping_stl(V, C.begin(), C.end()));
}
}// namespace balsa::geometry::triangle_mesh
#endif


/******************************************************************************
 * Author:   Laurent Kneip                                                    *
 * Contact:  kneip.laurent@gmail.com                                          *
 * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
 *                                                                            *
 * Redistribution and use in source and binary forms, with or without         *
 * modification, are permitted provided that the following conditions         *
 * are met:                                                                   *
 * * Redistributions of source code must retain the above copyright           *
 *   notice, this list of conditions and the following disclaimer.            *
 * * Redistributions in binary form must reproduce the above copyright        *
 *   notice, this list of conditions and the following disclaimer in the      *
 *   documentation and/or other materials provided with the distribution.     *
 * * Neither the name of ANU nor the names of its contributors may be         *
 *   used to endorse or promote products derived from this software without   *
 *   specific prior written permission.                                       *
 *                                                                            *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE  *
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
 * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE        *
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT         *
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY  *
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF     *
 * SUCH DAMAGE.                                                               *
 ******************************************************************************/

#include <opengv/sac_problems/relative_pose/MultiCentralRelativePoseSacProblem.hpp>

#include <opengv/relative_pose/methods.hpp>
#include <opengv/triangulation/methods.hpp>
#include <Eigen/NonLinearOptimization>
#include <Eigen/NumericalDiff>

bool
opengv::sac_problems::
    relative_pose::MultiCentralRelativePoseSacProblem::computeModelCoefficients(
    const std::vector<std::vector<int> > &indices,
    model_t & outModel) const
{
  std::vector<std::shared_ptr<translations_t> > multiTranslations;
  std::vector<std::shared_ptr<rotations_t> > multiRotations;

  for(size_t pairIndex = 0; pairIndex < _adapter.getNumberPairs(); pairIndex++)
  {
    std::vector<int> serializedIndices;
    for(
        size_t correspondenceIndex = 0;
        correspondenceIndex < indices[pairIndex].size();
        correspondenceIndex++ )
      serializedIndices.push_back(_adapter.convertMultiIndex(
          pairIndex, indices[pairIndex][correspondenceIndex] ));

    essential_t essentialMatrix =
        opengv::relative_pose::eightpt(_adapter,serializedIndices);

    //Decompose the essential matrix into rotations and translations
    std::shared_ptr<translations_t> translations(new translations_t());
    std::shared_ptr<rotations_t> rotations(new rotations_t());

    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    W(0,1) = -1;
    W(1,0) = 1;
    W(2,2) = 1;

    Eigen::JacobiSVD< Eigen::MatrixXd > SVD(
        essentialMatrix,
        Eigen::ComputeFullV | Eigen::ComputeFullU );
    Eigen::VectorXd singularValues = SVD.singularValues();

    // check for bad essential matrix
    if( singularValues[2] > 0.001 ) {};
    // continue; //singularity constraints not applied -> removed because too harsh
    if( singularValues[1] < 0.75 * singularValues[0] ) {};
    // continue; //bad essential matrix -> removed because too harsh

    // maintain scale
    double scale = singularValues[0];

    // get possible rotation and translation vectors
    rotation_t Ra = SVD.matrixU() * W * SVD.matrixV().transpose();
    rotation_t Rb = SVD.matrixU() * W.transpose() * SVD.matrixV().transpose();
    translation_t t = scale*SVD.matrixU().col(2);

    // change sign if det = -1
    if( Ra.determinant() < 0 ) Ra = -Ra;
    if( Rb.determinant() < 0 ) Rb = -Rb;

    //Store the decomposition, and already convert to our convention
    translations->push_back(t);
    translations->push_back(-t); //this is not needed actually!
    rotations->push_back(Ra);
    rotations->push_back(Rb);

    multiTranslations.push_back(translations);
    multiRotations.push_back(rotations);
  }

  for(size_t pairIndex = 0; pairIndex < _adapter.getNumberPairs(); pairIndex++)
  {
    //For each pair, find the right rotation and translation by our critera
    //
    //
    //fill outModel with that (it is a vector of transformations. A
    //transformation is 3x4 with R from frame 2 to 1, and position of 2 in 1)
  }

  return true;
}

void
opengv::sac_problems::
    relative_pose::MultiCentralRelativePoseSacProblem::getSelectedDistancesToModel(
    const model_t & model,
    const std::vector<std::vector<int> > & indices,
    std::vector<std::vector<double> > & scores) const
{
  Eigen::Matrix<double,4,1> p_hom;
  p_hom[3] = 1.0;

  for( size_t pairIndex = 0; pairIndex < indices.size(); pairIndex++ )
  {
    translation_t translation = model[pairIndex].col(3);
    rotation_t rotation = model[pairIndex].block<3,3>(0,0);

    for(
        size_t correspondenceIndex = 0;
        correspondenceIndex < indices[pairIndex].size();
        correspondenceIndex++ )
    {
      _adapter.sett12(translation);
      _adapter.setR12(rotation);

      transformation_t inverseSolution;
      inverseSolution.block<3,3>(0,0) = rotation.transpose();
      inverseSolution.col(3) =
          -inverseSolution.block<3,3>(0,0)*translation;

      p_hom.block<3,1>(0,0) =
          opengv::triangulation::triangulate2(
              _adapter,
              _adapter.convertMultiIndex(
              pairIndex, indices[pairIndex][correspondenceIndex] ));

      bearingVector_t reprojection1 = p_hom.block<3,1>(0,0);
      bearingVector_t reprojection2 = inverseSolution * p_hom;
      reprojection1 = reprojection1 / reprojection1.norm();
      reprojection2 = reprojection2 / reprojection2.norm();
      bearingVector_t f1 = _adapter.getBearingVector1(pairIndex,correspondenceIndex);
      bearingVector_t f2 = _adapter.getBearingVector2(pairIndex,correspondenceIndex);

      //bearing-vector based outlier criterium (select threshold accordingly):
      //1-(f1'*f2) = 1-cos(alpha) \in [0:2]
      double reprojError1 = 1.0 - (f1.transpose() * reprojection1);
      double reprojError2 = 1.0 - (f2.transpose() * reprojection2);
      scores[pairIndex].push_back(reprojError1 + reprojError2);
    }
  }
}

void
opengv::sac_problems::
    relative_pose::MultiCentralRelativePoseSacProblem::optimizeModelCoefficients(
    const std::vector<std::vector<int> > & inliers,
    const model_t & model,
    model_t & optimized_model)
{
  optimized_model = model; //todo: include non-linear optimization of model
}

std::vector<int>
opengv::sac_problems::
    relative_pose::MultiCentralRelativePoseSacProblem::getSampleSizes() const
{
  std::vector<int> sampleSizes;
  for(size_t pairIndex = 0; pairIndex < _adapter.getNumberPairs(); pairIndex++)
    sampleSizes.push_back(_sampleSize);

  return sampleSizes;
}


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_FEATURES_MATCHING_FAST_CORNER_DETECTOR_HPP
#define PIC_FEATURES_MATCHING_FAST_CORNER_DETECTOR_HPP

#include "../util/vec.hpp"
#include "../util/std_util.hpp"

#include "../image.hpp"
#include "../filtering/filter_luminance.hpp"
#include "../filtering/filter_gaussian_2d.hpp"

#include "../features_matching/general_corner_detector.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Dense"
#else
    #include <Eigen/Dense>
#endif

#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

class FastCornerDetector: public GeneralCornerDetector
{
protected:
    Image *lum_flt;
    bool bexecuteThreshold;

    float sigma, threshold;
    int radius;

public:
    /**
     * @brief FastCornerDetector
     * @param sigma
     * @param radius
     * @param threshold
     */
    FastCornerDetector(float sigma = 1.0f, int radius = 1, float threshold = 0.001f) : GeneralCornerDetector()
    {
        bexecuteThreshold = true;

        lum_flt = NULL;

        update(sigma, radius, threshold);
    }

    ~FastCornerDetector()
    {
        lum = delete_s(lum);
        lum_flt = delete_s(lum_flt);
    }

    /**
     * @brief update
     * @param sigma
     * @param radius
     * @param threshold
     */
    void update(float sigma = 1.0f, int radius = 1, float threshold = 0.001f)
    {
        this->sigma = sigma > 0.0f ? sigma : 1.0f;
        this->radius = radius > 0 ? radius : 1;
        this->threshold = threshold > 0.001f ? threshold : 0.001f;
    }

    /**
     * @brief execute
     * @param img
     * @param corners
     */
    void execute(Image *img, std::vector< Eigen::Vector2f > *corners)
    {
        if(img == NULL || corners == NULL) {
            return;
        }

        if(img->channels == 1) {
            bLum = false;
            lum = img;
        } else {
            bLum = true;
            lum = FilterLuminance::execute(img, lum, LT_CIE_LUMINANCE);
        }

        corners->clear();

        std::vector< Eigen::Vector3f > corners_w_quality;

        //filter the input image
        FilterGaussian2D flt(sigma);
        lum_flt = flt.Process(Single(lum), lum_flt);

        int x[] = {0, 1, 2, 3, 3,  3,  2,  1,  0, -1, -2, -3, -3, -3, -2, -1};
        int y[] = {3, 3, 2, 1, 0, -1, -2, -3, -3, -3, -2, -1,  0,  1,  2,  3};

        int width  = lum_flt->width;
        int height = lum_flt->height;

        bool *corners_map = new bool[width * height];

        memset(corners_map, 0, sizeof(bool) * width *  height);

        Image V(1,width, height, 1);
        V.setZero();

        for(int i=3; i<(height - 3); i++) {
            for(int j=3; j<(width - 3); j++) {
                int ind = i * width + j;

                float p = lum_flt->data[ind];

                bool bDark[16];
                bool bBright[16];
                float v[16];

                float sum = p;

                for(int k=0; k<16; k++){
                    float value = (*lum_flt)(j + x[k], i + y[k])[0];
                    v[k] = value;
                    sum += value;
                }

                //compute the threshold
                float thr;
                if(bexecuteThreshold) {
                    thr = 0.2f * sum / 16.0f;
                    thr = (thr > 1e-9f ) ? thr : threshold;

                } else {
                    thr = threshold;
                }

                //test
                float p_thr_dark   = p - thr;
                float p_thr_bright = p + thr;

                for(int k=0; k<16; k++){
                    bDark[k]   = v[k] <= p_thr_dark;
                    bBright[k] = v[k] >= p_thr_bright;
                }

                //first test: 0, 4, 8, 12
                int cDark   = bDark[0]   + bDark[4]   + bDark[8]   + bDark[12];
                int cBright = bBright[0] + bBright[4] + bBright[8] + bBright[12];

                if((cDark < 3) && (cBright < 3) ){
                    //corners_map[ind] = false;
                    continue;
                }

                //second test: check for 12 continuous true values in bDark or cBright
                int counter_dark   = 0;
                int counter_bright = 0;

                //corners_map[ind] = false;
                for(int k=0; k<16; k++){
                    if(bDark[k]){
                        counter_dark++;
                    } else {
                        counter_dark = 0;
                    }

                    if(bBright[k]){
                        counter_bright++;
                    } else {
                        counter_dark = 0;
                    }

                    if((counter_bright > 11) || (counter_dark > 11)) {
                        corners_map[ind] = true;

                        //Computing V function
                        float V_dark   = 0.0f;
                        float V_bright = 0.0f;
                        for(int k=0; k<16; k++){
                            if(bDark[k]) {
                                V_bright += fabsf(v[k] - p) - thr;
                            }

                            if(bBright[k]) {
                                V_dark += fabsf(p - v[k]) - thr;
                            }
                        }
                        V.data[ind] = MAX(V_bright, V_dark);

                        break;
                    }
                }
            }
        }

        //non-maximal supression
        int side = radius * 2 + 1;
        int *indices = new int[side * side];

        for(int i=3; i<(height - 3); i++) {
            for(int j=3; j<(width - 3); j++) {
                int ind = i * width + j;

                if(!corners_map[ind]){
                    continue;
                }

                indices[0] = ind;
                int counter = 1;

                for(int k = -radius; k <= radius; k++) {
                    int yy = CLAMP(i + k, height);

                    for(int l = -radius; l <= radius; l++) {

                        if((l == 0)&&(k == 0)) {
                            continue;
                        }

                        int xx = CLAMP(j + l, width);

                        ind = yy * width + xx;

                        if(corners_map[ind]){
                            indices[counter] = ind;
                            counter++;
                        }

                    }
                }

                //are other corners near-by?
                if(counter > 1) {
                    //finding the maximum value
                    float V_value = V.data[indices[0]];
                    int index = 0;

                    for(int k=1; k<counter; k++){
                        if(V.data[indices[k]] > V_value) {
                            V_value = V.data[indices[k]];
                            index = k;
                        }
                    }

                    if(index == 0){
                        corners_w_quality.push_back(Eigen::Vector3f (float(j), float(i), 1.0f) );
                    }
                } else {
                    corners_w_quality.push_back(Eigen::Vector3f (float(j), float(i), 1.0f) );
                }

            }
        }

        sortCornersAndTransfer(&corners_w_quality, corners);

        delete[] indices;
    }
};

#endif

} // end namespace pic

#endif /* PIC_FEATURES_MATCHING_FAST_CORNER_DETECTOR_HPP */


#pragma once

#include <armadillo>
#include "utils/macros.hpp"
#include "exception/NotImplementedException.hpp"

#include "spdlog/spdlog.h"
using namespace spdlog;
using namespace arma;
using namespace std;

class Kernel {
public:
    Kernel(double smoothingLenght);

    virtual double W(double distance);
    virtual double W(VEC r){ return  W(norm(r)); };

    virtual VEC gradW(double distance, const VEC& distanceVector);
    virtual VEC gradW(VEC r){ return  gradW(norm(r), r); };

    virtual double laplacianW(double distance, const VEC &distanceVector);
    virtual double laplacianW(const VEC r){ return  laplacianW(norm(r), r); };

    virtual double maxDistance();

protected:
    double smoothingLength;
    shared_ptr<logger> console = get("console");

};

typedef map<const char*, Kernel> mapKernel;


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

namespace py=pybind11;

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

  /// Use RGBA Color
  typedef OpenMesh::Vec4f Color;

    /// Status
    VertexAttributes(OpenMesh::Attributes::Status);
    FaceAttributes(OpenMesh::Attributes::Status);
    EdgeAttributes(OpenMesh::Attributes::Status);

//    VertexAttributes(
//            OpenMesh::Attributes::
//            )
};

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

class DR_feature{
public:
    DR_feature();
	void read_ref_mesh(std::string ref_mesh_name);
	void read_defor_mesh(std::string defor_mesh_name);
	std::vector<double> get_feature(std::string ref_mesh_name, std::string defor_mesh_name);

private:
	TriMesh ref_mesh_;
	TriMesh defor_mesh_;

	OpenMesh::EPropHandleT<double> LB_weights;
	OpenMesh::VPropHandleT<Eigen::Matrix3d> T_matrixs;
private:
	void compute_ref_LB_weight();
    void compute_Ti(TriMesh::VertexHandle v_it,TriMesh::VertexHandle v_to_it);
};

DR_feature::DR_feature(){
	ref_mesh_.add_property(LB_weights);
	defor_mesh_.add_property(T_matrixs);
}

void DR_feature::read_ref_mesh(std::string ref_mesh_name){
	if(!OpenMesh::IO::read_mesh(ref_mesh_, ref_mesh_name)){
        std::cout<<"read_ref_mesh_ : read file wrong!!!"<<std::endl;
        return ;
    }

    compute_ref_LB_weight();
}

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

            v0 = ref_mesh_.to_vertex_handle(h0);
            v1 = ref_mesh_.from_vertex_handle(h0);
            p0 = ref_mesh_.point(v0);
            p1 = ref_mesh_.point(v1);
            h1 = ref_mesh_.next_halfedge_handle(h0);
            p2 = ref_mesh_.point(ref_mesh_.to_vertex_handle(h1));
            d0 = (p0-p2).normalize();
            d1 = (p1-p2).normalize();
            w += 2.0 / tan(acos(std::min(0.99, std::max(-0.99, (d0|d1)))));
            w = std::max(0.0, w);

            if(std::isnan(w))
                std::cout<<"Some weight NAN"<<std::endl;
            ref_mesh_.property(LB_weights,e_it) = w;
            continue;
        }
        h0 = ref_mesh_.halfedge_handle(e_it.handle(),0);
        v0 = ref_mesh_.to_vertex_handle(h0);
        p0 = ref_mesh_.point(v0);

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

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

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

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

void DR_feature::read_defor_mesh(std::string _filename){
	if(!OpenMesh::IO::read_mesh(defor_mesh_,_filename))
    {
        std::cout<<"read_defor_mesh : read file wrong"<<std::endl;
        return ;
    }

    TriMesh::FaceIter f_it = defor_mesh_.faces_begin();
    for(;f_it!=defor_mesh_.faces_end();f_it++)
    {
        TriMesh::FaceHandle f_h = *f_it;
        TriMesh::FaceVertexIter fe_it0,fe_it1;
        fe_it0 = ref_mesh_.fv_iter(f_h);
        fe_it1 = defor_mesh_.fv_iter(f_h);
        int v00,v01,v02,v10,v11,v12;
        v00 = (*fe_it0).idx();fe_it0++;
        v01 = (*fe_it0).idx();fe_it0++;
        v02 = (*fe_it0).idx();fe_it0++;

        v10 = (*fe_it1).idx();fe_it1++;
        v11 = (*fe_it1).idx();fe_it1++;
        v12 = (*fe_it1).idx();fe_it1++;

        if(v00 == v10)
        {
            if(v01!=v11||v02!=v12)
            {
                std::cout<<"defor and ref are not compatible!!!"<<std::endl;
                return;
            }
        }
        else if(v00 == v11)
        {
            if(v01!=v12||v02!=v10)
            {
                std::cout<<"defor and ref are not compatible!!!"<<std::endl;
                return;
            }
        }
        else if(v00 == v12)
        {
            if(v01!=v10||v02!=v11)
            {
                std::cout<<"defor and ref are not compatible!!!"<<std::endl;
                return;
            }
        }
        else
        {
            std::cout<<"defor and ref are not compatible!!!"<<std::endl;
            return;
        }
    }

    TriMesh::VertexIter v_it, v_to_it;
    for(v_it=ref_mesh_.vertices_begin(),v_to_it=defor_mesh_.vertices_begin()
        ;v_it!=ref_mesh_.vertices_end()&&v_to_it!=defor_mesh_.vertices_end()
        ;v_it++,v_to_it++)
    {
        if((*v_it).idx()!=(*v_to_it).idx())
            std::cout<<"DR_feature::compute_ref_to_defor_Tmatrixs different topology!!!"<<std::endl;
        compute_Ti(*v_it,*v_to_it);
    }

}

void DR_feature::compute_Ti(TriMesh::VertexHandle v_it, TriMesh::VertexHandle v_to_it){
    if(v_it.idx()!=v_to_it.idx())
    	std::cout<<"compute_Ti correspond is wrong!!!"<<std::endl;
    TriMesh::VertexEdgeIter veiter=ref_mesh_.ve_iter(v_it);
    int v_id = v_it.idx();
    TriMesh::Point p0,p1;
    p0 = ref_mesh_.point(v_it);
    p1 = defor_mesh_.point(v_to_it);
    Eigen::Matrix3d L,RI;
    L.setZero();
    RI.setZero();
    double tolerance = 1.0e-6;
    TriMesh::HalfedgeHandle h_e=ref_mesh_.halfedge_handle(v_it);
    TriMesh::VertexHandle test_v = ref_mesh_.to_vertex_handle(h_e);
    TriMesh::Point tp0,tp1;
    tp0 = ref_mesh_.point(v_it); tp1 = ref_mesh_.point(test_v);
    double scale=1.0;
    if(((tp0[0]-tp1[0])*(tp0[0]-tp1[0])+(tp0[1]-tp1[1])*(tp0[1]-tp1[1])+(tp0[2]-tp1[2])*(tp0[2]-tp1[2]))<0.1)
        scale = 100;

    for(;veiter.is_valid();veiter++)
    {
        double weight = 1.0;
        int to_id;
        TriMesh::VertexHandle to_v=ref_mesh_.to_vertex_handle(ref_mesh_.halfedge_handle(*veiter, 0));
        if(to_v.idx()==v_id)
            to_v = ref_mesh_.from_vertex_handle(ref_mesh_.halfedge_handle(*veiter, 0));
        to_id = to_v.idx();
        Eigen::Vector3d eij0,eij1;
        TriMesh::Point q0,q1;
        q0 = ref_mesh_.point(to_v);
        q1 = defor_mesh_.point(to_v);
        eij0(0) = p0[0]-q0[0];
        eij0(1) = p0[1]-q0[1];
        eij0(2) = p0[2]-q0[2];
        eij0*=weight*scale;

        eij1(0) = p1[0]-q1[0];
        eij1(1) = p1[1]-q1[1];
        eij1(2) = p1[2]-q1[2];
        eij1*=weight*scale;

        L+=eij1*eij0.transpose();
        RI+=eij0*eij0.transpose();
    }
    Eigen::Matrix3d T;
    if(fabs(RI.determinant())>tolerance)
         T = L*RI.inverse();
    else
    {
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(RI, Eigen::ComputeThinU | Eigen::ComputeThinV);
        Eigen::Matrix3d U,V;
        U=svd.matrixU();
        V=svd.matrixV();
        Eigen::Matrix3d S_inv(svd.singularValues().asDiagonal());
        for(int i=0;i<3;i++)
        {
            if(fabs(S_inv(i,i))>tolerance)
                S_inv(i,i)=1.0/S_inv(i,i);
            else
                S_inv(i,i)=0.0;
        }
        T = L*V*S_inv*U.transpose();
    }
    defor_mesh_.property(T_matrixs,v_it) = T;
}

std::vector<double> DR_feature::get_feature(std::string ref_mesh_name, std::string defor_mesh_name){
	read_ref_mesh(ref_mesh_name);
	read_defor_mesh(defor_mesh_name);

	std::vector<double> dr_feature;

	TriMesh::VertexIter v_it;
    for(v_it=defor_mesh_.vertices_begin(); v_it!=defor_mesh_.vertices_end(); v_it++)
    {
        Eigen::Matrix3d T = defor_mesh_.property(T_matrixs,*v_it);
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(T, Eigen::ComputeThinU | Eigen::ComputeThinV);
        Eigen::Matrix3d U,V;
        U=svd.matrixU();
        V=svd.matrixV();
        Eigen::Matrix3d S(svd.singularValues().asDiagonal());
        Eigen::Matrix3d Temp=Eigen::Matrix3d::Identity();
        Temp(2,2) = (U*V.transpose()).determinant();
        Eigen::Matrix3d R=U*Temp*V.transpose();
        Eigen::Matrix3d Scale = V*Temp*S*V.transpose();
        Eigen::Matrix3d logR = rotation_log_exp::log(R);

        dr_feature.push_back(Scale(0, 0));
        dr_feature.push_back(Scale(0, 1));
        dr_feature.push_back(Scale(0, 2));
        dr_feature.push_back(Scale(1, 1));
        dr_feature.push_back(Scale(1, 2));
        dr_feature.push_back(Scale(2, 2));
        dr_feature.push_back(logR(0, 1));
        dr_feature.push_back(logR(0, 2));
        dr_feature.push_back(logR(1, 2));
    }
    return dr_feature;
}

py::array_t<double> get_dr(std::string ref_mesh_name, std::string defor_mesh_name){
	DR_feature feature;
	std::vector<double> temp = feature.get_feature(ref_mesh_name, defor_mesh_name);

    auto result = py::array_t<double>(temp.size());
    auto result_buffer = result.request();
    double *result_ptr = (double *)result_buffer.ptr;

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

    return result;

}

 PYBIND11_MODULE(get_dr, m){
    m.doc() = "get dr";
    m.def("get_dr", &get_dr, "get_dr");
 }


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2010 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <Eigen/SVD>

template<typename MatrixType> void upperbidiag(const MatrixType& m)
{
  const typename MatrixType::Index rows = m.rows();
  const typename MatrixType::Index cols = m.cols();

  typedef Matrix<typename MatrixType::RealScalar, MatrixType::RowsAtCompileTime,  MatrixType::ColsAtCompileTime> RealMatrixType;
  typedef Matrix<typename MatrixType::Scalar, MatrixType::ColsAtCompileTime,  MatrixType::RowsAtCompileTime> TransposeMatrixType;

  MatrixType a = MatrixType::Random(rows,cols);
  internal::UpperBidiagonalization<MatrixType> ubd(a);
  RealMatrixType b(rows, cols);
  b.setZero();
  b.block(0,0,cols,cols) = ubd.bidiagonal();
  MatrixType c = ubd.householderU() * b * ubd.householderV().adjoint();
  VERIFY_IS_APPROX(a,c);
  TransposeMatrixType d = ubd.householderV() * b.adjoint() * ubd.householderU().adjoint();
  VERIFY_IS_APPROX(a.adjoint(),d);
}

void test_upperbidiagonalization()
{
  for(int i = 0; i < g_repeat; i++) {
   CALL_SUBTEST_1( upperbidiag(MatrixXf(3,3)) );
   CALL_SUBTEST_2( upperbidiag(MatrixXd(17,12)) );
   CALL_SUBTEST_3( upperbidiag(MatrixXcf(20,20)) );
   CALL_SUBTEST_4( upperbidiag(Matrix<std::complex<double>,Dynamic,Dynamic,RowMajor>(16,15)) );
   CALL_SUBTEST_5( upperbidiag(Matrix<float,6,4>()) );
   CALL_SUBTEST_6( upperbidiag(Matrix<float,5,5>()) );
   CALL_SUBTEST_7( upperbidiag(Matrix<double,4,3>()) );
  }
}


//==============================================================================
//         Copyright 2009 - 2012   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
//==============================================================================
#include <nt2/exponential/include/functions/pow2.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>

#include <nt2/include/functions/splat.hpp>
#include <nt2/include/functions/unary_minus.hpp>
#include <nt2/include/constants/four.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/sdk/meta/as_integer.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/exceptions.hpp>

NT2_TEST_CASE_TPL ( pow2,  NT2_SIMD_REAL_TYPES)
{
  using nt2::pow2;
  using nt2::tag::pow2_;
  using boost::simd::native;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                         vT;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef native<iT,ext_t>                       viT;
  typedef typename nt2::meta::call<pow2_(vT)>::type r_t;

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ASSERT(pow2(nt2::Inf<vT>()));
  NT2_TEST_ASSERT(pow2(nt2::Minf<vT>()));
  NT2_TEST_ASSERT(pow2(nt2::Nan<vT>()));
  NT2_TEST_ASSERT(pow2(nt2::One<vT>(), nt2::Inf<vT>()));
  NT2_TEST_ASSERT(pow2(nt2::One<vT>(), nt2::Minf<vT>()));
  NT2_TEST_ASSERT(pow2(nt2::One<vT>(), nt2::Nan<vT>()));
#endif
  NT2_TEST_EQUAL(pow2(nt2::Inf<vT>(),  nt2::Two<viT>()), nt2::Inf<r_t>());
  NT2_TEST_EQUAL(pow2(nt2::Minf<vT>(), nt2::Two<viT>()), nt2::Minf<r_t>());
  NT2_TEST_EQUAL(pow2(nt2::Nan<vT>(),  nt2::Two<viT>()), nt2::Nan<r_t>());
  NT2_TEST_EQUAL(pow2(nt2::Inf<vT>(),  nt2::splat<vT>(T(2.5))), nt2::Inf<r_t>());
  NT2_TEST_EQUAL(pow2(nt2::Minf<vT>(), nt2::splat<vT>(T(2.5))), nt2::Minf<r_t>());
  NT2_TEST_EQUAL(pow2(nt2::Nan<vT>(),  nt2::splat<vT>(T(2.5))), nt2::Nan<r_t>());
  NT2_TEST_EQUAL(pow2(nt2::Mone<vT>(), nt2::Two<viT>()), -nt2::Four<r_t>());
  NT2_TEST_EQUAL(pow2(nt2::One<vT>(),  nt2::Two<viT>()), nt2::Four<r_t>());
  NT2_TEST_EQUAL(pow2(nt2::Zero<vT>(), nt2::Two<viT>()), nt2::Zero<r_t>());
  NT2_TEST_EQUAL(pow2(nt2::Mone<vT>(), nt2::splat<vT>(T(2.5))), -nt2::Four<r_t>());
  NT2_TEST_EQUAL(pow2(nt2::One<vT>(),  nt2::splat<vT>(T(2.5))), nt2::Four<r_t>());
  NT2_TEST_EQUAL(pow2(nt2::Zero<vT>(), nt2::splat<vT>(T(2.5))), nt2::Zero<r_t>());
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

//[register_linestring
//` Show the use of BOOST_GEOMETRY_REGISTER_LINESTRING

#include <iostream>

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

typedef boost::geometry::model::d2::point_xy<double> point_2d;

BOOST_GEOMETRY_REGISTER_LINESTRING(std::vector<point_2d>)

int main()
{
    // Normal usage of std::
    std::vector<point_2d> line;
    line.push_back(point_2d(1, 1));
    line.push_back(point_2d(2, 2));
    line.push_back(point_2d(3, 1));

    // Usage of Boost.Geometry's length and wkt functions
    std::cout << "Length: "
        << boost::geometry::length(line)
        << std::endl;

    std::cout << "WKT: "
        << boost::geometry::wkt(line)
        << std::endl;

    return 0;
}

//]

//[register_linestring_output
/*`
Output:
[pre
Length: 2.82843
WKT: LINESTRING(1 1,2 2,3 1)
]
*/
//]