// deal.II includes ----------------------------------------------
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/grid/grid_in.h>
#include <deal.II/lac/vector.h>

#include <deal.II/numerics/data_out.h>

// system includes -----------------------------------------------
#include <hdf5.h>
#include <omp.h>
#include <yaml-cpp/yaml.h>
#include <Eigen/Dense>
#include <algorithm>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>
#include <ctime>
#include <fstream>
#include <iostream>
// from eigen unsupported
#include <Eigen/KroneckerProduct>

// own includes --------------------------------------------------
#include "aux/eigen2hdf.hpp"
#include "aux/message.hpp"
#include "aux/timer.hpp"
#include "grid_transfer.hpp"
#include "init/import/load_coefficients.hpp"
#include "post_processing/energy.hpp"
#include "post_processing/mass.hpp"
#include "post_processing/momentum.hpp"

#include "l2errors.hpp"
#include "spectral/basis/indexer.hpp"
// class SimpleGridHandler, Solution
#include "grid/grid_tools.hpp"
#include "outer_product_helper.hpp"
#include "solution_handler.hpp"
#include "spectral_transfer_matrix.hpp"

#include "export/data_out_hdf5.hpp"

using namespace boltzmann;
using namespace std;

const int dim = 2;

namespace bf = boost::filesystem;
namespace po = boost::program_options;

template <typename SPECTRAL_BASIS>
void make_overlap(std::vector<double>& S, const SPECTRAL_BASIS& spectral_basis)
{
  typedef typename SPECTRAL_BASIS::elem_t elem_t;

  // angular basis
  typedef typename std::tuple_element<0, typename SPECTRAL_BASIS::elem_t::container_t>::type
      angular_elem_t;
  typename elem_t::Acc::template get<angular_elem_t> acc_ang;

  // inverse mass matrix
  S.resize(spectral_basis.n_dofs());
  for (unsigned int j = 0; j < spectral_basis.n_dofs(); ++j) {
    auto& elem = spectral_basis.get_elem(j);
    if (acc_ang(elem).get_id().l == 0)
      S[j] = numbers::PI;
    else
      S[j] = numbers::PI / 2;
  }
}

typedef dealii::DoFHandler<dim> dh_t;
typedef dealii::Vector<double> vector_t;

int main(int argc, char* argv[])
{
  // #pragma omp parallel
  // {
  //   #pragma omp master
  //   {
  //     int num_threads = omp_get_num_threads();
  //     Eigen::setNbThreads(num_threads);
  //     cout << "Eigen is using " << Eigen::nbThreads() << " threads \n";
  //   }

  // }

  boltzmann::Timer<> timer;

  po::options_description options("options");
  options.add_options()
      ("config,c", po::value<string>()->required(), "config file")
      ("help,h", "help");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  if (vm.count("help")) {
    cout << options << "\n";
    return 0;
  }

  po::notify(vm);

  string config_name = vm["config"].as<string>();

  if (!boost::filesystem::is_regular_file(config_name)) {
    cout << "config file not found\n";
    return 1;
  }

  // load yaml config
  YAML::Node config = YAML::LoadFile(config_name);

  string str_input_grid = config["input"]["grid"].as<string>();
  string str_input_path = config["input"]["path"].as<string>();

  string str_ref_grid = config["reference"]["grid"].as<string>();
  string str_ref_path = config["reference"]["path"].as<string>();

  auto cwd = bf::current_path();

  // initialize working directory
  bf::path working_dir = bf::path(str_input_path) / bf::path("convergence_plots");
  bf::create_directory(working_dir);

  // initialize logfile
  std::time_t result = std::time(nullptr);
  cout << argv[0] << "at: " << asctime(localtime(&result)) << ", executed in " << cwd.c_str()
       << endl
       << setw(14) << "Solution: " << (working_dir / bf::path(str_input_path)).c_str() << setw(14)
       << "Reference: " << (working_dir / bf::path(str_ref_path)).c_str() << endl;

  // ------------------------------------------------------------------------------------------
  // transfer matrix
  dealii::FE_Q<dim> fe(1);
  auto ref_grid_ptr = make_shared<SimpleGridHandler>(str_ref_path, str_ref_grid);
  auto grid_ptr = make_shared<SimpleGridHandler>(str_input_path, str_input_grid);

  const auto& ref_dh = ref_grid_ptr->get_dofhandler();
  const auto& input_dh = grid_ptr->get_dofhandler();

  cout << "Reference mesh: " << ref_dh.get_triangulation().n_used_vertices() << " vertices, "
       << ref_dh.get_triangulation().n_active_cells() << " cells." << endl;

  timer.start();
  GridTransfer<dim> grid_transfer;
  grid_transfer.init(ref_dh, input_dh);
  const auto& Tx = grid_transfer.get_transfer_matrix();
  print_timer(timer.stop(), "init GridTransfer");

  // output to hdf5
  hid_t file;
  file = H5Fcreate((working_dir / bf::path("transfer_matrix.h5")).c_str(),
                   H5F_ACC_TRUNC,
                   H5P_DEFAULT,
                   H5P_DEFAULT);
  eigen2hdf::save_sparse(file, "Tx", Tx);

  // spectral transfer matrix
  auto Tv =
      spectral_transfer_matrix(ref_grid_ptr->get_spectral_basis(), grid_ptr->get_spectral_basis());
  eigen2hdf::save_sparse(file, "Tv", Tv);

  // timer.start();
  // Eigen::SparseMatrix<double> T = Eigen::kroneckerProduct(Tx, Tv);
  // print_timer(timer.stop(), "kroneckerProduct");
  // timer.start();
  // eigen2hdf::save_sparse(file, "T", T);
  // print_timer(timer.stop(), "dump transfer matrix");

  // ------------------------------------------------------------------------------------------
  // load solution filenames from config
  // S contains (b_i (v), b_i (v))_R^2
  vector<double> S;
  make_overlap(S, ref_grid_ptr->get_spectral_basis());

  // load permutations from `vertex2dofidx.dat`
  std::vector<unsigned int> ref_perm(ref_grid_ptr->get_dofhandler().n_dofs());
  std::vector<unsigned int> input_perm(grid_ptr->get_dofhandler().n_dofs());
  // load permutation from ``
  load_permutation(ref_perm, str_ref_path);
  load_permutation(input_perm, str_input_path);

  auto ref_perm_tmp = v2d_permutation_vector(ref_dh);
  auto input_perm_tmp = v2d_permutation_vector(input_dh);

  Mass mass(ref_grid_ptr->get_spectral_basis());
  Momentum momentum(ref_grid_ptr->get_spectral_basis());
  Energy energy(ref_grid_ptr->get_spectral_basis());

  dealii::Vector<double> vmass(ref_dh.n_dofs());
  dealii::Vector<double> venergy(ref_dh.n_dofs());
  dealii::Vector<double> vux(ref_dh.n_dofs());  // momentum x
  dealii::Vector<double> vuy(ref_dh.n_dofs());  // momnetum y

  unsigned int nsteps = config["timesteps"].size();
  cout << "__ERRORS__\n";
  cout << setw(15) << "# i" << setw(15) << "t" << setw(15) << "l2_squared" << setw(15)
       << "l2_m_squared" << setw(15) << "l2_u_squared" << setw(15) << "l2_e_squared" << endl;

  for (unsigned int i = 0; i < nsteps; ++i) {
    string str_h5loc_ref = config["timesteps"][i]["reference"]["data"].as<string>();
    string str_h5loc_inp = config["timesteps"][i]["input"]["data"].as<string>();
    double time = config["timesteps"][i]["time"].as<double>();

    Eigen::VectorXd v_inp;  // approximate solution
    Eigen::VectorXd v_ref;  // reference solution

    load_solution_vector(v_inp, str_input_path, str_h5loc_inp);
    load_solution_vector(v_ref, str_ref_path, str_h5loc_ref);

    // transform to vertex ordering
    to_vertex_ordering(v_inp, input_perm);
    to_vertex_ordering(v_ref, ref_perm);

    // transform to active dofhandler ordering
    to_dof_ordering(v_inp, input_perm_tmp);
    to_dof_ordering(v_ref, ref_perm_tmp);

    // Eigen::VectorXd v_sol = T*v_inp;
    Eigen::VectorXd v_sol(v_ref.size());
    sparse_outer_product_multiply(v_sol, Tx, Tv, v_inp);

    Errors errors;
    double l2_error_sq =
        errors.compute(ref_dh, v_sol.data(), v_ref.data(), S, ref_grid_ptr->get_indexer());

    Eigen::VectorXd vdiff = v_sol - v_ref;
    mass.compute(vmass.begin(), vdiff.data(), ref_dh.n_dofs());
    energy.compute(venergy.begin(), vdiff.data(), ref_dh.n_dofs());
    momentum.compute(vux.begin(), vuy.begin(), vdiff.data(), ref_dh.n_dofs());
    std::for_each(vmass.begin(), vmass.end(), [](double v) { return std::abs(v); });
    std::for_each(venergy.begin(), venergy.end(), [](double v) { return std::abs(v); });
    std::transform(vux.begin(), vux.end(), vuy.begin(), vux.begin(), [](double x, double y) {
      return std::sqrt(x * x + y * y);
    });

    double l2diff_m = l2norm(ref_dh, vmass);
    double l2diff_u = l2norm(ref_dh, vux);
    double l2diff_e = l2norm(ref_dh, venergy);

    cout << setw(15) << i << setw(15) << time << setw(15) << scientific << setprecision(5)
         << l2_error_sq << setw(15) << scientific << setprecision(5) << l2diff_m << setw(15)
         << scientific << setprecision(5) << l2diff_u << setw(15) << scientific << setprecision(5)
         << l2diff_e << endl;

    // output
    {
      // dealii::DataOut<dim> data_out;
      dealii::DataOutHDF<dim> data_out;
      // data_out.attach_triangulation(ref_dh.get_triangulation());
      data_out.attach_dof_handler(ref_dh);
      const auto& cell_wise_error = errors.get_cell_wise_error();
      //      cout << "cell_wise_error.size = " << cell_wise_error.size() << endl;
      data_out.add_data_vector(cell_wise_error, "error^2");
      data_out.add_data_vector(vmass, "err_mass");
      data_out.add_data_vector(venergy, "err_energy");
      data_out.add_data_vector(vux, "err_abs(u)");
      //  data_out.add_data_vector(tmp_out, "test_direct");
      data_out.build_patches();

      dealii::DataOutBase::VtkFlags flags;
      data_out.set_flags(flags);
      // auto fname = "errors" + boost::lexical_cast<string>(i) + ".vtk";
      // std::ofstream vtk_output((working_dir / bf::path(fname)).c_str());
      // data_out.write_vtk(vtk_output);
      // vtk_output.close();
      typedef dealii::DataOutBase::DataOutFilterFlags data_out_filter_flags;
      dealii::DataOutBase::DataOutFilter data_out_filter(data_out_filter_flags(true, true));
      data_out.write_filtered_data(data_out_filter);
      string filename =
          (working_dir / bf::path("output" + boost::lexical_cast<string>(i) + ".h5")).c_str();
      string mesh_filename = (working_dir / bf::path("mesh.hdf5")).c_str();
      data_out.write_hdf5(filename, data_out_filter);
      auto xdmf_entry = data_out.create_xdmf_entry(data_out_filter, mesh_filename, filename, time);

      auto xdmf_file = working_dir / bf::path("solution.xdmf");
      std::ofstream fout(xdmf_file.c_str(), std::ios_base::out | std::ios_base::app);
      fout << xdmf_entry.get_xdmf_content(1) << std::endl;
      fout.close();

      if (i == 0) {
        data_out.write_mesh(mesh_filename, data_out_filter);
      }
    }
  }

  H5Fclose(file);

  return 0;
}


#include "HilbertOrder3D.hpp"
#include "HilbertOrder3D_Utils.hpp"
#include <boost/array.hpp>
#include <algorithm>
#include <ctime>
#include <iostream>
#ifdef RICH_MPI
#include <mpi.h>
#endif

