#include <Engine/MeshEdit/CParameterize.h>
#include <Engine/MeshEdit/ARAP.h>
#include <Engine/Primitive/TriMesh.h>
#include <Engine/Scene/CmptGeometry.h>
#include <Engine/Scene/CmptMaterial.h>
#include <Engine/Material/BSDF_Frostbite.h>
#include <Eigen/SVD>

using namespace Ubpa;
using namespace Eigen;

ARAP::ARAP(Ptr<SObj> triMeshObj, Ptr<TriMesh> triMesh, bool is_tex)
	: Paramaterize(triMeshObj, triMesh, is_tex, 0, 1)/* ASAP(triMeshObj, triMesh, true)*/ // Circle boundary and Naive mode
{
	this->is_tex = is_tex;
	Init(triMeshObj, triMesh);
}

void ARAP::Clear()
{
	ARAP_coeff.clear();
	Lt_array.clear();
	triangle_points.clear();
}

bool ARAP::Init(Ptr<SObj> triMeshObj, Ptr<TriMesh> triMesh)
{
	ARAP_mat_A = SparseMatrix<double>(nV , nV);
	ARAP_mat_A.setZero();
	ARAP_mat_b = MatrixXd(nV, 2);
	ARAP_mat_b.setZero();
	//Initialize 1.map 3D triangles to 2d
	CongruentMapping2D();

	// Initialize 2. Select anchor points
	auto triangle = heMesh->Polygons().back();
	auto v1 = triangle->BoundaryVertice()[0];
	anchor_v1_idx = heMesh->Index(v1);
	auto v2 = triangle->BoundaryVertice()[1];
	anchor_v2_idx = heMesh->Index(v2);
	size_t tri_idx = heMesh->Index(triangle);
	//anchor_pos1 = pointf2(points2d[tri_idx][v1][0], points2d[tri_idx][v1][1]);
	//anchor_pos2 = pointf2(points2d[tri_idx][v2][0], points2d[tri_idx][v2][1]);
	anchor_pos2 = pointf2(1, 1);
	// Initialize 3. set coefficients of A
	setCoefficientA(anchor_v1_idx, anchor_v2_idx);

	//Initialize 4. get initial parameterization
	this->Run();

	return true;
}

bool ARAP::RunARAP(int iter_n, double error_threshold = 0.01, int debug=5)
{
	// set debug mode
	if (debug < 50) this->is_debug = false;
	else this->is_debug = true;

	// do local/global iteration
	cout <<"iter:" << iter_n << endl;

	for (int i = 0; i < iter_n; i++)
	{
		localupdate();
		double max_error = globalupdate();
		cout << "iter:" << i << " error: " << max_error << endl;
		if (max_error < error_threshold) break;
	}

	// Finally, half-edge structure -> triangle mesh, end.
	vector<pointf3> positions;
	vector<unsigned> indice;
	positions.reserve(nV);
	indice.reserve(3 * nT);
	for (auto v : heMesh->Vertices())
		positions.push_back(v->pos.cast_to<pointf3>());
	for (auto f : heMesh->Polygons()) { // f is triangle
		for (auto v : f->BoundaryVertice()) // vertices of the triangle
			indice.push_back(static_cast<unsigned>(heMesh->Index(v)));
	}

	if (this->is_tex)
	{
		this->triMesh->Update(texCoor);
	}
	else
	{
		this->triMesh->Update(positions);
	}
	return true;
}

void ARAP::localupdate()
{
	getTrianglePoints();  // get the newest iteration result from trangle_points
	getLt();
}

double ARAP::globalupdate()
{
	// first, set b
	setb(anchor_v1_idx, anchor_pos1, anchor_v2_idx, anchor_pos2);
	double max_error = -1 ;
	// Then solve the equation
	ARAP_solution = ARAP_solver.solve(ARAP_mat_A.transpose() * ARAP_mat_b);

	if (is_debug) {
		//cout << MatrixXd(ARAP_mat_A) << endl << endl; // DEBUG
		//cout << "b:" << endl << ARAP_mat_b << endl << endl;
		//cout << "solu:" << endl << ARAP_solution << endl << endl;
	}

	// End, update textCoordinate
	this->texCoor.clear();
	for (size_t i = 0; i < nV; i++)
	{
		double dist = pointf3::distance(pointf3(ARAP_solution(i, 0), ARAP_solution(i, 1), 0.0), heMesh->Vertices()[i]->pos.cast_to<pointf3>());
		if (max_error < 0)
		{
			max_error = dist;
		}
		else if(max_error < dist)
		{
			max_error = dist;
		}

		heMesh->Vertices()[i]->pos[0] = ARAP_solution(i, 0);
		heMesh->Vertices()[i]->pos[1] = ARAP_solution(i, 1);
		heMesh->Vertices()[i]->pos[2] = 0.0;
		// update tex coordinates
		texCoor.push_back(pointf2(ARAP_solution(i, 0), ARAP_solution(i, 1)));
	}

	return max_error;
}

/* Get newest u per iteration */
void ARAP::getTrianglePoints()
{
	triangle_points.clear();	// remember to clear
	for (auto triangle : heMesh->Polygons())
	{
		if(triangle != nullptr)
		{
			this->triangle_points.push_back(triangle->BoundaryVertice());
		}
	}
}

void ARAP::getLt()
{
	assert(triangle_points.size() == nT);
	Lt_array.clear();

	for (size_t t = 0; t < nT; t++)
	{
		auto vec_u = triangle_points[t];
		auto mapped_u = points2d[t];
		Matrix2d St;
		St.setZero();

		// get St
		for (int i = 0; i < 3; i++)
		{
			V* u0 = vec_u[i];
			V* u1 = vec_u[(i+1)%3]; // % ?
			MatrixXd delta_u(2, 1);
			delta_u <<
				u0->pos[0] - u1->pos[0], u0->pos[1] - u1->pos[1];

			pointf3 x0 = mapped_u[u0];
			pointf3 x1 = mapped_u[u1];
			MatrixXd delta_x(2, 1);
			delta_x <<
				x0[0] - x1[0], x0[1] - x1[1];

			double cot = getCotan(t, vec_u[(i + 2) % 3]);

			St += cot * delta_u * delta_x.transpose();
		}

		// Do SVD Composition on St
		JacobiSVD<MatrixXd> svd(St, ComputeThinU | ComputeThinV);

		Matrix2d Lt = svd.matrixU() * svd.matrixV().transpose(); // Lt = U * V^T
		/*cout << svd.matrixV().transpose() << endl << endl;*/ // DEBUG
		//if (Lt.determinant() < 0 )
		//{
		//	if (is_debug) {
		//		//cout << "before" << Lt.determinant() << endl;
		//	}
		//	Matrix2d newV;
		//	newV <<
		//		svd.matrixV().transpose()(0, 0), svd.matrixV().transpose()(0, 1),
		//		-(svd.matrixV().transpose()(1, 0)), -(svd.matrixV().transpose()(1, 1));
		//	if (is_debug) {
		//		//cout << newV << endl;
		//	}
		//	Lt = svd.matrixU() * newV;
		//	assert(Lt.determinant() > 0);
		//}

		if (is_debug)
		{
			cout << Lt.determinant() << endl;
			//cout << svd.singularValues() << endl << endl;
		}

		//cout << Lt << endl; // DEBUG

		//Lt << 1, 0, 0, 1; // DEBUG

		Lt_array.push_back(Lt);

	}
	assert(Lt_array.size() == nT);
}

void ARAP::setb(size_t idx1, pointf2 pos1, size_t idx2, pointf2 pos2)
{
	ARAP_mat_b.setZero();

	// two anchor points
	//ARAP_mat_b(idx1, 0) = pos1[0];  // x
	//ARAP_mat_b(idx1, 1) = pos1[1]; // y

	ARAP_mat_b(idx2, 0) = pos2[0];  // x
	ARAP_mat_b(idx2, 1) = pos2[1]; // y

	// Other non-anchor points
	for (size_t i = 0; i < nV; i++)
	{
		if (/*i != idx1 && */i != idx2) {
			auto v = heMesh->Vertices()[i];
			MatrixXd b(2, 1);
			b.setZero();
			//cout << "v idx: " << heMesh->Index(v) << endl<<endl;
			// traverse adjecent vertrices
			for (auto adj_v : v->AdjVertices())
			{
				size_t adj_idx = heMesh->Index(adj_v);  // get index of adj_v
				// get adjacent triangles
				auto e = v->EdgeWith(adj_v);
				auto he1 = e->HalfEdge();
				auto he2 = e->HalfEdge()->Pair();

				// get Lt and cot
				auto triangle1 = he1->Polygon();
				if (triangle1 != nullptr)
				{
					auto tri_v1 = he1->Next()->End();
					assert(heMesh->Index(tri_v1) != heMesh->Index(v)
						&& heMesh->Index(tri_v1) != heMesh->Index(adj_v));

					size_t tri_idx = heMesh->Index(triangle1);
					double cot1 = getCotan(tri_idx, tri_v1);
					map<V*, pointf3> mapped_v = this->points2d[tri_idx]; // congruent mapping of triangle1
					MatrixXd Lt = Lt_array[tri_idx];
					MatrixXd delta_x(2, 1);
					delta_x <<
						mapped_v[v][0] - mapped_v[adj_v][0],
						mapped_v[v][1] - mapped_v[adj_v][1];
					/*cout << "cot1:" << cot1 << endl;
					cout << "mapped_v[v]:" << mapped_v[v][0] <<"," << mapped_v[v][1] << endl;
					cout << "mapped_[adj_v]:" << mapped_v[adj_v][0] << "," << mapped_v[adj_v][1] << endl;
					cout << "cot1 * Lt * delta_x:" << cot1 * Lt * delta_x << endl;
					cout << endl;*/

					b += cot1 * Lt * delta_x;
				}

				auto triangle2 = he2->Polygon();
				if (triangle2 != nullptr)
				{
					auto tri_v2 = he2->Next()->End();
					assert(heMesh->Index(tri_v2) != heMesh->Index(v)
						&& heMesh->Index(tri_v2) != heMesh->Index(adj_v));

					size_t tri_idx = heMesh->Index(triangle2);
					double cot2 = getCotan(tri_idx, tri_v2);
					map<V*, pointf3> mapped_v = this->points2d[tri_idx]; // congruent mapping of triangle1
					MatrixXd Lt = Lt_array[tri_idx];
					MatrixXd delta_x(2, 1);
					delta_x <<
						mapped_v[v][0] - mapped_v[adj_v][0],
						mapped_v[v][1] - mapped_v[adj_v][1];

					/*cout << "cot2:" << cot2 << endl;
					cout << "mapped_v[v]:" << mapped_v[v][0] << "," << mapped_v[v][1] << endl;
					cout << "mapped_v[adj_v]:" << mapped_v[adj_v][0] << "," << mapped_v[adj_v][1] << endl;
					cout << "cot2 * Lt * delta_x:" << cot2 * Lt * delta_x << endl;
					cout << endl;*/
					b += cot2 * Lt * delta_x;
				}
			}
			ARAP_mat_b(i, 0) = b(0); // x
			ARAP_mat_b(i, 1) = b(1); // y
		}
	}
}

