#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_edge.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
#include <boost/concept_check.hpp>

using namespace std;
using namespace g2o;
//Vertex数量n,这n个Vertex存储的数据类型
class powell_vertex: public BaseVertex<4, Eigen::Matrix<double,1,4>> {
	virtual void oplusImpl(const double* v) {
		//base_vertex.h: EstimateType _estimate;
		//cout << "oplus: "  << endl;
		//cout << Eigen::Matrix<double,1,4>(v) << endl;
		_estimate += Eigen::Matrix<double,1,4>(v);
		//cout << "_estimate: " << _estimate << endl;
	}
        virtual void setToOriginImpl() {
		//cout << "initial _estimate" << endl;
		_estimate << 0.5,1.5,2.5,3.5;
	}

	virtual bool read(std::istream& is) {}
        virtual bool write(std::ostream& os) const {}	//const 不能忽略
};
//e(xi)的维度、类型; vertex的类型
class powell_edge: public BaseUnaryEdge<4, Eigen::Matrix<double,1,4>, powell_vertex> {
	virtual void computeError() {
		//vertex* vtx = new vertex;
		const powell_vertex* v = static_cast<const powell_vertex*> (_vertices[0]);
		Eigen::Matrix<double,1,4> est =  v->estimate();
		//cout << "est: " << est(0,0)  << "---" << est(0,1)  <<"---" << est(0,2) <<  "---"<< est(0,3) << endl;
// 		_error(0,0) = (&_measurement)[0] - (est(0,0) + 10*est(0,1));
// 		_error(0,1) = (&_measurement)[1] - sqrt(5)*(est(0,2) - est(0,3));
// 		_error(0,2) = (&_measurement)[2] - (est(0,1) - 2*est(0,2))*(est(0,1) - 2*est(0,2));
// 		_error(0,3) = (&_measurement)[3] - sqrt(10)*(est(0,0) - est(0,2))*(est(0,0) - est(0,2));
		_error(0,0) = _measurement(0,0)- (est(0,0) + 10*est(0,1));
		_error(1,0) = _measurement(0,1) - sqrt(5)*(est(0,2) - est(0,3));
		_error(2,0) = _measurement(0,2) - (est(0,1) - 2*est(0,2))*(est(0,1) - 2*est(0,2));
		_error(3,0) = _measurement(0,3) - sqrt(10)*(est(0,0) - est(0,2))*(est(0,0) - est(0,2));
		cout << "_error: " << _error.transpose() << endl;
	}

	virtual bool read(std::istream& is) {}
        virtual bool write(std::ostream& os) const {}	//const 不能忽略
};

int main(int argc, char **argv) {
	// 构建图优化，先设定g2o
	//注意维度对应--边--顶点---
	typedef g2o::BlockSolver< g2o::BlockSolverTraits<4,4> > Block;  // int _PoseDim, int _LandmarkDim
	Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
	Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
	// 梯度下降方法，从GN, LM, DogLeg 中选
	cout << "config blocksolver" << endl;
	g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
	// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
	// g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
	g2o::SparseOptimizer optimizer;     // 图模型
	optimizer.setAlgorithm( solver );   // 设置求解器
	optimizer.setVerbose( true );       // 打开调试输出
	cout << "add Vertex" << endl;
	// 往图中增加顶点
	powell_vertex* v = new powell_vertex;
	Eigen::Matrix<double,1,4> estimate_initial;
	estimate_initial <<0.5,1.5,2.5,3.5;
	v->setEstimate(estimate_initial);
	v->setId(0);
	optimizer.addVertex( v );
	cout << "add Edge" << endl;
	//加边
	powell_edge* eg = new powell_edge;
	//double measurement_initial[4] = {21,-sqrt(5),16,9*sqrt(10)};
	//for (int i=0;i<4;i++) {
		eg->setId(0);
		eg->setVertex( 0, v );                // 设置连接的顶点
		Eigen::Matrix<double,1,4> measurement_initial;
		measurement_initial << 21,-sqrt(5),16,9*sqrt(10);
		eg->setMeasurement(measurement_initial);      // 观测数值
		//这个'4'对应于powell_edge第一个参数
		eg->setInformation(Eigen::Matrix<double,4,4>::Identity()*1/0.25); // 信息矩阵：协方差矩阵之逆
		optimizer.addEdge( eg );
	//}
	// 执行优化
	cout<<"start optimization"<<endl;
	chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
	cout<<"initial optimization"<<endl;
	optimizer.initializeOptimization();
	cout<<"start"<<endl;
	optimizer.optimize(200);
	chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
	chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
	cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;

	// 输出优化值
	Eigen::Matrix<double,1,4> abc_estimate = v->estimate();
	cout<<"estimated value: "<<abc_estimate.transpose()<<endl;

    return 0;
}


#pragma once

// -*- coding: utf-8 -*-
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <py2cpp/nx2bgl.hpp>
#include <utility> // for std::pair

using graph_t = boost::adjacency_list<boost::listS, boost::vecS,
    boost::directedS, boost::no_property,
    boost::property<boost::edge_weight_t, int,
        boost::property<boost::edge_index_t, int>>>;
using Vertex = boost::graph_traits<graph_t>::vertex_descriptor;
using Edge_it = boost::graph_traits<graph_t>::edge_iterator;

template <typename Container>
inline auto create_test_case1(const Container& weights) -> xn::grAdaptor<graph_t>
{
    using Edge = std::pair<int, int>;
    const auto num_nodes = 5;
    enum nodes
    {
        A,
        B,
        C,
        D,
        E
    };
    static Edge edge_array[] = {
        Edge {A, B}, Edge {B, C}, Edge {C, D}, Edge {D, E}, Edge {E, A}};
    // int weights[] = {-5, 1, 1, 1, 1};
    int num_arcs = sizeof(edge_array) / sizeof(Edge);
    auto g = graph_t(edge_array, edge_array + num_arcs, weights, num_nodes);
    return xn::grAdaptor<graph_t> {std::move(g)};
}

