/** nonfinite_num_facet.cpp
*
* Copyright (c) 2011 Paul A. Bristow
*
* Distributed under the Boost Software License, Version 1.0.
* (See accompanying file LICENSE_1_0.txt
* or copy at http://www.boost.org/LICENSE_1_0.txt)
*
* This very simple program illustrates how to use the
* `boost/math/nonfinite_num_facets.hpp' to obtain C99
* representation of infinity and NaN.
* (from the original
* Floating Point  Utilities contribution by Johan Rade.
* Floating Point Utility library has been accepted into Boost,
* but the utilities are incorporated into Boost.Math library.
*
\file

\brief A very simple example of using non_finite_num facet for
C99 standard output of infinity and NaN.

\detail Provided infinity and nan are supported,
this example shows how to create a C99 non-finite locale,
and imbue input and output streams with the non_finite_num put and get facets.
This allow output and input of infinity and NaN in a Standard portable way,
This permits 'loop-back' of output back into input (and portably across different system too).
This is particularly useful when used with Boost.Seralization so that non-finite NaNs and infinity
values in text and xml archives can be handled correctly and portably.

*/

#ifdef _MSC_VER
#  pragma warning (disable : 4127)  // conditional expression is constant.
#endif

#include <iostream>
using std::cout;
using std::endl;
using std::cerr;

#include <iomanip>
using std::setw;
using std::left;
using std::right;
using std::internal;

#include <string>
using std::string;

#include <sstream>
using std::istringstream;

#include <limits>
using std::numeric_limits;

#include <locale>
using std::locale;

#include <boost/math/special_functions/nonfinite_num_facets.hpp>
// from Johan Rade Floating Point Utilities.

int main ()
{
  std::cout << "Nonfinite_num_facet very simple example." << std::endl;

  if((std::numeric_limits<double>::has_infinity == 0) || (std::numeric_limits<double>::infinity() == 0))
  {
    std::cout << "Infinity not supported on this platform." << std::endl;
    return 0;
  }

  if((std::numeric_limits<double>::has_quiet_NaN == 0) || (std::numeric_limits<double>::quiet_NaN() == 0))
  {
    std::cout << "NaN not supported on this platform." << std::endl;
    return 0;
  }

  std::locale default_locale (std::locale::classic ()); // Note the currrent (default C) locale.

  // Create plus and minus infinity.
  double plus_infinity = +std::numeric_limits<double>::infinity();
  double minus_infinity = -std::numeric_limits<double>::infinity();

  // and create a NaN (NotANumber)
  double NaN = +std::numeric_limits<double>::quiet_NaN ();

  double negated_NaN = (boost::math::changesign)(std::numeric_limits<double>::quiet_NaN ());

  // Output the nonfinite values using the current (default C) locale.
  // The default representations differ from system to system,
  // for example, using Microsoft compilers, 1.#INF, -1.#INF, and 1.#QNAN.
  cout << "Using C locale" << endl;
  cout << "+std::numeric_limits<double>::infinity() = " << plus_infinity << endl;
  cout << "-std::numeric_limits<double>::infinity() = " << minus_infinity << endl;
  cout << "+std::numeric_limits<double>::quiet_NaN () = " << NaN << endl;

  // Display negated NaN.
  cout << "negated NaN " << negated_NaN << endl; // "-1.IND"

  // Create a new output locale, and add the nonfinite_num_put facet
  std::locale C99_out_locale (default_locale, new boost::math::nonfinite_num_put<char>);
  // and imbue the cout stream with the new locale.
  cout.imbue (C99_out_locale);

  // Or for the same effect more concisely:
  cout.imbue (locale(locale(), new boost::math::nonfinite_num_put<char>));

  // Output using the new locale
  cout << "Using C99_out_locale " << endl;
  cout << "+std::numeric_limits<double>::infinity() = " << plus_infinity << endl;
  cout << "-std::numeric_limits<double>::infinity() = " << minus_infinity << endl;
  cout << "+std::numeric_limits<double>::quiet_NaN () = " << NaN << endl;

  // Display negated NaN.
  cout << "negated NaN " << negated_NaN << endl; // -nan

  // Create a string with the expected C99 representation of plus infinity.
  std::string inf = "inf";
  { // Try to read an infinity value using the default C locale.
    // Create an input stream which will provide "inf"
    std::istringstream iss (inf);

     // Create a double ready to take the input,
    double infinity;
    // and read "inf" from the stringstream:
    iss >> infinity;

    // This will not work on all platforms!
    if (! iss)
    { // Reading infinity went wrong!
      std::cerr << "C locale input format error!" << std::endl;
    }
  } // Using default C locale.

  { // Now retry using C99 facets.
  // Create a new input locale and add the nonfinite_num_get facet.
  std::locale C99_in_locale (default_locale, new boost::math::nonfinite_num_get<char>);

  // Create an input stream which will provide "inf".
  std::istringstream iss (inf);
  // Imbue the stream with the C99 input locale.
  iss.imbue (C99_in_locale);

  // Create a double ready to take the input,
  double infinity;
  // and read from the stringstream:
  iss >> infinity;

  if (! iss)
  { // Reading infinity went wrong!
    std::cout << "C99 input format error!" << std::endl;
  }
  // Expect to get an infinity, which will display still using the C99 locale as "inf"
  cout << "infinity in C99 representation is " << infinity << endl;

  // To check, we can switch back to the default C locale.
  cout.imbue (default_locale);
  cout <<  "infinity in default C representation is " << infinity << endl;
  } // using C99 locale.

  {
    // A 'loop-back example, output to a stringstream, and reading it back in.
    // Create C99 input and output locales.
    std::locale C99_out_locale (default_locale, new boost::math::nonfinite_num_put<char>);
    std::locale C99_in_locale (default_locale, new boost::math::nonfinite_num_get<char>);

    std::ostringstream oss;
    oss.imbue(C99_out_locale);
    oss << plus_infinity;

    std::istringstream iss(oss.str()); // So stream contains "inf".
    iss.imbue (C99_in_locale);

    std::string s;

    iss >> s;

    cout.imbue(C99_out_locale);
    if (oss.str() != s)
    {
      cout << plus_infinity << " != " << s << " loopback failed!" << endl;
    }
    else
    {
      cout << plus_infinity << " == " << s << " as expected." << endl;
    }
  }

  // Example varying the width and position of the nonfinite representations.
  // With the nonfinite_num_put and _get facets, the width of the output is constant.

  #ifdef BOOST_NO_NUMERIC_LIMITS_LOWEST
  cout << "BOOST_NO_NUMERIC_LIMITS_LOWEST is defined, so no max_digits10 available." << endl;
  std::streamsize  max_digits10 = 2 + std::numeric_limits<double>::digits * 30103UL / 100000UL;
#else
  // Can use new C++0X max_digits10 (the maximum potentially significant digits).
  std::streamsize  max_digits10 = std::numeric_limits<double>::max_digits10;
#endif
  cout << "std::numeric_limits<double>::max_digits10 is " << max_digits10 << endl;
  cout.precision(max_digits10);

  double pi = 3.141592653589793238462643383279502884197169399375105820974944;
  // Expect 17 (probably) decimal digits (regardless of locale).
  // cout has the default locale.
  cout << "pi = " << pi << endl; // pi = 3.1415926535897931
  cout.imbue (C99_out_locale); // Use cout with the C99 locale
  // (expect the same output for a double).
  cout << "pi = " << pi << endl; // pi = 3.1415926535897931

  cout << "infinity in C99 representation is " << plus_infinity << endl;

  //int width = 2; // Check effect if width too small is OK.
  // (There was a disturbed layout on older MSVC?).
  int width = 20;

  // Similarly if we can switch back to the default C locale.
  cout.imbue (default_locale);
  cout <<  "infinity in default C representation is " << plus_infinity << endl;
  cout <<  "infinity in default C representation (setw(" << width << ") is " << setw(width) << plus_infinity <<'|' << endl;
  cout <<  "infinity in default C representation (setw(" << width << ") is " << left << setw(width) << plus_infinity <<'|' << endl;
  cout <<  "infinity in default C representation (setw(" << width << ") is " << internal << setw(width) << plus_infinity <<'|' << endl;

  cout.imbue (C99_out_locale);
  cout << "infinity in C99 representation (setw(" << width << ") is " << right << setw(width) << plus_infinity <<'|'<< endl;
  cout << "infinity in C99 representation (setw(" << width << ") is " << left << setw(width) << plus_infinity <<'|'<< endl;
  cout << "infinity in C99 representation (setw(" << width << ") is " << internal << setw(width) << plus_infinity <<'|'<< endl;

  return 0;
} // int main()

// end of test_nonfinite_num_facets.cpp

/*

Output:

simple_nonfinite_facet.vcxproj -> J:\Cpp\MathToolkit\test\Math_test\Release\nonfinite_facet_simple.exe
  Nonfinite_num_facet very simple example.
  Using C locale
  +std::numeric_limits<double>::infinity() = 1.#INF
  -std::numeric_limits<double>::infinity() = -1.#INF
  +std::numeric_limits<double>::quiet_NaN () = 1.#QNAN
  Using C99_out_locale
  +std::numeric_limits<double>::infinity() = inf
  -std::numeric_limits<double>::infinity() = -inf
  +std::numeric_limits<double>::quiet_NaN () = nan
  infinity in C99 representation is inf
  infinity in default C representation is 1.#INF
  3
  3
  inf == inf as expected.
  std::numeric_limits<double>::max_digits10 is 17
  pi = 3.1415926535897931
  C locale input format error!
  pi = 3.1415926535897931
  infinity in C99 representation is inf
  infinity in default C representation is 1.#INF
  infinity in default C representation (setw(20) is               1.#INF|
  infinity in default C representation (setw(20) is 1.#INF              |
  infinity in default C representation (setw(20) is               1.#INF|
  infinity in C99 representation (setw(20) is                  inf|
  infinity in C99 representation (setw(20) is inf                 |
  infinity in C99 representation (setw(20) is                  inf|

*/


#define CATCH_CONFIG_MAIN

#include "CALPHADFreeEnergyFunctionsTernary.h"
#include "InterpolationType.h"

#include "catch.hpp"

#include <boost/optional/optional.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <string>

#include <omp.h>

namespace pt = boost::property_tree;