void ARAP::setCoefficientA(size_t idx1, size_t idx2)
{
	// Two Anchor points
	//ARAP_coeff.push_back(Eigen::Triplet<double>(idx1, idx1, 1));
	ARAP_coeff.push_back(Eigen::Triplet<double>(idx2, idx2, 1));

	// Other non-anchor points
	for (size_t i = 0; i < nV; i++)
	{
		if (/*i != idx1 &&*/ i != idx2) {
			auto v = heMesh->Vertices()[i];
			double cotan_sum = 0.0;

			// traverse adjacent vertrices
			for (auto adj_v : v->AdjVertices())
			{
				size_t adj_idx = heMesh->Index(adj_v);

				// To get cotan
				auto e = v->EdgeWith(adj_v);
				auto he1 = e->HalfEdge();

				double cot1 = 0.0;
				if (he1->Polygon() != nullptr)
				{
					auto tri1_idx = heMesh->Index(he1->Polygon()); // get index of adjacent triangle
					try
					{
						auto tri_v1 = he1->Next()->End(); // get vertix of adjacent triangle
						assert(heMesh->Index(tri_v1) != heMesh->Index(v)
							&& heMesh->Index(tri_v1) != heMesh->Index(adj_v));

						cot1 = getCotan(tri1_idx, tri_v1);
					}
					catch (const std::exception& e)
					{
						cout << "cannot find cot 1 in map. " << endl;
					}
				}
				auto he2 = e->HalfEdge()->Pair();

				double cot2 = 0.0;
				if (he2->Polygon() != nullptr)
				{
					auto tri2_idx = heMesh->Index(he2->Polygon());
					try
					{
						auto tri_v2 = he2->Next()->End();
						assert(heMesh->Index(tri_v2) != heMesh->Index(v)
							&& heMesh->Index(tri_v2) != heMesh->Index(adj_v));
						cot2 = getCotan(tri2_idx, tri_v2);
					}
					catch (const std::exception & e)
					{
						cout << "cannot find cot 2 in map. " << endl;
					}
				}

				ARAP_coeff.push_back(Eigen::Triplet<double>(i, adj_idx, -(cot1 + cot2)));

				cotan_sum += (cot1 + cot2);
			}

			ARAP_coeff.push_back(Eigen::Triplet<double>(i, i, cotan_sum));
		}
	}
	ARAP_mat_A.setFromTriplets(ARAP_coeff.begin(), ARAP_coeff.end());
	// pre-computation
	ARAP_mat_A.makeCompressed();
	ARAP_solver.compute(ARAP_mat_A.transpose() * ARAP_mat_A);
}


#include "stdafx.h"
#include <cmath>
#include <fmt/format.h>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include "contest_types.h"
#include "solver_registry.h"
#include "visual_editor.h"
#include "solver_util.h"
#include "judge.h"

namespace FitHoleSolver {

namespace bg = boost::geometry;
using BoostPoint = bg::model::d2::point_xy<double>;
using BoostPolygon = bg::model::polygon<BoostPoint>;
using BoostLinestring = bg::model::linestring<BoostPoint>;

template <typename T>
void shrink(T& point_a, T& point_b) {
  constexpr double eps = 1e-6;
  auto shrink_a = point_a * (1.0 - eps) + point_b * eps;
  auto shrink_b = point_b * (1.0 - eps) + point_a * eps;
  point_a = shrink_a;
  point_b = shrink_b;
}

template <typename T>
BoostPoint ToBoostPoint(const T& point) {
  const auto [x, y] = point;
  return BoostPoint(x, y);
}

template <typename T>
BoostPolygon ToBoostPolygon(const std::vector<T>& points) {
  BoostPolygon polygon;
  for (std::size_t i = 0; i <= points.size(); ++i) {
    polygon.outer().push_back(ToBoostPoint(points[i % points.size()]));
  }
  if (bg::area(polygon) < 0.0) {
    bg::reverse(polygon);
  }
  return polygon;
}

template <typename T, typename U>
double SquaredDistance(const T& vertex0, const U& vertex1) {
  const auto [x0, y0] = vertex0;
  const auto [x1, y1] = vertex1;
  return (x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1);
}

template <typename T>
double SquaredEdgeLength(const T& vertices, const Edge& edge) {
  const auto [a, b] = edge;
  return SquaredDistance(vertices[a], vertices[b]);
}

class Solver : public SolverBase {
 public:
  SolverOutputs solve(const SolverArguments& args) override {
    hole_ = args.problem->hole_polygon;
    vertices_ = args.problem->vertices;
    edges_ = args.problem->edges;
    epsilon_ = args.problem->epsilon;
    hole_polygon_ = ToBoostPolygon(hole_);

    SVisualEditorPtr editor;
    if (args.visualize) {
      editor = std::make_shared<SVisualEditor>(args.problem, "FitHoleSolver", "visualize");
    }

    const int N = vertices_.size();
    auto pose = vertices_;
    double cost = std::numeric_limits<double>::infinity();
    bool found_fit_in_hole = false;
    std::vector<Point> best_feasible_pose;

    SPinnedIndex pinned_index(rng_, N, editor);

    const int num_iters = 10000;
    const double T0 = 1.0e1;
    const double T1 = 1.0e-2;
    double progress = 0.0;

    auto evaluate_and_descide_rollback = [&]() -> bool {
      auto [feasible, updated_cost] = Evaluate(pose);

      auto res = judge(*args.problem, pose);
      if (res.fit_in_hole()) {
        found_fit_in_hole = true;
      }

      const double T = std::pow(T0, 1.0 - progress) * std::pow(T1, progress);
      if (std::uniform_real_distribution(0.0, 1.0)(rng_) < std::exp(-(updated_cost - cost) / T)) {
        cost = updated_cost;
        return false; // accepted
      } else {
        return true; // rejected
      }
    };
    evaluate_and_descide_rollback();

    auto single_small_change = [&] { // ynasu87 original
      const int v = pinned_index.sample_movable_index();
      const int dx = std::uniform_int_distribution(-5, 5)(rng_);
      const int dy = std::uniform_int_distribution(-5, 5)(rng_);
      auto& [x, y] = pose[v];
      x += dx;
      y += dy;
      if (evaluate_and_descide_rollback()) {
        x -= dx;
        y -= dy;
      }
    };

    auto shift = [&] {
      const int dx = std::uniform_int_distribution(-2, 2)(rng_);
      const int dy = std::uniform_int_distribution(-2, 2)(rng_);
      auto pose_bak = pose;
      for (int i : pinned_index.movable_indices) {
        pose[i].first += dx;
        pose[i].second += dy;
      }
      if (evaluate_and_descide_rollback()) {
        pose = pose_bak;
      }
    };

    using Action = std::function<void()>;
    std::vector<std::pair<double, Action>> action_probs = {
      {0.9, single_small_change},
      {0.01, shift},
    };
    {
      // normalize probs
      double p = 0.0;
      for (int i = 0; i < action_probs.size(); ++i) { p += action_probs[i].first; }
      for (int i = 0; i < action_probs.size(); ++i) { action_probs[i].first /= p; }
    }

    for (int iter = 0; iter < num_iters; ++iter) {
      progress = 1.0 * iter / num_iters;

      if (found_fit_in_hole) {
        LOG(INFO) << fmt::format("found fit in hole iter = {}", iter);
        break;
      }

      if (iter < 100) {
        shift();
      } else {
        const double p_action = std::uniform_real_distribution(0.0, 1.0)(rng_);
        double p_accum = 0.0;
        for (int i = 0; i < action_probs.size(); ++i) {
          p_accum += action_probs[i].first;
          if (p_action < p_accum) {
            action_probs[i].second();
          }
        }
      }

      if (editor && iter % 100 == 0) {
        editor->set_oneshot_custom_stat(fmt::format("iter = {}/{}", iter, num_iters));
        editor->set_pose(args.problem->create_solution(pose));
        if (auto show_result = editor->show(1); show_result.edit_result) {
          pose = show_result.edit_result->pose_after_edit->vertices;
          pinned_index.update_movable_index();
        }
      }

    }

    SolverOutputs outputs;
    outputs.solution = args.problem->create_solution(pose);

    return outputs;
  }

  template <typename P>
  std::tuple<bool, double> Evaluate(const std::vector<P>& pose) const {
    double deformation_cost = 0.0;
    double protrusion_cost = 0.0;
    double dislikes_cost = 0.0;

    const double tolerance = epsilon_ / 1'000'000.0;
    for (const auto& vertex : pose) {
      protrusion_cost += bg::distance(ToBoostPoint(vertex), hole_polygon_);
    }
    for (const auto& edge : edges_) {
      const auto [a, b] = edge;
      Point2d pa = pose[a];
      Point2d pb = pose[b];
      shrink(pa, pb);
      BoostLinestring linestring{ToBoostPoint(pa), ToBoostPoint(pb)};
      std::vector<BoostLinestring> differences;
      bg::difference(linestring, hole_polygon_, differences);
      for (const auto& segment : differences) {
        protrusion_cost += 1.0e1 * bg::length(segment);
      }

      const auto d0 = SquaredEdgeLength(vertices_, edge);
      const auto d1 = SquaredEdgeLength(pose, edge);
      deformation_cost += 1.0e1 * std::max(0.0, std::abs(d1 / d0 - 1.0) - tolerance);
    }

    const bool feasible = deformation_cost + protrusion_cost == 0.0;
    const double cost = deformation_cost + protrusion_cost + dislikes_cost;

    return {feasible, cost};
  }

 private:
  std::mt19937 rng_;
  std::vector<Point> hole_;
  std::vector<Point> vertices_;
  std::vector<Edge> edges_;
  integer epsilon_;
  BoostPolygon hole_polygon_;
};

}

REGISTER_SOLVER("FitHoleSolver", FitHoleSolver::Solver);
// vim:ts=2 sw=2 sts=2 et ci


#include <boost/simd/include/functions/shuffle.hpp>
#include <boost/simd/sdk/simd/pack.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/mpl/int.hpp>
#include <iostream>

using boost::mpl::int_;
using boost::simd::pack;
using boost::simd::shuffle;

struct even_odd
{
  template<class Index, class Cardinal>
  struct apply  : int_< Index::value < Cardinal::value/2
                      ? Index::value * 2
                      : (Index::value-Cardinal::value/2) * 2 + 1
                      >
  {};
};

int main()
{
  pack<float,4> f(1,2,3,4);

  // Gather even and odd indexes on the side: [ 1 3 2 4 ]
  std::cout << shuffle<even_odd>(f) << "\n";
}


/*
 * This file is part of the Interpolated Polyline (https://github.com/fzi-forschungszentrum-informatik/P3IV),
 * copyright by FZI Forschungszentrum Informatik, licensed under the BSD-3 license (see LICENSE file in main directory)
 */

#pragma once

#include <chrono>
#include <boost/math/interpolators/barycentric_rational.hpp>

namespace p3iv_utils {

std::vector<double> chrono2double(const std::vector<std::chrono::milliseconds>& t) {
    std::vector<double> x;
    x.reserve(t.size());
    for (auto t_ : t) {
        x.push_back(t_.count());
    }
    return x;
}

template <typename T>
inline std::vector<T> interpolate(const std::vector<T>& xBase,
                                  const std::vector<T>& yBase,
                                  const std::vector<T>& xIntrp) {

    boost::math::barycentric_rational<T> interpolant(xBase.data(), yBase.data(), yBase.size());
    std::vector<T> yIntrp;
    yIntrp.reserve(xIntrp.size());

    for (auto& x : xIntrp) {
        yIntrp.push_back(interpolant(x));
    }

    return yIntrp;
}

template <typename T>
inline std::vector<T> interpolate(const std::vector<std::chrono::milliseconds>& tBase,
                                  const std::vector<T>& yBase,
                                  const std::vector<std::chrono::milliseconds>& tIntrp) {

    std::vector<double> xBase = chrono2double(tBase);
    std::vector<double> xIntrp = chrono2double(tIntrp);

    auto yIntrp = interpolate(xBase, yBase, xIntrp);

    return yIntrp;
}

} // namespace p3iv_utils


/*
 * EllipseIterator.hpp
 *
 *  Created on: Dec 2, 2015
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich, Autonomous Systems Lab
 */

#pragma once

#include "grid_map_core/GridMap.hpp"
#include "grid_map_core/iterators/SubmapIterator.hpp"

// Eigen
#include <Eigen/Core>

// unique_ptr
#include <memory>

namespace grid_map {

/*!
 * Iterator class to iterate through a ellipsoid area of the map.
 * The main axis of the ellipse are aligned with the map frame.
 */
class EllipseIterator
{
public:

  /*!
   * Constructor.
   * @param gridMap the grid map to iterate on.
   * @param center the position of the ellipse center.
   * @param length the length of the main axis.
   * @param angle the rotation angle of the ellipse (in [rad]).
   */
  EllipseIterator(const GridMap& gridMap, const Position& center, const Length& length, const double rotation = 0.0);

  /*!
   * Assignment operator.
   * @param iterator the iterator to copy data from.
   * @return a reference to *this.
   */
  EllipseIterator& operator =(const EllipseIterator& other);

  /*!
   * Compare to another iterator.
   * @return whether the current iterator points to a different address than the other one.
   */
  bool operator !=(const EllipseIterator& other) const;

  /*!
   * Dereference the iterator with const.
   * @return the value to which the iterator is pointing.
   */
  const Index& operator *() const;

  /*!
   * Increase the iterator to the next element.
   * @return a reference to the updated iterator.
   */
  EllipseIterator& operator ++();

  /*!
   * Indicates if iterator is past end.
   * @return true if iterator is out of scope, false if end has not been reached.
   */
  bool isPastEnd() const;

  /*!
   * Returns the size of the submap covered by the iterator.
   * @return the size of the submap covered by the iterator.
   */
  const Size& getSubmapSize() const;

private:

  /*!
   * Check if current index is inside the ellipse.
   * @return true if inside, false otherwise.
   */
  bool isInside() const;

  /*!
   * Finds the submap that fully contains the ellipse and returns the parameters.
   * @param[in] center the position of the ellipse center.
   * @param[in] length the length of the main axis.
   * @param[in] angle the rotation angle of the ellipse (in [rad]).
   * @param[out] startIndex the start index of the submap.
   * @param[out] bufferSize the buffer size of the submap.
   */
  void findSubmapParameters(const Position& center, const Length& length, const double rotation,
                            Index& startIndex, Size& bufferSize) const;

  //! Position of the circle center;
  Position center_;

  //! Square length of the semi axis.
  Eigen::Array2d semiAxisSquare_;

  //! Sine and cosine values of the rotation angle as transformation matrix.
  Eigen::Matrix2d transformMatrix_;

  //! Grid submap iterator. // TODO Think of using unique_ptr instead.
  std::shared_ptr<SubmapIterator> internalIterator_;

  //! Map information needed to get position from iterator.
  Length mapLength_;
  Position mapPosition_;
  double resolution_;
  Size bufferSize_;
  Index bufferStartIndex_;
};

} /* namespace */


/**
 * @file tests/ub_tree_test.cpp
 * @author Mikhail Lozhnikov
 *
 * Tests for the UB tree.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/core/tree/bounds.hpp>
#include <mlpack/methods/neighbor_search/neighbor_search.hpp>
#include <mlpack/core/tree/binary_space_tree.hpp>

#include <boost/test/unit_test.hpp>

using namespace mlpack;
using namespace mlpack::math;
using namespace mlpack::tree;
using namespace mlpack::metric;
using namespace mlpack::bound;
using namespace mlpack::neighbor;

BOOST_AUTO_TEST_SUITE(UBTreeTest);

BOOST_AUTO_TEST_CASE(AddressTest)
{
  typedef double ElemType;
  typedef typename std::conditional<sizeof(ElemType) * CHAR_BIT <= 32,
                                    uint32_t,
                                    uint64_t>::type AddressElemType;
  arma::Mat<ElemType> dataset(8, 1000);

  dataset.randu();
  dataset -= 0.5;
  arma::Col<AddressElemType> address(dataset.n_rows);
  arma::Col<ElemType> point(dataset.n_rows);

  // Ensure that this is one-to-one transform.
  for (size_t i = 0; i < dataset.n_cols; ++i)
  {
    addr::PointToAddress(address, dataset.col(i));
    addr::AddressToPoint(point, address);

    for (size_t k = 0; k < dataset.n_rows; ++k)
      BOOST_REQUIRE_CLOSE(dataset(k, i), point[k], 1e-13);
  }
}

template<typename TreeType>
void CheckSplit(const TreeType& tree)
{
  typedef typename TreeType::ElemType ElemType;
  typedef typename std::conditional<sizeof(ElemType) * CHAR_BIT <= 32,
                                    uint32_t,
                                    uint64_t>::type AddressElemType;

  if (tree.IsLeaf())
    return;

  arma::Col<AddressElemType> lo(tree.Bound().Dim());
  arma::Col<AddressElemType> hi(tree.Bound().Dim());

  lo.fill(std::numeric_limits<AddressElemType>::max());
  hi.fill(0);

  arma::Col<AddressElemType> address(tree.Bound().Dim());

  // Find the highest address of the left node.
  for (size_t i = 0; i < tree.Left()->NumDescendants(); ++i)
  {
    addr::PointToAddress(address,
        tree.Dataset().col(tree.Left()->Descendant(i)));

    if (addr::CompareAddresses(address, hi) > 0)
      hi = address;
  }

  // Find the lowest address of the right node.
  for (size_t i = 0; i < tree.Right()->NumDescendants(); ++i)
  {
    addr::PointToAddress(address,
        tree.Dataset().col(tree.Right()->Descendant(i)));

    if (addr::CompareAddresses(address, lo) < 0)
      lo = address;
  }

  // Addresses in the left node should be less than addresses in the right node.
  BOOST_REQUIRE_LE(addr::CompareAddresses(hi, lo), 0);

  CheckSplit(*tree.Left());
  CheckSplit(*tree.Right());
}

BOOST_AUTO_TEST_CASE(UBTreeSplitTest)
{
  typedef UBTree<EuclideanDistance, EmptyStatistic, arma::mat> TreeType;
  arma::mat dataset(8, 1000);

  dataset.randu();

  TreeType tree(dataset);
  CheckSplit(tree);
}

template<typename TreeType>
void CheckBound(const TreeType& tree)
{
  typedef typename TreeType::ElemType ElemType;
  for (size_t i = 0; i < tree.NumDescendants(); ++i)
  {
    arma::Col<ElemType> point = tree.Dataset().col(tree.Descendant(i));

    // Check that the point is contained in the bound.
    BOOST_REQUIRE_EQUAL(true, tree.Bound().Contains(point));

    const arma::Mat<ElemType>& loBound = tree.Bound().LoBound();
    const arma::Mat<ElemType>& hiBound = tree.Bound().HiBound();

    // Ensure that there is a hyperrectangle that contains the point.
    bool success = false;
    for (size_t j = 0; j < tree.Bound().NumBounds(); ++j)
    {
      success = true;
      for (size_t k = 0; k < loBound.n_rows; ++k)
      {
        if (point[k] < loBound(k, j) - 1e-14 * std::fabs(loBound(k, j)) ||
            point[k] > hiBound(k, j) + 1e-14 * std::fabs(hiBound(k, j)))
        {
          success = false;
          break;
        }
      }
      if (success)
        break;
    }

    BOOST_REQUIRE_EQUAL(success, true);
  }

  if (!tree.IsLeaf())
  {
    CheckBound(*tree.Left());
    CheckBound(*tree.Right());
  }
}

BOOST_AUTO_TEST_CASE(UBTreeBoundTest)
{
  typedef UBTree<EuclideanDistance, EmptyStatistic, arma::mat> TreeType;
  arma::mat dataset(8, 1000);

  dataset.randu();

  TreeType tree(dataset);
  CheckBound(tree);
}

// Ensure that MinDistance() and MaxDistance() works correctly.
template<typename TreeType, typename MetricType>
void CheckDistance(TreeType& tree, TreeType* node = NULL)
{
  typedef typename TreeType::ElemType ElemType;
  if (node == NULL)
  {
    node = &tree;

    while (node->Parent() != NULL)
      node = node->Parent();

    CheckDistance<TreeType, MetricType>(tree, node);

    for (size_t j = 0; j < tree.Dataset().n_cols; ++j)
    {
      const arma::Col<ElemType>& point = tree.  Dataset().col(j);
      ElemType maxDist = 0;
      ElemType minDist = std::numeric_limits<ElemType>::max();
      for (size_t i = 0; i < tree.NumDescendants(); ++i)
      {
        ElemType dist = MetricType::Evaluate(
            tree.Dataset().col(tree.Descendant(i)),
            tree.Dataset().col(j));

        if (dist > maxDist)
          maxDist = dist;
        if (dist < minDist)
          minDist = dist;
      }

      BOOST_REQUIRE_LE(tree.Bound().MinDistance(point), minDist *
          (1.0 + 10 * std::numeric_limits<ElemType>::epsilon()));
      BOOST_REQUIRE_LE(maxDist, tree.Bound().MaxDistance(point) *
          (1.0 + 10 * std::numeric_limits<ElemType>::epsilon()));

      math::RangeType<ElemType> r = tree.Bound().RangeDistance(point);

      BOOST_REQUIRE_LE(r.Lo(), minDist *
          (1.0 + 10 * std::numeric_limits<ElemType>::epsilon()));
      BOOST_REQUIRE_LE(maxDist, r.Hi() *
          (1.0 + 10 * std::numeric_limits<ElemType>::epsilon()));
    }

    if (!tree.IsLeaf())
    {
      CheckDistance<TreeType, MetricType>(*tree.Left());
      CheckDistance<TreeType, MetricType>(*tree.Right());
    }
  }
  else
  {
    if (&tree != node)
    {
      ElemType maxDist = 0;
      ElemType minDist = std::numeric_limits<ElemType>::max();
      for (size_t i = 0; i < tree.NumDescendants(); ++i)
        for (size_t j = 0; j < node->NumDescendants(); ++j)
        {
          ElemType dist = MetricType::Evaluate(
              tree.Dataset().col(tree.Descendant(i)),
              node->Dataset().col(node->Descendant(j)));

          if (dist > maxDist)
            maxDist = dist;
          if (dist < minDist)
            minDist = dist;
        }

      BOOST_REQUIRE_LE(tree.Bound().MinDistance(node->Bound()), minDist *
          (1.0 + 10 * std::numeric_limits<ElemType>::epsilon()));
      BOOST_REQUIRE_LE(maxDist, tree.Bound().MaxDistance(node->Bound()) *
          (1.0 + 10 * std::numeric_limits<ElemType>::epsilon()));

      math::RangeType<ElemType> r = tree.Bound().RangeDistance(node->Bound());

      BOOST_REQUIRE_LE(r.Lo(), minDist *
          (1.0 + 10 * std::numeric_limits<ElemType>::epsilon()));
      BOOST_REQUIRE_LE(maxDist, r.Hi() *
          (1.0 + 10 * std::numeric_limits<ElemType>::epsilon()));
    }
    if (!node->IsLeaf())
    {
      CheckDistance<TreeType, MetricType>(tree, node->Left());
      CheckDistance<TreeType, MetricType>(tree, node->Right());
    }
  }
}

BOOST_AUTO_TEST_CASE(UBTreeDistanceTest)
{
  typedef UBTree<EuclideanDistance, EmptyStatistic, arma::mat> TreeType;
  arma::mat dataset(8, 200);

  dataset.randu();

  TreeType tree(dataset);
  CheckDistance<TreeType, EuclideanDistance>(tree);
}

BOOST_AUTO_TEST_CASE(UBTreeTest)
{
  typedef UBTree<EuclideanDistance, EmptyStatistic, arma::mat> TreeType;

  size_t maxRuns = 10; // Ten total tests.
  size_t pointIncrements = 1000; // Range is from 2000 points to 11000.

  // We use the default leaf size of 20.
  for (size_t run = 0; run < maxRuns; run++)
  {
    size_t dimensions = run + 2;
    size_t maxPoints = (run + 1) * pointIncrements;

    size_t size = maxPoints;
    arma::mat dataset = arma::mat(dimensions, size);
    arma::mat datacopy; // Used to test mappings.

    // Mappings for post-sort verification of data.
    std::vector<size_t> newToOld;
    std::vector<size_t> oldToNew;

    // Generate data.
    dataset.randu();

    // Build the tree itself.
    TreeType root(dataset, newToOld, oldToNew);
    const arma::mat& treeset = root.Dataset();

    // Ensure the size of the tree is correct.
    BOOST_REQUIRE_EQUAL(root.NumDescendants(), size);

    // Check the forward and backward mappings for correctness.
    for (size_t i = 0; i < size; ++i)
    {
      for (size_t j = 0; j < dimensions; ++j)
      {
        BOOST_REQUIRE_EQUAL(treeset(j, i), dataset(j, newToOld[i]));
        BOOST_REQUIRE_EQUAL(treeset(j, oldToNew[i]), dataset(j, i));
      }
    }
  }
}

BOOST_AUTO_TEST_CASE(SingleTreeTraverserTest)
{
  arma::mat dataset;
  dataset.randu(8, 1000); // 1000 points in 8 dimensions.
  arma::Mat<size_t> neighbors1;
  arma::mat distances1;
  arma::Mat<size_t> neighbors2;
  arma::mat distances2;

  // Nearest neighbor search with the UB tree.
  NeighborSearch<NearestNS, metric::LMetric<2, true>, arma::mat,
      UBTree> knn1(dataset, SINGLE_TREE_MODE);

  knn1.Search(5, neighbors1, distances1);

  // Nearest neighbor search the naive way.
  KNN knn2(dataset, NAIVE_MODE);

  knn2.Search(5, neighbors2, distances2);

  for (size_t i = 0; i < neighbors1.size(); ++i)
  {
    BOOST_REQUIRE_EQUAL(neighbors1[i], neighbors2[i]);
    BOOST_REQUIRE_EQUAL(distances1[i], distances2[i]);
  }
}

BOOST_AUTO_TEST_CASE(DualTreeTraverserTest)
{
  arma::mat dataset;
  dataset.randu(8, 1000); // 1000 points in 8 dimensions.
  arma::Mat<size_t> neighbors1;
  arma::mat distances1;
  arma::Mat<size_t> neighbors2;
  arma::mat distances2;

  // Nearest neighbor search with the UB tree.
  NeighborSearch<NearestNS, metric::LMetric<2, true>, arma::mat,
      UBTree> knn1(dataset, DUAL_TREE_MODE);

  knn1.Search(5, neighbors1, distances1);

  // Nearest neighbor search the naive way.
  KNN knn2(dataset, NAIVE_MODE);

  knn2.Search(5, neighbors2, distances2);

  for (size_t i = 0; i < neighbors1.size(); ++i)
  {
    BOOST_REQUIRE_EQUAL(neighbors1[i], neighbors2[i]);
    BOOST_REQUIRE_EQUAL(distances1[i], distances2[i]);
  }
}

BOOST_AUTO_TEST_SUITE_END();


// The MIT License (MIT)
//
// Copyright (c) 2016 Darrell Wright
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files( the "Software" ), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and / or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <boost/algorithm/string/predicate.hpp>
#include <cmath>

#include "data_types.h"
#include "round_basal.h"

namespace ns {
	auto round_basal( double const & basal, profile_t const & profile ) {
		/*
		 * x23 and x54 pumps change basal increment depending on how much basal is being delivered:
		 * 0.025u for 0.025 < x < 0.975
		 * 0.05u for 1 < x < 9.95
		 * 0.1u for 10 < x
		 * To round numbers nicely for the pump, use a scale factor of (1 / increment).
		 */
		auto const lowest_rate_scale = [&profile]( ) -> double {
			if( profile.model ) {
				auto const & model = *profile.model;
				if( boost::algorithm::ends_with( model, "54" ) || boost::algorithm::ends_with( model, "23" ) ) {
					return 40;
				}
			}
			return 20;
		}( );
		assert( basal > 0 );
		if( basal < 1 ) {
			return round( basal * lowest_rate_scale)/lowest_rate_scale;
		} else if( basal < 10 ) {
			return round( basal * 20.0 )/20.0;
		}
		return round( basal * 10.0 )/10.0;
	}
}    // namespace ns