template <typename Container>
inline auto create_test_case_timing(const Container& weights)
    -> xn::grAdaptor<graph_t>
{
    using Edge = std::pair<int, int>;
    constexpr auto num_nodes = 3;
    enum nodes
    {
        A,
        B,
        C
    };
    static Edge edge_array[] = {Edge {A, B}, Edge {B, A}, Edge {B, C},
        Edge {C, B}, Edge {B, C}, Edge {C, B}, Edge {C, A}, Edge {A, C}};
    // int weights[] = {7, 0, 3, 1, 6, 4, 2, 5};
    constexpr int num_arcs = sizeof(edge_array) / sizeof(Edge);
    auto g = graph_t(edge_array, edge_array + num_arcs, weights, num_nodes);
    return xn::grAdaptor<graph_t> {std::move(g)};
}


/**
 * @ file PointEvaluationRhs_main.cc
 * @ brief NPDE homework PointEvaluationRhs code
 * @ author Christian Mitsch, Liaowang Huang (refactoring)
 * @ date 22/03/2019, 06/01/2020 (refactoring)
 * @ copyright Developed at ETH Zurich
 */

#include <lf/assemble/assemble.h>
#include <lf/base/base.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/test_utils/test_meshes.h>
#include <lf/mesh/utils/utils.h>
#include <lf/refinement/mesh_hierarchy.h>

#include <Eigen/Core>
#include <iomanip>
#include <memory>
#include <sstream>
#include <utility>
#include <vector>

#include "pointevaluationrhs.h"

int main() {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(1, 1.0);

  // Start of numerical experiment
  std::vector<double> dof_a{};
  std::vector<double> l2_a{};
  std::vector<double> h1_a{};
  Eigen::VectorXd sol_vec;

  // Runs with initial mesh
  lf::assemble::UniformFEDofHandler dofh_initial(
      mesh_p, {{lf::base::RefEl::kPoint(), 1}});
  auto result = PointEvaluationRhs::normsSolutionPointLoadDirichletBVP(
      dofh_initial, Eigen::Vector2d(1.3, 1.7), sol_vec);
  unsigned N_dofs = dofh_initial.NumDofs();
  dof_a.push_back(N_dofs);
  l2_a.push_back(result.first);
  h1_a.push_back(result.second);

  // Necessary for regular refinement
  std::unique_ptr<lf::mesh::hybrid2d::MeshFactory> mesh_factory2 =
      std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::refinement::MeshHierarchy my_hierarchy(mesh_p, std::move(mesh_factory2));

  for (int k = 1; k < 7; k++) {
    my_hierarchy.RefineRegular();

    mesh_p = my_hierarchy.getMesh(k);
    lf::assemble::UniformFEDofHandler dofh(mesh_p,
                                           {{lf::base::RefEl::kPoint(), 1}});
    unsigned N_dofs = dofh.NumDofs();
    dof_a.push_back(N_dofs);
    sol_vec.resize(N_dofs);

    result = PointEvaluationRhs::normsSolutionPointLoadDirichletBVP(
        dofh, Eigen::Vector2d(1.3, 1.7), sol_vec);
    l2_a.push_back(result.first);
    h1_a.push_back(result.second);
    // Write vtk file
    std::stringstream filename;
    filename << "rhseval" << k << ".vtk";
    lf::io::VtkWriter vtk_writer(mesh_p, filename.str());
    // need the newest pointer
    auto mds = lf::mesh::utils::make_CodimMeshDataSet<double>(mesh_p, 2);
    for (auto *node : mesh_p->Entities(2)) {
      mds->operator()(*node) = sol_vec(dofh.GlobalDofIndices(*node)[0]);
    }
    vtk_writer.WritePointData("solution_data", *mds);
  }

  // Print to std output
  std::cout << " dof      l2         h1 " << std::endl;
  for (int i = 0; i < dof_a.size(); i++) {
    std::cout << std::setw(5) << dof_a.at(i) << "   " << std::setw(5)
              << l2_a.at(i) << "   " << std::setw(5) << h1_a.at(i) << std::endl;
  }
}


/*
 * This file is part of the alglib project.
 *
 * (c) Divyanshu Kakwani <divkakwani@gmail.com>
 *
 * For the full copyright and license information, please view the LICENSE file
 * that was distributed with this source code.
 */

#include <iostream>
#include <alglib/heap/binary_heap.h>

using namespace alglib::heap;
using std::cout;
using std::endl;

int main() {
    /* unkeyed binary heap test */
    unkeyed_binary_heap<int> h;
    h.insert(4);
    h.insert(2);
    h.insert(12);
    while (!h.empty()) {
        cout << h.get_min() << "\t";
        h.delete_min();
    }
    cout << endl;

    /* keyed binary_heap_test */
    keyed_binary_heap<int, int> kh;
    kh.insert(4, 20);
    kh.insert(5, 10);
    kh.insert(10, 30);

    while (!kh.empty()) {
        cout << kh.get_min_elt() << "\t";
        kh.delete_min();
    }
    cout << endl;
}


#include <boost/test/unit_test.hpp>

#include "math/tensor.h" // header to test
#include "tools/mapper.h"

using namespace biosim;

BOOST_AUTO_TEST_SUITE(suite_tensor)