#define NUMBER_OF_SHAPES 24
#define MAX_ROTATION_LENGTH 5
#define PI 3.14159

using namespace std;

// The elementary Hilbert Curve shape:
class HilbertCurve3D_shape
{
public:
	// Constructor
	HilbertCurve3D_shape();
	// Comparison:
	bool operator==(HilbertCurve3D_shape & shape);
	// An array of the 7 unit vector steps defining the shape:
	boost::array<Vector3D, 7> m_vShapePoints;

};
// Constructor - uses the reference Hilbert curve shape:
HilbertCurve3D_shape::HilbertCurve3D_shape() : m_vShapePoints(boost::array<Vector3D, 7>())
{
	m_vShapePoints[0] = Vector3D(0, 0, -1);
	m_vShapePoints[1] = Vector3D(0, 1, 0);
	m_vShapePoints[2] = Vector3D(0, 0, 1);
	m_vShapePoints[3] = Vector3D(-1, 0, 0);
	m_vShapePoints[4] = Vector3D(0, 0, -1);
	m_vShapePoints[5] = Vector3D(0, -1, 0);
	m_vShapePoints[6] = Vector3D(0, 0, 1);
}

// Compare to a given Hilbert curve shape by comparing pairs of shape points:
bool HilbertCurve3D_shape::operator==(HilbertCurve3D_shape & shape)
{
	bool b = true;
	for (std::size_t ii = 0; ii < m_vShapePoints.size(); ++ii)
	{
		b = b && (m_vShapePoints[ii] == shape.m_vShapePoints[ii]);
	}

	return b;
}

// The Hilbert Curve class:
class HilbertCurve3D
{
public:
	// Constructor
	HilbertCurve3D(void);
	// Calculate the Hilbert curve distance of a given point, given a required number of iterations:
	unsigned long long int Hilbert3D_xyz2d(Vector3D const & rvPoint, int numOfIterations);

private:
	// Rotate a shape according to a given rotation scheme (in-place):
	void RotateShape(int iShapeIndex, vector<int> vAxes);
	// Rotate a shape according to rotation index, and return the rotated shape:
	void RotateShape(HilbertCurve3D_shape const & roShape, HilbertCurve3D_shape & roShapeOut, int iRotationIndex);
	/*!
	\brief Returns the rotation scheme, according to a rotation index
	\param piRotation - a pointer to the output rotation scheme vector
	\param iRotationIndex - the desired rotation index
	\return The rotation scheme length (the size of the array given by piRotation)
	*/
	int GetRotation(int * piRotation, int iRotationIndex);
	// Find the index of a given shape object:
	int FindShapeIndex(HilbertCurve3D_shape & roShape);
	// Create the recursion rule:
	void BuildRecursionRule();
	// Create the shape order, for all shapes (the order of octants):
	void BuildShapeOrder();

	// Stores all rotated shapes:
	boost::array<HilbertCurve3D_shape, NUMBER_OF_SHAPES> m_vRotatedShapes;
	// Stores all rotation schemes:
	boost::array < vector<int>, NUMBER_OF_SHAPES > m_vRotations;

	// An array of the 8 integers defining the recursion rule of the shape:
	boost::array< boost::array<int, 8> , NUMBER_OF_SHAPES> m_vShapeRecursion;

	// A 2x2x2 matrix indicating the 3 dimensional shape order
	// array< array<int , 8 > , NUMBER_OF_SHAPES > m_mShapeOrder;
	int m_mShapeOrder[NUMBER_OF_SHAPES][2][2][2];
};

// Constructor - performs all required initiallizations and preprocessing:
HilbertCurve3D::HilbertCurve3D() :m_vRotatedShapes(boost::array<HilbertCurve3D_shape, NUMBER_OF_SHAPES> ()), m_vRotations(boost::array < vector<int>, NUMBER_OF_SHAPES >()),
m_vShapeRecursion(boost::array< boost::array<int, 8> , NUMBER_OF_SHAPES>())
{
	int rot[MAX_ROTATION_LENGTH];
	int iRotLength;
	for (int iRotIndex = 1; iRotIndex < NUMBER_OF_SHAPES; ++iRotIndex)
	{
		iRotLength = GetRotation(rot, iRotIndex);
		m_vRotations[iRotIndex].assign(rot, rot + iRotLength);
	}

	for (int ii = 1; ii < NUMBER_OF_SHAPES; ++ii)
	{
		RotateShape(ii, m_vRotations[ii]);
	}

	BuildRecursionRule();
	BuildShapeOrder();
}

// FindShapeIndex - returns the index of a shape:
int HilbertCurve3D::FindShapeIndex(HilbertCurve3D_shape & roShape)
{
	for (int ii = 0; ii < NUMBER_OF_SHAPES; ++ii)
	{
		if (roShape == m_vRotatedShapes[ii])
		{
			return ii;
		}
	}
	// TODO - manage this kind of return value (error)
	return -1;
}

void HilbertCurve3D::BuildRecursionRule()
{
	// Reference recursion rule:
	m_vShapeRecursion[0][0] = 12;
	m_vShapeRecursion[0][1] = 16;
	m_vShapeRecursion[0][2] = 16;
	m_vShapeRecursion[0][3] = 2;
	m_vShapeRecursion[0][4] = 2;
	m_vShapeRecursion[0][5] = 14;
	m_vShapeRecursion[0][6] = 14;
	m_vShapeRecursion[0][7] = 10;

	HilbertCurve3D_shape oTempShape;
	// What about ii=0? not necessary
	for (int ii = 0; ii < NUMBER_OF_SHAPES; ++ii)
	{
		for (int jj = 0; jj < 8; ++jj)
		{
			// Rotate the appropriate block of the reference recursion rule, according to the ii rotation scheme:
			RotateShape(m_vRotatedShapes[m_vShapeRecursion[0][jj]], oTempShape, ii);
			// Find the shape index of the rotated shape:
			m_vShapeRecursion[ii][jj] = FindShapeIndex(oTempShape);
		}
	}

	return;
}

// Return the rotation scheme of the ii rotation (manually precalculated):
int HilbertCurve3D::GetRotation(int * piRotation, int iRotationIndex)
{
	switch (iRotationIndex)
	{
	case 1:
		piRotation[0] = 1;
		return 1;
	case 2:
		piRotation[0] = 1;
		piRotation[1] = 1;
		return 2;
	case 3:
		piRotation[0] = 1;
		piRotation[1] = 1;
		piRotation[2] = 1;
		return 3;

	case 4:
		piRotation[0] = 2;
		return 1;
	case 5:
		piRotation[0] = 2;
		piRotation[1] = 2;
		return 2;
	case 6:
		piRotation[0] = 2;
		piRotation[1] = 2;
		piRotation[2] = 2;
		return 3;

	case 7:
		piRotation[0] = 3;
		return 1;
	case 8:
		piRotation[0] = 3;
		piRotation[1] = 3;
		return 2;
	case 9:
		piRotation[0] = 3;
		piRotation[1] = 3;
		piRotation[2] = 3;
		return 3;

	case 10:
		piRotation[0] = 1;
		piRotation[1] = 2;
		return 2;
	case 11:
		piRotation[0] = 2;
		piRotation[1] = 1;
		return 2;
	case 12:
		piRotation[0] = 3;
		piRotation[1] = 1;
		return 2;
	case 13:
		piRotation[0] = 2;
		piRotation[1] = 3;
		return 2;

	case 14:
		piRotation[0] = 1;
		piRotation[1] = 1;
		piRotation[2] = 1;
		piRotation[3] = 3;
		return 4;
	case 15:
		piRotation[0] = 2;
		piRotation[1] = 2;
		piRotation[2] = 2;
		piRotation[3] = 1;
		return 4;
	case 16:
		piRotation[0] = 3;
		piRotation[1] = 3;
		piRotation[2] = 3;
		piRotation[3] = 2;
		return 4;
	case 17:
		piRotation[0] = 3;
		piRotation[1] = 2;
		piRotation[2] = 3;
		piRotation[3] = 2;
		return 4;
	case 18:
		piRotation[0] = 3;
		piRotation[1] = 2;
		piRotation[2] = 2;
		return 3;

	case 19:
		piRotation[0] = 1;
		piRotation[1] = 1;
		piRotation[2] = 1;
		piRotation[3] = 2;
		piRotation[4] = 2;
		return 5;
	case 20:
		piRotation[0] = 3;
		piRotation[1] = 3;
		piRotation[2] = 2;
		return 3;
	case 21:
		piRotation[0] = 3;
		piRotation[1] = 3;
		piRotation[2] = 1;
		return 3;
	case 22:
		piRotation[0] = 2;
		piRotation[1] = 2;
		piRotation[2] = 3;
		return 3;
	case 23:
		piRotation[0] = 1;
		piRotation[1] = 1;
		piRotation[2] = 2;
		return 3;

	default:
		return 0;
		break;
	}
}

// Rotate a shape:
void HilbertCurve3D::RotateShape(int iShapeIndex, vector<int> vAxes)
{
	int iSign = 0;

	for (std::size_t ii = 0; ii < 7; ++ii)
	{
		for (std::size_t iAx = 0; iAx < vAxes.size(); ++iAx)
		{
			// A trick to find the sign of vAxes[iAx]:
			iSign = (vAxes[iAx] > 0) - (vAxes[iAx] < 0);

			switch (abs(vAxes[iAx]))
			{
			case 1:
				m_vRotatedShapes[iShapeIndex].m_vShapePoints[ii].RotateX( iSign * PI / 2 );
				break;
			case 2:
				m_vRotatedShapes[iShapeIndex].m_vShapePoints[ii].RotateY( iSign * PI / 2 );
				break;
			case 3:
				m_vRotatedShapes[iShapeIndex].m_vShapePoints[ii].RotateZ( iSign * PI / 2 );
				break;
			default:
				break;
			}
		}
		// Round off the results:
		m_vRotatedShapes[iShapeIndex].m_vShapePoints[ii].Round();
	}
}

void HilbertCurve3D::RotateShape(HilbertCurve3D_shape const & roShape, HilbertCurve3D_shape & roShapeOut , int iRotationIndex)
{
	int iSign = 0;

	vector<int> vAxes = m_vRotations[iRotationIndex];
	roShapeOut = roShape;

	for (int ii = 0; ii < 7; ++ii)
	{
		for (std::size_t iAx = 0; iAx < vAxes.size(); ++iAx)
		{
			// A trick to find the sign of vAxes[iAx]:
			iSign = (vAxes[iAx] > 0) - (vAxes[iAx] < 0);

			switch (abs(vAxes[iAx]))
			{
			case 1:
				roShapeOut.m_vShapePoints[ii].RotateX(iSign * PI / 2);
				break;
			case 2:
				roShapeOut.m_vShapePoints[ii].RotateY(iSign * PI / 2);
				break;
			case 3:
				roShapeOut.m_vShapePoints[ii].RotateZ(iSign * PI / 2);
				break;
			default:
				break;
			}
		}
		// Round off the results:
		roShapeOut.m_vShapePoints[ii].Round();
	}

	return;
}