/**
 * @file newproblem_main.cc
 * @brief NPDE homework NewProblem code
 * @author Oliver Rietmann, Erick Schulz
 * @date 01.01.2020
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>
#include <iostream>

#include "newproblem.h"

int main() {
  Eigen::VectorXd v = NewProblem::dummyFunction(0.0, 0);

  const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision,
                                         Eigen::DontAlignCols, ", ", "\n");
  std::cout << v.transpose().format(CSVFormat) << std::endl;

  return 0;
}


// a random number generator supporting different distributions.
//
//  Copyright Steven Ross 2009.
//
// 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/libs/sort for library home page.

#include <stdio.h>
#include "stdlib.h"
#include <fstream>
#include <iostream>
#include <vector>
#include <string>
#include <boost/random.hpp>

using namespace std;
using namespace boost;

int main(int argc, const char ** argv) {
  //Always seed with the same value, to get the same results
  srand(1);
  //defaults
  int mod_shift = 32;
  unsigned count = 1000000;
  //Reading in user arguments
  if (argc > 2)
    count = atoi(argv[2]);
  if (argc > 1)
    mod_shift = atoi(argv[1]) - 1;
  std::ofstream ofile;
  ofile.open("input.txt", std::ios_base::out | std::ios_base::binary |
             std::ios_base::trunc);
  if (ofile.bad()) {
    printf("could not open input.txt for writing!\n");
    return 1;
  }
  int min_int = (numeric_limits<int>::min)();
  int max_int = (numeric_limits<int>::max)();
  if (mod_shift < 31 && mod_shift >= 0) {
    max_int %= 1 << mod_shift;
    if (-max_int > min_int)
      min_int = -max_int;
  }
  std::vector<int> result;
  result.resize(count);
  mt19937 rng;
  if (argc > 3 && (string(argv[3]) == "-normal")) {
    normal_distribution<> everything(0, max_int/4);
    variate_generator<mt19937&,normal_distribution<> > gen(rng, everything);
    generate(result.begin(), result.end(), gen);
  }
  else if (argc > 3 && (string(argv[3]) == "-lognormal")) {
    lognormal_distribution<> everything(max_int/2, max_int/4);
    variate_generator<mt19937&,lognormal_distribution<> > gen(rng, everything);
    generate(result.begin(), result.end(), gen);
  }
  else {
    uniform_int<> everything(min_int, max_int);
    variate_generator<mt19937&,uniform_int<> > gen(rng, everything);
    generate(result.begin(), result.end(), gen);
  }
  ofile.write(reinterpret_cast<char *>(&(result[0])), result.size() *
              sizeof(int));
  ofile.close();
  return 0;
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2016 Francisca Gil Ureta <gilureta@cs.nyu.edu>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "segment_segment_intersect.h"

#include <Eigen/Geometry>

template<typename DerivedSource, typename DerivedDir>
IGL_INLINE bool igl::segment_segment_intersect(
  const Eigen::MatrixBase <DerivedSource> &p,
  const Eigen::MatrixBase <DerivedDir> &r,
  const Eigen::MatrixBase <DerivedSource> &q,
  const Eigen::MatrixBase <DerivedDir> &s,
  double &a_t,
  double &a_u,
  double eps
)
{
  // http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect
  // Search intersection between two segments
  // p + t*r :  t \in [0,1]
  // q + u*s :  u \in [0,1]

  // p + t * r = q + u * s  // x s
  // t(r x s) = (q - p) x s
  // t = (q - p) x s / (r x s)

  // (r x s) ~ 0 --> directions are parallel, they will never cross
  Eigen::Matrix<typename DerivedDir::Scalar, 1, 3> rxs = r.cross(s);
  if (rxs.norm() <= eps)
    return false;

  int sign;

  double u;
  // u = (q − p) × r / (r × s)
  Eigen::Matrix<typename DerivedDir::Scalar, 1, 3> u1 = (q - p).cross(r);
  sign = ((u1.dot(rxs)) > 0) ? 1 : -1;
  u = u1.norm() / rxs.norm();
  u = u * sign;

  double t;
  // t = (q - p) x s / (r x s)
  Eigen::Matrix<typename DerivedDir::Scalar, 1, 3> t1 = (q - p).cross(s);
  sign = ((t1.dot(rxs)) > 0) ? 1 : -1;
  t = t1.norm() / rxs.norm();
  t = t * sign;

  a_t = t;
  a_u = u;

  if ((u - 1.) > eps || u < -eps)
    return false;

  if ((t - 1.) > eps || t < -eps)
    return false;

  return true;
};

#ifdef IGL_STATIC_LIBRARY
template bool igl::segment_segment_intersect<Eigen::Matrix<double, 1, 3, 1, 1, 3>, Eigen::Matrix<double, 1, 3, 1, 1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> > const&, double&, double&, double);
#endif


#include <iostream>
#include <armadillo>
#include "../config.hpp"

using namespace std;
using namespace arma;

pair<vec, double> entrenarPerceptron(const mat& datos,
                                     int nEpocas,
                                     double tasaAprendizaje,
                                     double toleranciaError);
double errorPerceptron(const vec& pesos,
                   const mat& patrones,
                   const vec& salidaDeseada);

using Particion = pair<mat, mat>;
vector<Particion> particionar(mat datos, int nParticiones, double porcentajeEnt);

int main()
{
    arma_rng::set_seed_random();
    mat datos;

    // Primera parte del ejercicio (spheres1d)
    {
        datos.load(config::sourceDir + "/guia1/icgtp1datos/spheres1d10.csv");

        // FIXME:
        // 1: Las particiones tendrían que contener los índices a usar, no replicar los datos.
        // 2: Las particiones tendrían que ser generadas una sola vez, guardadas
        //    en un archivo, y luego levantarlas para que de esta manera poder repetir
        //    sesiones de entrenamiento/prueba con distintos clasificadores en
        //    los mismos datos. Incluso fijar la semilla de generación de núeros aleatorios.
        const vector<Particion> particiones = particionar(datos, 5, 80);

        vec errores;
        errores.set_size(particiones.size());
        int i = 0;

        for (const Particion& particion : particiones) {
            vec pesos;
            tie(pesos, ignore) = entrenarPerceptron(particion.first, 100, 0.1, 20);

            const double tasaError = errorPerceptron(pesos,
                                                 particion.second.head_cols(3),
                                                 particion.second.tail_cols(1));
            errores[i++] = tasaError;
        }

        cout << "La validación cruzada del perceptron en spheres1d10 da como error:\n"
             << "Media: " << mean(errores) << '\n'
             << "Varianza: " << var(errores) << endl;
    }

    // Segunda parte del ejercicio (spheres2d)
    vector<string> archivos = {config::sourceDir + "/guia1/icgtp1datos/spheres2d10.csv",
                               config::sourceDir + "/guia1/icgtp1datos/spheres2d50.csv",
                               config::sourceDir + "/guia1/icgtp1datos/spheres2d70.csv"};

    for (string archivo : archivos) {
        datos.load(archivo);
        const vector<Particion> particiones = particionar(datos, 10, 80);

        vec errores;
        errores.set_size(particiones.size());
        int i = 0;

        for (const Particion& particion : particiones) {
            vec pesos;
            tie(pesos, ignore) = entrenarPerceptron(particion.first, 100, 0.1, 1);

            const double tasaError = errorPerceptron(pesos,
                                                 particion.second.head_cols(3),
                                                 particion.second.tail_cols(1));
            errores[i++] = tasaError;
        }

        // FIXME:
        // Para que los resultados sean más informativos habría que proporcionar tambíen lo siguiente:
        // (para cada partición)
        // - Epocas que demoró en converger
        // - Error
        // Esto sirve para detectar (por ejemplo):
        // - Si siempre está terminando el entrenamiento por límite de épocas
        // - Si los errores están dando siempre altos
        //      Esto puede querer decir que la tolerancia de error que le estamos pidiendo es
        //      demasiado alta.
        cout << "La validación cruzada del perceptron en " << archivo << " da como error:\n"
             << "Media: " << mean(errores) << '\n'
             << "Varianza: " << var(errores) << endl;
    }

    return 0;
}

// Lo siguiente es validación cruzada clásica
vector<Particion> particionar(mat datos, int nParticiones, double porcentajeEnt)
{
    vector<Particion> particiones;
    const int nPatronesEnt = datos.n_rows * porcentajeEnt / 100;

    for (int i = 0; i < nParticiones; ++i) {
        datos = shuffle(datos); // Mezcla las filas de los datos
        particiones.push_back({datos.head_rows(nPatronesEnt),
                               datos.tail_rows(datos.n_rows - nPatronesEnt)});
    }

    return particiones;
}

namespace ic {
int sign(double numero)
{
    if (numero >= 0)
        return 1;
    else
        return -1;
}
}

pair<vec, double> epocaPerceptron(const mat& patronesExt,
                                  const vec& salidaDeseada,
                                  double tasaAprendizaje,
                                  const vec& pesos)
{
    //Entrenamiento
    vec nuevosPesos = pesos;

    for (unsigned int i = 0; i < patronesExt.n_rows; ++i) {
        double z = dot(patronesExt.row(i), pesos);
        int y = ic::sign(z);

        // Actualizar pesos
        nuevosPesos += tasaAprendizaje * (salidaDeseada(i) - y) * patronesExt.row(i).t();
    } // Fin ciclo (Entrenamiento)

    // Validacion
    int errores = 0;

    for (unsigned int i = 0; i < patronesExt.n_rows; ++i) {
        double z = dot(patronesExt.row(i), pesos);
        int y = ic::sign(z);

        if (y != salidaDeseada(i))
            ++errores;
    }

    double tasaError = static_cast<double>(errores) / patronesExt.n_rows * 100;

    return {nuevosPesos, tasaError};
} // fin funcion Epoca

double errorPerceptron(const vec& pesos,
                   const mat& patrones,
                   const vec& salidaDeseada)
{
    // Se extiende la matriz de patrones con la entrada correspondiente al umbral
    const mat patronesExt = join_horiz(ones(patrones.n_rows) * (-1), patrones);

    int errores = 0;

    for (unsigned int i = 0; i < patronesExt.n_rows; ++i) {
        double z = dot(patronesExt.row(i), pesos);
        int y = ic::sign(z);

        if (y != salidaDeseada(i))
            ++errores;
    }

    double tasaError = static_cast<double>(errores) / patronesExt.n_rows * 100;

    return tasaError;
}

pair<vec, double> entrenarPerceptron(const mat& datos,
                                     int nEpocas,
                                     double tasaAprendizaje,
                                     double toleranciaError)
{
    const vec salidaDeseada = datos.tail_cols(1);
    // Extender la matriz de patrones con la entrada correspondiente al umbral
    const int nParametros = datos.n_cols - 1;
    const mat patronesExt = join_horiz(ones(datos.n_rows) * (-1), datos.head_cols(nParametros));

    // Inicializar pesos y tasa de error
    vec pesos = randu<vec>(patronesExt.n_cols) - 0.5;
    double tasaError = 0;

    // Ciclo de las epocas
    for (int epoca = 1; epoca <= nEpocas; ++epoca) {
        // Ciclo para una época
        tie(pesos, tasaError) = epocaPerceptron(patronesExt,
                                                salidaDeseada,
                                                tasaAprendizaje,
                                                pesos);

        if (tasaError < toleranciaError)
            break;
    }
    // Fin ciclo (epocas)

    return {pesos, tasaError};
}


/*=============================================================================
    Spirit v1.6.0
    Copyright (c) 2002-2003 Joel de Guzman
    http://spirit.sourceforge.net/

    Permission to copy, use, modify, sell and distribute this software is
    granted provided this copyright notice appears in all copies. This
    software is provided "as is" without express or implied warranty, and
    with no claim as to its suitability for any purpose.
=============================================================================*/
///////////////////////////////////////////////////////////////////////////////
//
//  A parser for summing a list of numbers.
//  [ demonstrating phoenix ]
//
//  [ JDG 6/28/2002 ]
//
///////////////////////////////////////////////////////////////////////////////
#include <boost/spirit/core.hpp>
#include <boost/spirit/phoenix/primitives.hpp>
#include <boost/spirit/phoenix/operators.hpp>
#include <iostream>
#include <string>