BOOST_AUTO_TEST_CASE(tensor_rank0) {
  double d(3.14);
  math::tensor<double> dbl_tensor({});
  BOOST_CHECK(dbl_tensor.get_rank() == 0);
  BOOST_CHECK(dbl_tensor.get_size(0) == 0);
  dbl_tensor({}) = d;
  BOOST_CHECK(dbl_tensor({}) == d);
  BOOST_REQUIRE_THROW(dbl_tensor({5}), std::out_of_range);

  float f(2.17);
  math::tensor<float> flt_tensor({});
  BOOST_CHECK(flt_tensor.get_rank() == 0);
  BOOST_CHECK(flt_tensor.get_size(0) == 0);
  flt_tensor({}) = f;
  BOOST_CHECK(flt_tensor({}) == f);
  BOOST_REQUIRE_THROW(flt_tensor({5}), std::out_of_range);

  int i(-10);
  math::tensor<int> int_tensor({});
  BOOST_CHECK(int_tensor.get_rank() == 0);
  BOOST_CHECK(int_tensor.get_size(0) == 0);
  int_tensor({}) = i;
  BOOST_CHECK(int_tensor({}) == i);
  BOOST_REQUIRE_THROW(int_tensor({5}), std::out_of_range);

  size_t s(25);
  math::tensor<size_t> szt_tensor({});
  BOOST_CHECK(szt_tensor.get_rank() == 0);
  BOOST_CHECK(szt_tensor.get_size(0) == 0);
  szt_tensor({}) = s;
  BOOST_CHECK(szt_tensor({}) == s);
  BOOST_REQUIRE_THROW(szt_tensor({5}), std::out_of_range);
}

BOOST_AUTO_TEST_CASE(tensor_rank1) {
  math::tensor<double> t({2});
  BOOST_CHECK(t.get_rank() == 1);
  BOOST_CHECK(t.get_size(0) == 2);
  t({0}) = 10;
  BOOST_CHECK(t({0}) == 10);
  t({1}) = 11;
  BOOST_CHECK(t({1}) == 11);
  BOOST_REQUIRE_THROW(t({2}), std::out_of_range);
  BOOST_REQUIRE_THROW(t({1, 5}), std::out_of_range);
  BOOST_REQUIRE_THROW(t({}), std::out_of_range);
}

BOOST_AUTO_TEST_CASE(tensor_rank2) {
  math::tensor<size_t> t({3, 2});
  BOOST_CHECK(t.get_rank() == 2);
  BOOST_CHECK(t.get_size(0) == 3);
  BOOST_CHECK(t.get_size(1) == 2);
  BOOST_REQUIRE_THROW(t({0, 3}), std::out_of_range);

  size_t input(10);
  for(size_t pos1(0); pos1 < t.get_size(1); ++pos1) {
    for(size_t pos0(0); pos0 < t.get_size(0); ++pos0, ++input) {
      t({pos0, pos1}) = input;
      BOOST_CHECK(t({pos0, pos1}) == input);
    } // for
  } // for
  BOOST_CHECK(t({2, 0}) == 12); // last value of first row
  BOOST_CHECK(t({0, 1}) == 13); // first value of second row

  t = math::tensor<size_t>({1, 1});
  BOOST_CHECK(t.get_rank() == 2);
  BOOST_CHECK(t.get_size(0) == 1);
  BOOST_CHECK(t.get_size(1) == 1);
  BOOST_REQUIRE_THROW(t({0, 1}), std::out_of_range);

  input = 10;
  for(size_t pos1(0); pos1 < t.get_size(1); ++pos1) {
    for(size_t pos0(0); pos0 < t.get_size(0); ++pos0, ++input) {
      t({pos0, pos1}) = input;
      BOOST_CHECK(t({pos0, pos1}) == input);
    } // for
  } // for
}

BOOST_AUTO_TEST_CASE(tensor_rank3) {
  math::tensor<size_t> t({4, 3, 2});
  BOOST_CHECK(t.get_rank() == 3);
  BOOST_CHECK(t.get_size(0) == 4);
  BOOST_CHECK(t.get_size(1) == 3);
  BOOST_CHECK(t.get_size(2) == 2);

  size_t input(10);
  for(size_t pos2(0); pos2 < t.get_size(2); ++pos2) {
    for(size_t pos1(0); pos1 < t.get_size(1); ++pos1) {
      for(size_t pos0(0); pos0 < t.get_size(0); ++pos0, ++input) {
        t({pos0, pos1, pos2}) = input;
        BOOST_CHECK(t({pos0, pos1, pos2}) == input);
      } // for
    } // for
  } // for
}

BOOST_AUTO_TEST_CASE(tensor_rank5) {
  math::tensor<size_t> t({2, 2, 2, 3, 2});
  size_t input(10);
  tools::mapper<std::vector<size_t>> m(t.get_mapper_alphabets());
  for(size_t i(m.get_min()), i_max(m.get_max()); i <= i_max; ++i, ++input) {
    std::vector<size_t> pos(m.encode(i));
    t(pos) = input;
    BOOST_CHECK(t(pos) == input);
  } // for
}