TEST_CASE("CALPHAD ternary equilibrium", "[ternary equilibrium]")
{
    std::cout << "Run test with " << omp_get_max_threads() << " threads"
              << std::endl;

    Thermo4PFM::EnergyInterpolationType energy_interp_func_type
        = Thermo4PFM::EnergyInterpolationType::PBG;
    Thermo4PFM::ConcInterpolationType conc_interp_func_type
        = Thermo4PFM::ConcInterpolationType::PBG;

    const double Tmin     = 2990;
    const double Tmax     = 3020.;
    const int nTintervals = 10;
    const double deltaT   = (Tmax - Tmin) / (double)nTintervals;

    std::cout << " Read CALPHAD database..." << std::endl;
    pt::ptree calphad_db;
    try
    {
        pt::read_json("../thermodynamic_data/calphadMoNbTa.json", calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    boost::optional<pt::ptree&> newton_db;

    // choose pair of phases: phaseL, phaseA, phaseB
    const Thermo4PFM::PhaseIndex pi0 = Thermo4PFM::PhaseIndex::phaseL;
    const Thermo4PFM::PhaseIndex pi1 = Thermo4PFM::PhaseIndex::phaseA;

    // initial guesses
    const double init_guess[5] = { 0.33, 0.38, 0.32, 0.33, 0.8 };

    // nominal composition
    double nominalc[2] = { 0.2, 0.3 };

    std::vector<double> cel(2 * (nTintervals + 1));
    std::vector<double> ces(2 * (nTintervals + 1));

    Thermo4PFM::CALPHADFreeEnergyFunctionsTernary* cafe
        = new Thermo4PFM::CALPHADFreeEnergyFunctionsTernary(calphad_db,
            newton_db, energy_interp_func_type, conc_interp_func_type);

    {
        // serial loop
        for (int i = 0; i < nTintervals + 1; i++)
        {
            const double temperature = Tmin + i * deltaT;

            double lceq[5] = { init_guess[0], init_guess[1], // liquid
                init_guess[2], init_guess[3], // solid
                init_guess[4] };

            // compute equilibrium concentrations in each phase
            // at ends of tie line
            bool found_ceq = cafe->computeTieLine(
                temperature, nominalc[0], nominalc[1], &lceq[0]);
            if (lceq[0] > 1.) found_ceq = false;
            if (lceq[0] < 0.) found_ceq = false;
            if (lceq[1] > 1.) found_ceq = false;
            if (lceq[1] < 0.) found_ceq = false;

            std::cout << "Temperature = " << temperature << std::endl;
            if (found_ceq)
            {
                std::cout << "Found solution: " << lceq[0] << ", " << lceq[1]
                          << ", " << lceq[2] << ", " << lceq[3] << ", "
                          << lceq[4] << "..." << std::endl;
            }
            CHECK(found_ceq);

            cel[2 * i]     = lceq[0];
            cel[2 * i + 1] = lceq[1];
            ces[2 * i]     = lceq[2];
            ces[2 * i + 1] = lceq[3];
        }
    }

// parallel loop
#pragma omp parallel for
    for (int i = 0; i < nTintervals + 1; i++)
    {
        const double temperature = Tmin + i * deltaT;

        double lceq[5] = { init_guess[0], init_guess[1], // liquid
            init_guess[2], init_guess[3], // solid
            init_guess[4] };

        // compute equilibrium concentrations in each phase
        bool found_ceq = cafe->computeTieLine(
            temperature, nominalc[0], nominalc[1], &lceq[0]);

        CHECK(found_ceq);
        CHECK(lceq[0] == Approx(cel[2 * i]).margin(1.e-6));
        CHECK(lceq[1] == Approx(cel[2 * i + 1]).margin(1.e-6));
        CHECK(lceq[2] == Approx(ces[2 * i]).margin(1.e-6));
        CHECK(lceq[3] == Approx(ces[2 * i + 1]).margin(1.e-6));
    }

#ifdef HAVE_OPENMP_OFFLOAD
    std::cout << "Run test loop on GPU..." << std::endl;

    double* xdev = new double[5 * (nTintervals + 1)];

// clang-format off
#pragma omp target map(to : cafe [0:1])                    \
                   map(to : init_guess[:5])                \
                   map(from : xdev[:5 * (nTintervals + 1)])
// clang-format on
#pragma omp parallel for
    for (int i = 0; i < nTintervals + 1; i++)
    {
        const double temperature = Tmin + i * deltaT;

        xdev[5 * i + 0] = init_guess[0];
        xdev[5 * i + 1] = init_guess[1]; // liquid
        xdev[5 * i + 2] = init_guess[2];
        xdev[5 * i + 3] = init_guess[3]; // solid
        xdev[5 * i + 4] = init_guess[4];

        // compute equilibrium concentrations in each phase
        bool found_ceq = cafe->computeTieLine(
            temperature, nominalc[0], nominalc[1], &xdev[5 * i]);
    }

    for (int i = 0; i < nTintervals + 1; i++)
    {
        std::cout << "Device: x=" << xdev[5 * i] << "," << xdev[5 * i + 1]
                  << "," << xdev[5 * i + 2] << "," << xdev[5 * i + 3]
                  << std::endl;
        CHECK(xdev[5 * i + 0] == Approx(cel[2 * i]).margin(1.e-6));
        CHECK(xdev[5 * i + 1] == Approx(cel[2 * i + 1]).margin(1.e-6));
        CHECK(xdev[5 * i + 2] == Approx(ces[2 * i]).margin(1.e-6));
        CHECK(xdev[5 * i + 3] == Approx(ces[2 * i + 1]).margin(1.e-6));
    }
    delete[] xdev;
#endif

    delete cafe;
}


#ifndef ROBOTOC_SE3_HPP_
#define ROBOTOC_SE3_HPP_

#include <Eigen/Core>
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/explog.hpp"

namespace robotoc {

///
/// @typedef SE3
/// @brief Using pinocchio::SE3 without its namespace.
///
using SE3 = pinocchio::SE3;

///
/// @brief Applies Log6 map that transforms the SE3 into 6-dimensional vector.
/// @param[in] SE3_obj SE3 object
/// @return Transformed 6-dimensional vector.
///
template <typename SE3Type>
const Eigen::Matrix<double, 6, 1> Log6Map(const SE3Type& SE3_obj) {
  return pinocchio::log6(SE3_obj).toVector();
}

///
/// @brief Computes the Jacobian of the Log6 map.
/// @param[in] SE3_obj SE3 object
/// @param[in, out] J Resultant Jacobian. Size must be 6x6.
///
template <typename SE3Type, typename MatrixType>
void computeJLog6Map(const SE3Type& SE3_obj,
                     const Eigen::MatrixBase<MatrixType>& J) {
  assert(J.rows() == 6);
  assert(J.cols() == 6);
  pinocchio::Jlog6(SE3_obj, const_cast<Eigen::MatrixBase<MatrixType>&>(J));
}

} // namespace robotoc

#endif // ROBOTOC_SE3_HPP_


/**
 * @file   test_ChromaticNumber.cpp
 * @brief  test chromatic number algorithm
 * @author Yibo Lin
 * @date   Feb 2015
 */

#include <iostream>
#include <fstream>
#include <string>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/undirected_graph.hpp>
#include <limbo/algorithms/coloring/ChromaticNumber.h>
#include <limbo/algorithms/coloring/GreedyColoring.h>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/graph/random.hpp>
#include <boost/graph/iteration_macros.hpp>

#include <boost/version.hpp>
#if BOOST_VERSION <= 14601
#include <boost/graph/detail/is_same.hpp>
#else
#include <boost/type_traits/is_same.hpp>
#endif

using std::cout;
using std::endl;
using std::ofstream;
using std::string;
using namespace boost;

/// maximum independent set visitor
/// @tparam GraphType graph type
template <typename GraphType>
struct MisVisitor
{
	GraphType& g; ///< graph

    /// constructor
    /// @param g_ graph
	MisVisitor(GraphType& g_) : g(g_) {}
    /// copy constructor
    /// @param rhs a MisVisitor object
	MisVisitor(MisVisitor const& rhs) : g(rhs.g) {}

    /// required callback for @ref limbo::algorithms::MaxIndependentSetByMaxClique
    /// @tparam MisType a container type, default is std::deque
    /// @param is independent set
	template <typename MisType>
	void mis(MisType const& is)
	{
		typename property_map<GraphType, vertex_index_t>::type vertex_index_map = get(vertex_index, g);

#if 1
		if (num_vertices(g) < 20)
		{
			cout << "independent sets" << endl;
			for (typename MisType::const_iterator it = is.begin();
					it != is.end(); ++it)
			{
				cout << vertex_index_map[*it] << " ";
			}
			cout << endl;
		}
#endif
	}
};

/// main function \n
/// generate a random graph, then \n
/// - test 1: find maximum independent set by @ref limbo::algorithms::MaxIndependentSetByMaxClique
/// - test 2: compute chromatic number by @ref limbo::algorithms::coloring::LawlerChromaticNumber
/// - test 3: greedy coloring by @ref limbo::algorithms::coloring::DsatColoring
int main()
{
	// do not use setS, it does not compile for subgraph
	// do not use custom property tags, it does not compile for most utilities
	typedef adjacency_list<vecS, vecS, undirectedS,
			property<vertex_index_t, std::size_t>,
			property<edge_index_t, std::size_t, property<edge_weight_t, int> >,
			property<graph_name_t, string> > graph_type;
	typedef subgraph<graph_type> subgraph_type;
	typedef property<vertex_index_t, std::size_t> VertexId;
	typedef property<edge_index_t, std::size_t> EdgeID;
	typedef typename boost::graph_traits<subgraph_type>::vertex_descriptor vertex_descriptor;

#if 1
	mt19937 gen;
    subgraph_type g;
    int N = 40;
    std::vector<vertex_descriptor> vertex_set;
    std::vector< std::pair<vertex_descriptor, vertex_descriptor> > edge_set;
    generate_random_graph(g, N, N * 2, gen,
                          std::back_inserter(vertex_set),
                          std::back_inserter(edge_set));
#else
	subgraph_type g;

	{
		vertex_descriptor v1 = add_vertex(g);
		vertex_descriptor v2 = add_vertex(g);
		vertex_descriptor v3 = add_vertex(g);
		vertex_descriptor v4 = add_vertex(g);
		vertex_descriptor v5 = add_vertex(g);
		add_edge(v1, v2, g);
		add_edge(v1, v3, g);
		add_edge(v1, v4, g);
		add_edge(v2, v3, g);
		add_edge(v2, v4, g);
		add_edge(v3, v4, g);
		add_edge(v3, v5, g);
	}

#endif

	property_map<subgraph_type, vertex_index_t>::type vertex_index_map = get(vertex_index, g);
	property_map<subgraph_type, edge_index_t>::type edge_index_map = get(edge_index, g);

	cout << "original graph" << endl;
	print_edges2(g, vertex_index_map, edge_index_map);
	print_graph(g, vertex_index_map);

	dynamic_properties dp;
	//dp.property("style", edge_style);
	//dp.property("pos", vertex_pos);
	//dp.property("len", weight);
	dp.property("node_id", get(vertex_index, g));
	ofstream out("graph.gv");
	write_graphviz_dp(out, g, dp);
	system("dot -Tpdf graph.gv -o graph.pdf");

	// test complement_graph
	{
		subgraph_type gp;
		std::map<vertex_descriptor, vertex_descriptor> mCompG2G;
		limbo::algorithms::complement_graph(g, gp, mCompG2G);

		cout << "complement graph" << endl;
		BGL_FORALL_VERTICES_T(v, gp, subgraph_type)
		{
			cout << vertex_index_map[mCompG2G[v]] << " <--> ";
			boost::graph_traits<subgraph_type>::out_edge_iterator e, e_end;
			for (boost::tie(e, e_end) = out_edges(v, gp); e != e_end; ++e)
				cout << vertex_index_map[mCompG2G[target(*e, gp)]] << " ";
			cout << endl;
		}
	}

	// test max_independent_set
	{
		limbo::algorithms::max_independent_set(g, MisVisitor<subgraph_type>(g), limbo::algorithms::MaxIndependentSetByMaxClique());
	}

	// test LawlerChromaticNumber
	{
		limbo::algorithms::coloring::LawlerChromaticNumber<graph_type> lcn;
		cout << "chromatic number = " << lcn(g) << endl;
	}

	// test DsatColoring
	{
		limbo::algorithms::coloring::DsatColoring<subgraph_type> dc (g);
		cout << "dsat coloring number = " << dc() << endl;
	}

	return 0;
}


/// @file  cmp.hpp
/// @brief Declarations for embedding methods using classic ordinal constraints

#pragma once
#ifndef OGT_EMBED_CMP_HPP
#define OGT_EMBED_CMP_HPP

#include <ogt/config.hpp>
#include <ogt/core/oracle.hpp>
#include <ogt/embed/embed.hpp>
#include <Eigen/Dense>
#include <vector>

namespace OGT_NAMESPACE {
namespace embed {

/// An ordinal constraint specifying that dist(a,b) < dist(c,d).
struct CmpConstraint {

	/// Default constructor.
	CmpConstraint() : a(0), b(0), c(0), d(0) {}

	/// Constructor with four points
	CmpConstraint(size_t a, size_t b, size_t c, size_t d)
		: a(a), b(b), c(c), d(d) {}

	/// Constructor with three points: dist(a,b) < dist(a,c).
	CmpConstraint(size_t a, size_t b, size_t c)
		: CmpConstraint(a, b, c, a) {}

	/// Constructor from an oracle comparison
	CmpConstraint(const OGT_NAMESPACE::core::CmpOutcome& cmp)
		: CmpConstraint(
			cmp.a,
			cmp.cmp == OGT_NAMESPACE::core::AB_LT_AC ? cmp.b : cmp.c,
			cmp.cmp == OGT_NAMESPACE::core::AB_LT_AC ? cmp.c : cmp.b) {}

	/// The objects being constrained
	size_t a, b, c, d;
};

/// Embeds a dataset by recovering a dissimilarity kernel using the Crowd Kernel
/// embedding algorithm.
/// The parameter lambda effects the scaling of the embedding.
///
/// Citation: O. Tamuz, C. Liu, S. Belongie, O. Shamir, and A. T. Kalai,
/// "Adaptively Learning the Crowd Kernel," International Conference on Machine
/// Learning, 2011.
EmbedResult embedCmpWithCKForK(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& K0, double lambda, EmbedConfig config);

/// Embeds a dataset using the Crowd Kernel embedding algorithm.
/// The parameter lambda effects the scaling of the embedding.
///
/// Citation: O. Tamuz, C. Liu, S. Belongie, O. Shamir, and A. T. Kalai,
/// "Adaptively Learning the Crowd Kernel," International Conference on Machine
/// Learning, 2011.
EmbedResult embedCmpWithCKForX(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& X0, double lambda, EmbedConfig config);

/// Embeds a dataset by recovering a dissimilarity kernel using Generalized
/// Non-metric Multidimensional Scaling.
/// The lambda parameter specifies the amount of regularization to apply to
/// keep the rank small.
/// An embedding can be recovered from the kernel using, for instance,
/// embeddingFromKernelSVD().
///
/// Citation: S. Agarwal, J. Wills, and L. Cayton, "Generalized non-metric
/// multidimensional scaling,"" International Conference on Machine Learning,
/// 2007.
EmbedResult embedCmpWithGNMDSForK(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& K0, double lambda, EmbedConfig config);

/// Embeds a dataset using Generalized Non-metric Multidimensional Scaling.
/// The lambda parameter specifies the amount of regularization to apply to
/// keep the rank small.
///
/// Citation: S. Agarwal, J. Wills, and L. Cayton, "Generalized non-metric
/// multidimensional scaling,"" International Conference on Machine Learning,
/// 2007.
EmbedResult embedCmpWithGNMDSForX(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& X0, double lambda, EmbedConfig config);

/// Embeds a dataset using Soft Ordinal Embedding.
/// This method uses the margin parameter to set the scale. A default value of
/// 0.1 will be used if no margin is provided.
///
/// Citation: Y. Terada and U. von Luxburg, "Local ordinal embedding," presented
/// at the Proceedings of the 31st International Conference on Machine Learning,
/// 2014.
EmbedResult embedCmpWithSOE(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& X0, EmbedConfig config);

/// Embeds using a weighted variant of Soft Ordinal Embedding. The loss function
/// is modified so that each constraint has a corresponding weight which is
/// multiplied to the loss incurred when that constraint is violated.
/// It is assumed, but not verified, that the weights are non-negative and sum
/// to one.
EmbedResult embedCmpWithSOEWeighted(std::vector<CmpConstraint> cons,
	const Eigen::VectorXd &weights, const Eigen::MatrixXd &X0,
	EmbedConfig config);

/// Embeds a point to satisfy a ranking of some set of fixed points, as much
/// as possible.
/// rank is an ordered list of rows from X. The resulting row in EmbedResult.X
/// gives a position which satisfies this ranking, if possible.
EmbedResult embedPtRankingWithSOE(const std::vector<size_t> &rank,
	const Eigen::MatrixXd& X, const Eigen::VectorXd& pos0, EmbedConfig config);

/// Embeds a dataset using Local Ordinal Embedding.
/// This method uses the margin parameter to set the scale. A default value of
/// 0.1 will be used if no margin is provided.
///
/// Citation: Y. Terada and U. von Luxburg, "Local ordinal embedding," presented
/// at the Proceedings of the 31st International Conference on Machine Learning,
/// 2014.
EmbedResult embedKnnWithLOE(const Eigen::MatrixXi& knn,
	const Eigen::MatrixXd& X0, EmbedConfig config);

/// Pick an optimal SOE scale parameter for the embedding.
double fitSOEScale(std::vector<CmpConstraint> cons, const Eigen::MatrixXd& X);

/// Embeds a dataset by recovering a dissimilarity kernel using Stochastic
/// Triplet Embedding.
/// The lambda parameter specifies the degree of regularization.
///
/// Citation: L. Van der Maaten and K. Weinberger, "Stochastic triplet
/// embedding," 2012 IEEE International Workshop on Machine Learning for Signal
/// Processing (MLSP), 2012.
EmbedResult embedCmpWithSTEForK(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& K0, double lambda, EmbedConfig config);

/// Embeds a dataset using Stochastic Triplet Embedding.
/// The lambda parameter specifies the degree of regularization.
///
/// Citation: L. Van der Maaten and K. Weinberger, "Stochastic triplet
/// embedding," 2012 IEEE International Workshop on Machine Learning for Signal
/// Processing (MLSP), 2012.
EmbedResult embedCmpWithSTEForX(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& X0, double lambda, EmbedConfig config);

/// Embeds a dataset using t-Distributed Stochastic Triplet Embedding.
/// The lambda parameter specifies the degree of regularization.
/// The alpha parameter indicates the degrees of freedom for the Student's-t
/// distribution.
///
/// Citation: L. Van der Maaten and K. Weinberger, "Stochastic triplet
/// embedding," 2012 IEEE International Workshop on Machine Learning for Signal
/// Processing (MLSP), 2012.
EmbedResult embedCmpWithTSTE(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& X0, double lambda, double alpha, EmbedConfig config);

/// Embeds a dataset using Rank-d Projected Gradient Descent (PGD), adapted
/// to work directly on a n x d matrix X rather than to project after each step.
///
/// Citation: L. Jain, K. Jamieson, and R. Nowak, "Finite Sample Prediction and
/// Recovery Bounds for Ordinal Embedding," NIPS, 2016.
EmbedResult embedCmpWithPGDForX(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& X0, EmbedConfig config);

/// Embeds a dataset using Rank-d Projected Gradient Descent (PGD).
/// This is the original version of the algorithm.
/// After each learning step, we project to the nearest rank d Gram matrix.
///
/// Citation: L. Jain, K. Jamieson, and R. Nowak, "Finite Sample Prediction and
/// Recovery Bounds for Ordinal Embedding," NIPS, 2016.
EmbedResult embedCmpWithPGDForK(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& X0, EmbedConfig config);

/// Embeds a dataset using Nuclear Norm Projected Gradient Descent.
/// After each learning step, we project onto the nuclear norm ball, which has
/// the effect of minimizing the rank.
///
/// Citation: L. Jain, K. Jamieson, and R. Nowak, "Finite Sample Prediction and
/// Recovery Bounds for Ordinal Embedding," NIPS, 2016.
EmbedResult embedCmpWithNNPGD(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& X0, double lambda, EmbedConfig config);

/// Embeds a dataset using Nuclear Norm Projected Gradient Descent Debiased.
/// This is similar to embedCmpWithNNPGD() but with an extra debiasing step.
/// This debiasing improves accuracy by rescaling the non-zero eigenvalues
/// to prevent them from shrinking toward zero. It is the default behavior.
///
/// Citation: L. Jain, K. Jamieson, and R. Nowak, "Finite Sample Prediction and
/// Recovery Bounds for Ordinal Embedding," NIPS, 2016.
EmbedResult embedCmpWithNNPGDDebiased(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& X0, double lambda, EmbedConfig config);

/// Compute a kernel matrix where K_ij is derived by how similarly points i and
/// j are ranked by the provided comparisons.
/// The parameters k1 and k2 indicate how much of the two possible kernels to
/// mix into the final output; k1 corresponds to an anchor-based kernel while k2
/// corresponds to a tail-based kernel.
///
/// Citation: M. Kleindessner and U. von Luxburg, “Kernel functions based on
/// triplet similarity comparisons,” arXiv.org, vol. stat.ML. 28-Jul-2016.
EmbedResult embedCmpWithTauForK(std::vector<CmpConstraint> cons, double k1,
	double k2);

} // end namespace embed
} // end namespace OGT_NAMESPACE
#endif /* OGT_EMBED_CMP_HPP */


/*
 * Copyright 2022 Sean McBane
 *
 * 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 NONINFRINGEMENT. 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.
 */

#ifndef LINEAR_ELASTICITY_HPP
#define LINEAR_ELASTICITY_HPP

#include <Eigen/Core>
#include <cassert>
#include <exception>

#include "C0_triangles.hpp"
#include "Galerkin.hpp"
#include "SymSparse.hpp"
#include "mesh_traits.hpp"
#include "src/FunctionBase.hpp"

namespace Elasticity
{

template <class Element>
struct basis_size_struct
{
};

template <>
struct basis_size_struct<C0Triangle<1>>
{
    constexpr static size_t value = 3;
};

template <>
struct basis_size_struct<C0Triangle<2>>
{
    constexpr static size_t value = 6;
};

template <>
struct basis_size_struct<C0Triangle<3>>
{
    constexpr static size_t value = 10;
};

template <>
struct basis_size_struct<C0Triangle<4>>
{
    constexpr static size_t value = 15;
};

template <class Element>
constexpr size_t basis_size = basis_size_struct<Element>::value;

template <class Element>
using coeffs_type = Eigen::Matrix<double, basis_size<Element>, 1>;

template <class Element>
using vector_coeffs_type = Eigen::Matrix<double, 2, basis_size<Element>>;

template <class Mesh>
auto get_scalar_coeffs(const Mesh &mesh, size_t eli, const Eigen::VectorXd &uc)
{
    const auto &el_info = mesh.element(eli);
    Eigen::Matrix<double, std::decay_t<decltype(el_info)>::num_nodes(), 1> coeffs;

    auto nn = el_info.node_numbers();
    for (size_t i = 0; i < el_info.num_nodes(); ++i)
    {
        coeffs[i] = uc[nn[i]];
    }

    return coeffs;
}

template <class Mesh>
auto get_vector_coeffs(const Mesh &mesh, size_t eli, const Eigen::Matrix2Xd &uc)
{
    const auto &el_info = mesh.element(eli);
    Eigen::Matrix<double, 2, std::decay_t<decltype(el_info)>::num_nodes()> coeffs;

    auto nn = el_info.node_numbers();
    for (size_t i = 0; i < el_info.num_nodes(); ++i)
    {
        coeffs.col(i) = uc.col(nn[i]);
    }

    return coeffs;
}

/*
 * Given the coefficients of a function u in the finite element basis for el,
 * construct a function object that evaluates that function (and whose
derivatives,
 * etc. can be used).
 */
template <class Element>
auto make_function(const Element &, const coeffs_type<Element> &uc)
{
    using Galerkin::Functions::ConstantFunction;
    return Galerkin::static_sum<1, basis_size<Element>>(
        [&uc](auto I) { return ConstantFunction(uc[I()]) * Galerkin::get<I()>(Element::basis); },
        ConstantFunction(uc[0]) * Galerkin::get<0>(Element::basis));
}

/*
 * Given the coefficients of a *vector* function u in the finite element basis
 * for el, construct 2 function objects evaluating the X and Y parts of the
 * vector function.
 */
template <class Element>
auto make_vector_functions(const Element &el, const vector_coeffs_type<Element> &uc)
{
    using Galerkin::Functions::ConstantFunction;

    auto ux = Galerkin::static_sum<1, basis_size<Element>>(
        [&](auto I) { return ConstantFunction(uc(0, I())) * Galerkin::get<I()>(Element::basis); },
        ConstantFunction(uc(0, 0)) * Galerkin::get<0>(Element::basis));
    auto uy = Galerkin::static_sum<1, basis_size<Element>>(
        [&](auto I) { return ConstantFunction(uc(1, I())) * Galerkin::get<I()>(Element::basis); },
        ConstantFunction(uc(1, 0)) * Galerkin::get<0>(Element::basis));

    return std::pair(ux, uy);
}

template <class Element>
using el_stiffness_type = Eigen::Matrix<double, 2 * basis_size<Element>, 2 * basis_size<Element>>;

template <class Element>
el_stiffness_type<Element> element_stiffness_matrix(const Element &el, double lambda, double mu);

extern template el_stiffness_type<C0Triangle<1>>
element_stiffness_matrix(const C0Triangle<1> &, double, double);
extern template el_stiffness_type<C0Triangle<2>>
element_stiffness_matrix(const C0Triangle<2> &, double, double);

#if ELASTICITY_MAX_ELEMENT_ORDER > 2
extern template el_stiffness_type<C0Triangle<3>>
element_stiffness_matrix(const C0Triangle<3> &, double, double);
#endif

#if ELASTICITY_MAX_ELEMENT_ORDER > 3
extern template el_stiffness_type<C0Triangle<4>>
element_stiffness_matrix(const C0Triangle<4> &, double, double);
#endif

template <class Element>
el_stiffness_type<Element> element_stiffness_matrix(
    const Element &el, const coeffs_type<Element> &lambda, const coeffs_type<Element> &mu);

extern template el_stiffness_type<C0Triangle<1>> element_stiffness_matrix(
    const C0Triangle<1> &, const coeffs_type<C0Triangle<1>> &,
    const coeffs_type<C0Triangle<1>> &);
extern template el_stiffness_type<C0Triangle<2>> element_stiffness_matrix(
    const C0Triangle<2> &, const coeffs_type<C0Triangle<2>> &,
    const coeffs_type<C0Triangle<2>> &);

#if ELASTICITY_MAX_ELEMENT_ORDER > 2
extern template el_stiffness_type<C0Triangle<3>> element_stiffness_matrix(
    const C0Triangle<3> &, const coeffs_type<C0Triangle<3>> &,
    const coeffs_type<C0Triangle<3>> &);
#endif

#if ELASTICITY_MAX_ELEMENT_ORDER > 3
extern template el_stiffness_type<C0Triangle<4>> element_stiffness_matrix(
    const C0Triangle<4> &, const coeffs_type<C0Triangle<4>> &,
    const coeffs_type<C0Triangle<4>> &);
#endif

template <class Element>
Eigen::Matrix<double, basis_size<Element>, basis_size<Element>>
element_mass_matrix(const Element &el);

extern template Eigen::Matrix<double, basis_size<C0Triangle<1>>, basis_size<C0Triangle<1>>>
element_mass_matrix(const C0Triangle<1> &);
extern template Eigen::Matrix<double, basis_size<C0Triangle<2>>, basis_size<C0Triangle<2>>>
element_mass_matrix(const C0Triangle<2> &);

#if ELASTICITY_MAX_ELEMENT_ORDER > 2
extern template Eigen::Matrix<double, basis_size<C0Triangle<3>>, basis_size<C0Triangle<3>>>
element_mass_matrix(const C0Triangle<3> &);
#endif

#if ELASTICITY_MAX_ELEMENT_ORDER > 3
extern template Eigen::Matrix<double, basis_size<C0Triangle<4>>, basis_size<C0Triangle<4>>>
element_mass_matrix(const C0Triangle<4> &);
#endif

template <class Mesh>
auto instantiate_element(const Mesh &mesh, size_t which)
{
    constexpr int order = msh::element_order<Mesh>;
    const auto &el = mesh.element(which);
    return C0Triangle<order>(
        mesh.coord(el.control_nodes[0]), mesh.coord(el.control_nodes[1]),
        mesh.coord(el.control_nodes[2]));
}

constexpr auto lame_parameters(double E, double nu) noexcept
{
    double lambda = E * nu / ((1 + nu) * (1 - nu));
    double mu = E / (2 * (1 + nu));
    return std::make_pair(lambda, mu);
}

typedef SymSparse::SymmetricSparseMatrix<double, 2 * max_node_adjacencies> StiffnessType;

template <class Mesh>
StiffnessType assemble_stiffness(const Mesh &mesh, double E, double nu)
{
    StiffnessType K(mesh.num_nodes() * 2);
    auto [lambda, mu] = lame_parameters(E, nu);

    for (size_t i = 0; i < mesh.num_elements(); ++i)
    {
        const auto &el_info = mesh.element(i);
        const auto nn = el_info.node_numbers();
        const auto el = instantiate_element(mesh, i);
        const auto local_K = element_stiffness_matrix(el, lambda, mu);
        for (size_t j = 0; j < nn.size(); ++j)
        {
            for (size_t k = j; k < nn.size(); ++k)
            {
                K.insert_entry(nn[j] * 2, nn[k] * 2, local_K(2 * j, 2 * k));
                if (k != j)
                {
                    K.insert_entry(nn[j] * 2 + 1, nn[k] * 2, local_K(2 * j + 1, 2 * k));
                }
                K.insert_entry(nn[j] * 2, nn[k] * 2 + 1, local_K(2 * j, 2 * k + 1));
                K.insert_entry(nn[j] * 2 + 1, nn[k] * 2 + 1, local_K(2 * j + 1, 2 * k + 1));
            }
        }
    }
    return K;
}

template <class Mesh>
StiffnessType
assemble_stiffness(const Mesh &mesh, const Eigen::VectorXd &lambda, const Eigen::VectorXd &mu)
{
    StiffnessType K(mesh.num_nodes() * 2);

    for (size_t eli = 0; eli < mesh.num_elements(); ++eli)
    {
        auto lc = get_scalar_coeffs(mesh, eli, lambda);
        auto uc = get_scalar_coeffs(mesh, eli, mu);
        const auto &el_info = mesh.element(eli);
        const auto nn = el_info.node_numbers();
        const auto el = instantiate_element(mesh, eli);
        const auto local_K = element_stiffness_matrix(el, lc, uc);

        for (size_t j = 0; j < nn.size(); ++j)
        {
            for (size_t k = j; k < nn.size(); ++k)
            {
                K.insert_entry(nn[j] * 2, nn[k] * 2, local_K(2 * j, 2 * k));
                if (k != j)
                {
                    K.insert_entry(nn[j] * 2 + 1, nn[k] * 2, local_K(2 * j + 1, 2 * k));
                }
                K.insert_entry(nn[j] * 2, nn[k] * 2 + 1, local_K(2 * j, 2 * k + 1));
                K.insert_entry(nn[j] * 2 + 1, nn[k] * 2 + 1, local_K(2 * j + 1, 2 * k + 1));
            }
        }
    }

    return K;
}

extern template StiffnessType assemble_stiffness<Mesh1>(const Mesh1 &, double, double);
extern template StiffnessType
assemble_stiffness<Mesh1>(const Mesh1 &, const Eigen::VectorXd &, const Eigen::VectorXd &);
extern template StiffnessType assemble_stiffness<Mesh2>(const Mesh2 &, double, double);
extern template StiffnessType
assemble_stiffness<Mesh2>(const Mesh2 &, const Eigen::VectorXd &, const Eigen::VectorXd &);

#if ELASTICITY_MAX_ELEMENT_ORDER > 2
extern template StiffnessType assemble_stiffness<Mesh3>(const Mesh3 &, double, double);
extern template StiffnessType
assemble_stiffness<Mesh3>(const Mesh3 &, const Eigen::VectorXd &, const Eigen::VectorXd &);
#endif

#if ELASTICITY_MAX_ELEMENT_ORDER > 3
extern template StiffnessType assemble_stiffness<Mesh4>(const Mesh4 &, double, double);
extern template StiffnessType
assemble_stiffness<Mesh4>(const Mesh4 &, const Eigen::VectorXd &, const Eigen::VectorXd &);
#endif

template <>
inline StiffnessType
assemble_stiffness<MeshVariant>(const MeshVariant &mv, double lambda, double mu)
{
    return std::visit(
        [lambda, mu](const auto &mesh) { return assemble_stiffness(mesh, lambda, mu); }, mv);
}

template <>
inline StiffnessType assemble_stiffness<MeshVariant>(
    const MeshVariant &mv, const Eigen::VectorXd &lambda, const Eigen::VectorXd &mu)
{
    return std::visit(
        [&lambda, &mu](const auto &mesh) { return assemble_stiffness(mesh, lambda, mu); }, mv);
}

/*
 * Add a homogeneous Dirichlet condition on the boundary segment of the mesh
 * indexed by `which`. K is the stiffness matrix obtained from
 * `assemble_stiffness`, and `rhs` is the forcing vector.
 */
template <class Mesh, class RHS>
void impose_homogeneous_condition(
    const Mesh &mesh, StiffnessType &K, RHS &rhs, size_t which, double scale = 1.0)
{
    if constexpr (std::is_same_v<Mesh, MeshVariant>)
    {
        std::visit(
            [&, which, scale](const auto &mesh)
            { impose_homogeneous_condition(mesh, K, rhs, which, scale); },
            mesh);
    }
    else
    {
        std::vector<size_t> adjacent;
        adjacent.reserve(2 * max_node_adjacencies);
        const auto &boundary = mesh.boundary(which);

        for (auto n : boundary.nodes)
        {
            adjacent.clear();
            // Get all of the adjacent DOFs to this one; since there are two
            // components of displacement there are 2 degrees of freedom (2*n,
            // 2*n+1) corresponding to each node.
            for (auto n2 : mesh.adjacent_nodes(n))
            {
                adjacent.push_back(2 * n2);
                adjacent.push_back(2 * n2 + 1);
            }
            K.eliminate_dof(2 * n, 0.0, scale, rhs, adjacent);
            K.eliminate_dof(2 * n + 1, 0.0, scale, rhs, adjacent);
        }
    }
}

struct PreEliminatedStiffness
{
};

template <class Mesh, class RHS>
void impose_homogeneous_condition(
    const Mesh &mesh, PreEliminatedStiffness, RHS &rhs, size_t which)
{
    if constexpr (std::is_same_v<Mesh, MeshVariant>)
    {
        std::visit(
            [&, which](const auto &mesh)
            { impose_homogeneous_condition(mesh, PreEliminatedStiffness{}, rhs, which); },
            mesh);
    }
    else
    {
        const auto &boundary = mesh.boundary(which);

        for (auto n : boundary.nodes)
        {
            rhs.row(2 * n).array() = 0;
            rhs.row(2 * n + 1).array() = 0;
        }
    }
}

template <class Mesh>
void impose_homogeneous_condition(
    const Mesh &mesh, StiffnessType &K, size_t which, double scale = 1.0)
{
    std::vector<size_t> adjacent;
    adjacent.reserve(2 * max_node_adjacencies);
    const auto &boundary = mesh.boundary(which);

    for (auto n : boundary.nodes)
    {
        adjacent.clear();
        // Get all of the adjacent DOFs to this one; since there are two
        // components of displacement there are 2 degrees of freedom (2*n,
        // 2*n+1) corresponding to each node.
        for (auto n2 : mesh.adjacent_nodes(n))
        {
            adjacent.push_back(2 * n2);
            adjacent.push_back(2 * n2 + 1);
        }
        K.eliminate_dof(2 * n, 0.0, scale, adjacent);
        K.eliminate_dof(2 * n + 1, 0.0, scale, adjacent);
    }
}

template <>
void impose_homogeneous_condition(const MeshVariant &, StiffnessType &, size_t, double);

struct OutOfBoundsIndex : public std::exception
{
    const char *msg;
    OutOfBoundsIndex(const char *m) : msg(m) {}
    const char *what() const noexcept { return msg; }
};

template <class Mesh, class RHS>
void impose_dirichlet_condition(
    const Mesh &mesh, StiffnessType &K, RHS &rhs, size_t which, const Eigen::Matrix2Xd &value,
    double scale = 1.0)
{
    if constexpr (std::is_same_v<Mesh, MeshVariant>)
    {
        std::visit(
            [&, which, scale](const auto &mesh)
            { impose_dirichlet_condition(mesh, K, rhs, which, value, scale); },
            mesh);
    }
    else
    {
        std::vector<size_t> adjacent;
        adjacent.reserve(2 * max_node_adjacencies);
        const auto &boundary = mesh.boundary(which);

        if ((size_t)value.cols() != boundary.nodes.size())
        {
            throw OutOfBoundsIndex(
                "Matrix given for Dirichlet condition has wrong number of values");
        }
        else if ((size_t)rhs.rows() != mesh.num_nodes() * 2)
        {
            throw OutOfBoundsIndex("RHS passed has wrong dimension - should be 2 * num_nodes");
        }

        size_t i = 0;
        for (auto n : boundary.nodes)
        {
            adjacent.clear();
            for (auto n2 : mesh.adjacent_nodes(n))
            {
                adjacent.push_back(2 * n2);
                adjacent.push_back(2 * n2 + 1);
            }
            K.eliminate_dof(2 * n, value(0, i), scale, rhs, adjacent);
            K.eliminate_dof(2 * n + 1, value(1, i), scale, rhs, adjacent);
            i += 1;
        }
    }
}

template <class Force, class RHS>
void add_point_force(const Force &force, size_t node, RHS &F)
{
    if (2 * node + 1 > F.size())
    {
        throw OutOfBoundsIndex("Node is out of bounds for given forcing vector");
    }
    F[2 * node] += force[0];
    F[2 * node + 1] += force[1];
}

template <class T, class RHS, int N, class IndexContainer, int... Options>
void add_point_forces(
    const Eigen::Matrix<T, 2, N, Options...> &force, const IndexContainer &nodes, RHS &F)
{
    static_assert(sizeof...(Options) == 3);
    size_t col = 0;
    for (size_t n : nodes)
    {
        add_point_force(force.col(col), n, F);
        col += 1;
    }
}

/*
 * Where fc is a matrix containing the coefficients of the X and Y components of
 * the volume forcing in the first and second rows, respectively, integrate this
 * volume force to obtain its contribution to the linear form vector.
 */
template <class Mesh>
Eigen::Matrix2Xd integrate_volume_force(const Mesh &mesh, const Eigen::Matrix2Xd &fc);

extern template Eigen::Matrix2Xd integrate_volume_force(const Mesh1 &, const Eigen::Matrix2Xd &);
extern template Eigen::Matrix2Xd integrate_volume_force(const Mesh2 &, const Eigen::Matrix2Xd &);

#if ELASTICITY_MAX_ELEMENT_ORDER > 2
extern template Eigen::Matrix2Xd integrate_volume_force(const Mesh3 &, const Eigen::Matrix2Xd &);
#endif

#if ELASTICITY_MAX_ELEMENT_ORDER > 3
extern template Eigen::Matrix2Xd integrate_volume_force(const Mesh4 &, const Eigen::Matrix2Xd &);
#endif

template <>
inline Eigen::Matrix2Xd integrate_volume_force(const MeshVariant &mv, const Eigen::Matrix2Xd &fc)
{
    return std::visit([&fc](const auto &mesh) { return integrate_volume_force(mesh, fc); }, mv);
}

/*
 * Where fc is a matrix containing coefficients of the X and Y parts of a
 * traction force on boundary bound_index, integrates this traction force to
 * find its contribution to the linear form vector.
 */
template <class Mesh>
Eigen::Matrix2Xd
integrate_traction_force(const Mesh &mesh, size_t bound_index, const Eigen::Matrix2Xd &fc);

extern template Eigen::Matrix2Xd
integrate_traction_force(const Mesh1 &, size_t, const Eigen::Matrix2Xd &);
extern template Eigen::Matrix2Xd
integrate_traction_force(const Mesh2 &, size_t, const Eigen::Matrix2Xd &);

#if ELASTICITY_MAX_ELEMENT_ORDER > 2
extern template Eigen::Matrix2Xd
integrate_traction_force(const Mesh3 &, size_t, const Eigen::Matrix2Xd &);
#endif

#if ELASTICITY_MAX_ELEMENT_ORDER > 3
extern template Eigen::Matrix2Xd
integrate_traction_force(const Mesh4 &, size_t, const Eigen::Matrix2Xd &);
#endif

template <>
inline Eigen::Matrix2Xd
integrate_traction_force(const MeshVariant &mv, size_t bound_index, const Eigen::Matrix2Xd &fc)
{
    return std::visit(
        [&fc, bound_index](const auto &mesh)
        { return integrate_traction_force(mesh, bound_index, fc); },
        mv);
}

/*
 * Where 'traction' is an integrated traction force on boundary 'bindex',
 * as returned from 'integrate_traction_force', adds this force to the
 * appropriate columns of 'forcing' (which, for example, might have come from
 * 'integrate_volume_force'.)
 */
void add_traction_force(
    const MeshVariant &mv, Eigen::Matrix2Xd &forcing, size_t bindex,
    const Eigen::Matrix2Xd &traction);

/*
 * Where u_true is a function taking X and Y coordinates and u is a matrix with
 * coefficients of X and Y parts of a FEM solution in its columns, compute the
 * L^2 norm of u - u_true and of u_true. Used for manufactured solution
 * verification.
 */
std::pair<double, double> integrate_error(
    const MeshVariant &mesh, const Eigen::Matrix2Xd &u,
    Eigen::Vector2d (*u_true)(std::array<double, 2>));

} // namespace Elasticity

#endif // LINEAR_ELASTICITY_HPP


/// @file  Test suit for Akima spline
/// @brief All tests data for these suite
///        were generated by gnu aspline:
///        $aspline --version
///        aspline version 1.2

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

#include <k52/dsp/akima_spline.h>

namespace
{
// The double and float values in this test suit may not differ more than kTolerance%
const double kTolerance = 0.001;
}

namespace k52
{
namespace dsp
{
namespace tests
{

class AkimaSplineMock : public AkimaSpline
{
public:
    std::size_t ResolveInterval(double value) const
    {
        return AkimaSpline::ResolveInterval(value);
    }
};

BOOST_AUTO_TEST_CASE(ConstructorMustNotThrow)
{
    AkimaSpline::Create();   // Empty Parameters
}

// Akima interpolation require at least 3 points
BOOST_AUTO_TEST_CASE(ResolveIntervalThrowOutOfRange)
{
    double x[] = { -2, -1, 0, 1, 2 }; // it must monotonically increase
    double y[] = {  0,  0, 0, 0, 0 };

    AkimaSplineMock spline;
    spline.Initialize(std::vector<double>(x, x + 5), std::vector<double>(y, y + 5));

    BOOST_REQUIRE_THROW(spline.ResolveInterval(-2.1), std::logic_error);
    BOOST_REQUIRE_THROW(spline.ResolveInterval(2.1), std::logic_error);
}

// Akima interpolation require at least 5 points
BOOST_AUTO_TEST_CASE(InitializeNegativeTrainingPoints)
{
    double x[] = { -0.126543, 0, 1, 2, 3}; // it must monotonically increase
    double y[] = { 0,         0, 0, 0, 0};

    AkimaSplineMock spline;
    BOOST_REQUIRE_THROW(spline.Initialize(std::vector<double>(x, x+2), std::vector<double>(y, y+2)),
                        std::logic_error);

    BOOST_REQUIRE_THROW(spline.Initialize(std::vector<double>(x, x+5), std::vector<double>(y, y+4)),
                        std::logic_error);
}

// Akima calculates values f(x) based on interval x[i] <= x <= x[i+1]
// Current implementation detect interval x[i] <= x < x[i+1]
// so it should correctly detects interval number i based on given value x
BOOST_AUTO_TEST_CASE(CheckResolveInterval)
{
    double x[] = { -0.126543, 0, 0.919, 1.4, 19 }; // it must monotonically increase
    double y[] = { 0,         0, 0,     0,   0 };

    AkimaSplineMock spline;
    spline.Initialize(std::vector<double>(x, x+5), std::vector<double>(y, y+5));

    BOOST_CHECK_EQUAL(spline.ResolveInterval(-0.1), 0);
    BOOST_CHECK_EQUAL(spline.ResolveInterval(0),    1);
    BOOST_CHECK_EQUAL(spline.ResolveInterval(0.8),  1);
    BOOST_CHECK_EQUAL(spline.ResolveInterval(0.92), 2);
    BOOST_CHECK_EQUAL(spline.ResolveInterval(1.5),  3);
    BOOST_CHECK_EQUAL(spline.ResolveInterval(18.9), 3);

    BOOST_CHECK_EQUAL(spline.ResolveInterval(19),   3);
}

// The main Akima spline feature - it doesn't oscillate near interpolated points
BOOST_AUTO_TEST_CASE(AkimaSplineShouldNotOscillate)
{
    double x[] = { -3, -2, -1, 0, 1, 2, 3 }; // it must monotonically increase
    double y[] = {  0,  0,  0, 0, 0, 0, 0 };

    std::vector<double> train_x(x, x+7);
    std::vector<double> train_y(y, y+7);

    AkimaSpline::shared_ptr spline = AkimaSpline::Create();
    spline->Initialize(train_x, train_y);

    // generated for the test points by
    // $aspline --points=10 <test.data>
    BOOST_CHECK_CLOSE(spline->Value(-2.9), 0, kTolerance);
    BOOST_CHECK_CLOSE(spline->Value(-1.9), 0, kTolerance);
    BOOST_CHECK_CLOSE(spline->Value(-0.9), 0, kTolerance);
    BOOST_CHECK_CLOSE(spline->Value(-0.1), 0, kTolerance);
    BOOST_CHECK_CLOSE(spline->Value(0.1),  0, kTolerance);
    BOOST_CHECK_CLOSE(spline->Value(0.9),  0, kTolerance);
    BOOST_CHECK_CLOSE(spline->Value(1.9),  0, kTolerance);
    BOOST_CHECK_CLOSE(spline->Value(2.9),  0, kTolerance);
}

// Actually check akima spline
BOOST_AUTO_TEST_CASE(CheckSimpleSet)
{
    double x[] = {  -3,  -2, -1, 0,  1,    2,   3 }; // it must monotonically increase
    double y[] = {-0.1, 0.1,  1, 0, -1, -0.1, 0.1 };

    std::vector<double> train_x(x, x+7);
    std::vector<double> train_y(y, y+7);

    AkimaSpline::shared_ptr spline = AkimaSpline::Create();
    spline->Initialize(train_x, train_y);

    // generated for the test points by
    // $aspline --points=10 <test.data>
    BOOST_CHECK_CLOSE(spline->Value(-3),    -0.1,       kTolerance);
    BOOST_CHECK_CLOSE(spline->Value(-2.4),  -0.0407385, kTolerance);
    BOOST_CHECK_CLOSE(spline->Value(-2),    0.1,        kTolerance);
    BOOST_CHECK_CLOSE(spline->Value(-1.8),  0.275323,   kTolerance);
    BOOST_CHECK_CLOSE(spline->Value(-1.2),  1.04683,    kTolerance);
    BOOST_CHECK_CLOSE(spline->Value(-1),    1,          kTolerance);
    BOOST_CHECK_CLOSE(spline->Value(0),     0,          kTolerance);
    BOOST_CHECK_CLOSE(spline->Value(0.6),   -0.6,       kTolerance);
    BOOST_CHECK_CLOSE(spline->Value(1.2),   -1.04683,   kTolerance);
    BOOST_CHECK_CLOSE(spline->Value(1.8),   -0.275323,  kTolerance);
    BOOST_CHECK_CLOSE(spline->Value(2),     -0.1,       kTolerance);
    BOOST_CHECK_CLOSE(spline->Value(2.4),   0.0407385,  kTolerance);
    BOOST_CHECK_CLOSE(spline->Value(3),     0.1,        kTolerance);
}

} // namespace tests
} // namespace dsp
} // namespace k52


/*
 * Copyright 2014-2017, Andrea Del Prete, Rohan Budhiraja LAAS-CNRS
 *
 */

#include <Eigen/Dense>

#include <dynamic-graph/factory.h>
#include <sot/core/debug.hh>
#include <sot/torque_control/numerical-difference.hh>
#include <sot/torque_control/commands-helper.hh>
#include <sot/torque_control/motor-model.hh>

namespace dynamicgraph {
namespace sot {
namespace torque_control {

#define ALL_INPUT_SIGNALS m_xSIN

#define ALL_OUTPUT_SIGNALS m_x_filteredSOUT << m_dxSOUT << m_ddxSOUT

namespace dynamicgraph = ::dynamicgraph;
using namespace dynamicgraph;
using namespace dynamicgraph::command;
using namespace Eigen;

/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
typedef NumericalDifference EntityClassName;

/* --- DG FACTORY ------------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(NumericalDifference, "NumericalDifference");

/* --- CONSTRUCTION ----------------------------------------------------- */
/* --- CONSTRUCTION ----------------------------------------------------- */
/* --- CONSTRUCTION ----------------------------------------------------- */
NumericalDifference::NumericalDifference(const std::string& name)
    : Entity(name),
      CONSTRUCT_SIGNAL_IN(x, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_OUT(x_filtered, dynamicgraph::Vector, m_x_dx_ddxSINNER),
      CONSTRUCT_SIGNAL_OUT(dx, dynamicgraph::Vector, m_x_dx_ddxSINNER),
      CONSTRUCT_SIGNAL_OUT(ddx, dynamicgraph::Vector, m_x_dx_ddxSINNER),
      CONSTRUCT_SIGNAL_INNER(x_dx_ddx, dynamicgraph::Vector, m_xSIN) {
  Entity::signalRegistration(ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS);

  /* Commands. */
  addCommand("getTimestep", makeDirectGetter(*this, &m_dt, docDirectGetter("Control timestep [s ]", "double")));
  addCommand("getDelay",
             makeDirectGetter(*this, &m_delay, docDirectGetter("Delay in the estimation of signal x", "double")));
  addCommand("getSize", makeDirectGetter(*this, &x_size, docDirectGetter("Size of the x signal", "int")));
  addCommand("init", makeCommandVoid4(*this, &NumericalDifference::init,
                                      docCommandVoid4("Initialize the estimator.", "Control timestep [s].",
                                                      "Size of the input signal x", "Estimation delay for signal x",
                                                      "Polynomial order")));
}

/* --- COMMANDS ---------------------------------------------------------- */
/* --- COMMANDS ---------------------------------------------------------- */
/* --- COMMANDS ---------------------------------------------------------- */
void NumericalDifference::init(const double& timestep, const int& xSize, const double& delay, const int& polyOrder) {
  assert(timestep > 0.0 && "Timestep should be > 0");
  assert(delay >= 1.5 * timestep && "Estimation delay should be >= 1.5*timestep");
  m_dt = timestep;
  m_delay = delay;
  x_size = xSize;
  int winSizeEnc = (int)(2 * delay / m_dt);
  assert(winSizeEnc >= 3 && "Estimation-window's length should be >= 3");

  if (polyOrder == 1)
    m_filter = new LinEstimator(winSizeEnc, x_size, m_dt);
  else if (polyOrder == 2)
    m_filter = new QuadEstimator(winSizeEnc, x_size, m_dt);
  else
    SEND_MSG("Only polynomial orders 1 and 2 allowed. Reinitialize the filter", MSG_TYPE_INFO);
  m_ddx_filter_std.resize(x_size);
  m_dx_filter_std.resize(x_size);
  m_x_filter_std.resize(x_size);
  m_x_std.resize(x_size);
}

/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */

/** Signal Filtering and Differentiation. */
DEFINE_SIGNAL_INNER_FUNCTION(x_dx_ddx, dynamicgraph::Vector) {
  sotDEBUG(15) << "Compute x_dx_ddx inner signal " << iter << std::endl;

  // read encoders and copy in std vector
  const dynamicgraph::Vector& base_x = m_xSIN(iter);
  COPY_VECTOR_TO_ARRAY(base_x, m_x_std);

  // Signal Filters
  m_filter->estimate(m_x_filter_std, m_x_std);
  m_filter->getEstimateDerivative(m_dx_filter_std, 1);
  m_filter->getEstimateDerivative(m_ddx_filter_std, 2);

  // copy data in signal vector
  if (s.size() != 3 * x_size) s.resize(3 * x_size);
  for (int i = 0; i < x_size; i++) s(i) = m_x_filter_std[i];
  for (int i = 0; i < x_size; i++) s(i + x_size) = m_dx_filter_std[i];
  for (int i = 0; i < x_size; i++) s(i + 2 * x_size) = m_ddx_filter_std[i];

  return s;
}

/// ************************************************************************* ///
/// The following signals depend only on other inner signals, so they
/// just need to copy the interested part of the inner signal they depend on.
/// ************************************************************************* ///

DEFINE_SIGNAL_OUT_FUNCTION(x_filtered, dynamicgraph::Vector) {
  sotDEBUG(15) << "Compute x_filtered output signal " << iter << std::endl;

  const dynamicgraph::Vector& x_dx_ddx = m_x_dx_ddxSINNER(iter);
  if (s.size() != x_size) s.resize(x_size);
  s = x_dx_ddx.head(x_size);
  return s;
}

DEFINE_SIGNAL_OUT_FUNCTION(dx, dynamicgraph::Vector) {
  sotDEBUG(15) << "Compute dx output signal " << iter << std::endl;

  const dynamicgraph::Vector& x_dx_ddx = m_x_dx_ddxSINNER(iter);
  if (s.size() != x_size) s.resize(x_size);
  s = x_dx_ddx.segment(x_size, x_size);
  return s;
}

DEFINE_SIGNAL_OUT_FUNCTION(ddx, dynamicgraph::Vector) {
  sotDEBUG(15) << "Compute ddx output signal " << iter << std::endl;

  const dynamicgraph::Vector& x_dx_ddx = m_x_dx_ddxSINNER(iter);
  if (s.size() != x_size) s.resize(x_size);
  s = x_dx_ddx.segment(2 * x_size, x_size);
  return s;
}

void NumericalDifference::display(std::ostream& os) const {
  os << "NumericalDifference " << getName() << ":\n";
  try {
    getProfiler().report_all(3, os);
  } catch (ExceptionSignal e) {
  }
}

}  // namespace torque_control
}  // namespace sot
}  // namespace dynamicgraph


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>
#include "MatrixFreeOperator.hpp"

#ifndef _DAVIDSON_OP_
#define _DAVIDSON_OP_

class DavidsonOperator : public MatrixFreeOperator
{
	public:

		DavidsonOperator(int n, double eps, bool d, bool reorder);
		Eigen::VectorXd col(int index) const;
		Eigen::ArrayXd _sort_index(Eigen::VectorXd& V) const;
		Eigen::VectorXd reorder_col(Eigen::VectorXd& col) const;

	private:
		double _sparsity = 0.1;
		bool _odiag = false;
		bool _reorder;
		Eigen::ArrayXd _order_index;
};

#endif


/**
 * Author	: Michael Fonder
 * Year		: 2016
 **/

#include <iostream>
#include <sstream>
#include <time.h>
#include <stdio.h>
#include <stdint.h>
#include <math.h>
#include <Eigen/Dense>
#include <Eigen/QR>
#include <Eigen/Dense>
#include <Eigen/Eigen>
#include <Eigen/Sparse>

#include <unordered_map>
#include <unordered_set>
#include <set>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/video.hpp>
#include <opencv2/opencv.hpp>
#include "opencv2/video/tracking.hpp"
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/core/eigen.hpp>

#include "dynamicVectorContainer.hpp"
#include "dynamicVectorReference.hpp"
#include "utils.hpp"
#include "imuState.hpp"
#include "MSCKF.hpp"

#ifndef _CRT_SECURE_NO_WARNINGS
# define _CRT_SECURE_NO_WARNINGS
#endif

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

#define Matrixfd Matrix<float, Eigen::Dynamic, Eigen::Dynamic>

MSCKF::MSCKF()
{
	imustate = new IMUstate();
	featureId_cnter = 150;
	maxNbrePoses = 24;
	maxNbreFeatures = 50;
	last_IMU_meas_time=0.0;

	camera.q_CI = Mat::zeros(4,1,CV_32FC1);

	// normal case orientation
	camera.q_CI.row(0) = 0.5;
	camera.q_CI.row(1) = -0.5;
	camera.q_CI.row(2) = 0.5;
	camera.q_CI.row(3) = -0.5;

	camProjMat = quatToProjMat(camera.q_CI);

	camera.p_CI = Mat::zeros(3,1,CV_32FC1);
	camera.p_CI = camProjMat*camera.p_CI;

	im_noise 			= 0.01;
	tracking_quality 	= 0.05;
	tracking_tresh 		= 0.55;
	posEst_tresh 		= 100;

}

void MSCKF::setCameraParams(const Mat &p_CI, const Mat &q_CI)
{
	q_CI.copyTo(camera.q_CI);
	camProjMat = quatToProjMat(camera.q_CI);
	camera.p_CI = camProjMat*p_CI;
}

void MSCKF::setFilterParams(const Mat &params)
{
	im_noise 			= params.at<float>(0,0);
	maxNbrePoses 		= params.at<float>(1,0);
	maxNbreFeatures 	= params.at<float>(2,0);
}

// Input == last measurement
void MSCKF::propagateIMUStateAndCovar(const Measurement &measurements, float timestamp)
{
	last_IMU_meas_time = timestamp;
	imustate->propagateState(measurements);
	Mat Phi = imustate->propagateCovar(measurements);
	if(cameraPoses.size() > 0)
		imuCamCovar = Phi*imuCamCovar;
}

/**
* Returns current position estimation
**/
Mat MSCKF::getPosition()
{
	return imustate->p_G;
}

/**
* Returns current attitude estimation
**/
Mat MSCKF::getOrientation()
{
	return imustate->q_IG;
}

/**
* Returns current filter covariance
**/
Mat MSCKF::getCovar()
{
	return imustate->covar;
}

void MSCKF::updateStateAndCovar()
{
	if(features_to_residualize.size() == 0)
		return;
}

// Augment state when new frame available
void MSCKF::augmentState(const Mat &frame, float timestamp)//const Feature &feature)
{
	Mat Jt;
	CameraPose cameraPose;
	imustate->getDelayedPose(cameraPose.pose, Jt, timestamp-last_IMU_meas_time);

	// store new IMU pose
	size_t N = cameraPoses.size();
	cameraPose.pose.p.copyTo(cameraPose.p_CG);
	cameraPose.pose.q.copyTo(cameraPose.q_CG);
	cameraPoses.push_back(cameraPose);

	addFrame(frame);

	if(points.size()==0) // If no features detected for current frame, remove current camera pose
		posesToPrune.insert(cameraPoses.last());

	// Build MSCKF covariance matrix for state augmentation
	size_t L = imustate->stateLength; // just for increased lisibility of the code
	Mat P = Mat::zeros(L+6*N, L+6*N, CV_32FC1);
	(imustate->covar).copyTo(P.rowRange(0,L).colRange(0,L));
	if(N>0)
	{
		camCovar.copyTo(P.rowRange(L, L+6*N).colRange(L,L+6*N));
		transpose(imuCamCovar, P.rowRange(L,L+6*N).colRange(0,L));
		imuCamCovar.copyTo(P.rowRange(0,L).colRange(L,L+6*N));
	}

	// Augment state
	Mat J = calcJ(N, Jt, cameraPose, timestamp-last_IMU_meas_time+imustate->delay);
	Mat p_aug = Mat::eye(6+L+6*N,L+6*N, CV_32FC1);
	J.copyTo(p_aug.rowRange(L+6*N,6+L+6*N).colRange(0,L+6*N));

	p_aug = p_aug*P*(p_aug.t());

	// assign result
	p_aug.rowRange(0,L)			.colRange(0,L)			.copyTo(imustate->covar);
	p_aug.rowRange(L,L+6+6*N)	.colRange(L,L+6+6*N)	.copyTo(camCovar);
	p_aug.rowRange(0,L)			.colRange(L, L+6+6*N)	.copyTo(imuCamCovar);

}

/**
* Extract and track features for a new incomming frame
* Params:
*	frame	: the new frame
* Returns the number of features tracked
**/
int MSCKF::addFrame(const Mat &frame)
{
	vector<uchar> status;
	vector<float> err;
	vector<float> err1;

	TermCriteria termcrit(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, tracking_quality);
	Size subPixWinSize(10,10), winSize(7,7);

	const int MAX_COUNT = 10;
	int discontinued = 0;

	if(cameraPoses.size()>1 && points.size()!= 0)
	{
		vector<Point2f> p1(points);
		vector<Point2f> p0r(points);

		// doc : http://docs.opencv.org/2.4/modules/video/doc/motion_analysis_and_object_tracking.html#calcopticalflowpyrlk
		calcOpticalFlowPyrLK(previous_frame, frame,points, p1, status, err, Size(39, 39), 4, termcrit, OPTFLOW_USE_INITIAL_FLOW);
		calcOpticalFlowPyrLK(frame, previous_frame,p1, p0r, status, err1, Size(39, 39), 4, termcrit, OPTFLOW_USE_INITIAL_FLOW);

		for(size_t i=0; i<points.size(); ++i)
		{
			if(norm(points[i]-p0r[i])>tracking_tresh)
			{
				status[i]= 0;
				discontinued++;
			}
			else
				points[i] = p1[i];
		}
	}

	int sx = frame.cols, sy= frame.rows;

	Mat mask = Mat::zeros(frame.rows, frame.cols,  CV_8UC1)*255;
	rectangle( mask, Point( sx*0.15, sy*0.15 ), Point( sx*0.85, sy*0.85), 255, -1, 8 );
	int size_circle = int(30*float(features.size()-discontinued)/float(maxNbreFeatures));
	for(size_t i=0; i<points.size(); ++i)
		circle(mask, points[i], size_circle, 0, -1);

	// Detect new features only if required
	if(features.size()-discontinued < maxNbreFeatures || float(countNonZero(mask))/float(sx*sy)>0.25)// && (cameraPoses.size()-1)%1 == 0)
	{

		imshow("mask", mask);

		Mat descriptors1;
		vector<KeyPoint> keypoints;
		BRISK extractor(48,3,1.5);
		extractor(frame, mask, keypoints, descriptors1);

		for(size_t i=0; i<keypoints.size(); ++i)
		{
			points.push_back(keypoints[i].pt);
			ids.push_back(featureId_cnter);
			status.push_back(1);
			Feature tmp;
			features.insert({featureId_cnter, tmp});
			++featureId_cnter;
		}
	}

	// Highlight tracked features in the image
	mask = frame;
	for(size_t i=0; i<points.size(); ++i)
		circle(mask, points[i], 3, 255, -1);

	imshow("frame", mask);
	waitKey(1);

	for(size_t i=0; i<points.size(); i++)
	{
		int x, y;
		x = points[i].x;
		y = points[i].y;

		// Discard features too close from the borders of the image
		if(status[i] == 1 && (x<0.15*sx || x>0.85*sx || y<0.15*sy || y>0.85*sy))
		{
			status[i] = 0;
		}
	}

	// Add point updates to data structure
	int longest = -1, max_size = 0;
	for(size_t i=0; i<points.size(); )
	{
		if(max_size<features[ids[i]].positions.size())
		{
			longest  = i;
			max_size = features[ids[i]].positions.size();
		}

		if(status[i] == 1)
		{
			Mat matPoint(1,1,CV_32FC2);
			float x = 160, y=120;
			matPoint = {points[i].x, points[i].y};
			undistortPoints(matPoint, matPoint,camera.intrisic, camera.distCoeffs);
			int test = ids[i];
			features[ids[i]].positions.push_back(matPoint.reshape(1,2));
			features[ids[i]].cameraPoses.push_back(cameraPoses.last());
			(cameraPoses.last())->featuresId.insert(ids[i]);
			++i;
		}
		else
		{
			features_to_residualize.insert(ids[i]);
			points.erase(points.begin()+i);
			ids.erase(ids.begin()+i);
			status.erase(status.begin()+i);
		}
	}

	if(features_to_residualize.size() < 1 && longest >= 0)
	{
		features_to_residualize.insert(ids[longest]);
		points.erase(points.begin()+longest);
		ids.erase(ids.begin()+longest);
		status.erase(status.begin()+longest);
	}

	frame.copyTo(previous_frame);
}

/**
* Performs the EKF update step
**/
void MSCKF::update()
{
	extractVariablesToPrune();

	// Exit function if nothing has to be done
	if(features_to_residualize.size() == 0 && posesToPrune.size()==0)
		return;

	// Assemble covariance matrix
	size_t N = imustate->stateLength + imuCamCovar.cols;
	Mat P = Mat::zeros(N,N,CV_32FC1);
	imustate->covar.copyTo(P.rowRange(0,imustate->stateLength).colRange(0, imustate->stateLength));
	imuCamCovar.copyTo(P.rowRange(0,imustate->stateLength).colRange(imustate->stateLength, N));
	transpose(imuCamCovar, P.rowRange(imustate->stateLength, N).colRange(0, imustate->stateLength));
	camCovar.copyTo(P.rowRange(imustate->stateLength, N).colRange(imustate->stateLength, N));

	Mat Ho, ro, Ro;

	int accepted_features = 0;

	for(std::unordered_set<int32_t>::iterator it = features_to_residualize.begin(); it != features_to_residualize.end(); it++)
	{
		Mat p_f_G, Hoj, Aj;
		int test = *it;
		Feature feature = features.find(*it)->second;

		if(feature.cameraPoses.size() < 3)
		{
			continue;
		}

		float d = calcGNPosEst(features[*it], p_f_G);
		if(d/float(features[*it].positions.size())>posEst_tresh)
		{
			continue;
		}

		calcHoj(p_f_G, feature, Hoj, Aj);

		//Compute residuals
		Mat rj(2*feature.positions.size(), 1, CV_32FC1); // NOTE : Previously : Mat rj = Mat::zeros(2*feature.positions.size(), 1, CV_32FC1);
		for(size_t i=0; i<feature.positions.size(); ++i)
		{
			Mat C_CG = camProjMat*quatToProjMat((feature.cameraPoses[i]).q_CG);
			Mat p_f_C = C_CG*(p_f_G-(feature.cameraPoses[i]).p_CG)-camera.p_CI;

			rj.at<float>(2*i,  0) = p_f_C.at<float>(0,0)/p_f_C.at<float>(2,0);
			rj.at<float>(2*i+1,0) = p_f_C.at<float>(1,0)/p_f_C.at<float>(2,0);
			rj.rowRange(2*i, 2*i+2) = feature.positions[i] - rj.rowRange(2*i, 2*i+2);
		}

		rj = Aj.t()*rj;

		// Mahanalobis gating test
		Mat Rj = Mat::eye(rj.rows, rj.rows, CV_32FC1)*im_noise;
		Mat S = Hoj*P*Hoj.t()+Rj;
		Mat MD;
		Mat tmp3;
		mySolve(S,rj,MD);
		MD = rj.t()*MD;
cout << "MD : " << MD <<chiSquared(rj.rows) << endl;
		if(isnan(abs(MD.at<float>(0,0))) || MD.at<float>(0,0) > chiSquared(rj.rows))
			continue;

		// Project matrices on the left nullspace
		Ho.push_back(Hoj);

		ro.push_back(rj);

		// Push Rj on the diag of Ro
		Mat tmp = Mat::zeros(Ro.rows+Rj.rows, Ro.cols+Rj.cols, CV_32FC1);
		Ro.copyTo(tmp.rowRange(0, Ro.rows).colRange(0, Ro.cols));// += Ro;
		Rj.copyTo(tmp.rowRange(Ro.rows, Ro.rows+Rj.rows).colRange(Ro.cols,Ro.cols+Rj.cols));// += Rj;
		tmp.copyTo(Ro);

		++accepted_features;
	}

	if(accepted_features > 0) //Do filter update only if there are some residuals to process
	{
		Mat Q1, Th;
		{	// Compute range space of Ho to speed up following computations
			MatrixXf eigenHo;
			cv2eigen(Ho, eigenHo);
			FullPivHouseholderQR<MatrixXf> qr(eigenHo);
			MatrixXf Q = qr.matrixQ();
			MatrixXf R = qr.matrixQR().triangularView<Eigen::Upper>();
			R = R*qr.colsPermutation().inverse();
			cout << "range space computed"<<endl;

			eigen2cv(Q, Q1);
			eigen2cv(R, Th);
		}

		// Speed boost by eliminating noise from the residuals
		for(size_t i=0; i<Th.rows; ++i)
		{
			size_t j=0;
			for(; j<Th.cols;++j)
				if(Th.at<float>(i,j)!=0)
					break;
			if(j==Th.cols)
			{
				Th.rowRange(0,i).copyTo(Th);
				Q1.colRange(0,i).copyTo(Q1);
				break;
			}
		}

		Ro = Q1.t()*Ro*Q1;
		Mat rn = Q1.t()*ro;
		rn.copyTo(ro);

		Mat tmp2 = Th*P*Th.t()+Ro;
		Mat K;
		{
			MatrixXf A, b;
			cv2eigen(tmp2.t(), A);
			cv2eigen((P*Th.t()).t(), b);

			// Choose your own solving method
// 				MatrixXf K2 = A.colPivHouseholderQr().solve(b);
			MatrixXf K2 = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
//				MatrixXf K2 = A.householderQr().solve(b);

			eigen2cv(K2, K);
		}
		transpose(K,K);

		// State correction term
		Mat deltaX = K*ro;
		cout << "DeltaX : " << endl << deltaX << endl;

		if(isnan(abs(deltaX.at<float>(0,0))) || isinf(abs(deltaX.at<float>(0,0))))
		{
			cout << "Warning ill conditionned matrices : State correction failed" << endl;
		}
		else
		{
			updateState(deltaX);

			// Covariance correction
			Mat tmp = Mat::eye(N,N, CV_32FC1) - K*Th;
			P = tmp*P*tmp.t() + K*Ro*K.t(); // P corrected

			P.rowRange(0,imustate->stateLength).colRange(0,imustate->stateLength)  .copyTo(imustate->covar);
			P.rowRange(0, imustate->stateLength).colRange(imustate->stateLength,N) .copyTo(imuCamCovar);
			P.rowRange(imustate->stateLength, N).colRange(imustate->stateLength, N).copyTo(camCovar);
		}
	}

	// State pruning
	pruneState();
}

/**
* Computes Jacobian used to augment the state
* Params:
*	N 			: Number of camera poses stored in the state;
*	Jt			: Jacobian of the time delay;
* 	cameraPose 	: camera pose;
* 	timeStep	: timestep between last received IMU measurements and current frame;
* Returns the Jacobian martix
**/
MatExpr MSCKF::calcJ(size_t N, const Mat &Jt, CameraPose &cameraPose, float timeStep)
{
	Mat rotMat = quatToProjMat(cameraPose.pose.q);
	Mat J = Mat::zeros(6, (imustate->stateLength)+6*N, CV_32FC1);
	J.rowRange(0,3).colRange(0,3) = Mat::eye(3,3,CV_32FC1);
	J.rowRange(3,6).colRange(12,15) = Mat::eye(3,3,CV_32FC1);
	J.rowRange(3,6).colRange(6,9) = Mat::eye(3,3,CV_32FC1)*timeStep;
	J.rowRange(0,6).colRange(15,16) = Jt;

	return J+0.0;
}

/**
* Computes Jacobian of a feature track for the state update
* Params:
*	p_f_G 	: feature position in the global frame;
*	feature	: feature track;
* 	Hoj 	: resulting matrix;
* 	Aj		: nullspace of Hfi
**/
void MSCKF::calcHoj(const Mat &p_f_G, const Feature &feature, Mat &Hoj, Mat &Aj) //const &feature?
{
	size_t IMU = imustate->stateLength; // length of IMU state
	size_t N = cameraPoses.size();
	size_t M = feature.positions.size();
	Mat Hfj = Mat::zeros(2*M, 3, CV_32FC1);
	Mat Hxj = Mat::zeros(2*M, IMU+6*N, CV_32FC1);

	for(size_t i=0; i<feature.positions.size(); i++)
	{
		int camPoseIndex = cameraPoses.getIndex(feature.cameraPoses.at(i));
		Mat p_CG = (feature.cameraPoses[i]).pose.p;
		Mat q_CG = (feature.cameraPoses[i]).pose.q;

		// Express feature position in the camera frame
		Mat p_f_C = camProjMat*quatToProjMat((feature.cameraPoses[i]).q_CG)*(p_f_G-(feature.cameraPoses[i]).p_CG)-camera.p_CI;
		Mat C_CG = camProjMat*quatToProjMat(q_CG);

		Mat Ji = Mat::zeros(2,3, CV_32FC1);
		Ji.at<float>(0,0) = 1;
		Ji.at<float>(1,1) = 1;
		Ji.at<float>(0,2) = -p_f_C.at<float>(0,0)/p_f_C.at<float>(2,0);
		Ji.at<float>(1,2) = -p_f_C.at<float>(1,0)/p_f_C.at<float>(2,0);
		Ji /= p_f_C.at<float>(2,0);

		Mat pos_diff = p_f_G-p_CG;
		Mat tmp = Mat::zeros(2,6, CV_32FC1);

		// Jacobian of the residual with respect to the feature position
		Hfj.rowRange(2*i, 2*i+2) = Ji*camProjMat*quatToProjMat((feature.cameraPoses[i]).q_CG);

		// Jacobian of the residual with respect to the IMU pose
		tmp.colRange(0, 3) = Hfj.rowRange(2*i, 2*i+2)*crossMat(pos_diff);//Ji*C_CG*crossMat(pos_diff);
		tmp.colRange(3, 6) = -Hfj.rowRange(2*i, 2*i+2);//Ji*C_CG;
		tmp.copyTo(Hxj.rowRange(2*i, 2*i+2).colRange(IMU+6*camPoseIndex, IMU+6*camPoseIndex+6));

		// Jacobian of the residual with respect to the time delay
		Hxj.rowRange(2*i, 2*i+2).colRange(15, 16) = -Ji*camProjMat*(crossMat((feature.cameraPoses[i]).pose.ang_vel).t())*quatToProjMat((feature.cameraPoses[i]).q_CG)*pos_diff-Ji*C_CG*(feature.cameraPoses[i]).pose.v;

		// Jacobian of the residual with respect to the camera pose
		Hxj.rowRange(2*i, 2*i+2).colRange(16, 19) = Ji*camProjMat*crossMat(quatToProjMat(q_CG)*pos_diff);
		Ji.copyTo(Hxj.rowRange(2*i, 2*i+2).colRange(19, 22));
	}

	// QR decomposition to have nullspace
	MatrixXf eigenHfj;
	cv2eigen(Hfj, eigenHfj);
	ColPivHouseholderQR<MatrixXf> qr(eigenHfj);
	MatrixXf Q = qr.matrixQ();
	Q = Q.rightCols(eigenHfj.rows()-eigenHfj.cols());
	eigen2cv(Q, Aj);

	// projection on the nullspace of the Jacobian of the residual with respect to the feature position
	Hoj = Aj.t()*Hxj;
}

/**
* Builds camera matrix from camera orientation and location
* Params:
*	C 	: rotation matrix;
*	t 	: translation matrix;
* Returns camera matrix
**/
Mat MSCKF::buildCameraMatrix(const Mat &C, const Mat &t)
{
	Mat matrix = Mat::zeros(3,4, CV_32FC1);
	C.copyTo(matrix.colRange(0,3));
	t.copyTo(matrix.col(3));
	return matrix+0.0;
}

/**
* Solves the AX=b linear system
* Params:
*	A 	: A matrix;
*	b 	: b matrix;
* 	res : matrix to store the result;
**/
void MSCKF::mySolve(const Mat &A, const Mat &b, Mat &res)
{
	Matrixfd eigA, eigb;
	cv2eigen(A, eigA);
	cv2eigen(b, eigb);
	Matrixfd eigRes = eigA.colPivHouseholderQr().solve(eigb);
	eigen2cv(eigRes, res);
}

/**
* Performs Gauss-Newton regression on a set of points corresponding to a same feature to estimate its 3d location
* Params:
*	feature : the feature whose position needs to be estimated;
*	pos 	: a matrix to store the result;
**/
float MSCKF::calcGNPosEst(const Feature &feature, Mat &pos)
{
	pos = Mat::zeros(3,1, CV_32FC1);
	Mat posCop;
	Mat p_IC = camProjMat.t()*camera.p_CI;

	{	// get estimate of feature position by triangulation for state initialisation
		Mat point_4d;
		Mat C1 = camProjMat*quatToProjMat(feature.cameraPoses.front().q_CG);
		Mat C2 = camProjMat*quatToProjMat(feature.cameraPoses.back().q_CG);
		Mat t1 = C1*feature.cameraPoses.front().p_CG+camera.p_CI;
		Mat t2 = C2*feature.cameraPoses.back().p_CG+camera.p_CI;

		triangulatePoints( buildCameraMatrix(C1, -t1), buildCameraMatrix(C2, -t2),
						feature.positions.front(), feature.positions.back(), point_4d);

		for(size_t i=0; i<3; ++i)
			pos.at<float>(i,0) = point_4d.at<float>(i,0)/point_4d.at<float>(3,0);

		pos = C1*(pos)-t1;
	}

	//Inverse depth parametrisation
	Mat xEst = Mat::ones(3,1, CV_32FC1);
	xEst.at<float>(0,0) = pos.at<float>(0,0)/pos.at<float>(2,0); //alphaBar
	xEst.at<float>(1,0) = pos.at<float>(1,0)/pos.at<float>(2,0); //betaBar
	xEst.at<float>(2,0) /= pos.at<float>(2,0);			 //rhoBar

	size_t n = feature.cameraPoses.size();
	int maxIter = 10;

	Mat prov;
	CameraPose cp_1;
	prov = quatLeftComp(camera.q_CI)*feature.cameraPoses.front().q_CG;
	prov = prov/norm(prov);
	prov.copyTo(cp_1.q_CG);
	prov = feature.cameraPoses.front().p_CG + quatToProjMat(cp_1.q_CG).t()*camera.p_CI;
	prov.copyTo(cp_1.p_CG);

	Mat sqrErr_prev = Mat::zeros(1,1,CV_32FC1);
	Mat sqrErr;
	for(size_t iter=0; iter<maxIter; iter++)
	{
		Mat E = Mat::zeros(2*n, 3, CV_32FC1);
		Mat errorVec = Mat::zeros(2*n,1, CV_32FC1);

		for(size_t i=0; i<n; i++)
		{
			CameraPose cp_i;
			prov = quatLeftComp(camera.q_CI)*feature.cameraPoses[i].q_CG;
			prov = prov/norm(prov);
			prov.copyTo(cp_i.q_CG);
			prov = feature.cameraPoses[i].p_CG + quatToProjMat(cp_i.q_CG).t()*camera.p_CI;
			prov.copyTo(cp_i.p_CG);

			// Compute evolution between first and current camera pose
			Mat C_i1 = quatToProjMat(cp_i.q_CG)*(quatToProjMat(cp_1.q_CG).t());
			Mat t_i1 = quatToProjMat(cp_i.q_CG)*(cp_1.p_CG - cp_i.p_CG);

			// Compute residuals
			Mat tmp = Mat::ones(3,1, CV_32FC1);
			xEst.rowRange(0,2).copyTo(tmp.rowRange(0,2));
			Mat h = C_i1*tmp + xEst.at<float>(2,0)*t_i1;
			tmp = Mat::ones(2,1,CV_32FC1);
			tmp.at<float>(0,0) = h.at<float>(0,0)/h.at<float>(2,0);
			tmp.at<float>(1,0) = h.at<float>(1,0)/h.at<float>(2,0);

			// Reproject estimated feature location in the 2d plane to compute the residuals
			errorVec.rowRange(2*i,2*i+2) = feature.positions[i]-tmp;

			// Form the Jacobian from eq. (39) in TR_MSCKF

			//dEdalpha
			E.at<float>(2*i, 0)   = -C_i1.at<float>(0,0)/h.at<float>(2,0) + (h.at<float>(0,0)/(h.at<float>(2,0)*h.at<float>(2,0)))*C_i1.at<float>(2,0);
			E.at<float>(2*i+1, 0) = -C_i1.at<float>(1,0)/h.at<float>(2,0) + (h.at<float>(1,0)/(h.at<float>(2,0)*h.at<float>(2,0)))*C_i1.at<float>(2,0);
			//dEdbeta
			E.at<float>(2*i, 1)   = -C_i1.at<float>(0,1)/h.at<float>(2,0) + (h.at<float>(0,0)/(h.at<float>(2,0)*h.at<float>(2,0)))*C_i1.at<float>(2,1);
			E.at<float>(2*i+1, 1) = -C_i1.at<float>(1,1)/h.at<float>(2,0) + (h.at<float>(1,0)/(h.at<float>(2,0)*h.at<float>(2,0)))*C_i1.at<float>(2,1);
			//dEdrho
			E.at<float>(2*i, 2)   = -t_i1.at<float>(0,0)/h.at<float>(2,0) + (h.at<float>(0,0)/(h.at<float>(2,0)*h.at<float>(2,0)))*t_i1.at<float>(2,0);
			E.at<float>(2*i+1, 2) = -t_i1.at<float>(1,0)/h.at<float>(2,0) + (h.at<float>(1,0)/(h.at<float>(2,0)*h.at<float>(2,0)))*t_i1.at<float>(2,0);
		}

		Mat delta;

		mySolve(E.t()*E, E.t()*errorVec, delta);
		xEst -= delta;

		sqrErr = 0.5*errorVec.t()*errorVec;
		Mat err = (sqrErr-sqrErr_prev)/n;//sqrErr;
		sqrErr.copyTo(sqrErr_prev);

		// Stop if gain in precision is too small
		if(err.at<float>(0,0) < 0.0000001)
			break;
	}

	// Reproject the feature location into the global referential
	Mat tmp = Mat::ones(3,1, CV_32FC1);
	xEst.rowRange(0,2).copyTo(tmp.rowRange(0,2));
	pos = (1/xEst.at<float>(2,0))*(quatToProjMat(cp_1.q_CG).t())*tmp +cp_1.p_CG;
	return sqrErr.at<float>(0,0);
}

/**
* Extracts the data to use for an EKF filter update
**/
void MSCKF::extractVariablesToPrune()
{
	// Prepare features pruning
	for(std::unordered_set<int32_t>::iterator it = features_to_residualize.begin(); it != features_to_residualize.end(); it++)
	{
		Feature feature = features.find(*it)->second;
		for(size_t i=0; i<feature.cameraPoses.size(); ++i)
		{
			feature.cameraPoses[i].featuresId.erase(*it);
			if(feature.cameraPoses[i].featuresId.size() == 0)
				posesToPrune.insert(feature.cameraPoses.at(i));
		}
	}

	// Prepare camera states pruning if too much poses in memory
	if(cameraPoses.size()-posesToPrune.size() < maxNbrePoses)
		return;

	std::unordered_map<int32_t, int32_t> features_correspondance;

	// Going through poses to prune
	for(size_t i=1; i<cameraPoses.size(); i+=3)
	{
		// Going through each feature detected for current pose

		for(std::unordered_set<int32_t>::iterator it = cameraPoses[i].featuresId.begin(); it != cameraPoses[i].featuresId.end(); ++it)
		{
			if(features_to_residualize.find(*it) != features_to_residualize.end())
				continue;

			if(features_correspondance.find(*it) == features_correspondance.end())
			{
				// Create temporary features to store information of features present in camera poses to prune
				features_correspondance.insert({*it, featureId_cnter});
				int index = features[*it].cameraPoses.getIndex(cameraPoses.at(i));
				Feature feature;
				features.insert({featureId_cnter, feature});
				features[*it].truePos.copyTo(features[featureId_cnter].truePos);
				features_to_residualize.insert(featureId_cnter);
				++featureId_cnter;

				features_to_prune.push_back(*it);
			}

			// If feature present in previous camstates, transfer information to correct temporary feature
			int id = features_correspondance[*it];
			int index = features[*it].cameraPoses.getIndex(cameraPoses.at(i));
			features[id].cameraPoses.push_back(cameraPoses.at(i));
			features[id].positions.push_back(features[*it].positions[index]);
			features[*it].positionsToPrune.insert(features[*it].positions.at(index));
		}
		posesToPrune.insert(cameraPoses.at(i));
	}
}

/**
* Deletes the data to use for an EKF filter update
**/
void MSCKF::pruneState()
{
	size_t nbre_remaining_states = cameraPoses.size() - posesToPrune.size();
	size_t L = imustate->stateLength;
	size_t N = 6*cameraPoses.size();
	Mat new_imuCamCovar = Mat:: zeros(L,6*nbre_remaining_states, CV_32FC1);

	size_t j=0;
	for(size_t i=0; i<cameraPoses.size(); ++i)
	{
		if(posesToPrune.find(cameraPoses.at(i)) != posesToPrune.end())
		{
			N = camCovar.rows-6; //6*(i-j);
			size_t size = camCovar.rows;
			Mat new_camCovar(N, N, CV_32FC1);

			if(j!=0)
				camCovar.rowRange(0, 6*j).colRange(0,6*j)			.copyTo(new_camCovar.rowRange(0, 6*j).colRange(0,6*j));
			if(j!=nbre_remaining_states)
			{
				if(j!=0)
				{
				camCovar.rowRange(6*j+6, size).colRange(0,6*j)		.copyTo(new_camCovar.rowRange(6*j, N).colRange(0,6*j));
				camCovar.rowRange(0,6*j).colRange(6*j+6, size)		.copyTo(new_camCovar.rowRange(0,6*j).colRange(6*j, N));
				}
				camCovar.rowRange(6*j+6, size).colRange(6*j+6, size).copyTo(new_camCovar.rowRange(6*j, N).colRange(6*j, N));
			}
			new_camCovar.copyTo(camCovar);
		}
		else
		{
			imuCamCovar.colRange(6*i, 6*i+6).copyTo(new_imuCamCovar.colRange(6*j, 6*j+6));
			++j;
		}

	}
	new_imuCamCovar.copyTo(imuCamCovar);
	for(std::unordered_set<int32_t>::iterator it = features_to_residualize.begin(); it != features_to_residualize.end(); ++it)
		features.erase(*it);
	for(size_t i=0; i<features_to_prune.size(); ++i)
	{
		int32_t id = features_to_prune[i];
		features[id].positions.remove(features[id].positionsToPrune);
		features[id].cameraPoses.remove(posesToPrune);
		features[id].positionsToPrune.clear();
	}

	cameraPoses.remove(posesToPrune);
	posesToPrune.clear();
	features_to_residualize.clear();
	features_to_prune.clear();
}

/**
* Applies the correction step
**/
void MSCKF::updateState(const Mat &deltaX)
{

	// Update IMU state
	imustate->q_IG 	= quatLeftComp( imustate->q_IG) *buildUpdateQuat(deltaX.rowRange(0,3));
	imustate->q_IG 	= imustate->q_IG/norm(imustate->q_IG);
	imustate->bg 	+= deltaX.rowRange(3,6);

	imustate->v_G 	+= deltaX.rowRange(6,9);
	imustate->ba 	+= deltaX.rowRange(9,12);
	imustate->p_G 	+= deltaX.rowRange(12,15);

	imustate->delay += deltaX.at<float>(16,1);
	camera.q_CI 	= quatLeftComp(camera.q_CI) *buildUpdateQuat(deltaX.rowRange(16,19));
	camera.q_CI 	/= float(norm(camera.q_CI));
	camProjMat 		= quatToProjMat(camera.q_CI);
	camera.p_CI 	+= camProjMat*deltaX.rowRange(19,22);

	cout << "Estimated time delay : " << imustate->delay << endl;

	// Update camera states
	size_t L = imustate->stateLength;
	for(size_t i=0; i<cameraPoses.size(); ++i)
	{
		cameraPoses[i].p_CG += deltaX.rowRange(L+6*i+3, L+6*i+6);
		cameraPoses[i].q_CG = quatLeftComp(cameraPoses[i].q_CG)*buildUpdateQuat(deltaX.rowRange(L+6*i, L+6*i+3));
		cameraPoses[i].q_CG /= float(norm(cameraPoses[i].q_CG));
	}
}


//==============================================================================
//         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 BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_ONEPLUS_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_ONEPLUS_HPP_INCLUDED

#include <boost/simd/toolbox/arithmetic/functions/oneplus.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/is_equal.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/valmax.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::oneplus_, tag::cpu_,
                          (A0)(X),
                          ((simd_<arithmetic_<A0>,X>))
                         )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
       return if_else(is_equal(a0, Valmax<result_type>()), a0, a0+One<A0>());
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::oneplus_, tag::cpu_
                                     , (A0)(X)
                                     , ((simd_< floating_<A0>,X >))
                                    )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
      {
        return a0+One<A0>();
      }
  };
} } }
#endif


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