void HilbertCurve3D::BuildShapeOrder()
{
	boost::array<int, 8> vShapeVerticesX;
	boost::array<int, 8> vShapeVerticesY;
	boost::array<int, 8> vShapeVerticesZ;

	vShapeVerticesX[0] = 0;
	vShapeVerticesY[0] = 0;
	vShapeVerticesZ[0] = 0;

	for (std::size_t iShapeInd = 0; iShapeInd < NUMBER_OF_SHAPES; ++iShapeInd)
	{
		for (std::size_t ii = 0; ii < m_vRotatedShapes[iShapeInd].m_vShapePoints.size(); ++ii)
		{
			vShapeVerticesX[ii + 1] = (int)(vShapeVerticesX[ii] + m_vRotatedShapes[iShapeInd].m_vShapePoints[ii].x);
			vShapeVerticesY[ii + 1] = (int)(vShapeVerticesY[ii] + m_vRotatedShapes[iShapeInd].m_vShapePoints[ii].y);
			vShapeVerticesZ[ii + 1] = (int)(vShapeVerticesZ[ii] + m_vRotatedShapes[iShapeInd].m_vShapePoints[ii].z);
		}

		int iMinX = *min_element(vShapeVerticesX.begin(), vShapeVerticesX.end());
		int iMinY = *min_element(vShapeVerticesY.begin(), vShapeVerticesY.end());
		int iMinZ = *min_element(vShapeVerticesZ.begin(), vShapeVerticesZ.end());

		for (std::size_t jj = 0; jj < vShapeVerticesX.size(); ++jj)
		{
			vShapeVerticesX[jj] -= iMinX;
			vShapeVerticesY[jj] -= iMinY;
			vShapeVerticesZ[jj] -= iMinZ;
		}

		for (std::size_t kk = 0; kk < vShapeVerticesX.size(); ++kk)
		{
			m_mShapeOrder[iShapeInd][vShapeVerticesX[kk]][vShapeVerticesY[kk]][vShapeVerticesZ[kk]] = static_cast<int>(kk);
		}
	}

	return;
}

unsigned long long int HilbertCurve3D::Hilbert3D_xyz2d(Vector3D const & rvPoint, int numOfIterations)
{
	// Extract the coordinates:
	double x = rvPoint.x;
	double y = rvPoint.y;
	double z = rvPoint.z;

	// The output distance along the 3D-Hilbert Curve:
	unsigned long long int d = 0;

	// The current shape index:
	int iCurrentShape = 0;
	// The octant number:
	int iOctantNum = 0;
	// A temp variable - storing the current (negative) power of 2
	double dbPow2;
	// Variables indicating the current octant:
	bool bX, bY, bZ;
	for (int iN = 1; iN <= numOfIterations; ++iN)
	{
		// Calculate the current power of 0.5:
		dbPow2 = ((double)1) / (1 << iN);
		bX = x > dbPow2;
		bY = y > dbPow2;
		bZ = z > dbPow2;

		x -= dbPow2*bX;
		y -= dbPow2*bY;
		z -= dbPow2*bZ;

		// Multiply the distance by 8 (for every recursion iteration):
		d = d << 3;
		iOctantNum = m_mShapeOrder[iCurrentShape][bX][bY][bZ];
		d = d + iOctantNum;
		iCurrentShape = m_vShapeRecursion[iCurrentShape][iOctantNum];
	}

	return d;
}

vector<std::size_t> GetGlobalHibertIndeces(vector<Vector3D> const& cor,Vector3D const& ll,Vector3D const& ur,size_t &Hmax)
{
	vector<std::size_t> res;
#ifdef RICH_MPI
	int Nlocal = static_cast<int>(cor.size());
	int Ntotal = Nlocal;
	MPI_Allreduce(&Nlocal, &Ntotal, 1, MPI_INT, MPI_SUM, MPI_COMM_WORLD);
#endif
	std::size_t Niter = 20;
	Hmax = static_cast<size_t>(std::pow(static_cast<size_t>(2), static_cast<size_t>(3*Niter)));
	HilbertCurve3D oHilbert;
	Vector3D dx = ur - ll,vtemp;
	std::size_t Ncor = cor.size();
	res.resize(Ncor);
	for (size_t i = 0; i < Ncor; ++i)
	{
		vtemp.x = (cor[i].x - ll.x) / dx.x;
		vtemp.y = (cor[i].y - ll.y) / dx.y;
		vtemp.z = (cor[i].z - ll.z) / dx.z;
		res[i]=static_cast<size_t>(oHilbert.Hilbert3D_xyz2d(vtemp,static_cast<int>(Niter)));
	}
	return res;
}