BOOST_AUTO_TEST_CASE(tensor_sub) {
  math::tensor<size_t> t({3, 3, 3});
  size_t input(10);
  for(size_t pos2(0); pos2 < t.get_size(2); ++pos2) {
    for(size_t pos1(0); pos1 < t.get_size(1); ++pos1) {
      for(size_t pos0(0); pos0 < t.get_size(0); ++pos0, ++input) {
        t({pos0, pos1, pos2}) = input;
        BOOST_CHECK(t({pos0, pos1, pos2}) == input);
      } // for
    } // for
  } // for

  math::tensor<size_t> subt(t.sub({}, {0, 0, 0}));
  BOOST_CHECK(subt.get_rank() == 0);
  BOOST_CHECK(subt({}) == 10);
  BOOST_REQUIRE_THROW(t.sub({}, {0, 1, 3}), std::out_of_range);
  BOOST_REQUIRE_THROW(t.sub({}, {0, 1, 2, 4}), std::out_of_range);

  subt = t.sub({2}, {2, 1});
  BOOST_CHECK(subt.get_rank() == 1);
  BOOST_CHECK(subt.get_size(0) == 3);
  BOOST_CHECK(subt({0}) == 15);
  BOOST_CHECK(subt({1}) == 24);
  BOOST_CHECK(subt({2}) == 33);
  subt = t.sub({1}, {2, 1});
  BOOST_CHECK(subt.get_rank() == 1);
  BOOST_CHECK(subt.get_size(0) == 3);
  BOOST_CHECK(subt({0}) == 21);
  BOOST_CHECK(subt({1}) == 24);
  BOOST_CHECK(subt({2}) == 27);
  subt = t.sub({0}, {2, 1});
  BOOST_CHECK(subt.get_rank() == 1);
  BOOST_CHECK(subt.get_size(0) == 3);
  BOOST_CHECK(subt({0}) == 25);
  BOOST_CHECK(subt({1}) == 26);
  BOOST_CHECK(subt({2}) == 27);

  subt = t.sub({1, 2}, {0});
  BOOST_CHECK(subt.get_rank() == 2);
  BOOST_CHECK(subt.get_size(0) == 3);
  BOOST_CHECK(subt.get_size(1) == 3);
  BOOST_CHECK(subt({0, 0}) == 10);
  BOOST_CHECK(subt({1, 0}) == 13);
  BOOST_CHECK(subt({2, 0}) == 16);
  BOOST_CHECK(subt({0, 1}) == 19);
  BOOST_CHECK(subt({0, 2}) == 28);
  BOOST_CHECK(subt({2, 2}) == 34);
  subt = t.sub({0, 2}, {0});
  BOOST_CHECK(subt.get_rank() == 2);
  BOOST_CHECK(subt.get_size(0) == 3);
  BOOST_CHECK(subt.get_size(1) == 3);
  BOOST_CHECK(subt({0, 0}) == 10);
  BOOST_CHECK(subt({1, 0}) == 11);
  BOOST_CHECK(subt({2, 0}) == 12);
  BOOST_CHECK(subt({0, 1}) == 19);
  BOOST_CHECK(subt({0, 2}) == 28);
  BOOST_CHECK(subt({2, 2}) == 30);
  subt = t.sub({0, 1}, {0});
  BOOST_CHECK(subt.get_rank() == 2);
  BOOST_CHECK(subt.get_size(0) == 3);
  BOOST_CHECK(subt.get_size(1) == 3);
  BOOST_CHECK(subt({0, 0}) == 10);
  BOOST_CHECK(subt({1, 0}) == 11);
  BOOST_CHECK(subt({2, 0}) == 12);
  BOOST_CHECK(subt({0, 1}) == 13);
  BOOST_CHECK(subt({0, 2}) == 16);
  BOOST_CHECK(subt({2, 2}) == 18);
  subt = t.sub({0, 1, 2}, {});
  BOOST_CHECK(subt.get_rank() == 3);
  BOOST_CHECK(subt.get_size(0) == 3);
  BOOST_CHECK(subt.get_size(1) == 3);
  BOOST_CHECK(subt.get_size(2) == 3);
  for(size_t pos2(0); pos2 < t.get_size(2); ++pos2) {
    for(size_t pos1(0); pos1 < t.get_size(1); ++pos1) {
      for(size_t pos0(0); pos0 < t.get_size(0); ++pos0, ++input) {
        BOOST_CHECK(t({pos0, pos1, pos2}) == subt({pos0, pos1, pos2}));
      } // for
    } // for
  } // for
  subt = t.sub({2, 1, 0}, {});
  BOOST_CHECK(subt.get_rank() == 3);
  BOOST_CHECK(subt.get_size(0) == 3);
  BOOST_CHECK(subt.get_size(1) == 3);
  BOOST_CHECK(subt.get_size(2) == 3);
  for(size_t pos2(0); pos2 < t.get_size(2); ++pos2) {
    for(size_t pos1(0); pos1 < t.get_size(1); ++pos1) {
      for(size_t pos0(0); pos0 < t.get_size(0); ++pos0, ++input) {
        BOOST_CHECK(t({pos0, pos1, pos2}) == subt({pos0, pos1, pos2}));
      } // for
    } // for
  } // for

  t = math::tensor<size_t>({2, 3, 4});
  subt = t.sub({0, 1, 2}, {});
  BOOST_CHECK(subt.get_rank() == 3);
  BOOST_CHECK(subt.get_size(0) == 2);
  BOOST_CHECK(subt.get_size(1) == 3);
  BOOST_CHECK(subt.get_size(2) == 4);
  for(size_t pos2(0); pos2 < t.get_size(2); ++pos2) {
    for(size_t pos1(0); pos1 < t.get_size(1); ++pos1) {
      for(size_t pos0(0); pos0 < t.get_size(0); ++pos0, ++input) {
        BOOST_CHECK(t({pos0, pos1, pos2}) == subt({pos0, pos1, pos2}));
      } // for
    } // for
  } // for
}

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MODULE SolutionTest

#include "solution.hpp"

//#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(SolutionSuite)

BOOST_AUTO_TEST_CASE(PlainTest1)
{
    vector<string> line = {"2", "1", "+", "3", "*"};
    int result = Solution().evalRPN(line);

    int expected = 9;

    BOOST_REQUIRE_EQUAL(result, expected);
}

BOOST_AUTO_TEST_CASE(PlainTest2)
{
    vector<string> line = {"4", "13", "5", "/", "+"};
    int result = Solution().evalRPN(line);

    int expected = 6;

    BOOST_REQUIRE_EQUAL(result, expected);
}