#include "shape_estimation/model/bounding_box.hpp"

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <autoware_perception_msgs/msg/shape.hpp>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <algorithm>
#include <cmath>
#include <utility>
#include <vector>

#define EIGEN_MPL2_ONLY

#include <Eigen/Core>

constexpr float epsilon = 0.001;

BoundingBoxShapeModel::BoundingBoxShapeModel() : reference_yaw_(boost::none) {}

BoundingBoxShapeModel::BoundingBoxShapeModel(const boost::optional<float> & reference_yaw)
: reference_yaw_(reference_yaw)
{
}

bool BoundingBoxShapeModel::estimate(
  const pcl::PointCloud<pcl::PointXYZ> & cluster,
  autoware_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output)
{
  float min_angle, max_angle;
  if (reference_yaw_) {
    min_angle = reference_yaw_.get() - autoware_utils::deg2rad(3);
    max_angle = reference_yaw_.get() + autoware_utils::deg2rad(3);
  } else {
    min_angle = 0.0;
    max_angle = M_PI / 2.0;
  }
  return fitLShape(cluster, min_angle, max_angle, shape_output, pose_output);
}

bool BoundingBoxShapeModel::fitLShape(
  const pcl::PointCloud<pcl::PointXYZ> & cluster, const float min_angle, const float max_angle,
  autoware_perception_msgs::msg::Shape & shape_output, geometry_msgs::msg::Pose & pose_output)
{
  // calc min and max z for height
  float min_z = cluster.empty() ? 0.0 : cluster.at(0).z;
  float max_z = cluster.empty() ? 0.0 : cluster.at(0).z;
  for (const auto & point : cluster) {
    min_z = std::min(point.z, min_z);
    max_z = std::max(point.z, max_z);
  }

  /*
   * Paper : IV2017, Efficient L-Shape Fitting for Vehicle Detection Using Laser Scanners
   * Authors : Xio Zhang, Wenda Xu, Chiyu Dong and John M. Dolan
   */

  // Paper : Algo.2 Search-Based Rectangle Fitting
  std::vector<std::pair<float /*theta*/, float /*q*/>> Q;
  constexpr float angle_resolution = M_PI / 180.0;
  for (float theta = min_angle; theta <= max_angle + epsilon; theta += angle_resolution) {
    Eigen::Vector2f e_1;
    e_1 << std::cos(theta), std::sin(theta);  // col.3, Algo.2
    Eigen::Vector2f e_2;
    e_2 << -std::sin(theta), std::cos(theta);  // col.4, Algo.2
    std::vector<float> C_1;                    // col.5, Algo.2
    std::vector<float> C_2;                    // col.6, Algo.2
    for (const auto & point : cluster) {
      C_1.push_back(point.x * e_1.x() + point.y * e_1.y());
      C_2.push_back(point.x * e_2.x() + point.y * e_2.y());
    }
    float q = calcClosenessCriterion(C_1, C_2);  // col.7, Algo.2
    Q.push_back(std::make_pair(theta, q));       // col.8, Algo.2
  }

  float theta_star{0.0};  // col.10, Algo.2
  float max_q = 0.0;
  for (size_t i = 0; i < Q.size(); ++i) {
    if (max_q < Q.at(i).second || i == 0) {
      max_q = Q.at(i).second;
      theta_star = Q.at(i).first;
    }
  }
  const float sin_theta_star = std::sin(theta_star);
  const float cos_theta_star = std::cos(theta_star);

  Eigen::Vector2f e_1_star;  // col.11, Algo.2
  Eigen::Vector2f e_2_star;
  e_1_star << cos_theta_star, sin_theta_star;
  e_2_star << -sin_theta_star, cos_theta_star;
  std::vector<float> C_1_star;  // col.11, Algo.2
  std::vector<float> C_2_star;  // col.11, Algo.2
  for (const auto & point : cluster) {
    C_1_star.push_back(point.x * e_1_star.x() + point.y * e_1_star.y());
    C_2_star.push_back(point.x * e_2_star.x() + point.y * e_2_star.y());
  }

  // col.12, Algo.2
  const float min_C_1_star = *std::min_element(C_1_star.begin(), C_1_star.end());
  const float max_C_1_star = *std::max_element(C_1_star.begin(), C_1_star.end());
  const float min_C_2_star = *std::min_element(C_2_star.begin(), C_2_star.end());
  const float max_C_2_star = *std::max_element(C_2_star.begin(), C_2_star.end());

  const float a_1 = cos_theta_star;
  const float b_1 = sin_theta_star;
  const float c_1 = min_C_1_star;
  const float a_2 = -1.0 * sin_theta_star;
  const float b_2 = cos_theta_star;
  const float c_2 = min_C_2_star;
  const float a_3 = cos_theta_star;
  const float b_3 = sin_theta_star;
  const float c_3 = max_C_1_star;
  const float a_4 = -1.0 * sin_theta_star;
  const float b_4 = cos_theta_star;
  const float c_4 = max_C_2_star;

  // calc center of bounding box
  float intersection_x_1 = (b_1 * c_2 - b_2 * c_1) / (a_2 * b_1 - a_1 * b_2);
  float intersection_y_1 = (a_1 * c_2 - a_2 * c_1) / (a_1 * b_2 - a_2 * b_1);
  float intersection_x_2 = (b_3 * c_4 - b_4 * c_3) / (a_4 * b_3 - a_3 * b_4);
  float intersection_y_2 = (a_3 * c_4 - a_4 * c_3) / (a_3 * b_4 - a_4 * b_3);

  // calc dimension of bounding box
  Eigen::Vector2f e_x;
  Eigen::Vector2f e_y;
  e_x << a_1 / (std::sqrt(a_1 * a_1 + b_1 * b_1)), b_1 / (std::sqrt(a_1 * a_1 + b_1 * b_1));
  e_y << a_2 / (std::sqrt(a_2 * a_2 + b_2 * b_2)), b_2 / (std::sqrt(a_2 * a_2 + b_2 * b_2));
  Eigen::Vector2f diagonal_vec;
  diagonal_vec << intersection_x_1 - intersection_x_2, intersection_y_1 - intersection_y_2;

  // calc yaw
  tf2::Quaternion quat;
  quat.setEuler(/* roll */ 0, /* pitch */ 0, /* yaw */ std::atan2(e_1_star.y(), e_1_star.x()));

  // output
  shape_output.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX;
  shape_output.dimensions.x = std::fabs(e_x.dot(diagonal_vec));
  shape_output.dimensions.y = std::fabs(e_y.dot(diagonal_vec));
  shape_output.dimensions.z = std::max((max_z - min_z), epsilon);
  pose_output.position.x = (intersection_x_1 + intersection_x_2) * 0.5;
  pose_output.position.y = (intersection_y_1 + intersection_y_2) * 0.5;
  pose_output.position.z = min_z + shape_output.dimensions.z * 0.5;
  pose_output.orientation = tf2::toMsg(quat);
  // check wrong output
  shape_output.dimensions.x = std::max(static_cast<float>(shape_output.dimensions.x), epsilon);
  shape_output.dimensions.y = std::max(static_cast<float>(shape_output.dimensions.y), epsilon);

  return true;
}