///////////////////////////////////////////////////////////////////////////////
using namespace std;
using namespace boost::spirit;
using namespace phoenix;

///////////////////////////////////////////////////////////////////////////////
//
//  Our adder
//
///////////////////////////////////////////////////////////////////////////////
template <typename IteratorT>
bool adder(IteratorT first, IteratorT last, double& n)
{
    return parse(first, last,

        //  Begin grammar
        (
            real_p[var(n) = arg1] >> *real_p[var(n) += arg1]
        )
        ,
        //  End grammar

        space_p).full;
}

////////////////////////////////////////////////////////////////////////////
//
//  Main program
//
////////////////////////////////////////////////////////////////////////////
int
main()
{
    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "\t\tA parser for summing a list of numbers...\n\n";
    cout << "/////////////////////////////////////////////////////////\n\n";

    cout << "Give me a space separated list of numbers.\n";
    cout << "The numbers are added using Phoenix.\n";
    cout << "Type [q or Q] to quit\n\n";

    string str;
    while (getline(cin, str))
    {
        if (str[0] == 'q' || str[0] == 'Q')
            break;

        double n;
        if (adder(str.begin(), str.end(), n))
        {
            cout << "-------------------------\n";
            cout << "Parsing succeeded\n";
            cout << str << " Parses OK: " << endl;

            cout << "sum = " << n;
            cout << "\n-------------------------\n";
        }
        else
        {
            cout << "-------------------------\n";
            cout << "Parsing failed\n";
            cout << "-------------------------\n";
        }
    }

    cout << "Bye... :-) \n\n";
    return 0;
}


//
// Created by abakfja on 3/28/21.
//

#ifndef LA_MATRIX_ENGINE_HPP
#define LA_MATRIX_ENGINE_HPP

#include <boost/numeric/ublas/matrix/storage_traits.hpp>
#include <cstddef>

namespace boost::numeric::ublas::experimental {

template<typename T, std::size_t R, std::size_t C>
struct static_matrix_engine {
    using scalar_type = T;
    using array_type = std::array<scalar_type, R * C>;
    using storage_traits_type = storage_traits<array_type>;
    using size_type = typename storage_traits_type::size_type;
    using resizable_tag = typename storage_traits_type::resizable_tag;
    static constexpr size_type rows = R;
    static constexpr size_type cols = C;
};

template<typename T>
struct dynamic_matrix_engine {
    using scalar_type = T;
    using array_type = std::vector<T>;
    using storage_traits_type = storage_traits<array_type>;
    using resizable_tag = typename storage_traits_type::resizable_tag;
};

}

#endif //LA_MATRIX_ENGINE_HPP


#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <laser_geometry/laser_geometry.h>
#include <visualization_msgs/Marker.h>

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

ros::Publisher marker_pub;
ros::Subscriber laser_sub;

double douglas_pecker_distance;
double neighbor_distance;
int min_cluster_size;

typedef boost::geometry::model::d2::point_xy<double> xy;
laser_geometry::LaserProjection lp;

void laser_cb(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    // Create marker for rviz
    visualization_msgs::Marker line_segments;
    line_segments.header.frame_id = msg->header.frame_id;
    line_segments.header.stamp = msg->header.stamp;
    line_segments.ns = "line_segments";
    line_segments.action = visualization_msgs::Marker::ADD;
    line_segments.type = visualization_msgs::Marker::LINE_LIST;
    line_segments.pose.orientation.w = 1.0;
    line_segments.id = 42;
    line_segments.scale.x = 0.1;
    line_segments.color.r = 1.0;
    line_segments.color.a = 1.0;

    // Convert laser data from polar to cartesian coordinate system
    sensor_msgs::PointCloud cloud;
    lp.projectLaser(*msg, cloud);

    // Cluster the point cloud
    boost::geometry::model::linestring<xy> cluster;
    for (int i=0; i<cloud.points.size(); i++)
    {
        boost::geometry::append(cluster, xy(cloud.points[i].x, cloud.points[i].y));
        bool is_cluster_ready = false;

        // If this is the last point, don't check the next one and complete the cluster
        if (i == cloud.points.size())
        {
            is_cluster_ready = true;
        }
        else
        {
            // Find if the point is in the cluster using euclidian distance
            double x1, y1, x2, y2;
            x1 = cloud.points[i].x;
            y1 = cloud.points[i].y;
            x2 = cloud.points[i+1].x;
            y2 = cloud.points[i+1].y;
            is_cluster_ready = ( pow(x1-x2, 2)+pow(y1-y2, 2) > pow(neighbor_distance, 2) );
        }

        if (is_cluster_ready)
        {
            if (cluster.size() >= min_cluster_size)
            {
                // simplify process
                boost::geometry::model::linestring<xy> simplified;
                boost::geometry::simplify(cluster, simplified, douglas_pecker_distance); // Using Douglas-Peucker algorithm

                // Parse results from "simplified" for visualization purposes
                for (int j=0; j<simplified.size()-1; j++)
                {
                    geometry_msgs::Point p1, p2;
                    p1.x = simplified[j].x();
                    p1.y = simplified[j].y();
                    p2.x = simplified[j+1].x();
                    p2.y = simplified[j+1].y();

                    line_segments.points.push_back(p1);
                    line_segments.points.push_back(p2);
                }
                cluster.clear();
            }
            else
            {
                cluster.clear();
            }
        }
    }

    marker_pub.publish(line_segments);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "extract_line_segments_node");
    ros::NodeHandle nh;
    ros::NodeHandle priv_nh("~");

    priv_nh.param("douglas_pecker_distance", douglas_pecker_distance, 0.1);
    priv_nh.param("neighbor_distance", neighbor_distance, 0.5);
    priv_nh.param("min_cluster_size", min_cluster_size, 5);

    marker_pub = nh.advertise<visualization_msgs::Marker>("line_segments", 2);
    laser_sub = nh.subscribe("scan", 2, laser_cb);

    ros::spin();
    return 0;
}


/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2017 Hidekazu Ikeno
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

///
/// \file partial_lanczos_bidiagonalization.hpp
///
#ifndef MXPFIT_PARTIAL_LANCZOS_BIDIAGONALIZATION_HPP
#define MXPFIT_PARTIAL_LANCZOS_BIDIAGONALIZATION_HPP

#include <Eigen/Core>

namespace mxpfit
{

///
/// ### PartialLanczosBidiagonalization
///
/// \brief Low-rank approximation of a matrix by the Lanczos bidiagonalization
/// with full reorthogonalization
///
/// \tparam MatrixT Matrix type to be decomposed. We expect that `MatrixT`
/// inherits Eigen::EigenBase class.
///
/// For a given \f$m \times n\f$ matrix \f$A\f$ and prescribed accuracy
/// \f$\epsilon,\f$ this class computes the approximate decomposition such that
///
/// \f[
///   \|A - P_{k}^{} B_{k} Q_{k}^{\ast} \|_{F} < \epsilon.
/// \f]
///
/// where \f$P_{k}\f$ is a \f$ m \times k \f$ matrix with orthonormal columns,
/// and \f$ Q_{k} \f$ is a \f$m \times k\f$ matrix with orthonormal columns.
/// \f$B_{k}\f$ is a real bidiagonal matrix
///
/// #### References
///
/// 1. D. Potts and M. Tasche, "Fast ESPRIT algorithms based on partial singular
///    value decompositions", Appl. Numer. Math. **88** (2015) 31-45.
///    [DOI: http://doi.org/10.1016/j.apnum.2014.10.003]
///
template <typename MatrixT>
class PartialLanczosBidiagonalization
{
public:
    using MatrixType    = MatrixT;
    using Scalar        = typename MatrixType::Scalar;
    using RealScalar    = typename MatrixType::RealScalar;
    using Index         = Eigen::Index;
    using Vector        = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using RealVector    = Eigen::Matrix<RealScalar, Eigen::Dynamic, 1>;
    using Matrix        = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using RealVectorRef = Eigen::Ref<RealVector>;
    using MatrixRef     = Eigen::Ref<Matrix>;