BOOST_AUTO_TEST_CASE(PlainTest3)
{
    vector<string> line = {"10", "6", "9", "3", "+", "-11", "*", "/", "*", "17", "+", "5", "+"};
    int result = Solution().evalRPN(line);

    int expected = 22;

    BOOST_REQUIRE_EQUAL(result, expected);
}

BOOST_AUTO_TEST_SUITE_END()


#include <Eigen/Core>
#include <igl/opengl/glfw/Viewer.h>
#include <igl/opengl/glfw/imgui/ImGuiMenu.h>

int main(int argc, char* argv[])
{
	Eigen::MatrixXd V(3, 3);
	V <<
		0, 0, 0,
		1, 0, 0,
		1, 1, 0,
		0, 1, 0;

	Eigen::MatrixXi F(2, 3);
	F <<
		0, 1, 2,
		1, 2, 3;

	igl::opengl::glfw::Viewer viewer;
	viewer.data().set_mesh(V, F);

	return 0;
}


/**
 * @file carDynamicsSim.hpp
 * @author Ezra Tal
 * @brief Car dynamics simulator class header file
 *
 */

#ifndef CARDYNAMICSSIM_H
#define CARDYNAMICSSIM_H

#include <random>
#include <Eigen/Dense>
#include <Eigen/Geometry>

/**
 * @brief Car dynamics simulator class
 *
 */
class CarDynamicsSim{
    public:
        CarDynamicsSim(
            const double maxSteeringAngle,
            const double minSpeed,
            const double maxSpeed,
            const Eigen::Vector2d & velProcNoiseAutoCorr);

        void setNoiseSeed(
            const unsigned seed);

        void setVehicleProperties(
            const double maxSteeringAngle,
            const double minSpeed,
            const double maxSpeed,
            const Eigen::Vector2d & velProcNoiseAutoCorr);

        void setVehicleState(
            const Eigen::Vector2d & position,
            const Eigen::Vector2d & velocity,
            const Eigen::Rotation2Dd & heading);

        void getVehicleState(
            Eigen::Vector2d & position,
            Eigen::Vector2d & velocity,
            Eigen::Rotation2Dd & heading);

        void setVehiclePosition(const Eigen::Vector2d & position);
        void setVehicleVelocity(const Eigen::Vector2d & velocity);
        void setVehicleHeading(const Eigen::Rotation2Dd & heading);
        Eigen::Vector2d getVehiclePosition(void);
        Eigen::Vector2d getVehicleVelocity(void);
        Eigen::Rotation2Dd getVehicleHeading(void);

        void proceedState_ExplicitEuler(
            const double dt_secs,
            const double speed,
            const double steeringAngle);

    private:
        /// @name Vehicle properties
        //@{
        double maxSteeringAngle_;
        double minSpeed_;
        double maxSpeed_;
        Eigen::Vector2d velocityProcessNoiseAutoCorrelation_;
        //@}

        /// @name Vehicle state
        //@{
        Eigen::Vector2d position_;
        Eigen::Vector2d velocity_;
        Eigen::Rotation2Dd heading_;
        //@}

        /// @name Std normal RNG
        //@{
        std::default_random_engine randomNumberGenerator_;
        std::normal_distribution<double> standardNormalDistribution_ = std::normal_distribution<double>(0.0,1.0);
        //@}

        Eigen::Vector2d getPositionDerivative(void);
        double getHeadingDerivative(const double steeringAngle);
};

#endif // CARDYNAMICSSIM_H


//   Boost pow.hpp header file
//   Computes a power with exponent known at compile-time

//  (C) Copyright Bruno Lalande 2008.
//  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)

//  See http://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_MATH_POW_HPP
#define BOOST_MATH_POW_HPP

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/policies/policy.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/tools/promotion.hpp>
#include <boost/mpl/greater_equal.hpp>

namespace boost {
namespace math {

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4702) // Unreachable code, only triggered in release mode and /W4
#endif

namespace detail {

template <int N, int M = N%2>
struct positive_power
{
    template <typename T>
    static T result(T base)
    {
        T power = positive_power<N/2>::result(base);
        return power * power;
    }
};

template <int N>
struct positive_power<N, 1>
{
    template <typename T>
    static T result(T base)
    {
        T power = positive_power<N/2>::result(base);
        return base * power * power;
    }
};

template <>
struct positive_power<1, 1>
{
    template <typename T>
    static T result(T base){ return base; }
};

template <int N, bool>
struct power_if_positive
{
    template <typename T, class Policy>
    static T result(T base, const Policy&)
    { return positive_power<N>::result(base); }
};

template <int N>
struct power_if_positive<N, false>
{
    template <typename T, class Policy>
    static T result(T base, const Policy& policy)
    {
        if (base == 0)
        {
            return policies::raise_overflow_error<T>(
                       "boost::math::pow(%1%)",
                       "Attempted to compute a negative power of 0",
                       policy
                   );
        }

        return T(1) / positive_power<-N>::result(base);
    }
};

template <>
struct power_if_positive<0, true>
{
    template <typename T, class Policy>
    static T result(T base, const Policy& policy)
    {
        if (base == 0)
        {
            return policies::raise_indeterminate_result_error<T>(
                       "boost::math::pow(%1%)",
                       "The result of pow<0>(%1%) is undetermined",
                       base,
                       T(1),
                       policy
                   );
        }

        return T(1);
    }
};

template <int N>
struct select_power_if_positive
{
    typedef typename mpl::greater_equal<
                         boost::integral_constant<int, N>,
                         boost::integral_constant<int, 0>
                     >::type is_positive;

    typedef power_if_positive<N, is_positive::value> type;
};

}  // namespace detail

template <int N, typename T, class Policy>
inline typename tools::promote_args<T>::type pow(T base, const Policy& policy)
{
   typedef typename tools::promote_args<T>::type result_type;
   return detail::select_power_if_positive<N>::type::result(static_cast<result_type>(base), policy);
}

template <int N, typename T>
inline typename tools::promote_args<T>::type pow(T base)
{ return pow<N>(base, policies::policy<>()); }

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

}  // namespace math
}  // namespace boost