float BoundingBoxShapeModel::calcClosenessCriterion(
  const std::vector<float> & C_1, const std::vector<float> & C_2)
{
  // Paper : Algo.4 Closeness Criterion
  const float min_c_1 = *std::min_element(C_1.begin(), C_1.end());  // col.2, Algo.4
  const float max_c_1 = *std::max_element(C_1.begin(), C_1.end());  // col.2, Algo.4
  const float min_c_2 = *std::min_element(C_2.begin(), C_2.end());  // col.3, Algo.4
  const float max_c_2 = *std::max_element(C_2.begin(), C_2.end());  // col.3, Algo.4

  std::vector<float> D_1;  // col.4, Algo.4
  for (const auto & c_1_element : C_1) {
    const float v = std::min(max_c_1 - c_1_element, c_1_element - min_c_1);
    D_1.push_back(v * v);
  }

  std::vector<float> D_2;  // col.5, Algo.4
  for (const auto & c_2_element : C_2) {
    const float v = std::min(max_c_2 - c_2_element, c_2_element - min_c_2);
    D_2.push_back(v * v);
  }
  constexpr float d_min = 0.1 * 0.1;
  constexpr float d_max = 0.4 * 0.4;
  float beta = 0;  // col.6, Algo.4
  for (size_t i = 0; i < D_1.size(); ++i) {
    if (d_max < std::min(D_1.at(i), D_2.at(i))) {
      continue;
    }
    const float d = std::max(std::min(D_1.at(i), D_2.at(i)), d_min);
    beta += 1.0 / d;
  }
  return beta;
}