    PartialLanczosBidiagonalization()
        : m_matP(),
          m_matQ(),
          m_alpha(),
          m_beta(),
          m_rank(),
          m_tolerance(Eigen::NumTraits<RealScalar>::epsilon()),
          m_error(),
          m_is_initialized()

    {
    }

    PartialLanczosBidiagonalization(Index nrows, Index ncols, Index nsteps)
        : m_matP(nrows, nsteps),
          m_matQ(ncols, nsteps),
          m_alpha(nsteps),
          m_beta(nsteps),
          m_rank(),
          m_tolerance(Eigen::NumTraits<RealScalar>::epsilon()),
          m_error(),
          m_is_initialized()
    {
    }

    PartialLanczosBidiagonalization(const MatrixType& mat, Index nsteps)
        : m_matP(mat.rows(), nsteps),
          m_matQ(mat.cols(), nsteps),
          m_alpha(nsteps),
          m_beta(nsteps),
          m_rank(),
          m_tolerance(Eigen::NumTraits<RealScalar>::epsilon()),
          m_error(),
          m_is_initialized()
    {
        compute(mat, nsteps);
    }

    void compute(const MatrixType& matA, Index nsteps);

    void setTolerance(RealScalar tolerance) noexcept
    {
        m_tolerance = tolerance;
    }

    RealScalar tolerance() const noexcept
    {
        return m_tolerance;
    }

    Index rank() const noexcept
    {
        assert(m_is_initialized &&
               "PartialLanczosBidiagonalization is not initialized.");
        return m_rank;
    }

    RealScalar error() const noexcept
    {
        assert(m_is_initialized &&
               "PartialLanczosBidiagonalization is not initialized.");
        return m_error;
    }

    const Matrix& matrixP() const noexcept
    {
        assert(m_is_initialized &&
               "PartialLanczosBidiagonalization is not initialized.");
        return m_matP;
    }

    const Matrix& matrixQ() const noexcept
    {
        assert(m_is_initialized &&
               "PartialLanczosBidiagonalization is not initialized.");
        return m_matQ;
    }

    const RealVector& diagonalAlpha() const noexcept
    {
        assert(m_is_initialized &&
               "PartialLanczosBidiagonalization is not initialized.");
        return m_alpha;
    }

    const RealVector& superdiagonalBeta() const noexcept
    {
        assert(m_is_initialized &&
               "PartialLanczosBidiagonalization is not initialized.");
        return m_beta;
    }

    Matrix reconstructedMatrix() const
    {
        assert(m_is_initialized &&
               "PartialLanczosBidiagonalization is not initialized.");

        if (m_rank == Index())
        {
            return Matrix();
        }

        auto viewP = m_matP.leftCols(m_rank);
        auto viewQ = m_matQ.leftCols(m_rank);
        Matrix matB(Matrix::Zero(m_rank, m_rank));
        matB.diagonal()  = m_alpha.head(m_rank);
        matB.diagonal(1) = m_beta.head(m_rank - 1);

        return viewP * matB * viewQ.adjoint();
    }

protected:
    Matrix m_matP;
    Matrix m_matQ;
    RealVector m_alpha;
    RealVector m_beta;

    Index m_rank;
    RealScalar m_tolerance;
    RealScalar m_error;
    bool m_is_initialized;
};

template <typename MatrixT>
void PartialLanczosBidiagonalization<MatrixT>::compute(const MatrixType& matA,
                                                       Index nsteps)
{
    assert(Index() <= nsteps && nsteps <= matA.rows() && nsteps <= matA.cols());

    m_matP.resize(matA.rows(), nsteps);
    m_matQ.resize(matA.cols(), nsteps);
    m_alpha.resize(nsteps);
    m_beta.resize(nsteps - 1);

    Vector workspace(nsteps);

    auto q0 = m_matQ.col(0);
    // Set q0 as unit vector (1,0,0,....)^T plus small perturbation
    q0.setRandom();
    q0(0) = RealScalar(1) /
            Eigen::numext::sqrt(Eigen::NumTraits<RealScalar>::epsilon());
    q0.normalize(); // make q0 normalized
    auto p0       = m_matP.col(0);
    p0            = matA * q0;
    const auto a1 = p0.norm();
    if (a1 > RealScalar())
    {
        p0 *= RealScalar(1) / a1;
    }
    m_alpha(0) = a1;

    const RealScalar tol2 = m_tolerance * m_tolerance;
    RealScalar fnorm_A    = a1 * a1; // Estimation of |A|_F
    m_error               = RealScalar();
    Index irank           = 0;

    while (++irank < nsteps)
    {
        auto p1 = m_matP.col(irank - 1);
        auto p2 = m_matP.col(irank);
        auto q1 = m_matQ.col(irank - 1);
        auto q2 = m_matQ.col(irank);
        //
        // --- Recursion for right Lanczos vector
        //
        q2 = matA.adjoint() * p1 - m_alpha(irank - 1) * q1;
        // Reorthogonalization
        auto tmp   = workspace.head(irank);
        auto viewQ = m_matQ.leftCols(irank);
        tmp        = viewQ.adjoint() * q2;
        q2 -= viewQ * tmp;
        auto b1 = q2.norm();
        if (b1 > RealScalar())
        {
            q2 *= RealScalar(1) / b1;
        }
        m_beta(irank - 1) = b1;
        //
        // --- Recursion for left Lanczos vector
        //
        // p2 <-- A * q2 - beta(i) * p1
        p2 = matA * q2 - m_beta(irank - 1) * p1;
        // Reorthogonalization
        auto viewP = m_matP.leftCols(irank);
        tmp        = viewP.adjoint() * p2;
        p2 -= viewP * tmp;

        auto a2 = p2.norm();
        if (a2 > RealScalar())
        {
            p2 *= RealScalar(1) / a2;
        }
        m_alpha(irank) = a2;
        //
        // Update frobenius norm of matrix A via
        //
        // ||A||_{F}^{2} = \sum_{K=1}^{rank(A)-1}
        //       (\alpha_{K}^{2} + \beta_{K}^{2}) + \alpha_{rank(A)}}
        //
        auto t = a2 * a2 + b1 * b1;
        fnorm_A += t;
        m_error = t / fnorm_A;
        if (m_error <= tol2)
        {
            break; // converged
        }
    }

    m_rank           = irank;
    m_is_initialized = true;
}

} // namespace mxpfit

#endif /* MXPFIT_PARTIAL_LANCZOS_BIDIAGONALIZATION_HPP*/


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
//////////////////////////////////////////////////////////////////////////////
// cover test behavior of arithmetic components in scalar mode
//////////////////////////////////////////////////////////////////////////////
#include <nt2/arithmetic/include/functions/sqr.hpp>
#include <vector>
#include <nt2/include/constants/valmin.hpp>
#include <nt2/include/constants/valmax.hpp>
#include <nt2/include/constants/sqrtvalmax.hpp>
#include <nt2/include/functions/multiplies.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

#include <nt2/sdk/unit/tests/cover.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/meta/as_floating.hpp>

NT2_TEST_CASE_TPL ( sqr_real__1_0,  NT2_REAL_TYPES)
{

  using nt2::sqr;
  using nt2::tag::sqr_;
  typedef typename nt2::meta::call<sqr_(T)>::type r_t;

  nt2::uint32_t NR = NT2_NB_RANDOM_TEST;
  std::vector<T> in1(NR);
  nt2::roll(in1, nt2::Valmin<T>()/2, nt2::Valmax<T>()/2);
  std::vector<r_t>  ref(NR);
  for(nt2::uint32_t i=0; i < NR ; ++i)
  {
    ref[i] = in1[i]*in1[i];
  }

  NT2_COVER_ULP_EQUAL(sqr_, ((T, in1)), ref, 0);
}

NT2_TEST_CASE_TPL ( sqr_uint,  NT2_INTEGRAL_UNSIGNED_TYPES)
{

  using nt2::sqr;
  using nt2::tag::sqr_;
  typedef typename nt2::meta::call<sqr_(T)>::type r_t;
  typedef typename boost::dispatch::meta::as_integer<T, unsigned>::type utype;

  nt2::uint32_t NR = NT2_NB_RANDOM_TEST;
  std::vector<T> in1(NR);
  nt2::roll(in1, nt2::Valmin<T>()/2, nt2::Valmax<T>()/2);
  std::vector<r_t>  ref(NR);
  for(nt2::uint32_t i=0; i < NR ; ++i)
  {
    ref[i] = T(nt2::multiplies(utype(in1[i]), utype(in1[i])));
  }

  NT2_COVER_ULP_EQUAL(sqr_, ((T, in1)), ref, 0);
}
NT2_TEST_CASE_TPL ( sqr_sint,  NT2_INTEGRAL_SIGNED_TYPES)
{

  using nt2::sqr;
  using nt2::tag::sqr_;
  typedef typename nt2::meta::call<sqr_(T)>::type r_t;
  typedef typename boost::dispatch::meta::as_integer<T, unsigned>::type utype;

  nt2::uint32_t NR = NT2_NB_RANDOM_TEST;
  std::vector<T> in1(NR);
  nt2::roll(in1, -nt2::Sqrtvalmax<T>()/2, nt2::Sqrtvalmax<T>()/2);
  std::vector<r_t>  ref(NR);
  for(nt2::uint32_t i=0; i < NR ; ++i)
  {
    ref[i] = T(nt2::multiplies(utype(in1[i]), utype(in1[i])));
  }

  NT2_COVER_ULP_EQUAL(sqr_, ((T, in1)), ref, 0);
}


/**
 * Copyright (c) 2020 Melown Technologies SE
 *
 * 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.
 *
 * 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 HOLDER 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.
 */
#include "estimate-point-normals.hpp"
#include "bvh.hpp"
#include "math/math.hpp"
#include "math/geometry.hpp"

#include <boost/iterator/counting_iterator.hpp>

namespace geometry {

namespace ublas = math::ublas;

Eigen::VectorXd estimateNormal(const Eigen::MatrixXd& data)
{
    using namespace Eigen;
    // calculate centroid (1 x K) of all (N) data points
    RowVectorXd centroid = data.colwise().mean();
    // mean-center sample matrix
    MatrixXd centered = data.rowwise() - centroid;
    // calculate the covariance matrix (K x K), for PCA we may ommit
    // scaling by the factor 1/(N - 1)
    MatrixXd covariance = centered.transpose() * centered;

    // calculate SVD (ordered by the magnitude of SV)
    JacobiSVD<MatrixXd> svd(covariance, ComputeFullU);
    // the normal is the last column of U, singular vector of the covariance
    // matrix corresponding to the smallest singular value
    // (i.e., the direction of the least variability in the data)
    VectorXd sgVec(svd.matrixU().col(data.cols() - 1));

    // normalization shouldn't be needed as U should be a unitary matrix.
    return sgVec;//.normalized();
}

namespace {

    class BvhDisk : public BvhPrimitive {
        math::Point3 center_;
        math::Point3 normal_;
        double radius_;

    public:
        BvhDisk() = default;

        BvhDisk(const math::Point3& center
                , const math::Point3& normal
                , const double radius
                , const std::uint32_t index)
            : center_(center)
            , normal_(normal)
            , radius_(radius) {
            assert(radius_ > 0.);
            userData = index;
        }

        bool getIntersection(const Ray& ray, IntersectionInfo& intersection) const {
            // ray-plane intersection
            const double denom = inner_prod(normal_, ray.direction());
            if (std::fabs(denom) < 1.e-6) {
                return false;
            }
            const math::Point3 diff = center_ - ray.origin();
            const double t = inner_prod(diff, normal_) / denom;
            if (t > 0.) {
                // find the distance of the intersection from the disk center
                const math::Point3 dp = ray.origin() + ray.direction() * t - center_;
                const double distSqr = inner_prod(dp, dp);
                if (distSqr < math::sqr(radius_)) {
                    intersection.object = this;
                    intersection.t = t;
                    return true;
                }
            }

            return false;
        }