#endif


#ifndef STAN_MATH_PRIM_SCAL_PROB_INV_GAMMA_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_INV_GAMMA_RNG_HPP

#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_greater_or_equal.hpp>
#include <stan/math/prim/scal/err/check_less_or_equal.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/fun/lgamma.hpp>
#include <stan/math/prim/scal/fun/gamma_q.hpp>
#include <stan/math/prim/scal/fun/digamma.hpp>
#include <stan/math/prim/scal/meta/length.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>
#include <stan/math/prim/scal/meta/return_type.hpp>
#include <stan/math/prim/scal/fun/grad_reg_inc_gamma.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
  namespace math {

    template <class RNG>
    inline double
    inv_gamma_rng(double alpha,
                  double beta,
                  RNG& rng) {
      using boost::variate_generator;
      using boost::random::gamma_distribution;

      static const char* function("inv_gamma_rng");

      check_positive_finite(function, "Shape parameter", alpha);
      check_positive_finite(function, "Scale parameter", beta);

      variate_generator<RNG&, gamma_distribution<> >
        gamma_rng(rng, gamma_distribution<>(alpha, 1 / beta));
      return 1 / gamma_rng();
    }

  }
}
#endif


#include <boost/functional/hash.hpp>
#include <unordered_map>
#include <memory>
#include "MPIProvider.hpp"
#include "BayesOptVQEBackend.hpp"
#include <boost/math/constants/constants.hpp>

namespace xacc {
namespace vqe {

const VQETaskResult BayesOptVQEBackend::minimize(Eigen::VectorXd parameters) {

	VQETaskResult r;
	const int dim = parameters.size();
	auto pi = boost::math::constants::pi<double>();
	auto computeTask = std::make_shared<ComputeEnergyVQETask>(program);
	bayesopt::Parameters par;
	par = initialize_parameters_to_default();
	par.n_iterations = xacc::optionExists("bo-n-iter") ? std::stoi(xacc::getOption("bo-n-iter")) : par.n_iterations;
	par.noise = xacc::optionExists("bo-noise") ? std::stod(xacc::getOption("bo-noise")) : par.noise;
	par.l_type = xacc::optionExists("bo-learn-type") ? str2learn(xacc::getOption("bo-learn-type").c_str()) : par.l_type;
	par.n_init_samples = xacc::optionExists("bo-n-init-iter") ? std::stoi(xacc::getOption("bo-n-init-iter")) : par.n_init_samples;
	par.verbose_level = xacc::optionExists("bo-verbose-level") ? std::stoi(xacc::getOption("bo-verbose-level")) : par.verbose_level;
	par.epsilon = xacc::optionExists("bo-epsilon") ? std::stod(xacc::getOption("bo-epsilon")) : par.epsilon;
	par.crit_name = xacc::optionExists("bo-crit-name") ? std::string(xacc::getOption("bo-crit-name")) : par.crit_name;
	par.kernel.name = xacc::optionExists("bo-kernel-name") ? std::string(xacc::getOption("bo-kernel-name")) : par.kernel.name;
	par.sc_type = xacc::optionExists("bo-score-type") ? str2score(xacc::getOption("bo-score-type").c_str()) : par.sc_type;
	par.surr_name = xacc::optionExists("bo-surrogate-name") ? std::string(xacc::getOption("bo-surrogate-name")) : par.surr_name;
	par.mean.name = xacc::optionExists("bo-mean-name") ? std::string(xacc::getOption("bo-mean-name")) : par.mean.name;
	//par.kernel.hp_mean = xacc::optionExists("bo-kernel-mean") ? std::stod(xacc::getOption("bo-kernel-mean")) : par.kernel.hp_mean;
	//par.kernel.hp_std = xacc::optionExists("bo-kernel-std") ? std::stod(xacc::getOption("bo-kernel-std")) : par.kernel.hp_std;
	VQEBayesOptFunction f(par, computeTask, dim);

	// Map parameters to boost vector

	boost::numeric::ublas::vector<double> result(dim);
	double* p = parameters.data();
	std::copy(result.begin(), result.end(), p);

	boost::numeric::ublas::vector<double> lowerBound(dim);
	boost::numeric::ublas::vector<double> upperBound(dim);
	for (int i = 0; i < dim; i++) {lowerBound[i] = -1*pi;upperBound[i] = pi;}

	f.setBoundingBox(lowerBound,upperBound);

	f.optimize(result);

	r.energy = f.getValueAtMinimum();
	auto resultAngles = f.getFinalResult();
	const double * data = &(resultAngles.data()[0]);
  r.angles = Eigen::Map<const Eigen::VectorXd>(data, parameters.size());

	return r;

}

}
}


#include <cstdlib>
#include <iostream>
#include <complex>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <Eigen/Core>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/eigen/vector.hpp>
#include <boost/numeric/bindings/eigen/matrix.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include "print.hpp"
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