vector<std::size_t> HilbertOrder3D(vector<Vector3D> const& cor)
{
	// If only 1 or 2 points are provided - do not reorder them
	if ( 2 >= cor.size() )
	{
		vector<std::size_t> vIndSort( cor.size() );
		for (std::size_t ii = 0; ii < cor.size(); ++ii)
		{
			vIndSort[ii] = ii;
		}
		return vIndSort;
	}
	// Create a 3D-Hilbert Curve Object:
	HilbertCurve3D oHilbert;

	// Allocate an output vector:
	size_t N = cor.size();
	vector<unsigned long long int> vOut;
	vOut.resize(N);

	// Estimate the number of required iterations:
	int numOfIterations = EstimateHilbertIterationNum(cor);

	vector<Vector3D> vAdjustedPoints;

	// Adjust the points coordinates to the unit cube:
	AdjustPoints(cor, vAdjustedPoints);

	// Run throught the points, and calculate the Hilbert distance of each:
	for (size_t ii = 0; ii < N; ++ii)
	{
		vOut[ii]=oHilbert.Hilbert3D_xyz2d(vAdjustedPoints[ii], numOfIterations+8);
		//vOut.push_back(oHilbert.Hilbert3D_xyz2d(vAdjustedPoints[ii], 2));
	}
	// Get the sorting indices:
	vector<std::size_t> vIndSort;
	ordered(vOut, vIndSort);
	// Reorder the Hilbert distances vector (according to the sorting indices):
	reorder( vOut, vIndSort );

	// Find indices with repeated Hilbert distance:
	vector<vector<std::size_t> > vEqualIndices;
	FindEqualIndices(vOut, vEqualIndices);

	// If all points have different Hilbert distances, return the sorting indices:
	if (vEqualIndices.empty())
	{
		return vIndSort;
	}
	else
	{
		for (std::size_t ii = 0; ii < vEqualIndices.size(); ++ii)
		{
			vector<Vector3D> vPointsInner(vEqualIndices[ii].size() );
			vector<std::size_t> vIndInner(vEqualIndices[ii].size());
			vector<std::size_t> vIndSortInner(vEqualIndices[ii].size());
			vector<std::size_t> vIndSortInner_cpy(vEqualIndices[ii].size());

			// Store the points with the equal indices
			for (std::size_t jj = 0; jj < vEqualIndices[ii].size() ; ++jj)
			{
				vIndInner[jj] = vIndSort[vEqualIndices[ii][jj]];
				vPointsInner[jj] = cor[vIndInner[jj]];
				vIndSortInner_cpy[jj] = vIndSort[vIndInner[jj]];
			}

			// Sort the repeated points:
			vIndSortInner = HilbertOrder3D(vPointsInner);
	//		vector<std::size_t> vIndSortTemp = vIndSort;
			for (std::size_t kk = 0; kk < vIndSortInner.size(); ++kk)
			{
				vIndSort[vIndInner[kk]] = vIndSortInner_cpy[vIndSortInner[kk]];
			}
		}

		// Return the sorting indices:
		return vIndSort;
	}
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2009 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef SCHURBLOCKLU_SOLVER_HH
#define SCHURBLOCKLU_SOLVER_HH

/**
 * @file
 * @brief  Routines for the solution of (sparse) linear systems
 * @author Anton Schiela
 */

#include <iostream>
#include <memory> // std::unique_ptr

#include "dune/common/fmatrix.hh"
#include "dune/istl/matrix.hh"

#include "linalg/umfpack_solve.hh"

#include "linalg/linearsystem.hh"
#include "linalg/simpleLAPmatrix.hh"

#include <boost/timer/timer.hpp>

namespace Kaskade
{

/** \ingroup linalg
 *\ brief Adapter class for DUNE::IterativeSolver
 */

void printVec(std::vector<double> const&v, int vend=1000000);

template<class Factorization=UMFFactorization<double> >
class BlockLUFactorization
{
public:
/// needs a matrix
  static const bool needMatrix = true;
template<class Sys>
BlockLUFactorization(Sys const& lin, int start2, int end2, int start3, int end3)
  {
    MatrixAsTriplet<double> matL;
    lin.getMatrixBlocks(matL,start3,end3,start2,end2);
    matA.resize(0);
    lin.getMatrixBlocks(matA,start2,end2,start2,end2);
    std::cout << "Inner, " << std::flush;
    boost::timer::cpu_timer timer;
    factoredL.reset(new Factorization(matL.nrows(),
                                      2,
                                      matL.ridx,
                                      matL.cidx,
                                      matL.data,
                                      MatrixProperties::GENERAL));
    std::cout << "Finished: " << (double)(timer.elapsed().user)/1e9 << " sec." << std::endl;
  }

  void resetBlock22(MatrixAsTriplet<double>const & matA_) { matA = matA_; };

  void solve(std::vector<double>const& rhs, std::vector<double>& sol, int nr=1);
  MatrixAsTriplet<double> matA;
  std::unique_ptr<Factorization> factoredL;
};

struct BlockSchurParameters
{
  BlockSchurParameters(bool reg_)
    : refactorizeInner(true), refactorizeOuter(true)
  {
    if(reg_) regularizationMethod=AddId; else regularizationMethod=None;
  }

  BlockSchurParameters(bool reg_, bool innerf_, bool outerf_)
    : refactorizeInner(innerf_), refactorizeOuter(outerf_)
  {
    if(reg_) regularizationMethod=AddId; else regularizationMethod=None;
  }

  bool refactorizeInner, refactorizeOuter;
  typedef enum { None=0, AddId=1, CG = 2} RegularizationMethod;

  RegularizationMethod regularizationMethod;
};

/// Solver, which is especially designed for the hyperthermia planning problem.
template<class Factorization>
class DirectBlockSchurSolver
{
public:
  int start1, end1, start2, end2, start3, end3;

/// needs a matrix
  static const bool needMatrix = true;

  DirectBlockSchurSolver(bool doregularize = false) :
    start1(0), end1(1),
    start2(1), end2(2),
    start3(2), end3(3),
    report(false), paras(doregularize)
  {}

  void ax(std::vector<double>& sol, std::vector<double>const &r) const;

  void resolve(std::vector<double>& sol, SparseLinearSystem const& lin) const;

  void resolveAdjAndNormal(std::vector<double>& sol, SparseLinearSystem const& lin) const;

  void resolveNormal(std::vector<double>& sol, SparseLinearSystem const& lin,std::vector<double>const *addrhs=0);

  void solve(std::vector<double>& sol,
             SparseLinearSystem const& lin);

  void solveAdjAndNormal(std::vector<double>& sol,
             SparseLinearSystem const& lin);

  void solveTCG(std::vector<double>& sol1, std::vector<double>& sol2,
                SparseLinearSystem const& linT, SparseLinearSystem const& linN, std::vector<double>const & normalStep, double nu0);

  /// Solves always exactly
  void setRelativeAccuracy(double) {}

  /// Always exact solution
  double getRelativeAccuracy() {return 0.0;}

  void onChangedLinearization() {flushFactorization(); }

  void flushFactorization()
  {
    if(factorization.get() && paras.refactorizeInner) factorization.reset();
    mC.setSize(0,0);
    B.resize(0);
    AinvB.resize(0);
    matANormal.flush();
  }

  bool report;

  /// Always exact solution
  double getAbsoluteAccuracy() {return 0.0;}

  bool improvementPossible() { return false; }

  void resetParameters(BlockSchurParameters const& p_) { paras=p_; }

private:
  void resolveN(std::vector<double>& sol, std::vector<double>const &r,std::vector<double>const &s,std::vector<double>const &t) const;

  void tsolve(std::vector<double>& sol1,std::vector<double>& sol2,
              std::vector<double>const &r,std::vector<double>const &s,std::vector<double>const &t) const;

  void fwd(std::vector<double>& sol, std::vector<double>const &r,std::vector<double>const &s,std::vector<double>const &t) const;
  void bwd(std::vector<double>& sol, std::vector<double>const &x2,std::vector<double>const &s,std::vector<double>const &t) const;

  void buildNewSchurComplement(SparseLinearSystem const& lin,int task);

  std::unique_ptr<BlockLUFactorization<Factorization> > factorization;
  MatrixAsTriplet<double> matANormal;
  Dune::Matrix<Dune::FieldMatrix<double,1,1> > mC,mCNormal;
  std::vector<double> B,AinvB;

  int rowsB, colsBC, rowsC;
  int rows1, rows2, rows3;

  BlockSchurParameters paras;

};

class ModifiedSparseSystem : public SparseLinearSystem
{
public:
  ModifiedSparseSystem(SparseLinearSystem const &lin_, MatrixAsTriplet<double> const& mat_, MatrixAsTriplet<double> const& mat2_
                       , std::vector<double> const& scaling_) :
    scaling(scaling_), lin(&lin_), mat(mat_), mat2(mat2_)
  {
  }

  virtual int rows(int rbegin, int rend) const { return lin->rows(rbegin,rend);}
  virtual int cols(int colbegin, int colend) const { return lin->cols(colbegin,colend);}

  /// Return matrix blocks of the linear system in triplet format
  virtual void getMatrixBlocks(MatrixAsTriplet<double>& m, int rbegin, int rend, int colbegin, int colend) const
  {
    lin->getMatrixBlocks(m,rbegin,rend,colbegin,colend);
    MatrixAsTriplet<double> mm(mat);
    mm *= -1.0;
    if(rbegin==0 && colbegin == 0) m+=mm;
  }

  void resetLin(SparseLinearSystem const& lin_)
  {
    lin=&lin_;
  }

  /// value of function
  virtual double getValue() const { return lin->getValue();}

  /// Return components of the right hand side of the linear system
  virtual void getRHSBlocks(std::vector<double>& rhs, int rbegin, int rend) const
  {
    rhs.resize(0);
    lin->getRHSBlocks(rhs,rbegin,rend);
    if(rbegin==0)
    {
      std::vector<double> t(lin->rows(3,4),0.0);
      std::vector<double> Mt(lin->rows(0,1),0.0);
      lin->getRHSBlocks(t,3,4);
      for(int i=0; i<t.size();++i)
      {
        t[i] *=scaling[i];
      }
      mat2.ax(Mt,t);
      for(int i=0; i<Mt.size();++i)
      {
//        std::cout << Mt[i] << std::endl;
      }
      for(int i=0; i<lin->rows(0,1); ++i)
        rhs[i] -= Mt[i];
    }
  }

  /// number of column blocks
  virtual int nColBlocks() const { return lin->nColBlocks();};

  /// number of row blocks
  virtual int nRowBlocks() const { return lin->nRowBlocks();};
private:
  std::vector<double> scaling;
  SparseLinearSystem const* lin;
  MatrixAsTriplet<double> mat;
  MatrixAsTriplet<double> mat2;
};

/// Solver, which is especially designed for the hyperthermia planning problem with amplitude ratio
template<class Factorization>
class ARDirectBlockSchurSolver
{
public:
/// needs a matrix
  static const bool needMatrix = true;

  ARDirectBlockSchurSolver(bool doregularize = false) : report(false), DBSSolver(doregularize), justsolved(false)
{
}

  void resolve(std::vector<double>& sol, SparseLinearSystem const& lin) const;

  void solve(std::vector<double>& sol,
             SparseLinearSystem const& lin);

  /// Solves always exactly
  void setRelativeAccuracy(double) {}

  /// Always exact solution
  double getRelativeAccuracy() {return 0.0;}

  void onChangedLinearization() {flushFactorization(); }

  void flushFactorization()
  {
    DBSSolver.flushFactorization();
    F.setSize(0,0);
    FT.setSize(0,0);
    FTVinv.setSize(0,0);
    FTVinvF.setSize(0,0);
    Vinv.setSize(0,0);
  }

  bool report;

  /// Always exact solution
  double getAbsoluteAccuracy() {return 0.0;}

  bool improvementPossible() { return false; }

private:
  std::vector<double> scaling;
  DirectBlockSchurSolver<Factorization> DBSSolver;
  Dune::Matrix<Dune::FieldMatrix<double,1,1> > Vinv,F,FT, FTVinv, FTVinvF;
  std::unique_ptr<MatrixAsTriplet<double> > L,FTVi;
  std::unique_ptr<ModifiedSparseSystem> linMod;
  bool justsolved;
};

}  // namespace Kaskade
#endif


// Copyright 2020 The Defold Foundation
// Licensed under the Defold License version 1.0 (the "License"); you may not use
// this file except in compliance with the License.
//
// You may obtain a copy of the License, together with FAQs at
// https://www.defold.com/license
//
// Unless required by applicable law or agreed to in writing, software distributed
// under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <assert.h>
#include "shared_library.h"
#include "crypt.h"

#include <dlib/endian.h>
#include <mbedtls/md5.h>
#include <mbedtls/base64.h>
#include <mbedtls/sha1.h>
#include <mbedtls/sha256.h>
#include <mbedtls/sha512.h>
#include <mbedtls/pk.h>
#include <mbedtls/pk_internal.h>
#include <mbedtls/rsa.h>
#include <mbedtls/entropy.h>
#include <mbedtls/ctr_drbg.h>
//Added by dotGears
#include <mbedtls/md.h>

#include <dlib/log.h> // For debugging the manifest verification issue

namespace dmCrypt
{
    const uint32_t NUM_ROUNDS = 32;

    static inline uint64_t EncryptXTea(uint64_t v, uint32_t* key)
    {
        uint32_t v0 = (uint32_t) (v >> 32);
        uint32_t v1 = (uint32_t) (v & 0xffffffff);

        uint32_t sum = 0, delta = 0x9e3779b9;
        for (uint32_t i = 0; i < NUM_ROUNDS; i++) {
            v0 += (((v1 << 4) ^ (v1 >> 5)) + v1) ^ (sum + dmEndian::ToHost(key[sum & 3]));
            sum += delta;
            v1 += (((v0 << 4) ^ (v0 >> 5)) + v0) ^ (sum + dmEndian::ToHost(key[(sum>>11) & 3]));
        }
        uint64_t ret = dmEndian::ToHost((((uint64_t) v0) << 32 | v1));
        return ret;
    }

    static void EncryptXTeaCTR(uint8_t* data, uint32_t datalen, const uint8_t* key, uint32_t keylen)
    {
        assert(keylen <= 16);
        const uint32_t block_len = 8;
        uint8_t paddedkey[16] = {0};
        memcpy(paddedkey, key, keylen);

        uint64_t counter = 0;

        uint32_t i = 0;
        uint64_t* d = (uint64_t*) data;
        for (i = 0; i < datalen / block_len; i++) {
            uint64_t enc_counter = EncryptXTea(counter, (uint32_t*) paddedkey);
            d[i] ^= enc_counter;
            data += block_len;
            counter++;
        }

        uint64_t enc_counter = EncryptXTea(counter, (uint32_t*) paddedkey);
        uint32_t rest = datalen & (block_len - 1);
        uint8_t* ec = (uint8_t*) &enc_counter;
        for (uint32_t j = 0; j < rest; j++) {
            data[j] ^= ec[j];
        }
    }

    Result Encrypt(Algorithm algo, uint8_t* data, uint32_t datalen, const uint8_t* key, uint32_t keylen)
    {
        EncryptXTeaCTR(data, datalen, key, keylen);
        return RESULT_OK;
    }

    Result Decrypt(Algorithm algo, uint8_t* data, uint32_t datalen, const uint8_t* key, uint32_t keylen)
    {
        EncryptXTeaCTR(data, datalen, key, keylen);
        return RESULT_OK;
    }

    // Same as rsa_alt_decrypt_wrap() except with a MBEDTLS_RSA_PUBLIC
    static int rsa_alt_decrypt_public_wrap( void *ctx,
                        const unsigned char *input, size_t ilen,
                        unsigned char *output, size_t *olen, size_t osize,
                        int (*f_rng)(void *, unsigned char *, size_t), void *p_rng )
    {
        mbedtls_rsa_context * rsa = (mbedtls_rsa_context *) ctx;

        if( ilen != mbedtls_rsa_get_len( rsa ) )
            return( MBEDTLS_ERR_RSA_BAD_INPUT_DATA );

        return( mbedtls_rsa_pkcs1_decrypt( rsa, f_rng, p_rng,
                    MBEDTLS_RSA_PUBLIC, olen, input, output, osize ) );
    }

    Result Decrypt(const uint8_t* key, uint32_t keylen, const uint8_t* data, uint32_t datalen, uint8_t** output, uint32_t* outputlen)
    {
        // https://tls.mbed.org/discussions/generic/parsing-public-key-from-memory
        Result result = RESULT_OK;

        const char* pers = "defold_pk_decrypt";
        mbedtls_pk_context pk;
        mbedtls_entropy_context entropy;
        mbedtls_ctr_drbg_context ctr_drbg;
        mbedtls_pk_init(&pk);
        mbedtls_ctr_drbg_init( &ctr_drbg );
        mbedtls_entropy_init( &entropy );

        uint32_t signature_hash_len = MBEDTLS_MD_MAX_SIZE;

        int ret;
        if( ( ret = mbedtls_ctr_drbg_seed( &ctr_drbg, mbedtls_entropy_func, &entropy, (const unsigned char *) pers, strlen(pers) ) ) != 0 )
        {
            dmLogError("Decrypt: mbedtls_ctr_drbg_seed failed: %d", ret);
            result = RESULT_ERROR;
            goto exit;
        }

        if ((ret = mbedtls_pk_parse_public_key(&pk, key, keylen) != 0))
        {
            dmLogError("Decrypt: mbedtls_pk_parse_public_key failed: %d", ret);
            result = RESULT_ERROR;
            goto exit;
        }

        *output = (uint8_t*)malloc(signature_hash_len);
        size_t _outputlen;
        if ((ret = rsa_alt_decrypt_public_wrap(pk.pk_ctx,
                    data, datalen,
                    (uint8_t*)*output, &_outputlen, signature_hash_len,
                    mbedtls_ctr_drbg_random, &ctr_drbg )) != 0)
        {
            dmLogError("Decrypt: rsa_alt_decrypt_public_wrap failed: %d", ret);
            free(*output);
            result = RESULT_ERROR;
            goto exit;
        }

        *outputlen = (uint32_t)_outputlen;

    exit:
        mbedtls_ctr_drbg_free( &ctr_drbg );
        mbedtls_entropy_free( &entropy );
        mbedtls_pk_free(&pk);
        return result;
    }

    void HashSha1(const uint8_t* buf, uint32_t buflen, uint8_t* digest)
    {
        mbedtls_sha1_context ctx;
        mbedtls_sha1_init(&ctx);
        mbedtls_sha1_starts_ret(&ctx);
        mbedtls_sha1_update_ret(&ctx, (const unsigned char*)buf, (size_t)buflen);
        int ret = mbedtls_sha1_finish_ret(&ctx, (unsigned char*)digest);
        mbedtls_sha1_free(&ctx);
        if (ret != 0) {
            memset(digest, 0, 20);
        }
    }

    void HashSha256(const uint8_t* buf, uint32_t buflen, uint8_t* digest)
    {
        int ret = mbedtls_sha256_ret((const unsigned char*)buf, (size_t)buflen, (unsigned char*)digest, 0);
        if (ret != 0) {
            memset(digest, 0, 20);
        }
    }

    void HashSha512(const uint8_t* buf, uint32_t buflen, uint8_t* digest)
    {
        int ret = mbedtls_sha512_ret((const unsigned char*)buf, (size_t)buflen, (unsigned char*)digest, 0);
        if (ret != 0) {
            memset(digest, 0, 20);
        }
    }

    void HashMd5(const uint8_t* buf, uint32_t buflen, uint8_t* digest)
    {
        int ret = mbedtls_md5_ret((const unsigned char*)buf, (size_t)buflen, (unsigned char*)digest);
        if (ret != 0) {
            memset(digest, 0, 20);
        }
    }

    bool Base64Encode(const uint8_t* src, uint32_t src_len, uint8_t* dst, uint32_t* dst_len)
    {
        size_t out_len = 0;
        int r = mbedtls_base64_encode(dst, *dst_len, &out_len, src, src_len);
        if (r != 0)
        {
            if (*dst_len == 0)
                *dst_len = (uint32_t)out_len; // Seems to return 1 more than necessary, but better to err on the safe side! (see test_crypt.cpp)
            else
                *dst_len = 0xFFFFFFFF;
            return false;
        }
        *dst_len = (uint32_t)out_len;
        return true;
    }

    bool Base64Decode(const uint8_t* src, uint32_t src_len, uint8_t* dst, uint32_t* dst_len)
    {
        size_t out_len = 0;
        int r = mbedtls_base64_decode(dst, *dst_len, &out_len, src, src_len);
        if (r != 0)
        {
            if (*dst_len == 0)
                *dst_len = (uint32_t)out_len;
            else
                *dst_len = 0xFFFFFFFF;
            return false;
        }
        *dst_len = (uint32_t)out_len;
        return true;
    }

    /* Added by dotGears/TrungB
     * This function will sign your content with given private key by RSA PKCS v15, and digest with SHA256.
     */
    unsigned char * RS256SignKey( unsigned char * signing_content, unsigned char * private_key )
    {
        int ret = 1;
        mbedtls_pk_context pk;
        mbedtls_entropy_context entropy;
        mbedtls_ctr_drbg_context ctr_drbg;

        mbedtls_pk_init( &pk );
        mbedtls_entropy_init( &entropy );
        mbedtls_ctr_drbg_init( &ctr_drbg );

        unsigned char hash[32];
        unsigned char buf[MBEDTLS_MPI_MAX_SIZE]; // As [1024]

        size_t  olen = 0,
                dlen = 344+1,
                buflen = 256;
        unsigned char dst[344+1];

        const char * pers = "rsa_sign_pss";
        size_t pkey_len = strlen((char*)private_key)+1;
        /*
         * Key Sign need a Random Generator Function, so here's one :
         */
        if(( ret = mbedtls_ctr_drbg_seed( &ctr_drbg,  mbedtls_entropy_func,  &entropy, (const unsigned char *) pers, strlen( pers ))) != 0 )
        {
             printf( "\ncrypt -- error: mbedtls_ctr_drbg_seed returned %d", ret );
             goto exit;
        }
        /*
         * Parse private key, wonder why pkey_len had to +1 ?
         */
        if(( ret = mbedtls_pk_parse_key( &pk, private_key, pkey_len, NULL, NULL)) != 0)
        {
            printf( "  ! mbedtls_pk_parse_public_keyfile returned %d", ret );
            goto exit;
        }
        /*
         * Check for valid RSA key.
         */
        if( !mbedtls_pk_can_do( &pk, MBEDTLS_PK_RSA ))
        {
            printf( "\ncrypt -- error: Key is not an RSA key: %s\n", private_key);
            goto exit;
        }
        /*
         * Important: MBEDTLS_RSA_PKCS_V21 won't work for Google OAuth v2, but V15.
         */
        mbedtls_rsa_set_padding( mbedtls_pk_rsa(pk), MBEDTLS_RSA_PKCS_V15, MBEDTLS_MD_SHA256 );
        /*
         * Compute the SHA-256 hash of the input file.
         */
        if(( ret = mbedtls_md(mbedtls_md_info_from_type(MBEDTLS_MD_SHA256), signing_content, strlen((char*)signing_content), hash)) != 0 )
        {
            printf( "\ncrypt -- mbedtls_md_info_from_type\n" );
            goto exit;
        }
        /*
         * then calculate the RSA signature of the hash.
         */
        if(( ret = mbedtls_pk_sign(&pk, MBEDTLS_MD_SHA256, hash, 0, buf, &olen, mbedtls_ctr_drbg_random, &ctr_drbg)) != 0 )
        {
            printf( "\ncrypt -- error: mbedtls_pk_sign returned %d\n", ret );
            goto exit;
        }
        /*
         * encode given signature > base64
         */
        mbedtls_base64_encode(dst, dlen, &olen, buf, buflen);

    exit:
        /*
         * free resources.
         */
        mbedtls_ctr_drbg_free( &ctr_drbg );
        mbedtls_entropy_free( &entropy );
        mbedtls_pk_free(&pk);

        return dst;
    }
}

extern "C" {
    DM_DLLEXPORT int EncryptXTeaCTR(uint8_t* data, uint32_t datalen, const uint8_t* key, uint32_t keylen)
    {
        return dmCrypt::Encrypt(dmCrypt::ALGORITHM_XTEA, data, datalen, key, keylen);
    }

    DM_DLLEXPORT int DecryptXTeaCTR(uint8_t* data, uint32_t datalen, const uint8_t* key, uint32_t keylen)
    {
        return dmCrypt::Decrypt(dmCrypt::ALGORITHM_XTEA, data, datalen, key, keylen);
    }

}


/**
 * @file DynamicsOptimizer.hpp
 * @author Brahayam Ponton (brahayam.ponton@tuebingen.mpg.de)
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
 * @date 2019-10-08
 */

#pragma once

#include <string>
#include <vector>
#include <iostream>
#include <Eigen/Eigen>

#include <solver/interface/Solver.hpp>
#include <momentumopt/utilities/Clock.hpp>
#include <momentumopt/setting/PlannerSetting.hpp>
#include <momentumopt/kinopt/KinematicsState.hpp>
#include <momentumopt/cntopt/ContactPlanInterface.hpp>

namespace momentumopt {

  /**
   * Helper class to define a linear approximation of a friction cone.
   */
  struct FrictionCone
  {
    typedef Eigen::Matrix< double, 4, 3> ConeMat;
    void getCone(double fcoeff, FrictionCone::ConeMat& cone_mat)
    {
      if (fcoeff <= 0.001) { fcoeff = 0.001; }
      Eigen::Vector3d fconevec = Eigen::Vector3d(0.5*sqrt(2.0), 0.5*sqrt(2.0), -fcoeff); fconevec.normalize();
      Eigen::Matrix3d fconemat = Eigen::Matrix3d::Identity();
      double angle = 2*M_PI/4;
      fconemat(0,0) = cos(angle);    fconemat(0,1) = -sin(angle);
      fconemat(1,0) = sin(angle);    fconemat(1,1) =  cos(angle);
      for (int i=0; i<4; i++) {
        cone_mat.row(i) = fconevec;
        fconevec = fconemat * fconevec;
      }
    }
  };

  /*! main class of the dynamics optimization problem  */
  class DynamicsOptimizer
  {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    public:
      /*! default class constructor and destructor */
	  DynamicsOptimizer(){}
      ~DynamicsOptimizer(){}

      /**
       * function to initialize and configure the optimization
       * @param[in]  PlannerSetting               setting of planner configuration variables
       */
      void initialize(PlannerSetting& planner_setting);

      /**
       * function to parse equations and objective into optimization problem and attempt to find a solution
       * @param[in]  ini_state                    initial state of the robot
       * @param[in]  contact_plan                 container of contact sequence to be used for dynamics planning
       * @param[in]  kin_sequence                 kinematics sequence to be used as momentum tracking reference (can be zeros).
       * @param[in]  update_tracking_objective    changes weights from regulation to tracking of momentum in ref_sequence
       * @return     ExitCode                     flag that indicates the optimization result (for example: optimal, infeasible)
       */
      solver::ExitCode optimize(const DynamicsState& ini_state, ContactPlanInterface* contact_plan,
                                const KinematicsSequence& kin_sequence, bool update_tracking_objective = false);

      /**
       * this function gives access to the optimized motion plan
       * @return     DynamicsSequence             reference to dynamics sequence, which is a collection of dynamic states,
       *                                          each of which contains information about all variables, all end-effectors
       *                                          of the motion plan for one time step
       */
      DynamicsSequence& dynamicsSequence() { return dyn_sequence_; }
      const DynamicsSequence& dynamicsSequence() const { return dyn_sequence_; }

      /*! function to have access to time required to solve the optimization problem */
      const double& solveTime() const { return solve_time_; }

    private:
      /*! Getter and setter methods for getting the planner variables  */
      inline PlannerSetting& getSetting() { return *planner_setting_; }
      inline const PlannerSetting& getSetting() const { return *planner_setting_; }

      /*! helper functions for the optimization problem */
      const ContactType& contactType(int time_id, int eff_id) const;
      Eigen::Matrix3d contactRotation(int time_id, int eff_id) const;
      double contactLocation(int time_id, int eff_id, int axis_id) const;

      /**
       * function to initialize optimization variables: type [continuous or binary],
       * guess value [if any], upper and lower bounds for the variable
       */
      void initializeOptimizationVariables();

      /**
       * function to add each variable to the Model and assign a unique identifier
       * to it, to be used by the optimizer to construct the problem
       * @param[in]  opt_var                      helper optimization variable for model predictive control
       * @param[in]  model                        instance of solver interface to collect constraints, objective and solve problem
       * @param[in]  vars                         vector of optimization variables
       */
      void addVariableToModel(const solver::OptimizationVariable& opt_var, solver::Model& model, std::vector<solver::Var>& vars);

      /**
       * functions to update tracking objective for momentum from penalty to tracking
       * and attempt to find a solution to the optimization problem
       * @param[in]  ref_sequence                 dynamics sequence to be used as momentum tracking reference (can be zeros).
       * @param[in]  is_first_time                flag to indicate if this is the first time the solution is being constructed
       */
      void internalOptimize(const KinematicsSequence& kin_sequence, bool is_first_time = false);
      void updateTrackingObjective();

      /**
       * functions that transfers optimal solution from model to helper class OptimizationVariable,
       * from helper class OptimizationVariable to dynamics sequence to be accessed by the user, and
       * from dynamics sequence to a file.
       * @param[in]  opt_var                      helper optimization variable for model predictive control
       * @param[in]  ref_sequence                 dynamics sequence to be used as momentum tracking reference (can be zeros).
       */
      void saveSolution(solver::OptimizationVariable& opt_var);
      void storeSolution();
      void saveToFile(const KinematicsSequence& kin_sequence);

    private:
      /*! Variables required for optimization */
      PlannerSetting* planner_setting_;

      /**
       * class that stores all information about the problem, interfaces between high level definition
       * of the problem and its corresponding mathematical construction to solve it with a solver instance
       */
      solver::Model model_;

      /*! helper variables to construct linear and quadratic expressions */
      solver::LinExpr lin_cons_;
      solver::DCPQuadExpr quad_objective_, quad_cons_;

      /*! exit code of the optimization problem */
      solver::ExitCode exitcode_;

      /**
       * c++ vector containing all problem variables defined by the user. Does not include extra
       * variables required to write the problem in standard conic form.
       */
      std::vector<solver::Var> vars_;

      /**
       * initial configuration of the robot, including center of mass position, linear and angular momenta,
       * end-effectors configurations: activation, position, orientation
       */
      DynamicsState ini_state_;

      /*! simple helper class to build a linear approximation of a friction cone */
      FrictionCone friction_cone_;
      FrictionCone::ConeMat cone_matrix_;

      /**
       * dynamics sequence of dynamic states. This is the main interface between the user
       * and the planner. The user can find in this variable all the optimization results
       */
      DynamicsSequence dyn_sequence_;

      /**
       * helper class to store information about the contact plan, such as surfaces,
       * position and orientation of a sequence of contacts
       */
      ContactPlanInterface* contact_plan_;

      /*! clock for timing purposes */
      Clock timer_;

      /*! type for optimization variable: continuous 'C' or binary 'B' and type of heuristic */
      char variable_type_;

      /*! helper boolean variables for the optimization problem */
      bool has_converged_;

      /*! helper integer variables for the optimization problem */
      int size_, num_vars_;

      /*! helper double variables for the optimization problem */
      double solve_time_, convergence_err_, last_convergence_err_;

      /*! helper vector variables for the optimization problem */
      Eigen::Vector3d com_pos_goal_, weight_desired_com_tracking_;

      /*! helper optimization variables for the optimization problem */
      solver::OptimizationVariable dt_, com_, lmom_, amom_, lmomd_, amomd_;
      std::array<solver::OptimizationVariable, Problem::n_endeffs_> frc_world_, trq_local_, cop_local_, ub_var_, lb_var_;

      /*! helper matrices and vectors for the optimization problem */
      solver::OptimizationVariable::OptVector solution_;
      solver::OptimizationVariable::OptMatrix mat_lb_, mat_ub_, mat_guess_, com_guess_, lmom_guess_, amom_guess_;
  };

}


/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
 *     its contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: Jan 7, 2014
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *********************************************************************************/

/**
 * @file ode.hpp
 * @brief File for ODE integration functionality.
 * @author Stefan Leutenegger
 */

#ifndef INCLUDE_OKVIS_CERES_ODE_ODE_HPP_
#define INCLUDE_OKVIS_CERES_ODE_ODE_HPP_

#include <Eigen/Core>
#include <okvis/kinematics/Transformation.hpp>
#include <okvis/kinematics/operators.hpp>
#include <okvis/FrameTypedefs.hpp>
#include <okvis/Measurements.hpp>
#include <okvis/Variables.hpp>
#include <okvis/assert_macros.hpp>

/// \brief okvis Main namespace of this package.
namespace okvis {
/// \brief ceres Namespace for ceres-related functionality implemented in okvis.
namespace ceres {
/// \brief ode Namespace for functionality related to ODE integration implemented in okvis.
namespace ode {

// to make things a bit faster than using angle-axis conversion:
inline double sinc(double x) {
  if (fabs(x) > 1e-6) {
   return sin(x) / x;
   } else{
    static const double c_2 = 1.0 / 6.0;
    static const double c_4 = 1.0 / 120.0;
    static const double c_6 = 1.0 / 5040.0;
    const double x_2 = x * x;
    const double x_4 = x_2 * x_2;
    const double x_6 = x_2 * x_2 * x_2;
    return 1.0 - c_2 * x_2 + c_4 * x_4 - c_6 * x_6;
  }
}

// world-centric velocities
inline void evaluateContinuousTimeOde(const Eigen::Vector3d& gyr, const Eigen::Vector3d& acc, double g,
                                          const Eigen::Vector3d& p_WS_W, const Eigen::Quaterniond& q_WS,
                                          const okvis::SpeedAndBias& sb, Eigen::Vector3d& p_WS_W_dot,
                                          Eigen::Vector4d& q_WS_dot, okvis::SpeedAndBias& sb_dot,
                                          Eigen::Matrix<double, 15, 15>* F_c_ptr = 0){
  // "true" rates and accelerations
  const Eigen::Vector3d omega_S = gyr - sb.segment<3>(3);
  const Eigen::Vector3d acc_S = acc - sb.tail<3>();

  // nonlinear states
  // start with the pose
  p_WS_W_dot = sb.head<3>();

  // now the quaternion
  Eigen::Vector4d dq;
  q_WS_dot.head<3>() = 0.5 * omega_S;
  q_WS_dot[3] = 0.0;
  Eigen::Matrix3d C_WS = q_WS.toRotationMatrix();

  // the rest is straightforward
  // consider Earth's radius. Model the Earth as a sphere, since we neither
  // know the position nor yaw (except if coupled with GPS and magnetometer).
  Eigen::Vector3d G = -p_WS_W - Eigen::Vector3d(0, 0, 6371009); // vector to Earth center
  sb_dot.head<3>() = (C_WS * acc_S + g * G.normalized()); // s
  // biases
  sb_dot.tail<6>().setZero();

  // linearized system:
  if (F_c_ptr) {
    F_c_ptr->setZero();
    F_c_ptr->block<3, 3>(0, 6) += Eigen::Matrix3d::Identity();
    F_c_ptr->block<3, 3>(3, 9) -= C_WS;
    F_c_ptr->block<3, 3>(6, 3) -= okvis::kinematics::crossMx(C_WS * acc_S);
    F_c_ptr->block<3, 3>(6, 12) -= C_WS;
  }
}

/*/ robo-centric velocities
__inline__ void evaluateContinuousTimeOde(
    const Eigen::Vector3d& gyr, const Eigen::Vector3d& acc, double g,
    const Eigen::Vector3d& p_WS_W, const Eigen::Quaterniond& q_WS,
    const okvis::SpeedAndBias& sb, Eigen::Vector3d& p_WS_W_dot,
    Eigen::Vector4d& q_WS_dot, okvis::SpeedAndBias& sb_dot,
    Eigen::Matrix<double, 15, 15>* F_c_ptr = 0) {

  // "true" rates and accelerations
  const Eigen::Vector3d omega_S = gyr - sb.segment<3>(3);
  const Eigen::Vector3d acc_S = acc - sb.tail<3>();

  // rotation matrix
  Eigen::Matrix3d C_WS = q_WS.toRotationMatrix();
  Eigen::Matrix3d C_SW = C_WS.transpose();

  // nonlinear states
  // start with the pose
  p_WS_W_dot = C_WS*sb.head<3>();

  // now the quaternion
  Eigen::Vector4d dq;
  q_WS_dot.head<3>() = 0.5 * omega_S;
  q_WS_dot[3] = 0.0;

  // the rest is straightforward
  // consider Earth's radius. Model the Earth as a sphere, since we neither
  // know the position nor yaw (except if coupled with GPS and magnetometer).
  Eigen::Vector3d G = -p_WS_W - Eigen::Vector3d(0, 0, 6371009);  // vector to Earth center
  Eigen::Vector3d g_W = g * G.normalized();
  sb_dot.head<3>() = acc_S - okvis::kinematics::crossMx(omega_S)*sb.head<3>() + C_SW * g_W;
  // biases
  sb_dot.tail<6>().setZero();
  //sb_dot.tail<3>()=-sb.tail<3>()/360.0;

  // linearized system:
  if (F_c_ptr) {
    F_c_ptr->setZero();
    F_c_ptr->block<3, 3>(0, 3)  += -okvis::kinematics::crossMx(C_WS * sb.head<3>());
    F_c_ptr->block<3, 3>(0, 6)  += C_WS;
    F_c_ptr->block<3, 3>(3, 9)  += -C_WS;
    F_c_ptr->block<3, 3>(6, 3)  += C_SW * okvis::kinematics::crossMx(g_W);
    F_c_ptr->block<3, 3>(6, 6)  += -okvis::kinematics::crossMx(omega_S);
    F_c_ptr->block<3, 3>(6, 9)  += -okvis::kinematics::crossMx(sb.head<3>());
    F_c_ptr->block<3, 3>(6, 12) += -Eigen::Matrix3d::Identity();
    //F_c_ptr->block<3, 3>(12, 12) = - 1.0/360.0*Eigen::Matrix3d::Identity();
  }
}*/

inline void integrateOneStep_RungeKutta(
    const Eigen::Vector3d& gyr_0, const Eigen::Vector3d& acc_0,
    const Eigen::Vector3d& gyr_1, const Eigen::Vector3d& acc_1, double g,
    double sigma_g_c, double sigma_a_c, double sigma_gw_c, double sigma_aw_c,
    double dt, Eigen::Vector3d& p_WS_W, Eigen::Quaterniond& q_WS,
    okvis::SpeedAndBias& sb, Eigen::Matrix<double, 15, 15>* P_ptr = 0,
    Eigen::Matrix<double, 15, 15>* F_tot_ptr = 0) {

  Eigen::Vector3d k1_p_WS_W_dot;
  Eigen::Vector4d k1_q_WS_dot;
  okvis::SpeedAndBias k1_sb_dot;
  Eigen::Matrix<double, 15, 15> k1_F_c;
  evaluateContinuousTimeOde(gyr_0, acc_0, g, p_WS_W, q_WS, sb, k1_p_WS_W_dot,
                            k1_q_WS_dot, k1_sb_dot, &k1_F_c);

  Eigen::Vector3d p_WS_W1 = p_WS_W;
  Eigen::Quaterniond q_WS1 = q_WS;
  okvis::SpeedAndBias sb1 = sb;
  // state propagation:
  p_WS_W1 += k1_p_WS_W_dot * 0.5 * dt;
  Eigen::Quaterniond dq;
  double theta_half = k1_q_WS_dot.head<3>().norm() * 0.5 * dt;
  double sinc_theta_half = sinc(theta_half);
  double cos_theta_half = cos(theta_half);
  dq.vec() = sinc_theta_half * k1_q_WS_dot.head<3>() * 0.5 * dt;
  dq.w() = cos_theta_half;
  q_WS1 = q_WS * dq;
  sb1 += k1_sb_dot * 0.5 * dt;

  Eigen::Vector3d k2_p_WS_W_dot;
  Eigen::Vector4d k2_q_WS_dot;
  okvis::SpeedAndBias k2_sb_dot;
  Eigen::Matrix<double, 15, 15> k2_F_c;
  evaluateContinuousTimeOde(0.5 * (gyr_0 + gyr_1), 0.5 * (acc_0 + acc_1), g,
                            p_WS_W1, q_WS1, sb1, k2_p_WS_W_dot, k2_q_WS_dot,
                            k2_sb_dot, &k2_F_c);

  Eigen::Vector3d p_WS_W2 = p_WS_W;
  Eigen::Quaterniond q_WS2 = q_WS;
  okvis::SpeedAndBias sb2 = sb;
  // state propagation:
  p_WS_W2 += k2_p_WS_W_dot * dt;
  theta_half = k2_q_WS_dot.head<3>().norm() * dt;
  sinc_theta_half = sinc(theta_half);
  cos_theta_half = cos(theta_half);
  dq.vec() = sinc_theta_half * k2_q_WS_dot.head<3>() * dt;
  dq.w() = cos_theta_half;
  //std::cout<<dq.transpose()<<std::endl;
  q_WS2 = q_WS2 * dq;
  sb2 += k1_sb_dot * dt;

  Eigen::Vector3d k3_p_WS_W_dot;
  Eigen::Vector4d k3_q_WS_dot;
  okvis::SpeedAndBias k3_sb_dot;
  Eigen::Matrix<double, 15, 15> k3_F_c;
  evaluateContinuousTimeOde(0.5 * (gyr_0 + gyr_1), 0.5 * (acc_0 + acc_1), g,
                            p_WS_W2, q_WS2, sb2, k3_p_WS_W_dot, k3_q_WS_dot,
                            k3_sb_dot, &k3_F_c);

  Eigen::Vector3d p_WS_W3 = p_WS_W;
  Eigen::Quaterniond q_WS3 = q_WS;
  okvis::SpeedAndBias sb3 = sb;
  // state propagation:
  p_WS_W3 += k3_p_WS_W_dot * dt;
  theta_half = k3_q_WS_dot.head<3>().norm() * dt;
  sinc_theta_half = sinc(theta_half);
  cos_theta_half = cos(theta_half);
  dq.vec() = sinc_theta_half * k3_q_WS_dot.head<3>() * dt;
  dq.w() = cos_theta_half;
  //std::cout<<dq.transpose()<<std::endl;
  q_WS3 = q_WS3 * dq;
  sb3 += k3_sb_dot * dt;

  Eigen::Vector3d k4_p_WS_W_dot;
  Eigen::Vector4d k4_q_WS_dot;
  okvis::SpeedAndBias k4_sb_dot;
  Eigen::Matrix<double, 15, 15> k4_F_c;
  evaluateContinuousTimeOde(gyr_1, acc_1, g, p_WS_W3, q_WS3, sb3, k4_p_WS_W_dot,
                            k4_q_WS_dot, k4_sb_dot, &k4_F_c);

  // now assemble
  p_WS_W +=
      (k1_p_WS_W_dot + 2 * (k2_p_WS_W_dot + k3_p_WS_W_dot) + k4_p_WS_W_dot) * dt
          / 6.0;
  Eigen::Vector3d theta_half_vec = (k1_q_WS_dot.head<3>()
      + 2 * (k2_q_WS_dot.head<3>() + k3_q_WS_dot.head<3>())
      + k4_q_WS_dot.head<3>()) * dt / 6.0;
  theta_half = theta_half_vec.norm();
  sinc_theta_half = sinc(theta_half);
  cos_theta_half = cos(theta_half);
  dq.vec() = sinc_theta_half * theta_half_vec;
  dq.w() = cos_theta_half;
  q_WS = q_WS * dq;
  sb += (k1_sb_dot + 2 * (k2_sb_dot + k3_sb_dot) + k4_sb_dot) * dt / 6.0;

  q_WS.normalize(); // do not accumulate errors!

  if (F_tot_ptr) {
    // compute state transition matrix
    Eigen::Matrix<double, 15, 15>& F_tot = *F_tot_ptr;
    const Eigen::Matrix<double, 15, 15>& J1 = k1_F_c;
    const Eigen::Matrix<double, 15, 15> J2=k2_F_c*(Eigen::Matrix<double, 15, 15>::Identity()+0.5*dt*J1);
    const Eigen::Matrix<double, 15, 15> J3=k3_F_c*(Eigen::Matrix<double, 15, 15>::Identity()+0.5*dt*J2);
    const Eigen::Matrix<double, 15, 15> J4=k4_F_c*(Eigen::Matrix<double, 15, 15>::Identity()+dt*J3);
    Eigen::Matrix<double, 15, 15> F = Eigen::Matrix<double, 15, 15>::Identity()
        + dt * (J1+2*(J2+J3)+J4) / 6.0;
        //+ dt * J1;
    //std::cout<<F<<std::endl;
    F_tot = (F * F_tot).eval();

    if (P_ptr) {
      Eigen::Matrix<double, 15, 15>& cov = *P_ptr;
      cov = F * (cov * F.transpose()).eval();

      // add process noise
      const double Q_g = sigma_g_c * sigma_g_c * dt;
      const double Q_a = sigma_a_c * sigma_a_c * dt;
      const double Q_gw = sigma_gw_c * sigma_gw_c * dt;
      const double Q_aw = sigma_aw_c * sigma_aw_c * dt;
      cov(3, 3) += Q_g;
      cov(4, 4) += Q_g;
      cov(5, 5) += Q_g;
      cov(6, 6) += Q_a;
      cov(7, 7) += Q_a;
      cov(8, 8) += Q_a;
      cov(9, 9) += Q_gw;
      cov(10, 10) += Q_gw;
      cov(11, 11) += Q_gw;
      cov(12, 12) += Q_aw;
      cov(13, 13) += Q_aw;
      cov(14, 14) += Q_aw;

      // force symmetric - TODO: is this really needed here?
      // cov = 0.5 * cov + 0.5 * cov.transpose().eval();
    }
  }

}

}

}  // namespace ceres
}  // namespace okvis

#endif /* INCLUDE_OKVIS_CERES_ODE_ODE_HPP_ */


/**
 * @file
 * @license BSD 3-clause
 * @copyright Copyright (c) 2020, New York University and Max Planck
 * Gesellschaft
 *
 * @brief Implements the "Walking Control Based on Step Timing Adaptation" foot
 * trajectory QP. The pdf can be found in https://arxiv.org/abs/1704.01271, and
 * in the `doc/` folder in this repository.
 */

#pragma once

#include <eigen-quadprog/QuadProg.h>
#include <Eigen/Eigen>
#include <chrono>
#include <iostream>
#include <limits>
#define RESET "\033[0m"
#define RED "\033[31m"  /* Red */
#define BLUE "\033[34m" /* Blue */
#define MAX_VAR 500
#define EPSILON 1e-9

namespace reactive_planners
{
/**
 * @brief
 */
class DynamicallyConsistentEndEffectorTrajectory
{
    /*
     * Private methods
     */
public:
    /** @brief Constructor. */
    DynamicallyConsistentEndEffectorTrajectory();

    /** @brief Destructor. */
    ~DynamicallyConsistentEndEffectorTrajectory();

    bool compute(const Eigen::Ref<const Eigen::Vector3d>& start_pose,
                 const Eigen::Ref<const Eigen::Vector3d>& current_pose,
                 const Eigen::Ref<const Eigen::Vector3d>& current_velocity,
                 const Eigen::Ref<const Eigen::Vector3d>& target_pose,
                 const double& start_time,
                 const double& current_time,
                 const double& end_time,
                 const bool& is_left_leg_in_contact);

    void update_robot_status(Eigen::Ref<Eigen::Vector3d> next_pose,
                             Eigen::Ref<Eigen::Vector3d> next_velocity,
                             Eigen::Ref<Eigen::Vector3d> next_acceleration);

    /** @brief Get all the forces until landing foot. Returns the number of
     * forces.*/
    int get_forces(Eigen::Ref<Eigen::VectorXd> forces,
                   Eigen::Ref<Eigen::Vector3d> next_pose,
                   Eigen::Ref<Eigen::Vector3d> next_velocity,
                   Eigen::Ref<Eigen::Vector3d> next_acceleration);

    /** @brief Display the matrices of the Problem. */
    void print_solver() const;

    /** @brief Convert the inner data to a string format. */
    std::string to_string() const;

    /*
     * Getters
     */

    /** @brief Get the height of the flying foot. */
    double get_mid_air_height() const
    {
        return mid_air_height_;
    }

    /*
     * Setters
     */
    /** @brief Set the height of the flying foot.
     *
     * @param mid_air_height
     */
    void set_planner_loop(double planner_loop)
    {
        planner_loop_ = planner_loop;
        init_acceleration_velocity_terms();
    }

    /** @brief Set the height of the flying foot.
     *
     * @param mid_air_height
     */
    void set_mid_air_height(double mid_air_height)
    {
        mid_air_height_ = mid_air_height;
    }

    /**
     * @brief return cost.
     *
     * @return double
     */
    double cost()
    {
        x_opt_.resize(nb_var_);
        x_opt_ = qp_solver_.result();
        return (0.5 * x_opt_.transpose() * Q_ * x_opt_ +
                q_.transpose() * x_opt_)(0, 0);
    }

    /**
     * @brief Return the slack values from the last solution.
     **/
    const Eigen::Vector3d& get_slack_variables() const
    {
        return slack_variables_;
    }

    double calculate_t_min(
        const Eigen::Ref<const Eigen::Vector3d>& current_pose,
        const Eigen::Ref<const Eigen::Vector3d>& current_velocity,
        const bool& is_left_leg_in_contact);
    /*
     * Private methods
     */
private:
    /** @brief resize QP variable at each step.
     */
    void resize_matrices()
    {
        Q_.resize(nb_var_, nb_var_);
        Q_.setZero();
        q_.resize(nb_var_);
        q_.setZero();

        A_eq_.resize(nb_eq_, nb_var_);
        A_eq_.setZero();
        B_eq_.resize(nb_eq_);
        B_eq_.setZero();

        A_ineq_.resize(nb_ineq_, nb_var_);
        A_ineq_.setZero();
        B_ineq_.resize(nb_ineq_);
        B_ineq_.setZero();
        qp_solver_.problem(nb_var_, nb_eq_, nb_ineq_);
    }
    /** @brief resize QP variable at each step.
     */
    void resize_matrices_t_min(int index)
    {
        Q_t_min_.resize(3 * index, 3 * index);
        Q_t_min_ = Eigen::MatrixXd::Identity(3 * index, 3 * index);
        q_t_min_.resize(3 * index);
        q_t_min_.setZero();

        A_eq_t_min_.resize(2, 3 * index);
        A_eq_t_min_.setZero();
        B_eq_t_min_.resize(2);
        B_eq_t_min_.setZero();

        A_ineq_t_min_.resize(2 * 3 * index, 3 * index);
        A_ineq_t_min_.setZero();
        B_ineq_t_min_.resize(2 * 3 * index);
        B_ineq_t_min_.setZero();
        qp_solver_t_min_.problem(3 * index, 2, 2 * 3 * index);
    }

    /** @brief initialie acceleration_terms and velocity_terms.
     */
    void init_acceleration_velocity_terms();

    /*
     * Constant problem parameters.
     */

    /** @brief Flying foot apex to be reach mid-air. */
    double mid_air_height_;

    /** @brief Number of force variables in the optimization problem on the
     * one of the axes. */
    int nb_local_sampling_time_;

    /** @brief non_linear_terms.
     */
    Eigen::Vector3d h_c;

    /*
     * Variable problem parameters.
     */

    /** @brief affect of forces_x to the positions.
     * There are two separate matrices for each leg.
     */
    Eigen::MatrixXd* position_terms_F_x_;

    /** @brief affect of forces_y to the positions.
     * There are two separate matrices for each leg.
     */
    Eigen::MatrixXd* position_terms_F_y_;

    /** @brief affect of forces_z to the positions.
     * There are two separate matrices for each leg.
     */
    Eigen::MatrixXd* position_terms_F_z_;

    /** @brief Desired velocity. */
    Eigen::Vector3d v_des_;

    /** @brief Slack variable values corresponding to last solution. */
    Eigen::Vector3d slack_variables_;

    /** @brief affect of non_linear_terms in position. */
    Eigen::MatrixXd* non_linear_terms;

    /** @brief Initial position before the motion. */
    Eigen::Vector3d start_pose_;

    /** @brief Current position. */
    Eigen::Vector3d current_pose_;

    /** @brief Previous computed position from the QP. */
    Eigen::Vector3d previous_solution_pose_;

    /** @brief Current velocity. */
    Eigen::Vector3d current_velocity_;

    /** @brief Current acceleration. */
    Eigen::Vector3d current_acceleration_;

    /** @brief Target pose after the motion. */
    Eigen::Vector3d target_pose_;

    /** @brief Initial time. */
    double start_time_;

    /** @brief Current time. */
    double current_time_;

    /** @brief Last end time register when we computed the QP. */
    double last_end_time_seen_;

    /** @brief Control loop. */
    double control_loop_;

    /** @brief planner loop. */
    double planner_loop_;

    /*
     * QP variables
     */

    /** @brief Number of variables in the optimization problem. */
    int nb_var_;

    /** @brief Number of equality constraints in the optimization problem. */
    int nb_eq_;

    /** @brief Number of inequality in the optimization problem. */
    int nb_ineq_;

    /** @brief Quadratic program solver.
     *
     * This is an eigen wrapper around the quad_prog fortran solver.
     */
    Eigen::QuadProgDense qp_solver_;

    /** @brief Quadratic program solver.
     *
     * This is an eigen wrapper around the quad_prog fortran solver.
     */
    Eigen::QuadProgDense qp_solver_t_min_;

    /** @brief Solution of the optimization problem.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::VectorXd x_opt_;

    /** @brief Quadratic term of the quadratic cost.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::MatrixXd Q_;
    /** @brief Quadratic term of the quadratic cost.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::MatrixXd Q_t_min_;

    /** @brief inverse estimation of mass matrix.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::MatrixXd* M_inv_;

    /** @brief Is the left foot in contact? otherwise the right foot is. */
    bool is_left_leg_in_contact_;

    /** @brief Quadratic term added to the quadratic cost in order regularize
     * the system.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::MatrixXd Q_regul_;

    /** @brief Cost weights for forces. */
    double cost_;

    /** @brief Cost weights for the epsilon_z_mid. */
    double cost_epsilon_z_mid_;

    /** @brief Cost weights for the epsilon_x. */
    double cost_epsilon_x_;

    /** @brief Cost weights for the epsilon_y. */
    double cost_epsilon_y_;

    /** @brief Cost weights for the epsilon_z. */
    double cost_epsilon_z_;

    /** @brief Cost weights for the epsilon_z. */
    double cost_epsilon_vel_;

    /** @brief Linear term of the quadratic cost.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::VectorXd q_;

    /** @brief Linear equality matrix.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::MatrixXd A_eq_;

    /** @brief Linear equality vector.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::VectorXd B_eq_;

    /** @brief Linear inequality matrix.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::MatrixXd A_ineq_;

    /** @brief Linear inequality vector.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::VectorXd B_ineq_;

    /** @brief Linear term of the quadratic cost.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::VectorXd q_t_min_;

    /** @brief Linear equality matrix.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::MatrixXd A_eq_t_min_;

    /** @brief Linear equality vector.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::VectorXd B_eq_t_min_;

    /** @brief Linear inequality matrix.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::MatrixXd A_ineq_t_min_;

    /** @brief Linear inequality vector.
     * @see DynamicallyConsistentEndEffectorTrajectory */
    Eigen::VectorXd B_ineq_t_min_;
};

}  // namespace reactive_planners


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

// Own include
#include <asiAlgo_MeshOBB.h>

// asiAlgo includes
#include <asiAlgo_Utils.h>

// OCCT includes
#include <BRepBuilderAPI_Transform.hxx>
#include <BRepPrimAPI_MakeBox.hxx>

// Eigen includes
#pragma warning(push, 0)
#include <Eigen/Dense>
#pragma warning(pop)

// STL includes
#include <vector>

#undef COUT_DEBUG
#if defined COUT_DEBUG
  #pragma message("===== warning: COUT_DEBUG is enabled")
#endif

#undef DRAW_DEBUG
#if defined DRAW_DEBUG
  #pragma message("===== warning: DRAW_DEBUG is enabled")
#endif

//-----------------------------------------------------------------------------

static bool compare(const std::pair<double, int>& p1,
                    const std::pair<double, int>& p2)
{
  return p1.first > p2.first;
}

//-----------------------------------------------------------------------------

asiAlgo_MeshOBB::asiAlgo_MeshOBB(const Handle(Poly_Triangulation)& mesh,
                                 ActAPI_ProgressEntry              progress,
                                 ActAPI_PlotterEntry               plotter)
//
: ActAPI_IAlgorithm (progress, plotter),
  m_input           (mesh)
{}

//-----------------------------------------------------------------------------

bool asiAlgo_MeshOBB::Perform()
{
  // Check if triangulation exists.
  if ( m_input.IsNull() || !m_input->NbNodes() )
  {
    this->GetProgress().SendLogMessage( LogErr(Normal) << "Cannot build OBB for empty triangulation." );
    return false;
  }

  // Calculate principal axes.
  gp_Ax1 ax_X, ax_Y, ax_Z;
  gp_XYZ mu;
  //
  this->calculateByCovariance(ax_X, ax_Y, ax_Z, mu);

  { // Imperative drawing
    // NOTE: that looks a bit funny, but in order to visualize the just calculated
    //       principal axes of OBB we use AABB as it gives us hint for sizing.

    double xMin, yMin, zMin, xMax, yMax, zMax;
    asiAlgo_Utils::Bounds(m_input, xMin, yMin, zMin, xMax, yMax, zMax);
    //
    gp_Pnt bndMin(xMin, yMin, zMin), bndMax(xMax, yMax, zMax);
    const double magnitude = bndMax.Distance(bndMin)*0.1;

    this->GetPlotter().DRAW_VECTOR_AT(mu, ax_X.Direction().XYZ()*magnitude, Color_Red);
    this->GetPlotter().DRAW_VECTOR_AT(mu, ax_Y.Direction().XYZ()*magnitude, Color_Green);
    this->GetPlotter().DRAW_VECTOR_AT(mu, ax_Z.Direction().XYZ()*magnitude, Color_Blue);
  }

  /* =============================================
   *  Calculate extremities on the principal axes
   * ============================================= */

  double ax_X_param_max = -RealLast(),
         ax_Y_param_max = -RealLast(),
         ax_Z_param_max = -RealLast();
  double ax_X_param_min =  RealLast(),
         ax_Y_param_min =  RealLast(),
         ax_Z_param_min =  RealLast();
  //
  const TColgp_Array1OfPnt& nodes = m_input->Nodes();
  //
  for ( int i = nodes.Lower(); i <= nodes.Upper(); ++i )
  {
    const gp_Pnt& P = nodes(i);
    //
    const double node_X = P.X();
    const double node_Y = P.Y();
    const double node_Z = P.Z();
    //
    gp_XYZ p(node_X, node_Y, node_Z);
    gp_Vec p_local = p - mu;

    const double ax_X_param = p_local.Dot( ax_X.Direction() );
    const double ax_Y_param = p_local.Dot( ax_Y.Direction() );
    const double ax_Z_param = p_local.Dot( ax_Z.Direction() );

    if ( ax_X_param > ax_X_param_max )
      ax_X_param_max = ax_X_param;
    //
    if ( ax_Y_param > ax_Y_param_max )
      ax_Y_param_max = ax_Y_param;
    //
    if ( ax_Z_param > ax_Z_param_max )
      ax_Z_param_max = ax_Z_param;
    //
    if ( ax_X_param < ax_X_param_min )
      ax_X_param_min = ax_X_param;
    //
    if ( ax_Y_param < ax_Y_param_min )
      ax_Y_param_min = ax_Y_param;
    //
    if ( ax_Z_param < ax_Z_param_min )
      ax_Z_param_min = ax_Z_param;
  }

#if defined DRAW_DEBUG
  { // Imperative drawing.
    const gp_XYZ ax_X_max = mu + ax_X.Direction().XYZ()*ax_X_param_max;
    const gp_XYZ ax_Y_max = mu + ax_Y.Direction().XYZ()*ax_Y_param_max;
    const gp_XYZ ax_Z_max = mu + ax_Z.Direction().XYZ()*ax_Z_param_max;
    //
    const gp_XYZ ax_X_min = mu + ax_X.Direction().XYZ()*ax_X_param_min;
    const gp_XYZ ax_Y_min = mu + ax_Y.Direction().XYZ()*ax_Y_param_min;
    const gp_XYZ ax_Z_min = mu + ax_Z.Direction().XYZ()*ax_Z_param_min;

    this->Plotter().DRAW_POINT(ax_X_max, Color_Red);
    this->Plotter().DRAW_POINT(ax_Y_max, Color_Green);
    this->Plotter().DRAW_POINT(ax_Z_max, Color_Blue);
    //
    this->Plotter().DRAW_POINT(ax_X_min, Color_Magenta);
    this->Plotter().DRAW_POINT(ax_Y_min, Color_Yellow);
    this->Plotter().DRAW_POINT(ax_Z_min, Color_Violet);
  }
#endif

  /* =====================
   *  STAGE 4: Set result
   * ===================== */

  // Set placement and corner positions to the result.
  gp_Ax3 ax3_placement( mu, ax_Z.Direction(), ax_X.Direction() );
  gp_Pnt corner_min(ax_X_param_min, ax_Y_param_min, ax_Z_param_min);
  gp_Pnt corner_max(ax_X_param_max, ax_Y_param_max, ax_Z_param_max);
  //
  m_obb.Placement      = ax3_placement;
  m_obb.LocalCornerMin = corner_min;
  m_obb.LocalCornerMax = corner_max;

  return true;
}

//-----------------------------------------------------------------------------

const asiAlgo_OBB& asiAlgo_MeshOBB::GetResult() const
{
  return m_obb;
}

//-----------------------------------------------------------------------------

gp_Trsf asiAlgo_MeshOBB::GetResultTrsf() const
{
  gp_Trsf T;
  T.SetTransformation(m_obb.Placement);
  T.Invert();
  return T;
}

//-----------------------------------------------------------------------------

TopoDS_Solid asiAlgo_MeshOBB::GetResultBox() const
{
  gp_Trsf T = this->GetResultTrsf();

  // Build a properly located box solid representing OBB
  TopoDS_Shape solid;
  try
  {
    BRepPrimAPI_MakeBox mkOBB(m_obb.LocalCornerMin, m_obb.LocalCornerMax);
    solid = BRepBuilderAPI_Transform(mkOBB.Solid(), T, true);
  }
  catch ( ... ) {}

  return TopoDS::Solid(solid);
}

//-----------------------------------------------------------------------------

void asiAlgo_MeshOBB::calculateByCovariance(gp_Ax1& xAxis,
                                            gp_Ax1& yAxis,
                                            gp_Ax1& zAxis,
                                            gp_XYZ& meanVertex) const
{
  // Get number of nodes.
  const int nNodes = m_input->NbNodes();

  /* =======================
   *  Calculate mean vertex
   * ======================= */

  gp_XYZ mu;
  const TColgp_Array1OfPnt& nodes = m_input->Nodes();
  //
  for ( int i = nodes.Lower(); i <= nodes.Upper(); ++i )
  {
    const gp_Pnt& P = nodes(i);

    const double node_X = P.X();
    const double node_Y = P.Y();
    const double node_Z = P.Z();

    mu += gp_XYZ(node_X, node_Y, node_Z);
  }
  mu /= nNodes;

  this->GetPlotter().DRAW_TRIANGULATION(m_input, Color_Magenta, 0.5);
  this->GetPlotter().DRAW_POINT(mu, Color_Red);

  /* =====================
   *  Calculate main axes
   * ===================== */

  Eigen::Matrix3d C;
  for ( int j = 1; j <= 3; ++j )
  {
    for ( int k = 1; k <= 3; ++k )
    {
      C(j-1, k-1) = 0.0; // TODO: is that necessary?
    }
  }

  for ( int i = nodes.Lower(); i <= nodes.Upper(); ++i )
  {
    const gp_Pnt& P = nodes(i);
    //
    const double node_X = P.X();
    const double node_Y = P.Y();
    const double node_Z = P.Z();
    //
    gp_XYZ p(node_X, node_Y, node_Z);
    gp_XYZ p_dash = p - mu;

    for ( int j = 1; j <= 3; ++j )
    {
      for ( int k = 1; k <= 3; ++k )
      {
        C(j-1, k-1) += ( p_dash.Coord(j)*p_dash.Coord(k) );
      }
    }
  }

  for ( int j = 1; j <= 3; ++j )
  {
    for ( int k = 1; k <= 3; ++k )
    {
      C(j-1, k-1) /= (nNodes);
    }
  }

  Eigen::EigenSolver<Eigen::Matrix3d> EigenSolver(C);

#if defined COUT_DEBUG
  std::cout << "\tThe eigen values of A are:" << std::endl << EigenSolver.eigenvalues() << std::endl;
  std::cout << "\tThe matrix of eigenvectors, V, is:" << std::endl << EigenSolver.eigenvectors() << std::endl << std::endl;
#endif

  Eigen::Vector3cd v1 = EigenSolver.eigenvectors().col(0);
  Eigen::Vector3cd v2 = EigenSolver.eigenvectors().col(1);
  Eigen::Vector3cd v3 = EigenSolver.eigenvectors().col(2);

  gp_Vec V[3] = { gp_Vec( v1.x().real(), v1.y().real(), v1.z().real() ),
                  gp_Vec( v2.x().real(), v2.y().real(), v2.z().real() ),
                  gp_Vec( v3.x().real(), v3.y().real(), v3.z().real() ) };
  //
  std::vector< std::pair<double, int> >
    lambda { std::pair<double, int>( EigenSolver.eigenvalues()(0).real(), 0 ),
             std::pair<double, int>( EigenSolver.eigenvalues()(1).real(), 1 ),
             std::pair<double, int>( EigenSolver.eigenvalues()(2).real(), 2 ) };
  //
  std::sort(lambda.begin(), lambda.end(), compare);
  //
  gp_Ax1 ax_X(mu, V[lambda[0].second]);
  gp_Ax1 ax_Y(mu, V[lambda[1].second]);
  gp_Ax1 ax_Z(mu, V[lambda[2].second]);

  // Check if the system is right-handed.
  const double ang = ax_X.Direction().AngleWithRef( ax_Y.Direction(), ax_Z.Direction() );
  if ( ang < 0 )
    ax_X.Reverse();

  // Store results.
  meanVertex = mu;
  xAxis      = ax_X;
  yAxis      = ax_Y;
  zAxis      = ax_Z;
}