        math::Extents3 getBBox() const {
            ublas::scalar_vector<double> size(3, radius_);
            return math::Extents3(center_ - size, center_ + size);
        }

        math::Point3 getCenter() const {
            return center_;
        }
    };

} // namespace

static constexpr float DX = 0.25;
static constexpr float DZ = 1.;
static math::Points3 ALL_DIRS = {
    math::Point3(0., 0., DZ),
    math::Point3(0., DX, DZ),
    math::Point3(0., -DX, DZ),
    math::Point3(DX, 0., DZ),
    math::Point3(-DX, 0., DZ),
    math::Point3(DX, DX, DZ),
    math::Point3(DX, -DX, DZ),
    math::Point3(-DX, -DX, DZ),
    math::Point3(-DX, DX, DZ),
};

void reorientNormals(const std::vector<math::Point3>& pc
                     , std::vector<math::Point3>& normals
                     , const double pointRadius)
{
    std::vector<BvhDisk> disks(pc.size());
    UTILITY_OMP(parallel for)
    for (std::uint32_t i = 0; i < pc.size(); ++i) {
        disks[i] = BvhDisk(pc[i], normals[i], pointRadius, i);
    }

    LOG(info2) << "Building BVH for " << disks.size() << " points";
    geometry::Bvh<BvhDisk> bvh(20);
    bvh.build(std::move(disks));

    // find order in z-direction
    std::vector<std::uint32_t> orderZ(pc.size());
    std::copy(boost::counting_iterator<std::uint32_t>(0)
              , boost::counting_iterator<std::uint32_t>(pc.size())
              , orderZ.begin());
    std::sort(orderZ.begin(), orderZ.end()
              , [&](std::uint32_t i1, std::uint32_t i2) { return pc[i1](2) > pc[i2](2); });

    // step 1 - determine normal orientation based on normals of already determined points,
    // assuming the topmost points (roofs) have z>0 orientation.
    LOG(info2) << "Estimating normal orientations";
    UTILITY_OMP(parallel for)
    for (std::uint32_t rankZ = 0; rankZ < orderZ.size(); ++rankZ) {
        const std::uint32_t i = orderZ[rankZ];
        int doFlip = 0;
        for (math::Point3 dir : ALL_DIRS) {
            dir = math::normalize(dir);
            // whether the ray is outward or inward with respect to current normal orientation
            const int outward = math::sgn(inner_prod(dir, normals[i]));
            const Ray ray(pc[i] + pointRadius * dir, dir);
            IntersectionInfo is;
            if (bvh.getFirstIntersection(ray, is)) {
                const std::uint32_t j = is.object->userData;
                // if this is an outward ray, flip the normal if the intersected point has
                // the same orientation as the ray (i.e. likely a backface);
                // for an inward ray, flip the normal if the intersected point is NOT a backface.
                doFlip += int(inner_prod(normals[j], dir) > 0.) * outward;
            } else {
                // no occlusion -> normal correct if this is an outward ray
                doFlip -= outward;
            }
        }

        if (doFlip > 0) {
            // more votes for flipping the normal
            normals[i] *= -1;
        }
    }

    // step 2 - flip normals which have opposite orientation than their neighbors
    LOG(info2) << "Flipping outliers";
    KdTree<math::Point3, 3> tree(pc.begin(), pc.end());
    std::vector<uint8_t> doFlip(pc.size(), false);
    std::vector<math::Points3::const_iterator> neighs;
    UTILITY_OMP(parallel for private(neighs))
    for (std::uint32_t i = 0; i < pc.size(); ++i) {
        neighs.clear();
        tree.range(pc[i], 2 * pointRadius, neighs);

        double count = 0.;
        for (auto& n : neighs) {
            const std::uint32_t j = std::uint32_t(n - pc.begin());
            count += inner_prod(normals[i], normals[j]);
        }
        if (count < 0.) {
            doFlip[i] = true;
        }
    }
    UTILITY_OMP(parallel for)
    for (std::uint32_t i = 0; i < pc.size(); ++i) {
        if (doFlip[i]) {
            normals[i] *= -1;
        }
    }
}

} // namespace geometry


#include "trim_dht.hpp"
#include "dht.hpp"

#include <Eigen/Dense>
#include <algorithm>
#include <boost/math/special_functions/bessel.hpp>
#include <cmath>
#include <iostream>
#include <iterator>

using namespace Eigen;
using boost::math::cyl_bessel_j;
using boost::math::cyl_bessel_j_zero;
using std::pow;

TrimDHT::TrimDHT(int order, int nr, double rmax, int nexp)
    : DiscreteHankelTransform(order, nr * pow(2, nexp)),
      shift_matrix_(nr_, nr_) {
  // nr_ = nr * pow(2, nexp)
  rmax_extend_ = cyl_bessel_j_zero(float(order), nr_ + 1) /
                 cyl_bessel_j_zero(float(order), nr + 1) * rmax;
  shift_matrix_ = tmatrix_.leftCols(nr) * tmatrix_.topRows(nr);
}

TrimDHT::~TrimDHT() = default;

VectorXd TrimDHT::perform(const Ref<const VectorXd> &src) {
  VectorXd ret;
  ret = shift_matrix_ * src;
  return ret;
}

VectorXd TrimDHT::r_sampling() {
  return DiscreteHankelTransform::r_sampling(rmax_extend_);
}

VectorXd TrimDHT::k_sampling() {
  return DiscreteHankelTransform::k_sampling(rmax_extend_);
}

int TrimDHT::get_nr() const { return nr_; }


#include <iostream>
#include <math.h>

#include <boost/bind/bind.hpp>
#include "boost/math/special_functions/pow.hpp"
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/tuple/tuple.hpp>

#include "Filtering.hpp"

using namespace boost::math;
using namespace boost::numeric::ublas;

#define POW(a,b) pow(a,b)

class AbstractRadarCoordinates {
public:

	RealMatrix ENU2AER(RealVector E, RealVector N, RealVector U) {
		RealMatrix AER(E.size(), 3);
		AER(0, 0) = fmod(atan2(N(0), E(0)), (2.0*constants::pi<double>()));  // azimuth
		AER(0, 1) = atan2(U(0), sqrt(pow<2>(E(0)) + pow<2>(N(0))));
		AER(0, 2) = sqrt(pow<2>(E(0)) + pow<2>(N(0)) + pow<2>(U(0)));
		return AER;
	};