int main(int argc, char *argv[]) {
  {
    typedef ublas::vector<double> vector;
    typedef ublas::matrix<double, ublas::column_major> matrix;
    typedef vector::size_type size_type;
    rand_normal<double>::reset();
    size_type m=6, n=8;
    matrix A(m, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<m; ++i)
  	A(i, j)=rand_normal<double>::get();
    vector x(m);
    for (size_type i=0; i<m; ++i)
      x(i)=rand_normal<double>::get();
    vector y(n);
    for (size_type i=0; i<n; ++i)
      y(i)=rand_normal<double>::get();
    double alpha(rand_normal<double>::get());
    matrix A1(alpha*ublas::outer_prod(x, y)+A);
    matrix A2(A);
    blas::gerh(alpha, x, y, A2);
    std::cout << print_mat(A1) << '\n'
  	      << print_mat(A2) << '\n';
  }
  {
    typedef std::complex<double> complex;
    typedef ublas::vector<complex> vector;
    typedef ublas::matrix<complex, ublas::column_major> matrix;
    typedef vector::size_type size_type;
    rand_normal<complex>::reset();
    size_type m=6, n=8;
    matrix A(m, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<m; ++i)
 	A(i, j)=rand_normal<complex>::get();
    vector x(m);
    for (size_type i=0; i<m; ++i)
      x(i)=rand_normal<complex>::get();
    vector y(n);
    for (size_type i=0; i<n; ++i)
      y(i)=rand_normal<complex>::get();
    complex alpha(rand_normal<complex>::get());
    matrix A1(alpha*ublas::outer_prod(x, ublas::conj(y))+A);
    matrix A2(A);
    blas::gerh(alpha, x, y, A2);
    std::cout << print_mat(A1) << '\n'
	      << print_mat(A2) << '\n';
  }
  return EXIT_SUCCESS;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2014, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_math_sqrt
#endif

#include <cmath>
#include <iostream>

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

#include <boost/config.hpp>
#include <boost/type_traits/is_fundamental.hpp>

#include "number_types.hpp"

// important: the include above must precede the include below,
// otherwise the test will fail for the custom number type:
// custom_with_global_sqrt

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/algorithms/not_implemented.hpp>

#ifdef HAVE_TTMATH
#  include <boost/geometry/extensions/contrib/ttmath_stub.hpp>
#endif

namespace bg = boost::geometry;

// call BOOST_CHECK
template <typename Argument, bool IsFundamental /* true */>
struct check
{
    template <typename Result>
    static inline void apply(Argument const& arg, Result const& result)
    {
        BOOST_CHECK_CLOSE(static_cast<double>(bg::math::sqrt(arg)),
                          static_cast<double>(result),
                          0.00001);
    }
};

template <typename Argument>
struct check<Argument, false>
{
    template <typename Result>
    static inline void apply(Argument const& arg, Result const& result)
    {
        Result const tol(0.00001);
        BOOST_CHECK( bg::math::abs(bg::math::sqrt(arg) - result) < tol );
    }
};

// test sqrt return type and value
template
<
    typename Argument,
    typename ExpectedResult,
    typename Result = typename bg::math::detail::square_root
        <
            Argument
        >::return_type,
    bool IsFundamental = boost::is_fundamental<Argument>::value
>
struct check_sqrt
    : bg::not_implemented<Argument, Result, ExpectedResult>
{};

template <typename Argument, typename Result, bool IsFundamental>
struct check_sqrt<Argument, Result, Result, IsFundamental>
{
    static inline void apply(Argument const& arg, Result const& result)
    {
#ifdef BOOST_GEOMETRY_TEST_DEBUG
        std::cout << "testing: " << typeid(Result).name()
                  << " sqrt(" << typeid(Argument).name()
                  << ")" << std::endl;
#endif
        check<Argument, IsFundamental>::apply(arg, result);
    }
};

// test cases
BOOST_AUTO_TEST_CASE( test_math_sqrt_fundamental )
{
    static const double sqrt2 = std::sqrt(2.0);
    static const long double sqrt2L = std::sqrt(2.0L);
    static const float sqrt2F = std::sqrt(2.0F);

    check_sqrt<float, float>::apply(2.0F, sqrt2F);
    check_sqrt<double, double>::apply(2.0, sqrt2);
    check_sqrt<long double, long double>::apply(2.0L, sqrt2L);

    check_sqrt<char, double>::apply(2, sqrt2);
    check_sqrt<signed char, double>::apply(2, sqrt2);
    check_sqrt<short, double>::apply(2, sqrt2);
    check_sqrt<int, double>::apply(2, sqrt2);
    check_sqrt<long, double>::apply(2L, sqrt2);
#if !defined(BOOST_NO_LONG_LONG)
    check_sqrt<long long, double>::apply(2LL, sqrt2);
#endif
#ifdef BOOST_HAS_LONG_LONG
    check_sqrt
        <
            boost::long_long_type, double
        >::apply(boost::long_long_type(2), sqrt2);
#endif
}

BOOST_AUTO_TEST_CASE( test_math_sqrt_custom )
{
    typedef number_types::custom<double> custom1;
    typedef custom_global<double> custom2;
    typedef number_types::custom_with_global_sqrt<double> custom3;

    static const double sqrt2 = std::sqrt(2.0);

    check_sqrt<custom1, custom1>::apply(custom1(2.0), custom1(sqrt2));
    check_sqrt<custom2, custom2>::apply(custom2(2.0), custom2(sqrt2));
    check_sqrt<custom3, custom3>::apply(custom3(2.0), custom3(sqrt2));

#ifdef HAVE_TTMATH
    typedef ttmath_big custom4;
    typedef ttmath::Big<1, 4> custom5;

    check_sqrt<custom4, custom4>::apply(custom4(2.0), custom4(sqrt2));
    check_sqrt<custom5, custom5>::apply(custom5(2.0), custom5(sqrt2));
#endif
}


/*******
edit_distance: STL and Boost compatible edit distance functions for C++

Copyright (c) 2013 Erik Erlandson

Author:  Erik Erlandson <erikerlandson@yahoo.com>

Distributed under the Boost Software License, Version 1.0.
See accompanying file LICENSE or copy at
http://www.boost.org/LICENSE_1_0.txt
*******/

#include <ctype.h>

#include "edit_distance_common.hpp"

// get the edit_alignment() function
#include <boost/algorithm/sequence/edit_distance.hpp>
using boost::algorithm::sequence::edit_distance;
using boost::algorithm::sequence::unit_cost;
using namespace boost::algorithm::sequence::parameter;

// define a custom cost function where case changes cost less
struct cost_case_less {
    typedef float cost_type;     // edit costs may be fractional

    cost_type insertion(char c) const { return 1; }
    cost_type deletion(char c) const { return 1; }

    // changes in case cost less than other edit operations:
    cost_type substitution(char c, char d) const {
        if (toupper(c) == toupper(d)) return 0.5;
        return 1;
    }
};

int main(int argc, char** argv) {
    char const* str1 = "Try to find XXX capitalized";
    char const* str2 = "xxx";

    // Match the substring 'xxx' against the larger string, with cheap case changes,
    // identifies the correct location of 'XXX' in the larger string
    stringstream_tuple_output<cost_case_less, char const*> out;
    float dist = edit_distance(str1, str2, _script = out, _cost = cost_case_less(), _substitution=true);
    std::cout << "dist= " << dist << "   edit operations=  \"" << out.ss.str() << "\"\n";

    // compare to the behavior with the default cost function:
    out.ss.str("");
    dist = edit_distance(str1, str2, _script = out, _substitution=true);
    std::cout << "dist= " << dist << "   edit operations=  \"" << out.ss.str() << "\"\n";

    return 0;
}


#pragma once

#include <polyvec/core/macros.hpp>
#include <polyvec/core/types.hpp>
#include <polyvec/utils/num.hpp>
#include <polyvec/geometry/path.hpp>
#include <polyvec/utils/matrix.hpp>

#include <Eigen/Geometry>

#define PF_RAD(deg) ((double)deg * M_PI / 180.)
#define PF_DEG(rad) ((double)rad * 180. / M_PI)
#define PF_ISNAN(v) (!((v) == (v)))

NAMESPACE_BEGIN(polyfit)
NAMESPACE_BEGIN(AngleUtils)

/*
	Returns true if both points are in the same quadrant. The test is inclusive
	if any of the two points lie exactly along one of the axis.
*/
PV_INLINE bool lie_in_the_same_quadrant(
	const vec2& p0,
	const vec2& p1
);

PV_INLINE double deviation_from_horizontal(const vec2& d);
PV_INLINE double deviation_from_horizontal(double angle);

PV_INLINE double deviation_from_vertical(const vec2& d);
PV_INLINE double deviation_from_vertical(double angle);

PV_INLINE double spanned_shortest(const vec2 & p0, const vec2 & p1, const vec2 & p2);

PV_INLINE double spanned_shortest_between(const vec2 & d0, const vec2 & d1);

PV_INLINE double spanned_clockwise_between(const vec2& d0, const vec2& d1);

PV_INLINE bool have_opposite_convexity(const vec2 & p0, const vec2 & p1, const vec2 & p2, const vec2 & p3);

PV_INLINE bool have_opposite_convexity_with_tol(const vec2 & p0, const vec2 & p1, const vec2 & p2, const vec2 & p3);

PV_INLINE int count_inflections(const mat2x & points, bool circular = true);

PV_INLINE bool is_flat(const vec2 pp, const vec2 p, const vec2 pn);

PV_INLINE int convexity(const vec2 pp, const vec2 p, const vec2 pn, int orientation);

PV_INLINE int number_of_neighbors_with_different_convexity(const Eigen::Matrix2Xd& polygon, int corner);

// convexity should be writable for 1 + 2 * neighborhood_size elements
// the polygon should contain enough elements before and after the first/last neighboring corners to be able to compute the convexity
PV_INLINE void neighborhood_convexity(const Eigen::Matrix2Xd& P, const int corner, const int neighborhood_size, int* convexity);

/*
	Returns the number of edges in P which overlap 1-pixel axis-aligned steps in B.

	B is assumed to be the raster boundary which P is approximating, though there are
	no requirements on the points except that they should have the same ordering.
*/
PV_INLINE int count_visually_degenerate_edges(
	const mat2x& B,
	const vecXi& P,
	const bool circular = false
);

PV_INLINE int count_visually_inconsistent_edges(const mat2x & P, bool circular = false);

#include "angle.inl"

NAMESPACE_END(AngleUtils)
NAMESPACE_END(polyfit)


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

/**
 * \file arun.hpp
 * \brief Arun's method for computing the rotation between two point sets.
 */

#ifndef OPENGV_ARUN_HPP_
#define OPENGV_ARUN_HPP_

#include <stdlib.h>
#include <Eigen/Eigen>
#include <opengv/types.hpp>

/**
 * \brief The namespace of this library.
 */
namespace opengv
{
/**
 * \brief The namespace of the math tools.
 */
namespace math
{

/**
 * \brief Arun's method for computing the rotation between two point sets.
 *        Core function [13].
 *
 * \param[in] Hcross The summation over the exterior products between the
 *            normalized points.
 * \return The rotation matrix that aligns the points.
 */
rotation_t arun( const Eigen::MatrixXd & Hcross );

/**
 * \brief Arun's method for complete point cloud alignment [13]. The method
 *        actually does the same than threept_arun, but has a different
 *        interface.
 *
 * \param[in] p1 The points expressed in the first frame.
 * \param[in] p2 The points expressed in the second frame.
 * \return The Transformation from frame 2 to frame 1 (
 *         \f$ \mathbf{T} = \left(\begin{array}{cc} \mathbf{R} & \mathbf{t} \end{array}\right) \f$,
 *         with \f$ \mathbf{t} \f$ being the position of frame 2 seen from
 *         frame 1, and \f$ \mathbf{R} \f$ being the rotation from
 *         frame 2 to frame 1).
 */
transformation_t arun_complete( const points_t & p1, const points_t & p2 );

}
}

#endif /* OPENGV_ARUN_HPP_ */