	RealMatrix AER2ENU(RealVector A, RealVector E, RealVector R) {
		RealMatrix ENU(A.size(), 3);
		ENU(0, 0) = R(0) * cos(E(0)) * sin(A(0));
		ENU(0, 1) = R(0) * cos(E(0)) * cos(A(0));
		ENU(0, 2) = R(0) * sin(E(0));
		return ENU;
	};

protected:
	virtual double d1AzimuthdENU1(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d2AzimuthdENU2(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d3AzimuthdENU3(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d4AzimuthdENU4(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d5AzimuthdENU5(const RealVector E, const RealVector N, const RealVector U) = 0;

	virtual double d1ElevationdENU1(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d2ElevationdENU2(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d3ElevationdENU3(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d4ElevationdENU4(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d5ElevationdENU5(const RealVector E, const RealVector N, const RealVector U) = 0;

	virtual double d1RangedENU1(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d2RangedENU2(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d3RangedENU3(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d4RangedENU4(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d5RangedENU5(const RealVector E, const RealVector N, const RealVector U) = 0;

	virtual double d1EastdAER1(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d2EastdAER2(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d3EastdAER3(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d4EastdAER4(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d5EastdAER5(const RealVector A, const RealVector E, const RealVector R) = 0;

	virtual double d1NorthdAER1(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d2NorthdAER2(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d3NorthdAER3(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d4NorthdAER4(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d5NorthdAER5(const RealVector A, const RealVector E, const RealVector R) = 0;

	virtual double d1UpdAER1(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d2UpdAER2(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d3UpdAER3(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d4UpdAER4(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d5UpdAER5(const RealVector A, const RealVector E, const RealVector R) = 0;

};


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_AVX_SIMD_FUNCTION_RSQRT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_AVX_SIMD_FUNCTION_RSQRT_HPP_INCLUDED

#include <boost/simd/function/abs.hpp>
#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_zero_else.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/fnms.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/function/raw.hpp>
#include <boost/simd/function/pedantic.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD( rsqrt_
                         , (typename A0)
                         , bs::avx_
                         , bs::raw_tag
                         , bs::pack_< bd::single_<A0>, bs::avx_>
                         )
   {
     BOOST_FORCEINLINE A0 operator()( const bs::raw_tag &, A0 const& a0) const
      {
        return _mm256_rsqrt_ps( a0 );
      }
   };

  BOOST_DISPATCH_OVERLOAD( rsqrt_
                         , (typename A0)
                         , bs::avx_
                         , bs::raw_tag
                         , bs::pack_< bd::double_<A0>, bs::avx_>
                         )
   {
     BOOST_FORCEINLINE A0 operator()( const bs::raw_tag &, A0 const& a0) const
      {
        return _mm256_cvtps_pd(_mm_rsqrt_ps( _mm256_cvtpd_ps(a0) ));//The error for this approximation is no more than 1.5.e-12
      }
   };

  BOOST_DISPATCH_OVERLOAD( rsqrt_
                         , (typename A0)
                         , bs::avx_
                         , bs::pack_< bd::single_<A0>, bs::avx_>
                         )
   {
     BOOST_FORCEINLINE A0 operator()(A0 const& a00) const
      {
        A0 a0 =  raw_(rsqrt)(a00);
        A0 y = sqr(a0)*a00;
        a0 = a0*Ratio<A0, 1, 8>()*fnms(y, fnms(A0(3), y, A0(10)), A0(15)); //this is Halley cubically convergent iteration
        #ifndef BOOST_SIMD_NO_INFINITIES
        a0 = if_zero_else(a00 == Inf<A0>(),a0);
        #endif
        return if_else(is_eqz(a00), Inf<A0>(), a0);
      }
   };

  BOOST_DISPATCH_OVERLOAD( rsqrt_
                         , (typename A0)
                         , bs::avx_
                         , bs::pack_< bd::double_<A0>, bs::avx_>
                         )
   {
     BOOST_FORCEINLINE A0 operator()(A0 const& a00) const
      {
        // To obtain accuracy we need 3 Newton steps or one Halley step followed by one Newton from the raw estimate
        // the second method is a bit faster by half a cycle
        A0 a0 =  raw_(rsqrt)(a00);
        A0 y = sqr(a0)*a00;
        a0 = a0*Ratio<A0, 1, 8>()*fnms(y, fnms(A0(3), y, A0(10)), A0(15)); //this is Halley cubically convergent iteration
        a0 = refine_rsqrt(a00, a0);
        #ifndef BOOST_SIMD_NO_INFINITIES
        a0 = if_zero_else(a00 == Inf<A0>(),a0);
        #endif
        return if_else(is_eqz(a00), Inf<A0>(), a0);
      }
   };

   BOOST_DISPATCH_OVERLOAD ( rsqrt_
                          , (typename A0)
                          , bs::avx_
                          , bs::pedantic_tag
                          , bs::pack_<bd::single_<A0>, bs::avx_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (pedantic_tag const&
                                    ,const A0 & a00) const BOOST_NOEXCEPT
    {
      A0 a0 = a00;
      auto is_den = bs::abs(a00) < Smallestposval<A0>();
      #ifndef BOOST_SIMD_NO_DENORMALS
      a0 *= if_else(is_den, Denormalfactor<A0>(), One<A0>());
      #endif
      a0 = refine_rsqrt(a0, refine_rsqrt(a0, raw_(rsqrt)(a0)));
      #ifndef BOOST_SIMD_NO_DENORMALS
      a0 *= if_else(is_den, Denormalsqrtfactor<A0>(), One<A0>());
      #endif

      #ifndef BOOST_SIMD_NO_INFINITIES
      a0 = if_zero_else(a00 == Inf<A0>(),a0);
      #endif
      return if_else(is_eqz(a00), Inf<A0>(), a0);
    }
  };

   BOOST_DISPATCH_OVERLOAD ( rsqrt_
                          , (typename A0)
                          , bs::avx_
                          , bs::pedantic_tag
                          , bs::pack_<bd::double_<A0>, bs::avx_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (pedantic_tag const&
                                    ,const A0 & a00) const BOOST_NOEXCEPT
    {
      A0 a01 =  a00;
      auto is_den = bs::abs(a00) < Smallestposval<A0>();
      #ifndef BOOST_SIMD_NO_DENORMALS
      a01 *= if_else(is_den, Denormalfactor<A0>(), One<A0>());
      #endif
      A0 a0 =  raw_(rsqrt)(a01);
      A0 y = sqr(a0)*a01;
      a0 = a0*Ratio<A0, 1, 8>()*fnms(y, fnms(A0(3), y, A0(10)), A0(15)); //this is Halley cubically convergent iteration
      a0 = refine_rsqrt(a00, a0);
      #ifndef BOOST_SIMD_NO_DENORMALS
      a0 *= if_else(is_den, Denormalsqrtfactor<A0>(), One<A0>());
      #endif

      #ifndef BOOST_SIMD_NO_INFINITIES
      a0 = if_zero_else(a00 == Inf<A0>(),a0);
      #endif
      return if_else(is_eqz(a00), Inf<A0>(), a0);
    }
  };
} } }

#endif


// Boost.Geometry Index
//
// squared distance between point and nearest point of the box or point
//
// Copyright (c) 2011-2014 Adam Wulkiewicz, Lodz, Poland.
//
// This file was modified by Oracle on 2021.
// Modifications copyright (c) 2021 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_INDEX_DETAIL_ALGORITHMS_COMPARABLE_DISTANCE_NEAR_HPP
#define BOOST_GEOMETRY_INDEX_DETAIL_ALGORITHMS_COMPARABLE_DISTANCE_NEAR_HPP

#include <boost/geometry/algorithms/detail/comparable_distance/interface.hpp>
#include <boost/geometry/core/access.hpp>

#include <boost/geometry/index/detail/algorithms/sum_for_indexable.hpp>

namespace boost { namespace geometry { namespace index { namespace detail {

struct comparable_distance_near_tag {};

template <
    typename Point,
    typename PointIndexable,
    size_t N>
struct sum_for_indexable<Point, PointIndexable, point_tag, comparable_distance_near_tag, N>
{
    typedef typename geometry::default_comparable_distance_result<Point, PointIndexable>::type result_type;

    inline static result_type apply(Point const& pt, PointIndexable const& i)
    {
        return geometry::comparable_distance(pt, i);
    }
};

template <
    typename Point,
    typename BoxIndexable,
    size_t DimensionIndex>
struct sum_for_indexable_dimension<Point, BoxIndexable, box_tag, comparable_distance_near_tag, DimensionIndex>
{
    typedef typename geometry::default_comparable_distance_result<Point, BoxIndexable>::type result_type;

    inline static result_type apply(Point const& pt, BoxIndexable const& i)
    {
        typedef typename coordinate_type<Point>::type point_coord_t;
        typedef typename coordinate_type<BoxIndexable>::type indexable_coord_t;

        point_coord_t pt_c = geometry::get<DimensionIndex>(pt);
        indexable_coord_t ind_c_min = geometry::get<geometry::min_corner, DimensionIndex>(i);
        indexable_coord_t ind_c_max = geometry::get<geometry::max_corner, DimensionIndex>(i);

        result_type diff = 0;

        if ( pt_c < ind_c_min )
            diff = ind_c_min - pt_c;
        else if ( ind_c_max < pt_c )
            diff = pt_c - ind_c_max;

        return diff * diff;
    }
};

template <typename Point, typename Indexable>
typename geometry::default_comparable_distance_result<Point, Indexable>::type
comparable_distance_near(Point const& pt, Indexable const& i)
{
    return detail::sum_for_indexable<
        Point,
        Indexable,
        typename tag<Indexable>::type,
        detail::comparable_distance_near_tag,
        dimension<Indexable>::value
    >::apply(pt, i);
}

}}}} // namespace boost::geometry::index::detail

#endif // BOOST_GEOMETRY_INDEX_DETAIL_ALGORITHMS_COMPARABLE_DISTANCE_NEAR_HPP


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <scitbx/math/mean_and_variance.h>
#include <boost/python/class.hpp>

namespace scitbx { namespace af { namespace boost_python { namespace {

  struct mean_and_variance_wrappers
  {
    typedef math::mean_and_variance<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("mean_and_variance", no_init)
        .def(init<af::const_ref<double> const&>())
        .def(init<af::const_ref<double> const&,
                  af::const_ref<double> const&>())
        .def("have_weights", &w_t::have_weights)
        .def("mean", &w_t::mean)
        .def("gsl_stats_wvariance", &w_t::gsl_stats_wvariance)
        .def("gsl_stats_wsd", &w_t::gsl_stats_wsd)
        .def("standard_error_of_mean_calculated_from_sample_weights",
          &w_t::standard_error_of_mean_calculated_from_sample_weights)
        .def("unweighted_sample_variance", &w_t::unweighted_sample_variance)
        .def("unweighted_sample_standard_deviation",
          &w_t::unweighted_sample_standard_deviation)
        .def("unweighted_standard_error_of_mean",
          &w_t::unweighted_standard_error_of_mean)
        .def("sum_weights", &w_t::sum_weights)
        .def("sum_weights_sq", &w_t::sum_weights_sq)
        .def("sum_weights_values", &w_t::sum_weights_values)
        .def("sum_weights_delta_sq", &w_t::sum_weights_delta_sq)
      ;
    }
  };

} // namespace <anonymous>

  void wrap_flex_mean_and_variance()
  {
    mean_and_variance_wrappers::wrap();
  }

}}} // namespace scitbx::af::boost_python


#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>

#include <boost/make_shared.hpp>

#include "uxabiped/utils/Walking.h"
#include "uxabiped/utils/Kinematics.h"

namespace Bipedal
{

VirtualRobot::MathTools::ConvexHull2DPtr ComputeFootContact(const VirtualRobot::CollisionModelPtr& colModel)
{
    // let the feet collide with the floor and get the collision points
    VirtualRobot::MathTools::Plane plane =  VirtualRobot::MathTools::getFloorPlane();
    VirtualRobot::CollisionCheckerPtr colChecker = VirtualRobot::CollisionChecker::getGlobalCollisionChecker();
    std::vector< VirtualRobot::MathTools::ContactPoint > pointsFoot;
    std::vector< Eigen::Vector2f > points2D;

    // let the collision begin
    colChecker->getContacts(plane, colModel, pointsFoot, 5.0f);

    // project the points on the floor
    for (size_t u = 0; u < pointsFoot.size(); u++)
    {
        Eigen::Vector2f pt2d = VirtualRobot::MathTools::projectPointToPlane2D(pointsFoot[u].p, plane);
        points2D.push_back(pt2d);
    }

    // calculate the convex hulls and the appropriate centers
    VirtualRobot::MathTools::ConvexHull2DPtr hull = VirtualRobot::MathTools::createConvexHull2D(points2D);

    return hull;
}

Eigen::Vector2f CenterConvexHull(const VirtualRobot::MathTools::ConvexHull2DPtr& hull)
{
    Eigen::Vector2f center = VirtualRobot::MathTools::getConvexHullCenter(hull);

    // translate points of FootShape so, that center of convex hull is (0|0)
    for (unsigned i =  0; i < hull->vertices.size(); i++)
    {
        hull->vertices[i] -= center;
    }

    return center;
}

void TransformConvexHull(const VirtualRobot::MathTools::ConvexHull2DPtr& hull, const Eigen::Matrix3f& frame)
{
    // translate points of FootShape so, that center of convex hull is (0|0)
    for (unsigned i =  0; i < hull->vertices.size(); i++)
    {
        hull->vertices[i] = frame.block(0, 0, 2, 2) * hull->vertices[i] + frame.block(0, 2, 2, 1);
    }
}

Eigen::Matrix2f ComputeWalkingDirection(const Eigen::Vector2f& leftFootCenter, const Eigen::Vector2f& rightFootCenter)
{
    Eigen::Vector2f center = (leftFootCenter + rightFootCenter) * 0.5;
    Eigen::Vector2f centerToLeft = (leftFootCenter - center);
    centerToLeft.normalize();
    Eigen::Matrix2f rotNinety;
    rotNinety << 0, 1, -1, 0;
    Eigen::Vector2f walkingDirection = rotNinety * centerToLeft;

    // Note: The TCP coordinate system in Armar4 uses y as forward direction
    Eigen::Matrix2f pose;
    pose << -centerToLeft.x(), walkingDirection.x(), -centerToLeft.y(), walkingDirection.y();

    return pose;
}

Eigen::Vector2f computeHullContactPoint(const Eigen::Vector2f p, const VirtualRobot::MathTools::ConvexHull2DPtr& hull)
{
    double min = std::numeric_limits<double>::max();
    VirtualRobot::MathTools::Segment2D min_segment;
    for(const auto& segment : hull->segments)
    {
        double dist = VirtualRobot::MathTools::distPointSegment(hull->vertices[segment.id1],
                hull->vertices[segment.id2], p);
        if (dist < min)
        {
            min = dist;
            min_segment = segment;
        }
    }
    return VirtualRobot::MathTools::nearestPointOnSegment(hull->vertices[min_segment.id1], hull->vertices[min_segment.id2], p);
}

VirtualRobot::MathTools::ConvexHull2DPtr computeConvexHull(const VirtualRobot::RobotNodePtr& foot,
                                                           const VirtualRobot::RobotNodePtr& tcp)
{
    auto colModel = foot->getCollisionModel()->clone();
    Eigen::Matrix4f relPose = tcp->getGlobalPose().inverse() * colModel->getGlobalPose();
    colModel->setGlobalPose(relPose);
    auto hull = Bipedal::ComputeFootContact(colModel);
    Bipedal::CenterConvexHull(hull);
    return hull;
}

VirtualRobot::MathTools::ConvexHull2DPtr computeSupportPolygone(const Eigen::Matrix4f& leftFootPose,
                                                                const Eigen::Matrix4f& rightFootPose,
                                                                const VirtualRobot::MathTools::ConvexHull2DPtr& leftFootHull,
                                                                const VirtualRobot::MathTools::ConvexHull2DPtr& rightFootHull,
                                                                Bipedal::SupportPhase phase)
{
    if (phase == SUPPORT_LEFT)
        return boost::make_shared<VirtualRobot::MathTools::ConvexHull2D>(*leftFootHull);

    if (phase == SUPPORT_RIGHT)
        return boost::make_shared<VirtualRobot::MathTools::ConvexHull2D>(*rightFootHull);

    if (phase == SUPPORT_BOTH)
    {
        Eigen::Vector2f offset = rightFootPose.block(0, 3, 2, 1) - leftFootPose.block(0, 3, 2, 1);

        std::vector<Eigen::Vector2f> points;
        for (const auto& v : leftFootHull->vertices)
        {
            points.push_back(v - offset/2);
        }
        for (const auto& v : rightFootHull->vertices)
        {
            points.push_back(v + offset/2);
        }

        auto dualSupportHull = VirtualRobot::MathTools::createConvexHull2D(points);
        Bipedal::CenterConvexHull(dualSupportHull);

        return dualSupportHull;
    }

    return VirtualRobot::MathTools::ConvexHull2DPtr();
}

}


#include <boost/math/quadrature/exp_sinh.hpp>