#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/maptbx/structure_factors.h>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>

namespace cctbx { namespace maptbx { namespace structure_factors {

namespace {

  struct to_map_wrappers
  {
    typedef to_map<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("structure_factors_to_map", no_init)
        .def(init<
          sgtbx::space_group const&,
          bool,
          af::const_ref<miller::index<> > const&,
          af::const_ref<std::complex<double> > const&,
          af::int3 const&,
          af::flex_grid<> const&,
          bool,
          optional<bool> >((
            arg("space_group"),
            arg("anomalous_flag"),
            arg("miller_indices"),
            arg("structure_factors"),
            arg("n_real"),
            arg("map_grid"),
            arg("conjugate_flag"),
            arg("treat_restricted")=true)))
        .def("complex_map", &w_t::complex_map)
      ;
    }
  };

  struct from_map_wrappers
  {
    typedef from_map<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("structure_factors_from_map", no_init)
        .def(init<
          uctbx::unit_cell const&,
          sgtbx::space_group_type const&,
          bool,
          double,
          af::const_ref<std::complex<double>,
            af::c_grid_padded<3> > const&,
          bool,
          optional<bool> >((
            arg("unit_cell"),
            arg("space_group_type"),
            arg("anomalous_flag"),
            arg("d_min"),
            arg("complex_map"),
            arg("conjugate_flag"),
            arg("discard_indices_affected_by_aliasing")=false)))
        .def(init<
          bool,
          af::const_ref<miller::index<> > const&,
          af::const_ref<std::complex<double>,
            af::c_grid_padded<3> > const&,
          bool,
          optional<bool> >((
            arg("anomalous_flag"),
            arg("miller_indices"),
            arg("complex_map"),
            arg("conjugate_flag"),
            arg("allow_miller_indices_outside_map")=false)))
        .def(init<
          sgtbx::space_group const&,
          bool,
          af::const_ref<miller::index<> > const&,
          af::const_ref<std::complex<double>,
          af::c_grid_padded<3> > const&,
          bool>((
            arg("space_group"),
            arg("anomalous_flag"),
            arg("miller_indices"),
            arg("complex_map"),
            arg("conjugate_flag"))))
        .def("miller_indices", &w_t::miller_indices)
        .def("data", &w_t::data)
        .def("n_indices_affected_by_aliasing",
             &w_t::n_indices_affected_by_aliasing)
        .def("outside_map", &w_t::outside_map)
      ;
    }
  };

}} // namespace structure_factors::<anoymous>

namespace boost_python {

  void wrap_structure_factors()
  {
    structure_factors::to_map_wrappers::wrap();
    structure_factors::from_map_wrappers::wrap();
  }

}}} // namespace cctbx::maptbx::boost_python


#ifndef STAN_MATH_PRIM_SCAL_PROB_BERNOULLI_LOGIT_LPMF_HPP
#define STAN_MATH_PRIM_SCAL_PROB_BERNOULLI_LOGIT_LPMF_HPP

#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/scal/meta/operands_and_partials.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_bounded.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/fun/size_zero.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/inv_logit.hpp>
#include <stan/math/prim/scal/fun/log1m.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <boost/random/bernoulli_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>

namespace stan {
namespace math {

/**
 * Returns the log PMF of the logit-parametrized Bernoulli distribution. If
 * containers are supplied, returns the log sum of the probabilities.
 *
 * @tparam T_n type of integer parameter
 * @tparam T_prob type of chance of success parameter
 * @param n integer parameter
 * @param theta logit-transformed chance of success parameter
 * @return log probability or log sum of probabilities
 * @throw std::domain_error if theta is infinite.
 * @throw std::invalid_argument if container sizes mismatch.
 */
template <bool propto, typename T_n, typename T_prob>
typename return_type<T_prob>::type bernoulli_logit_lpmf(const T_n& n,
                                                        const T_prob& theta) {
  static const char* function = "bernoulli_logit_lpmf";
  typedef
      typename stan::partials_return_type<T_n, T_prob>::type T_partials_return;

  using std::exp;

  if (size_zero(n, theta))
    return 0.0;

  T_partials_return logp(0.0);

  check_bounded(function, "n", n, 0, 1);
  check_not_nan(function, "Logit transformed probability parameter", theta);
  check_consistent_sizes(function, "Random variable", n,
                         "Probability parameter", theta);

  if (!include_summand<propto, T_prob>::value)
    return 0.0;

  scalar_seq_view<T_n> n_vec(n);
  scalar_seq_view<T_prob> theta_vec(theta);
  size_t N = max_size(n, theta);
  operands_and_partials<T_prob> ops_partials(theta);

  for (size_t n = 0; n < N; n++) {
    const T_partials_return theta_dbl = value_of(theta_vec[n]);

    const int sign = 2 * n_vec[n] - 1;
    const T_partials_return ntheta = sign * theta_dbl;
    const T_partials_return exp_m_ntheta = exp(-ntheta);

    // Handle extreme values gracefully using Taylor approximations.
    static const double cutoff = 20.0;
    if (ntheta > cutoff)
      logp -= exp_m_ntheta;
    else if (ntheta < -cutoff)
      logp += ntheta;
    else
      logp -= log1p(exp_m_ntheta);

    if (!is_constant_struct<T_prob>::value) {
      if (ntheta > cutoff)
        ops_partials.edge1_.partials_[n] -= exp_m_ntheta;
      else if (ntheta < -cutoff)
        ops_partials.edge1_.partials_[n] += sign;
      else
        ops_partials.edge1_.partials_[n]
            += sign * exp_m_ntheta / (exp_m_ntheta + 1);
    }
  }
  return ops_partials.build(logp);
}

template <typename T_n, typename T_prob>
inline typename return_type<T_prob>::type bernoulli_logit_lpmf(
    const T_n& n, const T_prob& theta) {
  return bernoulli_logit_lpmf<false>(n, theta);
}

}  // namespace math
}  // namespace stan
#endif


/*
*  @file 		ex9.cpp
*  @details  	This file is the solution to exercise 9.
*  @author    	Alexander Rettkowski
*  @date      	12.07.2017
*/
#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_object.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/fusion/include/io.hpp>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <boost/property_map/property_map.hpp>

#include <boost/timer/timer.hpp>
#include <boost/chrono.hpp>

#include <iostream>
#include <string>
#include <complex>
#include <fstream>
#include <queue>

#include <omp.h>

using namespace boost;

namespace exercise9
{
	namespace qi = boost::spirit::qi;
	namespace ascii = boost::spirit::ascii;
	struct edge
	{
		int startNode;
		int endNode;
		int length;
	};
}

BOOST_FUSION_ADAPT_STRUCT(
	exercise9::edge,
	(int, startNode)
	(int, endNode)
	(int, length)
)

namespace exercise9
{
	template <typename Iterator>
	struct line_parser : qi::grammar<Iterator, edge()>
	{
		line_parser() : line_parser::base_type(start)
		{
			using qi::int_;
			start %= int_ >> ' ' >> int_ >> ' ' >> int_;
		}

		qi::rule<Iterator, edge()> start;
	};
}

/**
* This method checks (in a naive way), if a given number is prime or not.
* @param number The number to check.
* @returns True, if number is prime; false otherwise.
*/
bool isPrime(int number) {
	if (number == 2)
		return true;
	if (number % 2 == 0)
		return false;
	for (int i = 3; (i*i) <= number; i += 2) {
		if (number % i == 0) return false;
	}
	return true;
}

/**
* The function that calculates the shortest paths using Dijkstra's method.
* @param numberOfNodes Number of nodes in the graph.
* @param graph The graph represented as a vector auf edge-vectors.
* @param startNode The id of the node where the search starts.
* @returns The weight of the Steiner Tree and the edge list of the tree coupled as a std::pair
*/
std::pair<int, std::list<std::pair<int, int>>>  steinerTree(int numberOfNodes, std::vector< std::vector< std::pair<int, int> > > graph, int startNode)
{
	std::vector<int> connectedVertices;
	connectedVertices.push_back(startNode);
	std::list<std::pair<int, int>> edgeList;

	std::vector<int> predecessors(numberOfNodes, -1);
	std::vector<int> distanceTo(numberOfNodes, INT_MAX);
	std::priority_queue< std::pair<int, int>, std::vector< std::pair<int, int> >, std::greater< std::pair<int, int> > > queue;
	queue.push(std::pair<int, int>(startNode, 0));
	distanceTo[startNode] = 0;
	//predecessors[startNode] = -1;
	int steinerWeight = 0;

	int currentNode, compareNode, compareNodeDistance, currentNodeDistance;

	while (!queue.empty()) {
		currentNode = queue.top().first;
		currentNodeDistance = queue.top().second;
		queue.pop();

		if (distanceTo[currentNode] < currentNodeDistance) continue;

		for (int i = 0; i < graph[currentNode].size(); i++) {
			compareNode = graph[currentNode][i].first;
			compareNodeDistance = graph[currentNode][i].second;
			if (distanceTo[compareNode] > distanceTo[currentNode] + compareNodeDistance) {
				if (isPrime(compareNode))
				{
					predecessors[compareNode] = currentNode;

					int backtracknode = compareNode;
					while (std::find(connectedVertices.begin(), connectedVertices.end(), backtracknode) == connectedVertices.end())
					{
						for (std::pair<int, int> edge : graph[predecessors[backtracknode]])
						{
							if (edge.first == backtracknode)
							{
								edgeList.push_back(std::pair<int, int>(predecessors[backtracknode], backtracknode));
								steinerWeight += edge.second;
							}
						}

						distanceTo[backtracknode] = 0;
						connectedVertices.push_back(backtracknode);
						backtracknode = predecessors[backtracknode];
						queue.push(std::pair<int, int>(backtracknode, 0));
					}
				}
				else
				{
					distanceTo[compareNode] = distanceTo[currentNode] + compareNodeDistance;
					queue.push(std::pair<int, int>(compareNode, distanceTo[compareNode]));
					predecessors[compareNode] = currentNode;
				}
			}
		}
	}

	return std::pair<int, std::list<std::pair<int, int>>>(steinerWeight, edgeList);
}

/**
* The main function that reads in a file and processes it.
* @param argc Number of command line arguments.
* @param *argv a pointer to the array of command line arguments.
*/
int main(int argc, char *argv[])
{
	timer::cpu_timer boostTimer;

	using boost::spirit::ascii::space;
	typedef std::string::const_iterator iterator_type;
	typedef exercise9::line_parser<iterator_type> line_parser;
	line_parser parser;
	std::string currentLine;
	std::ifstream file(argv[1]);

	// get number of nodes
	char delimiter = ' ';
	getline(file, currentLine, delimiter);
	const int numberOfNodes = stoi(currentLine);
	getline(file, currentLine);
	int constSub = 1;

	std::vector<std::vector<std::pair<int, int>>> edges(numberOfNodes);
	while (getline(file, currentLine))
	{
		exercise9::edge parsedLine;
		std::string::const_iterator currentPosition = currentLine.begin();
		std::string::const_iterator lineEnd = currentLine.end();
		bool parsingSucceeded = phrase_parse(currentPosition, lineEnd, parser, space, parsedLine);

		if (parsingSucceeded && currentPosition == lineEnd)
		{
			std::pair<int, int> *tempEdge = new std::pair<int, int>();
			tempEdge->first = parsedLine.endNode - constSub;
			tempEdge->second = parsedLine.length;
			edges[parsedLine.startNode - constSub].push_back(*tempEdge);

			std::pair<int, int> *reverseEdge = new std::pair<int, int>();
			reverseEdge->first = parsedLine.startNode - constSub;
			reverseEdge->second = parsedLine.length;
			edges[parsedLine.endNode - constSub].push_back(*reverseEdge);
		}
	}

	file.close();

	timer::cpu_timer runTimer;
	omp_set_dynamic(0);
	omp_set_num_threads(std::stoi(argv[2]));
	std::vector<int> terminalIds;
	for (int i = 0; i < numberOfNodes; i++)
	{
		if (isPrime(i))
			terminalIds.push_back(i);
	}
	std::vector<int> lengths(terminalIds.size());

#pragma omp parallel for
	for (int i = 0; i < terminalIds.size(); i++)
		{
		    std::pair<int, std::list<std::pair<int, int>>> currentRun = steinerTree(numberOfNodes, edges, terminalIds[i]);
			lengths[i] = currentRun.first;
			std::cout << "Length starting from " << i << ": " << lengths[i] << std::endl;
		}

	int minlength = INT32_MAX;
	int minId = -1;
	for (int i = 0; i < terminalIds.size(); i++)
	{
		if (lengths[i] < minlength)
		{
			minlength = lengths[i];
			minId = i;
		}
	}

	timer::cpu_times runtime = boostTimer.elapsed();
	std::list<std::pair<int, int>> edgelist = steinerTree(numberOfNodes, edges, minId).second;

	std::cout << "TLEN: " << minlength << std::endl;
	std::cout << "TREE: ";

	for (std::pair<int, int> edge : edgelist)
	{
		std::cout << "(" << edge.first << ", " << edge.second << ") ";
	}
	std::cout << std::endl;

	timer::cpu_times time = boostTimer.elapsed();
	std::cout << "TIME: " << (time.user + time.system) / 1e9 << "s\n";
	std::cout << "WALL: " << runtime.wall / 1e9 << "s\n";

	return 0;
}


/*!
@file
Defines `boost::hana::Monoid::laws`.

@copyright Louis Dionne 2014
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_HANA_MONOID_LAWS_HPP
#define BOOST_HANA_MONOID_LAWS_HPP

#include <boost/hana/monoid/monoid.hpp>

#include <boost/hana/comparable/comparable.hpp>
#include <boost/hana/core/datatype.hpp>
#include <boost/hana/logical/logical.hpp>
#include <boost/hana/searchable/searchable.hpp>

namespace boost { namespace hana {
    struct Monoid::laws {
        template <typename MonoidObjects>
        static constexpr auto check(MonoidObjects objs) {
            return all(objs, [=](auto x) {
                return all(objs, [=](auto y) {
                    return all(objs, [=](auto z) {
                        using M = datatype_t<decltype(x)>;
                        return and_(
                            equal(plus(zero<M>, x), x),
                            equal(plus(x, zero<M>), x),
                            equal(plus(x, plus(y, z)), plus(plus(x, y), z))
                        );
                    });
                });
            });
        }
    };
}} // end namespace boost::hana

#endif // !BOOST_HANA_MONOID_LAWS_HPP


// ROS includes.
#include <ros/ros.h>
#include <ros/time.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <tf_conversions/tf_eigen.h>
#include <eigen_conversions/eigen_msg.h>

#include <Eigen/Geometry>

#include "rwth_visual_odometry/VisualOdometry.h"

ros::Publisher pub_message;

int main(int argc, char **argv)
{
    // Set up ROS.
    ros::init(argc, argv, "tf2visual");
    ros::NodeHandle n;

    std::string pub_topic;
    std::string sensor_frame_id;
    std::string odom_frame_id;

    // Initialize node parameters from launch file or command line.
    // Use a private node handle so that multiple instances of the node can be run simultaneously
    // while using different parameters.
    ros::NodeHandle private_node_handle_("~");

    private_node_handle_.param("motion_parameters", pub_topic, std::string("/spencer/sensors/visual_odometry/motion_matrix"));
    private_node_handle_.param("sensor_frame_id", sensor_frame_id, std::string("rgbd_front_top_rgb_optical_frame"));
    private_node_handle_.param("odom_frame_id", odom_frame_id, std::string("odom"));
    pub_message = n.advertise<rwth_visual_odometry::VisualOdometry>(pub_topic.c_str(), 3);

    tf::TransformListener listener;

    ros::Rate updateRate(30.0);
    while(ros::ok()) {
        ros::spinOnce();
        updateRate.sleep();

        tf::StampedTransform transform;
        try{
          listener.waitForTransform(sensor_frame_id, odom_frame_id, ros::Time(), ros::Duration(1.0));
          listener.lookupTransform(sensor_frame_id, odom_frame_id, ros::Time(0), transform);
        }
        catch (tf::TransformException &ex) {
          ROS_WARN_THROTTLE(10.0, "Failed to lookup transform in tf2visual: %s",ex.what());
        }

        Eigen::Affine3d finalTransform;
        tf::transformTFToEigen(transform, finalTransform);

        Eigen::Matrix4d ros2tracker_R;
        ros2tracker_R << 0,-1,0,0, 0,0,-1,0, 1,0,0,0, 0,0,0,1;

        Eigen::Matrix4d finalMatrix = finalTransform.matrix().inverse();

        Eigen::Matrix4d rotatedFinalMatrix = (ros2tracker_R*finalMatrix);

        // and mirror at x-z-plane (turns a +0 in a -0 in t and does sth. to R, but still...)
        /*Eigen::Affine3d mirrorTransform;
        mirrorTransform.setIdentity();
        Eigen::Matrix3d A_rh = mirrorTransform.linear();
        Eigen::Vector3d b_rh = mirrorTransform.translation();

        Eigen::Matrix3d S_y; S_y << 1, 0, 0,   0, -1, 0,  0, 0, 1;

        Eigen::Matrix3d A_lh = S_y * A_rh * S_y;
        Eigen::Vector3d b_lh = S_y * b_rh;

        Eigen::Affine3d finalMirrorTransform;
        finalMirrorTransform.linear() = A_lh;
        finalMirrorTransform.translation() = b_lh;*/

        //Eigen::Matrix4d reallyFinalMatrix = (finalMirrorTransform*rotatedFinalMatrix).matrix().transpose();
        Eigen::Matrix4d reallyFinalMatrix = rotatedFinalMatrix.transpose();

        rwth_visual_odometry::VisualOdometry fovis_info_msg;
        fovis_info_msg.header.frame_id = sensor_frame_id;
        fovis_info_msg.header.stamp = transform.stamp_;
        fovis_info_msg.motion_estimate_valid = true;
        fovis_info_msg.transformation_matrix.resize(4*4);
        for(int i = 0; i < 4*4; i++)
            fovis_info_msg.transformation_matrix[i] = reallyFinalMatrix.data()[i];
         pub_message.publish(fovis_info_msg);
    }

    return 0;
}


#include <armadillo>

#include "settings.h"

#include "landmark/LandmarkIO.h"

#include "model/Model.h"
#include "model/ModelReader.h"
#include "mesh/MeshIO.h"
#include "mesh/Mesh.h"
#include "mesh/MeshSmooth.h"

#include "optimization/fitmodel/Energy.h"
#include "optimization/fitmodel/EnergyMinimizer.h"
#include "optimization/fitmodel/MinimizerSettings.h"

int main(int argc, char* argv[]) {

  Settings settings(argc, argv);

  // read input data
  Mesh target = MeshIO::read(settings.target);
  ModelReader reader(settings.model);
  Model model = reader.get_model();

  // deal with target meshes that do not provide normals
  if( settings.fixedNeighbors == false && target.has_normals() == false ) {

    // try to estimate normals if the mesh has faces
    if( target.has_faces() == true ) {
      NormalEstimation estimation(target);
      target.set_vertex_normals(estimation.compute());
    } // end if
    else {
      // use point-to-point distance measure
      settings.energySettings.searchStrategy =
        fitModel::EnergySettings::SearchStrategy::BASIC;
    } // end else

  } // end if

  fitModel::EnergyData data(model, target);

  if(settings.landmarksPresent == true) {
    data.landmarks = LandmarkIO::read(settings.landmarks);

    // use landmarks only for initialization if wanted
    if(settings.useLandmarksOnlyForInitialization == true) {

      fitModel::Energy energy(data, settings.energySettings);

      fitModel::MinimizerSettings minimizerSettings = settings.minimizerSettings;

      // use one iteration
      minimizerSettings.iterationAmount = 1;
      fitModel::EnergyMinimizer minimizer(energy, minimizerSettings);

      minimizer.minimize();

      // copy weights
      data.speakerWeights = energy.data().speakerWeights;
      data.phonemeWeights = energy.data().phonemeWeights;

      // remove landmarks
      data.landmarks.clear();

    } // end if useLandmarks
  } // end if landmarksPresent

  // configure for fitting only
  settings.energySettings.weights["dataTerm"] = 1;
  settings.energySettings.weights["landmarkTerm"] = settings.landmarkTermWeight;

  fitModel::Energy energy(data, settings.energySettings);

  fitModel::EnergyMinimizer minimizer(energy, settings.minimizerSettings);

  minimizer.minimize();

  Mesh result = energy.derived_data().source;

  // remove normals
  result.get_vertex_normals().clear();

  MeshSmooth(result).apply(settings.meshSmoothIterations);

  MeshIO::write(result, settings.output);

  return 0;

}


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

#ifndef SOLVERS_SUITESPARSESOLVER_HPP
#define SOLVERS_SUITESPARSESOLVER_HPP

#include <Eigen/CholmodSupport>
#include <Eigen/Dense>
#include <Eigen/KLUSupport>
#include <Eigen/UmfPackSupport>

#include <solvers/Solver.hpp>
#include <solvers/SparseSystem.hpp>

namespace solvers {

enum class SuiteSparseMethod {
    SimplicialLLT,
    SimplicialLDLT,
    SupernodalLLT,
    LU,
    KLU
};

/**
 * \brief SuiteSparseSolver Direct solver based on SuiteSparse library.
 */
class SuiteSparseSolver : public Solver {
private:
    /**
     * \brief _method Resolution algorithm.
     */
    SuiteSparseMethod _method;

    /**
     * \brief _gpu Whether to use GPU (even if GPU support is very limited).
     */
    bool _gpu;

    /**
     * \brief _cholmod_solve Solve a sparse linear system using CHOLMOD module.
     *
     * \param[in] system Sparse linear system.
     * \param[out] duration Running time of the resolution in seconds.
     * \return Solution vector of the sparse linear system in Eigen format.
     */
    Eigen::VectorXd _cholmod_solve(const SparseSystem& system,
                                   double& duration) const;

    /**
     * \brief _cholmod_gpu_solve Solve a sparse linear system using CHOLMOD module on GPU.
     *
     * \param[in] system Sparse linear system.
     * \param[out] duration Running time of the resolution in seconds.
     * \return Solution vector of the sparse linear system in Eigen format.
     */
    Eigen::VectorXd _cholmod_gpu_solve(const SparseSystem& system,
                                       double& duration) const;

    /**
     * \brief _lu_solve Solve a sparse linear system using LU or KLU modules.
     *
     * \param[in] system Sparse linear system.
     * \param[out] duration Running time of the resolution in seconds.
     * \return Solution vector of the sparse linear system in Eigen format.
     */
    Eigen::VectorXd _lu_solve(const SparseSystem& system,
                              double& duration) const;

public:
    explicit SuiteSparseSolver(
        const SuiteSparseMethod method = SuiteSparseMethod::SupernodalLLT,
        const bool gpu = false)
        : _method(method), _gpu(gpu){};

    Eigen::VectorXd solve(const SparseSystem& system,
                          double& duration) const override;
};

}    // namespace solvers

#endif    // SOLVERS_SUITESPARSESOLVER_HPP


#pragma once

#include <Eigen/Dense>

#include <PnC/WBC/Task.hpp>

enum BasicTaskType { JOINT, LINKXYZ, LINKORI, CENTROID, COM };

class BasicTask : public Task {
   public:
    BasicTask(RobotSystem* robot_, const BasicTaskType& taskType_,
              const int& _dim, const int& _link_idx = 0);
    virtual ~BasicTask(){};

    int getLinkID(){
        return link_idx_;
    }

   private:
    /* Update op_cmd, pos_err, vel_des, acc_des
     *
     * LINKORI :
     * pos_des_ = [quat_w, quat_x, quat_y, quat_z]
     * vel_des_ = [w_x, w_y, w_z]
     * acc_des_ = [a_x, a_y, a_z]
     *
     * CENTROID :
     *  p -- center of mass position
     *  k -- angular momentum
     *  l -- linear momentum
     *  pos_des_ = [dummy , dummy , dummy ,  px   , py    , pz     ]
     *  vel_des_ = [kx    , ky    , kz    ,  lx   , ly    , lz     ]
     *  acc_des_ = [kx_dot, ky_dot, kz_dot, lx_dot, ly_dot, lz_dot ]
     *
     */
    virtual bool _UpdateCommand(const Eigen::VectorXd& pos_des,
                                const Eigen::VectorXd& vel_des,
                                const Eigen::VectorXd& acc_des);
    virtual bool _UpdateTaskJacobian();
    virtual bool _UpdateTaskJDotQdot();

    BasicTaskType task_type_;
    int link_idx_;
    std::string task_type_string_;
};


/*
 [auto_generated]
 boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp

 [begin_description]
 Implementation of the Dormand-Prince 5(4) method. This stepper can also be used with the dense-output controlled stepper.
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

 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)
 */

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA_DOPRI5_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA_DOPRI5_HPP_INCLUDED

#include <boost/numeric/odeint/util/bind.hpp>

#include <boost/numeric/odeint/stepper/base/explicit_error_stepper_fsal_base.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>

#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/util/same_instance.hpp>

namespace boost {
namespace numeric {
namespace odeint {

template<
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = typename algebra_dispatcher< State >::algebra_type ,
class Operations = typename operations_dispatcher< State >::operations_type ,
class Resizer = initially_resizer
>
class runge_kutta_dopri5
#ifndef DOXYGEN_SKIP
: public explicit_error_stepper_fsal_base<
  runge_kutta_dopri5< State , Value , Deriv , Time , Algebra , Operations , Resizer > ,
  5 , 5 , 4 , State , Value , Deriv , Time , Algebra , Operations , Resizer >
#else
: public explicit_error_stepper_fsal_base
#endif
{

public :

    #ifndef DOXYGEN_SKIP
    typedef explicit_error_stepper_fsal_base<
    runge_kutta_dopri5< State , Value , Deriv , Time , Algebra , Operations , Resizer > ,
    5 , 5 , 4 , State , Value , Deriv , Time , Algebra , Operations , Resizer > stepper_base_type;
    #else
    typedef explicit_error_stepper_fsal_base< runge_kutta_dopri5< ... > , ... > stepper_base_type;
    #endif

    typedef typename stepper_base_type::state_type state_type;
    typedef typename stepper_base_type::value_type value_type;
    typedef typename stepper_base_type::deriv_type deriv_type;
    typedef typename stepper_base_type::time_type time_type;
    typedef typename stepper_base_type::algebra_type algebra_type;
    typedef typename stepper_base_type::operations_type operations_type;
    typedef typename stepper_base_type::resizer_type resizer_type;

    #ifndef DOXYGEN_SKIP
    typedef typename stepper_base_type::stepper_type stepper_type;
    typedef typename stepper_base_type::wrapped_state_type wrapped_state_type;
    typedef typename stepper_base_type::wrapped_deriv_type wrapped_deriv_type;
    #endif // DOXYGEN_SKIP

    runge_kutta_dopri5( const algebra_type &algebra = algebra_type() ) : stepper_base_type( algebra )
    { }

    template< class System , class StateIn , class DerivIn , class StateOut , class DerivOut >
    void do_step_impl( System system , const StateIn &in , const DerivIn &dxdt_in , time_type t ,
            StateOut &out , DerivOut &dxdt_out , time_type dt )
    {
        const value_type a2 = static_cast<value_type> ( 1 ) / static_cast<value_type>( 5 );
        const value_type a3 = static_cast<value_type> ( 3 ) / static_cast<value_type> ( 10 );
        const value_type a4 = static_cast<value_type> ( 4 ) / static_cast<value_type> ( 5 );
        const value_type a5 = static_cast<value_type> ( 8 )/static_cast<value_type> ( 9 );

        const value_type b21 = static_cast<value_type> ( 1 ) / static_cast<value_type> ( 5 );

        const value_type b31 = static_cast<value_type> ( 3 ) / static_cast<value_type>( 40 );
        const value_type b32 = static_cast<value_type> ( 9 ) / static_cast<value_type>( 40 );

        const value_type b41 = static_cast<value_type> ( 44 ) / static_cast<value_type> ( 45 );
        const value_type b42 = static_cast<value_type> ( -56 ) / static_cast<value_type> ( 15 );
        const value_type b43 = static_cast<value_type> ( 32 ) / static_cast<value_type> ( 9 );

        const value_type b51 = static_cast<value_type> ( 19372 ) / static_cast<value_type>( 6561 );
        const value_type b52 = static_cast<value_type> ( -25360 ) / static_cast<value_type> ( 2187 );
        const value_type b53 = static_cast<value_type> ( 64448 ) / static_cast<value_type>( 6561 );
        const value_type b54 = static_cast<value_type> ( -212 ) / static_cast<value_type>( 729 );

        const value_type b61 = static_cast<value_type> ( 9017 ) / static_cast<value_type>( 3168 );
        const value_type b62 = static_cast<value_type> ( -355 ) / static_cast<value_type>( 33 );
        const value_type b63 = static_cast<value_type> ( 46732 ) / static_cast<value_type>( 5247 );
        const value_type b64 = static_cast<value_type> ( 49 ) / static_cast<value_type>( 176 );
        const value_type b65 = static_cast<value_type> ( -5103 ) / static_cast<value_type>( 18656 );

        const value_type c1 = static_cast<value_type> ( 35 ) / static_cast<value_type>( 384 );
        const value_type c3 = static_cast<value_type> ( 500 ) / static_cast<value_type>( 1113 );
        const value_type c4 = static_cast<value_type> ( 125 ) / static_cast<value_type>( 192 );
        const value_type c5 = static_cast<value_type> ( -2187 ) / static_cast<value_type>( 6784 );
        const value_type c6 = static_cast<value_type> ( 11 ) / static_cast<value_type>( 84 );

        typename odeint::unwrap_reference< System >::type &sys = system;

        m_k_x_tmp_resizer.adjust_size( in , detail::bind( &stepper_type::template resize_k_x_tmp_impl<StateIn> , detail::ref( *this ) , detail::_1 ) );

        //m_x_tmp = x + dt*b21*dxdt
        stepper_base_type::m_algebra.for_each3( m_x_tmp.m_v , in , dxdt_in ,
                typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , dt*b21 ) );

        sys( m_x_tmp.m_v , m_k2.m_v , t + dt*a2 );
        // m_x_tmp = x + dt*b31*dxdt + dt*b32*m_k2
        stepper_base_type::m_algebra.for_each4( m_x_tmp.m_v , in , dxdt_in , m_k2.m_v ,
                typename operations_type::template scale_sum3< value_type , time_type , time_type >( 1.0 , dt*b31 , dt*b32 ));

        sys( m_x_tmp.m_v , m_k3.m_v , t + dt*a3 );
        // m_x_tmp = x + dt * (b41*dxdt + b42*m_k2 + b43*m_k3)
        stepper_base_type::m_algebra.for_each5( m_x_tmp.m_v , in , dxdt_in , m_k2.m_v , m_k3.m_v ,
                typename operations_type::template scale_sum4< value_type , time_type , time_type , time_type >( 1.0 , dt*b41 , dt*b42 , dt*b43 ));

        sys( m_x_tmp.m_v, m_k4.m_v , t + dt*a4 );
        stepper_base_type::m_algebra.for_each6( m_x_tmp.m_v , in , dxdt_in , m_k2.m_v , m_k3.m_v , m_k4.m_v ,
                typename operations_type::template scale_sum5< value_type , time_type , time_type , time_type , time_type >( 1.0 , dt*b51 , dt*b52 , dt*b53 , dt*b54 ));

        sys( m_x_tmp.m_v , m_k5.m_v , t + dt*a5 );
        stepper_base_type::m_algebra.for_each7( m_x_tmp.m_v , in , dxdt_in , m_k2.m_v , m_k3.m_v , m_k4.m_v , m_k5.m_v ,
                typename operations_type::template scale_sum6< value_type , time_type , time_type , time_type , time_type , time_type >( 1.0 , dt*b61 , dt*b62 , dt*b63 , dt*b64 , dt*b65 ));

        sys( m_x_tmp.m_v , m_k6.m_v , t + dt );
        stepper_base_type::m_algebra.for_each7( out , in , dxdt_in , m_k3.m_v , m_k4.m_v , m_k5.m_v , m_k6.m_v ,
                typename operations_type::template scale_sum6< value_type , time_type , time_type , time_type , time_type , time_type >( 1.0 , dt*c1 , dt*c3 , dt*c4 , dt*c5 , dt*c6 ));

        // the new derivative
        sys( out , dxdt_out , t + dt );
    }

    template< class System , class StateIn , class DerivIn , class StateOut , class DerivOut , class Err >
    void do_step_impl( System system , const StateIn &in , const DerivIn &dxdt_in , time_type t ,
            StateOut &out , DerivOut &dxdt_out , time_type dt , Err &xerr )
    {
        const value_type c1 = static_cast<value_type> ( 35 ) / static_cast<value_type>( 384 );
        const value_type c3 = static_cast<value_type> ( 500 ) / static_cast<value_type>( 1113 );
        const value_type c4 = static_cast<value_type> ( 125 ) / static_cast<value_type>( 192 );
        const value_type c5 = static_cast<value_type> ( -2187 ) / static_cast<value_type>( 6784 );
        const value_type c6 = static_cast<value_type> ( 11 ) / static_cast<value_type>( 84 );

        const value_type dc1 = c1 - static_cast<value_type> ( 5179 ) / static_cast<value_type>( 57600 );
        const value_type dc3 = c3 - static_cast<value_type> ( 7571 ) / static_cast<value_type>( 16695 );
        const value_type dc4 = c4 - static_cast<value_type> ( 393 ) / static_cast<value_type>( 640 );
        const value_type dc5 = c5 - static_cast<value_type> ( -92097 ) / static_cast<value_type>( 339200 );
        const value_type dc6 = c6 - static_cast<value_type> ( 187 ) / static_cast<value_type>( 2100 );
        const value_type dc7 = static_cast<value_type>( -1 ) / static_cast<value_type> ( 40 );

        /* ToDo: copy only if &dxdt_in == &dxdt_out ? */
        if( same_instance( dxdt_in , dxdt_out ) )
        {
            m_dxdt_tmp_resizer.adjust_size( in , detail::bind( &stepper_type::template resize_dxdt_tmp_impl<StateIn> , detail::ref( *this ) , detail::_1 ) );
            boost::numeric::odeint::copy( dxdt_in , m_dxdt_tmp.m_v );
            do_step_impl( system , in , dxdt_in , t , out , dxdt_out , dt );
            //error estimate
            stepper_base_type::m_algebra.for_each7( xerr , m_dxdt_tmp.m_v , m_k3.m_v , m_k4.m_v , m_k5.m_v , m_k6.m_v , dxdt_out ,
                                                    typename operations_type::template scale_sum6< time_type , time_type , time_type , time_type , time_type , time_type >( dt*dc1 , dt*dc3 , dt*dc4 , dt*dc5 , dt*dc6 , dt*dc7 ) );

        }
        else
        {
            do_step_impl( system , in , dxdt_in , t , out , dxdt_out , dt );
            //error estimate
            stepper_base_type::m_algebra.for_each7( xerr , dxdt_in , m_k3.m_v , m_k4.m_v , m_k5.m_v , m_k6.m_v , dxdt_out ,
                                                    typename operations_type::template scale_sum6< time_type , time_type , time_type , time_type , time_type , time_type >( dt*dc1 , dt*dc3 , dt*dc4 , dt*dc5 , dt*dc6 , dt*dc7 ) );

        }

    }

    /*
     * Calculates Dense-Output for Dopri5
     *
     * See Hairer, Norsett, Wanner: Solving Ordinary Differential Equations, Nonstiff Problems. I, p.191/192
     *
     * y(t+theta) = y(t) + h * sum_i^7 b_i(theta) * k_i
     *
     * A = theta^2 * ( 3 - 2 theta )
     * B = theta^2 * ( theta - 1 )
     * C = theta^2 * ( theta - 1 )^2
     * D = theta   * ( theta - 1 )^2
     *
     * b_1( theta ) = A * b_1 - C * X1( theta ) + D
     * b_2( theta ) = 0
     * b_3( theta ) = A * b_3 + C * X3( theta )
     * b_4( theta ) = A * b_4 - C * X4( theta )
     * b_5( theta ) = A * b_5 + C * X5( theta )
     * b_6( theta ) = A * b_6 - C * X6( theta )
     * b_7( theta ) = B + C * X7( theta )
     *
     * An alternative Method is described in:
     *
     * www-m2.ma.tum.de/homepages/simeon/numerik3/kap3.ps
     */
    template< class StateOut , class StateIn1 , class DerivIn1 , class StateIn2 , class DerivIn2 >
    void calc_state( time_type t , StateOut &x ,
                     const StateIn1 &x_old , const DerivIn1 &deriv_old , time_type t_old ,
                     const StateIn2 & /* x_new */ , const DerivIn2 &deriv_new , time_type t_new ) const
    {
        const value_type b1 = static_cast<value_type> ( 35 ) / static_cast<value_type>( 384 );
        const value_type b3 = static_cast<value_type> ( 500 ) / static_cast<value_type>( 1113 );
        const value_type b4 = static_cast<value_type> ( 125 ) / static_cast<value_type>( 192 );
        const value_type b5 = static_cast<value_type> ( -2187 ) / static_cast<value_type>( 6784 );
        const value_type b6 = static_cast<value_type> ( 11 ) / static_cast<value_type>( 84 );

        const time_type dt = ( t_new - t_old );
        const value_type theta = ( t - t_old ) / dt;
        const value_type X1 = static_cast< value_type >( 5 ) * ( static_cast< value_type >( 2558722523LL ) - static_cast< value_type >( 31403016 ) * theta ) / static_cast< value_type >( 11282082432LL );
        const value_type X3 = static_cast< value_type >( 100 ) * ( static_cast< value_type >( 882725551 ) - static_cast< value_type >( 15701508 ) * theta ) / static_cast< value_type >( 32700410799LL );
        const value_type X4 = static_cast< value_type >( 25 ) * ( static_cast< value_type >( 443332067 ) - static_cast< value_type >( 31403016 ) * theta ) / static_cast< value_type >( 1880347072LL ) ;
        const value_type X5 = static_cast< value_type >( 32805 ) * ( static_cast< value_type >( 23143187 ) - static_cast< value_type >( 3489224 ) * theta ) / static_cast< value_type >( 199316789632LL );
        const value_type X6 = static_cast< value_type >( 55 ) * ( static_cast< value_type >( 29972135 ) - static_cast< value_type >( 7076736 ) * theta ) / static_cast< value_type >( 822651844 );
        const value_type X7 = static_cast< value_type >( 10 ) * ( static_cast< value_type >( 7414447 ) - static_cast< value_type >( 829305 ) * theta ) / static_cast< value_type >( 29380423 );

        const value_type theta_m_1 = theta - static_cast< value_type >( 1 );
        const value_type theta_sq = theta * theta;
        const value_type A = theta_sq * ( static_cast< value_type >( 3 ) - static_cast< value_type >( 2 ) * theta );
        const value_type B = theta_sq * theta_m_1;
        const value_type C = theta_sq * theta_m_1 * theta_m_1;
        const value_type D = theta * theta_m_1 * theta_m_1;

        const value_type b1_theta = A * b1 - C * X1 + D;
        const value_type b3_theta = A * b3 + C * X3;
        const value_type b4_theta = A * b4 - C * X4;
        const value_type b5_theta = A * b5 + C * X5;
        const value_type b6_theta = A * b6 - C * X6;
        const value_type b7_theta = B + C * X7;

        // const state_type &k1 = *m_old_deriv;
        // const state_type &k3 = dopri5().m_k3;
        // const state_type &k4 = dopri5().m_k4;
        // const state_type &k5 = dopri5().m_k5;
        // const state_type &k6 = dopri5().m_k6;
        // const state_type &k7 = *m_current_deriv;

        stepper_base_type::m_algebra.for_each8( x , x_old , deriv_old , m_k3.m_v , m_k4.m_v , m_k5.m_v , m_k6.m_v , deriv_new ,
                typename operations_type::template scale_sum7< value_type , time_type , time_type , time_type , time_type , time_type , time_type >( 1.0 , dt * b1_theta , dt * b3_theta , dt * b4_theta , dt * b5_theta , dt * b6_theta , dt * b7_theta ) );
    }

    template< class StateIn >
    void adjust_size( const StateIn &x )
    {
        resize_k_x_tmp_impl( x );
        resize_dxdt_tmp_impl( x );
        stepper_base_type::adjust_size( x );
    }

private:

    template< class StateIn >
    bool resize_k_x_tmp_impl( const StateIn &x )
    {
        bool resized = false;
        resized |= adjust_size_by_resizeability( m_x_tmp , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_k2 , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_k3 , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_k4 , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_k5 , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_k6 , x , typename is_resizeable<deriv_type>::type() );
        return resized;
    }

    template< class StateIn >
    bool resize_dxdt_tmp_impl( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_dxdt_tmp , x , typename is_resizeable<deriv_type>::type() );
    }

    wrapped_state_type m_x_tmp;
    wrapped_deriv_type m_k2 , m_k3 , m_k4 , m_k5 , m_k6 ;
    wrapped_deriv_type m_dxdt_tmp;
    resizer_type m_k_x_tmp_resizer;
    resizer_type m_dxdt_tmp_resizer;
};

/************* DOXYGEN ************/
/**
 * \class runge_kutta_dopri5
 * \brief The Runge-Kutta Dormand-Prince 5 method.
 *
 * The Runge-Kutta Dormand-Prince 5 method is a very popular method for solving ODEs, see
 * .
 * The method is explicit and fulfills the Error Stepper concept. Step size control
 * is provided but continuous output is available which make this method favourable for many applications.
 *
 * This class derives from explicit_error_stepper_fsal_base and inherits its interface via CRTP (current recurring
 * template pattern). The method possesses the FSAL (first-same-as-last) property. See
 * explicit_error_stepper_fsal_base for more details.
 *
 * \tparam State The state type.
 * \tparam Value The value type.
 * \tparam Deriv The type representing the time derivative of the state.
 * \tparam Time The time representing the independent variable - the time.
 * \tparam Algebra The algebra type.
 * \tparam Operations The operations type.
 * \tparam Resizer The resizer policy type.
 */

    /**
     * \fn runge_kutta_dopri5::runge_kutta_dopri5( const algebra_type &algebra )
     * \brief Constructs the runge_kutta_dopri5 class. This constructor can be used as a default
     * constructor if the algebra has a default constructor.
     * \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
     */

    /**
     * \fn runge_kutta_dopri5::do_step_impl( System system , const StateIn &in , const DerivIn &dxdt_in , time_type t , StateOut &out , DerivOut &dxdt_out , time_type dt )
     * \brief This method performs one step. The derivative `dxdt_in` of `in` at the time `t` is passed to the
     * method. The result is updated out-of-place, hence the input is in `in` and the output in `out`. Furthermore,
     * the derivative is update out-of-place, hence the input is assumed to be in `dxdt_in` and the output in
     * `dxdt_out`.
     * Access to this step functionality is provided by explicit_error_stepper_fsal_base and
     * `do_step_impl` should not be called directly.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved. in is not modified in this method
     * \param dxdt_in The derivative of x at t. dxdt_in is not modified by this method
     * \param t The value of the time, at which the step should be performed.
     * \param out The result of the step is written in out.
     * \param dxdt_out The result of the new derivative at time t+dt.
     * \param dt The step size.
     */

    /**
     * \fn runge_kutta_dopri5::do_step_impl( System system , const StateIn &in , const DerivIn &dxdt_in , time_type t , StateOut &out , DerivOut &dxdt_out , time_type dt , Err &xerr )
     * \brief This method performs one step. The derivative `dxdt_in` of `in` at the time `t` is passed to the
     * method. The result is updated out-of-place, hence the input is in `in` and the output in `out`. Furthermore,
     * the derivative is update out-of-place, hence the input is assumed to be in `dxdt_in` and the output in
     * `dxdt_out`.
     * Access to this step functionality is provided by explicit_error_stepper_fsal_base and
     * `do_step_impl` should not be called directly.
     * An estimation of the error is calculated.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved. in is not modified in this method
     * \param dxdt_in The derivative of x at t. dxdt_in is not modified by this method
     * \param t The value of the time, at which the step should be performed.
     * \param out The result of the step is written in out.
     * \param dxdt_out The result of the new derivative at time t+dt.
     * \param dt The step size.
     * \param xerr An estimation of the error.
     */

    /**
     * \fn runge_kutta_dopri5::calc_state( time_type t , StateOut &x , const StateIn1 &x_old , const DerivIn1 &deriv_old , time_type t_old , const StateIn2 &  , const DerivIn2 &deriv_new , time_type t_new ) const
     * \brief This method is used for continuous output and it calculates the state `x` at a time `t` from the
     * knowledge of two states `old_state` and `current_state` at time points `t_old` and `t_new`. It also uses
     * internal variables to calculate the result. Hence this method must be called after two successful `do_step`
     * calls.
     */

    /**
     * \fn runge_kutta_dopri5::adjust_size( const StateIn &x )
     * \brief Adjust the size of all temporaries in the stepper manually.
     * \param x A state from which the size of the temporaries to be resized is deduced.
     */

} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA_DOPRI5_HPP_INCLUDED


#ifndef CANNON_ML_LLOYD_H
#define CANNON_ML_LLOYD_H

/*!
 * \file cannon/ml/lloyd.hpp
 * \brief File containing functions relating to Lloyd's algorithm for Voronoi
 * relaxation.
 */

#include <Eigen/Dense>

using namespace Eigen;

namespace cannon {
  namespace ml {

    /*!
     * \brief Enum class representing the different ways that the centroid
     * integral used by Lloyd's algorithm can be numerically approximated.
     */
    enum class LloydSamplingStrategy {
      Grid,
      Uniform,
    };

    /*!
     * \brief Function to compute a single iteration of the classic Lloyd's
     * algorithm in place within the specified bounds.
     *
     * \param pts The current points making up the Voronoi diagram.
     * \param strat Sampling strategy to use for approximating centroid
     * \param x_low X axis lower bound
     * \param x_high X axis upper bound
     * \param y_low Y axis lower bound
     * \param y_high Y axis upper bound
     */
    void do_lloyd_iteration(
        Matrix2Xd &pts,
        LloydSamplingStrategy strat = LloydSamplingStrategy::Grid,
        double x_low = -1.0, double x_high = 1.0, double y_low = -1.0,
        double y_high = 1.0);

    /*!
     * \brief Function to compute a single iteration of Lloyd's
     * algorithm with respect to the input weighting function in place within
     * the specified bounds.
     *
     * \param pts The current points making up the Voronoi diagram.
     * \param f The weighting function to use. Should be everywhere positive.
     * \param strat Sampling strategy to use for approximating centroid
     * \param x_low X axis lower bound
     * \param x_high X axis upper bound
     * \param y_low Y axis lower bound
     * \param y_high Y axis upper bound
     */
    void do_lloyd_iteration(
        Matrix2Xd &pts, std::function<double(const Vector2d&)> f,
        LloydSamplingStrategy strat = LloydSamplingStrategy::Grid,
        double x_low = -1.0, double x_high = 1.0, double y_low = -1.0,
        double y_high = 1.0);
  }
}

#endif /* ifndef CANNON_ML_LLOYD_H */


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/constant/logpi.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check logpi behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::logpi;
  using boost::simd::Logpi;

  STF_TYPE_IS(decltype(Logpi<T>()), T);
  STF_EQUAL(Logpi<T>(), T(1));
  STF_EQUAL(logpi( as(T{}) ),T(1));
}

STF_CASE_TPL( "Check logpi behavior for floating types"
            , (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::logpi;
  using boost::simd::Logpi;

  STF_TYPE_IS(decltype(Logpi<T>()), T);
  STF_IEEE_EQUAL(Logpi<T>(), T(1.1447298858494001741434273513531));
  STF_IEEE_EQUAL(logpi( as(T{}) ), T(1.1447298858494001741434273513531));
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "EntropyRegularizedEisner"

#include <boost/test/unit_test.hpp>
namespace utf = boost::unit_test;

#include "diffdp/algorithm/eisner.h"

// using boost test with intolerance fails (too precise),
// so let's just use the same test as in Dynet.
bool check_grad(float g, float g_act)
{
    float f = std::fabs(g - g_act);
    float m = std::max(std::fabs(g), std::fabs(g_act));
    if (f > 0.01 && m > 0.f)
        f /= m;

    if (f > 0.01 || std::isnan(f))
        return false;
    else
        return true;
}

BOOST_AUTO_TEST_CASE(gradient)
{
    const unsigned size = 10;
    const float sensitivity = 1e-2;

    diffdp::AlgorithmicDifferentiableEisner alg_diff_eisner(size);

    // check gradient
    std::vector<float> weights(size * size);
    for (unsigned output_head = 0 ; output_head < size ; ++output_head)
    {
        for (unsigned output_mod = 1 ; output_mod < size ; ++output_mod)
        {
            if (output_head == output_mod)
            continue;

            for (unsigned input_head = 0 ; input_head < size ; ++input_head)
            {
                for (unsigned input_mod = 1; input_mod < size; ++input_mod)
                {
                    if (input_head == input_mod)
                        continue;

                    // compute gradient using the algorithm
                    alg_diff_eisner.forward(
                        [&] (const unsigned head, const unsigned mod) -> float
                        {
                            return weights.at(head + mod * size);
                        }
                        );
                    alg_diff_eisner.backward(
                        [&] (const unsigned head, const unsigned mod) -> float
                        {
                            if (head == output_head && mod == output_mod)
                                return 1.f;
                            else
                                return 0.f;
                        }
                    );

                    const double computed_gradient = alg_diff_eisner.gradient(input_head, input_mod);

                    // estimate the gradient
                    const float sensitivity = 1e-3;
                    const double original_weights = weights.at(input_head + input_mod * size);

                    weights.at(input_head + input_mod * size) = original_weights + sensitivity;
                    alg_diff_eisner.forward(
                        [&] (const unsigned head, const unsigned mod) -> float
                        {
                            return weights.at(head + mod * size);
                        }
                        );

                    const double output_a = alg_diff_eisner.output(output_head, output_mod);

                    weights.at(input_head + input_mod * size) = original_weights - sensitivity;
                        alg_diff_eisner.forward(
                        [&] (const unsigned head, const unsigned mod) -> float
                        {
                            return weights.at(head + mod * size);
                        }
                    );
                    const double output_b = alg_diff_eisner.output(output_head, output_mod);

                    // restore
                    weights.at(input_head + input_mod * size) = original_weights;

                    const double estimated_gradient = (output_a - output_b) / (2.f * sensitivity);

                    BOOST_CHECK(check_grad(computed_gradient, estimated_gradient));

                }
            }
        }
    }
}


//
// Copyright 2013 Christian Henning
// Copyright 2020 Mateusz Loskot <mateusz@loskot.net>
//
// 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
//
#include <boost/gil.hpp>
#include <boost/gil/extension/toolbox/color_spaces/hsl.hpp>

#include <boost/core/lightweight_test.hpp>

#include <cstdint>

namespace gil = boost::gil;

void test_rgb_to_hsl()
{
    gil::rgb8_pixel_t p{128, 0, 128};
    gil::hsl32f_pixel_t h;
    gil::color_convert(p, h);

    BOOST_TEST(gil::get_color(h, gil::hsl_color_space::hue_t()) > 0.8);         // 0.83333331
    BOOST_TEST(gil::get_color(h, gil::hsl_color_space::saturation_t()) == 1.0); // 1.00000000
    BOOST_TEST(gil::get_color(h, gil::hsl_color_space::lightness_t()) > 0.25);  // 0.25098040
}

void test_hsl_to_rgb()
{
    gil::rgb8_pixel_t p(64, 0, 64);
    gil::hsl32f_pixel_t h;
    gil::color_convert(p, h);

    gil::rgb8_pixel_t b;
    gil::color_convert(h, b);
    BOOST_TEST(b == p);
}

void test_image_assign_hsl()
{
    std::ptrdiff_t const w = 320;
    std::ptrdiff_t const h = 240;
    gil::hsl32f_image_t hsl_img(w, h);

    for (std::ptrdiff_t y = 0; y < h; y++)
    {
        gil::hsl32f_view_t::x_iterator hsl_x_it = view(hsl_img).row_begin(y);
        float v = static_cast<float>(h - y) / h;
        for (std::ptrdiff_t x = 0; x < w; x++)
        {
            float const hue = (x + 1.f) / w;
            gil::hsl32f_pixel_t const p(hue, 1.0, v);
            hsl_x_it[x] = p;
            BOOST_TEST(gil::view(hsl_img)(x, y) == p);
        }
    }
}

void test_copy_pixels_rgb_to_hsl()
{
    gil::rgb8_image_t rgb_img(320, 240);
    gil::rgb8_pixel_t rgb_pix(64, 32, 64);
    gil::fill_pixels(view(rgb_img), rgb_pix);
    gil::hsl32f_image_t hsl_img(view(rgb_img).dimensions());
    gil::copy_pixels(gil::color_converted_view<gil::hsl32f_pixel_t>(view(rgb_img)), view(hsl_img));

    auto view = gil::view(hsl_img);
    for (auto it = view.begin(), end = view.end(); it != end; ++it)
    {
        gil::rgb8_pixel_t p;
        gil::color_convert(*it, p);
        BOOST_TEST(p == rgb_pix);
    }
}

int main()
{
    test_rgb_to_hsl();
    test_hsl_to_rgb();
    test_image_assign_hsl();
    test_copy_pixels_rgb_to_hsl();

    return ::boost::report_errors();
}


/**
 * @example netcdf/test/read_bathy_test.cc
 */
#include <boost/test/unit_test.hpp>
#include <usml/netcdf/netcdf_files.h>
#include <iostream>
#include <fstream>

BOOST_AUTO_TEST_SUITE(read_profile_test)

using namespace boost::unit_test ;
using namespace usml::netcdf ;

/**
 * @ingroup netcdf_test
 * @{
 */

/**
 * Tests the ability of the netcdf_woa class to splice deep and shallow
 * databases into a combined product. Extract Hawaii ocean temperature and
 * dumps the resulting data to the read_woa.csv file.
 *
 * Automatically compares results for a handfull of individual location
 * to data that was extracted by hand from the woacmp.log file created
 * by the Makefile. Generates BOOST errors if these values differ by
 * more that 1E-6 percent.
 */
BOOST_AUTO_TEST_CASE( read_woa ) {
    cout << "=== profile_test: read_woa ===" << endl;
    int month = 6 ;
    const double earth_radius = 6378137.0 ;
    netcdf_woa profile(
	USML_DATA_DIR "/woa09/temperature_seasonal_1deg.nc",
	USML_DATA_DIR "/woa09/temperature_monthly_1deg.nc",
        month, 18.5, 22.5, 200.5, 205.5, earth_radius ) ;

    // compare depth axis to values read using ncdump

    const seq_vector& altitude = *(profile.axis(0)) ;
    size_t num_alt = altitude.size() ;
    cout << "altitude[" << num_alt << "] =" ;
    for ( size_t n=0 ; n < num_alt ; ++n ) {
        cout << " " << ( altitude(n) - earth_radius ) ;
    }
    cout << endl ;
    BOOST_CHECK_EQUAL( num_alt, 33 ) ;
    BOOST_CHECK_CLOSE( altitude(0)-earth_radius, 0.0, 1e-6 ) ;
    BOOST_CHECK_CLOSE( altitude(num_alt-1)-earth_radius, -5500.0, 1e-6 ) ;

    // compare latitude axis to values read using ncdump

    const seq_vector& latitude = *(profile.axis(1)) ;
    size_t num_lat = latitude.size() ;
    double lat1 = to_latitude( latitude(0) ) ;
    double lat2 = to_latitude( latitude(num_lat-1) ) ;
    double inc_lat = to_degrees( latitude.increment(0) ) ;
    cout << "latitude[" << num_lat
         << "] = " << lat1 << " to " << lat2 << " by " << inc_lat << endl ;
    BOOST_CHECK_EQUAL( num_lat, 5 ) ;
    BOOST_CHECK_CLOSE( lat1, 18.5, 1e-6 ) ;
    BOOST_CHECK_CLOSE( lat2, 22.5, 1e-6 ) ;

    // compare longitude axis to values read using ncdump

    const seq_vector& longitude = *(profile.axis(2)) ;
    size_t num_lng = longitude.size() ;
    double lng1 = to_degrees( longitude(0) ) ;
    double lng2 = to_degrees( longitude(num_lng-1) ) ;
    double inc_lng = to_degrees( longitude.increment(0) ) ;
    cout << "longitude[" << num_lng
         << "] = " << lng1 << " to " << lng2 << " by " << inc_lng << endl ;
    BOOST_CHECK_EQUAL( num_lng, 6 ) ;
    BOOST_CHECK_CLOSE( lng1, 200.5, 1e-6 ) ;
    BOOST_CHECK_CLOSE( lng2, 205.5, 1e-6 ) ;

    // compare depth to some known values

    size_t index[3] ;
    index[0]=0 ; index[1]=0 ; index[2]=0 ;  // 1st depth, 1st lat, 1st long
    BOOST_CHECK_CLOSE( profile.data(index), 25.8543f, 1e-6f ) ;
    index[0]=0 ; index[1]=0 ; index[2]=1 ;  // 1st depth, 1st lat, 2nd long
    BOOST_CHECK_CLOSE( profile.data(index), 25.7594f, 1e-6f ) ;
    index[0]=0 ; index[1]=1 ; index[2]=0 ;  // 1st depth, 2nd lat, 1st long
    BOOST_CHECK_CLOSE( profile.data(index), 25.8541f, 1e-6f ) ;
    index[0]=1 ; index[1]=0 ; index[2]=0 ;  // 2nd depth, 1st lat, 1st long
    BOOST_CHECK_CLOSE( profile.data(index), 25.8575f, 1e-6f ) ;

    // dump CSV file for processing by graphics program
    // each row is a depth, cycle through all longitudes
    // before moving to next latitude

    std::ofstream ofile( USML_TEST_DIR "/netcdf/test/read_woa.csv" ) ;

    // print header row
    ofile << "depth" ;
    for ( size_t n=0 ; n < num_lat ; ++n ) {
        for ( size_t m=0 ; m < num_lng ; ++m ) {
            ofile << ",\"" << to_latitude( latitude(n) ) << "N "
                  << to_degrees( longitude(m) ) << "E\"" ;
        }
    }
    ofile << std::endl ;

    // print data
    for ( size_t d=0 ; d < num_alt ; ++d ) {
        index[0] = d ;
        ofile << ( altitude(d) - earth_radius ) ;
        for ( size_t n=0 ; n < num_lat ; ++n ) {
            index[1] = n ;
            for ( size_t m=0 ; m < num_lng ; ++m ) {
                index[2] = m ;
                ofile << "," << profile.data(index) ;
            }
        }
        ofile << std::endl ;
    }
}

/**
 * Tests the ability of the netcdf_profile class to span a longitude
 * cut point in the database.  To test this, it reads data from WOA09
 * across the area from -1E (aka 1W) to 1E.  It relies on the fact that
 * WOA09 defines its data in terms of longitudes from 0 to 360.
 * Dumps the resulting profiles to the span_profile.csv file.
 *
 * Automatically compares results for a handfull of individual location
 * to data that was extracted by hand from the woacmp2.log file created
 * by the Makefile. Generates BOOST errors if these values differ by
 * more that 1E-6 percent.
 */
BOOST_AUTO_TEST_CASE( span_profile ) {
    cout << "=== profile_test: span_profile ===" << endl;
    const double earth_radius = 0.0 ;
    netcdf_profile profile(
	USML_DATA_DIR "/woa09/temperature_monthly_1deg.nc",
	15.0, -1.0, 2.0, -1.0, 2.0, earth_radius ) ;

    // compare depth axis to values read using ncdump

    const seq_vector& altitude = *(profile.axis(0)) ;
    size_t num_alt = altitude.size() ;
    cout << "altitude = " << altitude << endl ;
    BOOST_CHECK_EQUAL( num_alt, 24 ) ;
    BOOST_CHECK_CLOSE( altitude(0)-earth_radius, 0.0, 1e-6 ) ;
    BOOST_CHECK_CLOSE( altitude(num_alt-1)-earth_radius, -1500.0, 1e-6 ) ;

    // compare latitude axis to values read using ncdump

    const seq_vector& latitude = *(profile.axis(1)) ;
    size_t num_lat = latitude.size() ;
    double lat1 = to_latitude( latitude(0) ) ;
    double lat2 = to_latitude( latitude(num_lat-1) ) ;
    double inc_lat = -to_degrees( latitude.increment(0) ) ;
    cout << "latitude[" << num_lat
         << "] = " << lat1 << " to " << lat2 << " by " << inc_lat << endl ;
    BOOST_CHECK_EQUAL( num_lat, 5 ) ;
    BOOST_CHECK_CLOSE( lat1, -1.5, 1e-6 ) ;
    BOOST_CHECK_CLOSE( lat2, 2.5, 1e-6 ) ;

    // compare longitude axis to values read using ncdump

    const seq_vector& longitude = *(profile.axis(2)) ;
    size_t num_lng = longitude.size() ;
    double lng1 = to_degrees( longitude(0) ) ;
    double lng2 = to_degrees( longitude(num_lng-1) ) ;
    double inc_lng = to_degrees( longitude.increment(0) ) ;
    cout << "longitude[" << num_lng
         << "] = " << lng1 << " to " << lng2 << " by " << inc_lng << endl ;
    BOOST_CHECK_EQUAL( num_lng, 5 ) ;
    BOOST_CHECK_CLOSE( lng1, -1.5, 1e-6 ) ;
    BOOST_CHECK_CLOSE( lng2, 2.5, 1e-6 ) ;

    // compare temperature to some known values

    size_t index[3] ;
    index[0]=0 ; index[1]=2 ; index[2]=2 ;  // zero depth at 0.5N 0.5E
    BOOST_CHECK_CLOSE( profile.data(index), 28.2047f, 1e-6f ) ;
    index[0]=23 ; index[1]=2 ; index[2]=2 ; // max depth at 0.5N 0.5E
    BOOST_CHECK_CLOSE( profile.data(index), 4.1774f, 1e-6f ) ;

    // dump CSV file for processing by graphics program
    // each row is a depth, cycle through all longitudes
    // before moving to next latitude

    std::ofstream ofile( USML_TEST_DIR "/netcdf/test/span_profile.csv" ) ;

    // print header row
    ofile << "depth" ;
    for ( size_t n=0 ; n < num_lat ; ++n ) {
        for ( size_t m=0 ; m < num_lng ; ++m ) {
            ofile << ",\"" << to_latitude( latitude(n) ) << "N "
                  << to_degrees( longitude(m) ) << "E\"" ;
        }
    }
    ofile << std::endl ;

    // print data
    for ( size_t d=0 ; d < num_alt ; ++d ) {
        index[0] = d ;
        ofile << ( altitude(d) - earth_radius ) ;
        for ( size_t n=0 ; n < num_lat ; ++n ) {
            index[1] = n ;
            for ( size_t m=0 ; m < num_lng ; ++m ) {
                index[2] = m ;
                ofile << "," << profile.data(index) ;
            }
        }
        ofile << std::endl ;
    }
}

/**
 * Tests the ability of the netcdf_profile class to extract data
 * from a netcdf file that does not have a global range. This
 * would in return prevent the data from wrapping around and
 * instead limit the bounds of the data to the database provided.
 */
BOOST_AUTO_TEST_CASE( nonglobal_database ) {
    cout << "=== profile_test: nonglobal_database ===" << endl;
    netcdf_profile profile( USML_TEST_DIR "/netcdf/test/flstrts_temperature.nc",
	9, -90.0, 90.0, 0.0, 360.0 ) ;

    // compare latitude axis to values read using ncdump

    const seq_vector& latitude = *(profile.axis(1)) ;
    size_t num_lat = latitude.size();
    double lat1 = to_latitude( latitude(0) ) ;
    double lat2 = to_latitude( latitude(num_lat-1) ) ;
    double inc_lat = to_degrees( latitude.increment(0) ) ;
    cout << "latitude[" << num_lat
         << "] = " << lat1 << " to " << lat2 << " by " << inc_lat << endl ;
    BOOST_CHECK_EQUAL( num_lat, 7 ) ;
    BOOST_CHECK_CLOSE( lat1, 27.5, 1e-6 ) ;
    BOOST_CHECK_CLOSE( lat2, 33.5, 1e-6 ) ;

    // compare longitude axis to values read using ncdump

    const seq_vector& longitude = *(profile.axis(2)) ;
    size_t num_lng = longitude.size();
    double lng1 = to_degrees( longitude(0) ) ;
    double lng2 = to_degrees( longitude(num_lng-1) ) ;
    double inc_lng = to_degrees( longitude.increment(0) ) ;
    cout << "longitude[" << num_lng
         << "] = " << lng1 << " to " << lng2 << " by " << inc_lng << endl ;
    BOOST_CHECK_EQUAL( num_lng, 8 ) ;
    BOOST_CHECK_CLOSE( lng1, 278.5, 1e-6 ) ;
    BOOST_CHECK_CLOSE( lng2, 285.5, 1e-6 ) ;
}

/// @}

BOOST_AUTO_TEST_SUITE_END()


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/global_pose_estimation/compute_triplet_baseline_ratios.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <ceres/rotation.h>
#include <glog/logging.h>
#include <vector>

#include "theia/sfm/feature.h"
#include "theia/sfm/triangulation/triangulation.h"
#include "theia/sfm/view_triplet.h"

namespace theia {
namespace {

using Eigen::Vector3d;

// Triangulate the point and return the depth of the point relative to each
// view. Returns true if the depth was recovered successfully and false if the
// point could not be triangulated.
bool GetTriangulatedPointDepths(const TwoViewInfo& info,
                                const Vector3d& feature1,
                                const Vector3d& feature2,
                                double* depth1,
                                double* depth2) {
  static const double kMinTriangulationAngle = 2.0;

  const std::vector<Vector3d> origins = {Vector3d::Zero(), info.position_2};

  Eigen::Matrix3d rotation2;
  ceres::AngleAxisToRotationMatrix(
      info.rotation_2.data(), ceres::ColumnMajorAdapter3x3(rotation2.data()));
  const std::vector<Vector3d> directions = {feature1,
                                            rotation2.transpose() * feature2};

  // Make sure the rays have are viewed from a sufficient angle, otherwise the
  // depth computation is unstable.
  if (!SufficientTriangulationAngle(directions, kMinTriangulationAngle)) {
    return false;
  }

  Eigen::Vector4d point;
  if (!TriangulateMidpoint(origins, directions, &point)) {
    return false;
  }

  // Compute depths.
  const Vector3d point3d = point.hnormalized();
  *depth1 = point3d.norm();
  *depth2 = (point3d - info.position_2).norm();
  return true;
}

}  // namespace

bool ComputeTripletBaselineRatios(const ViewTriplet& triplet,
                                  const std::vector<Feature>& feature1,
                                  const std::vector<Feature>& feature2,
                                  const std::vector<Feature>& feature3,
                                  Eigen::Vector3d* baseline) {
  CHECK_NOTNULL(baseline)->setZero();
  CHECK_EQ(feature1.size(), feature2.size())
      << "The feature containers must be the same size when computing the "
         "triplet baseline ratios.";
  CHECK_EQ(feature1.size(), feature3.size())
      << "The feature containers must be the same size when computing the "
         "triplet baseline ratios.";

  Eigen::Vector4d point12, point13, point23;
  double depth1_12, depth2_12, depth1_13, depth3_13, depth2_23, depth3_23;

  std::vector<double> baseline2, baseline3;
  baseline2.reserve(feature2.size());
  baseline3.reserve(feature3.size());
  for (int i = 0; i < feature1.size(); i++) {
    const Vector3d normalized_feature1 =
        feature1[i].point_.homogeneous().normalized();
    const Vector3d normalized_feature2 =
        feature2[i].point_.homogeneous().normalized();
    const Vector3d normalized_feature3 =
        feature3[i].point_.homogeneous().normalized();
    if (!GetTriangulatedPointDepths(triplet.info_one_two,
                                    normalized_feature1,
                                    normalized_feature2,
                                    &depth1_12,
                                    &depth2_12)) {
      continue;
    }

    // Compute triangulation from views 1, 3.
    if (!GetTriangulatedPointDepths(triplet.info_one_three,
                                    normalized_feature1,
                                    normalized_feature3,
                                    &depth1_13,
                                    &depth3_13)) {
      continue;
    }

    // Compute triangulation from views 2, 3.
    if (!GetTriangulatedPointDepths(triplet.info_two_three,
                                    normalized_feature2,
                                    normalized_feature3,
                                    &depth2_23,
                                    &depth3_23)) {
      continue;
    }

    baseline2.emplace_back(depth1_12 / depth1_13);
    baseline3.emplace_back(depth2_12 / depth2_23);
  }

  if (baseline2.size() == 0) {
    VLOG(2) << "Could not compute the triplet baseline ratios. An inusfficient "
               "number of well-constrained 3D points were observed.";
    return false;
  }

  // Take the median as the baseline ratios.
  const int mid_index = baseline2.size() / 2;
  std::nth_element(
      baseline2.begin(), baseline2.begin() + mid_index, baseline2.end());
  std::nth_element(
      baseline3.begin(), baseline3.begin() + mid_index, baseline3.end());
  *baseline = Vector3d(1.0, baseline2[mid_index], baseline3[mid_index]);
  return true;
}

}  // namespace theia


#define BOOST_TEST_MODULE pcraster model_engine ldddownstream
#include <boost/test/unit_test.hpp>
#include "stddefx.h"
#include "calc_asispacking.h"
#include "calc_maskpacking.h"

#define private public
#include "calc_ldddownstream.h"

BOOST_AUTO_TEST_CASE(testSimple)
{
  using namespace calc;

  geo::RasterDim rd_aip(1,3);
  geo::RasterDim rd_mp(2,3);
  // UINT1 mask[6] = {0,0,0,1,1,1};
  std::vector<bool> mask = {0,0,0,1,1,1};
  AsIsPacking aip(rd_aip);
  MaskPacking mp(rd_mp,mask);
  IFieldRDConversion *conv[2]={&aip,&mp};

  const UINT1 lddField[3]={6,5,4}; // -> P <-

  for (size_t i=0; i<2; ++i) {
    LddDownstream ld(lddField,*conv[i]);

    BOOST_CHECK(ld.d_pit.size()==1);
    BOOST_CHECK(ld.d_pit[0]    ==1);

    BOOST_CHECK(ld.d_edge.size()==2);
    BOOST_CHECK(ld.d_edge[1].source()==2);
    BOOST_CHECK(ld.d_edge[1].target()==1);
    BOOST_CHECK(ld.d_edge[0].source()==0);
    BOOST_CHECK(ld.d_edge[0].target()==1);
  }
}

BOOST_AUTO_TEST_CASE(testUpstream)
{
  using namespace calc;

  geo::RasterDim rd(2,4);
  AsIsPacking aip(rd);
  UINT1 lddField[8]={6,5,4,4,   // -> X <-<-
                     6,6,6,8 }; // -> ->->|^
  REAL4  resultR[8]={1,0,1,2,
                     6,5,4,3 };
  // init to something detectable
  REAL4  resultC[8]={9,9,9,9,
                     9,9,9,9 };
  LddDownstream ld(lddField,aip);

  BOOST_CHECK(ld.d_edge.size()==7);
  BOOST_CHECK(ld.d_pit.size()==1);

  resultC[ld.d_pit[0]]=0;
  for(LddDownstream::upIterator i=ld.upBegin();i!=ld.upEnd();++i)
    resultC[i->up()] = resultC[i->down()]+1;
  BOOST_CHECK(std::equal(resultC,resultC+8,resultR));
}

BOOST_AUTO_TEST_CASE(testDownstream)
{
  using namespace calc;

  geo::RasterDim rd(2,4);
  AsIsPacking aip(rd);
  UINT1 lddField[8]={6,5,4,4,   // -> X <-<-
                     6,6,6,8 }; // -> ->->|^
  // total of upstream edges as a
  // downstream traversal
  REAL4  resultR[8]={0,7,5,4,
                     0,1,2,3 };
  REAL4  resultC[8]={0,0,0,0,
                     0,0,0,0 };
  LddDownstream ld(lddField,aip);

  BOOST_CHECK(ld.d_edge.size()==7);
  BOOST_CHECK(ld.d_pit.size()==1);

  resultC[ld.d_pit[0]]=0;
  for(LddDownstream::downIterator i=ld.downBegin();i!=ld.downEnd();++i)
    resultC[i->down()] += resultC[i->up()]+1;
  BOOST_CHECK(std::equal(resultC,resultC+8,resultR));
}

BOOST_AUTO_TEST_CASE(testDiagonal)
{
  using namespace calc;

  geo::RasterDim rd(2,4);
  AsIsPacking aip(rd);
  UINT1 lddField[8]={6,5,4,4,   // -> X <-<-
                     9,6,6,7 }; // -/ ->->\-
  // total of upstream edges as a
  // downstream traversal
  LddDownstream ld(lddField,aip);

  BOOST_CHECK(ld.d_edge.size()==7);
  BOOST_CHECK(ld.d_pit.size()==1);
/*
 * TODO translate iterators to index
  resultC[ld.d_pit[0]]=0;
  for(LddDownstream::reverse_iterator i=ld.rbegin();i!=ld.rend();++i)
    resultC[i->down()] += resultC[i->up()]+1;
  BOOST_CHECK(std::equal(resultC,resultC+8,resultR));
*/
}

BOOST_AUTO_TEST_CASE(testUnsound)
{
  using namespace calc;

  geo::RasterDim rd_aip(1,4);
  AsIsPacking aip(rd_aip);
  UINT1 lddField[4]={6,5,6,4}; // -> P -><-
  bool catched=false;
  try {
    LddDownstream ld(lddField,aip);
  } catch(const LddDownstream::Unsound& u) {
    catched=true;
  }
  BOOST_CHECK(catched);
}


//
// $Id: base.hpp 7297 2015-03-12 05:30:33Z paragmallick $
//
//
// Original author: Witold Wolski <wewolski@gmail.com>
//
// Copyright : ETH Zurich
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//

#ifndef BASE_H
#define BASE_H

#include <math.h>
#include <algorithm>
#include <vector>
#include <functional>
#include <numeric>
#include <stdexcept>
#include <limits>
#include <iterator>
#include <cmath>
#include <string>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_integral.hpp>
#include "pwiz/utility/findmf/base/base/utilities/base.hpp"

namespace ralab
{
  namespace base
  {
    namespace base
    {

      /// generates the sequence from, from+/-1, ..., to (identical to from:to).
      template<typename TReal>
      void seq
      (
          TReal from, //!<[in] the starting  value of the sequence
          TReal to, //!<[in] the end value of the sequence
          std::vector<TReal> &result //!<[out] result sequence
          )
      {
        result.clear();
        typedef typename std::vector<TReal>::size_type size_type;
        if( from <= to )
        {
          size_type length = static_cast<size_type>(to - from) + 1;
          result.resize(length);
          std::generate(result.begin() , result.end() , utilities::SeqPlus<TReal>(from));
        }
        else
        {
          size_type length = static_cast<size_type>(from - to) + 1 ;
          result.resize(length);
          std::generate(result.begin() , result.end() , utilities::SeqMinus<TReal>(from));
        }
      }

      /// generates sequence: from, from+by, from + 2*by, ..., up to a sequence value less than, or equal than to.
      /// Specifying to < from and by of positive sign is an error.
      template<typename TReal>
      void seq(
          TReal from,//!<[in] the starting value of the sequence
          TReal to,//!<[in] the end value of the sequence
          TReal by, //!<[in] number: increment of the sequence
          std::vector<TReal> &result //!<[out] result sequence
          )
      {
        result.clear();
        typedef typename std::vector<TReal>::size_type size_type;
        size_type size = static_cast<size_type>(  (to - from)  / by ) + 1u ;
        result.reserve( size );

        if(from <= to)
        {
          if(!(by > 0)){
            throw std::logic_error(std::string( "by > 0" ));
          }
          for(; from <= to; from += by)
          {
            result.push_back(from);
          }
        }
        else
        {
          if(! (by < 0) ){
            throw std::logic_error(std::string( "by < 0" ));
          }
          for(; from >= to; from += by)
          {
            result.push_back(from);
          }
        }
      }

      /// generates sequence: from, to of length
      /// calls seq with \$[ by = ( ( to - from ) / (  length  - 1. ) ) \$]
      template<typename TReal>
      void seq_length(
          TReal from,//!<[in] the starting value of the sequence
          TReal to,//!<[in] the end value of the sequence
          unsigned int length, //!<[in] length of sequence
          std::vector<TReal> &result //!<[out] result sequence
          )
      {
        TReal by = ( ( to - from ) / ( static_cast<TReal>( length ) - 1. ) );
        seq(from, to, by, result);

        //this is required because of machine precision...
        // sometimes by does not add's up to precisely _to_ nad
        if(result.size() < length)
        {
          result.push_back(result[result.size()-1] + by );
        }
        else
        {
          result.resize(length);
        }
      }

      /// generates the sequence [1, 2, ..., length(ref)]
      /// (as if argument along.with had been specified),
      /// unless the argument is numeric of length 1 when it is interpreted as
      ///  1:from (even for seq(0) for compatibility with S).

      template< typename T1 , typename T2 >
      void seq
      (
          std::vector<T1> & ref, //!<[in] take the length from the length of this argument.
          std::vector<T2> & res //!<[out] result sequence
          )
      {
        T2 t(1);
        res.assign(ref.size() , t );
        std::partial_sum( res.begin() , res.end() , res.begin() );
      }

      /// Generates Sequence 1,2,3,....length .
      /// Generates 1, 2, ..., length unless length.out = 0, when it generates
      /// integer(0).

      template<typename TSize, typename TReal>
      typename boost::enable_if<boost::is_integral<TSize>, void>::type
      seq(
          TSize length, //!< [in] length of sequence
          std::vector<TReal> &res //!< [out] result sequence
          )
      {
        TReal t(1);
        res.assign(length , t );
        std::partial_sum(res.begin(),res.end(),res.begin());
      }

      /// MEAN Trimmed arithmetic mean.

      /// ## Default S3 method:
      /// mean(x, trim = 0, na.rm = FALSE, ...)

      /// Arguments
      /// x 	An R object. Currently there are methods for numeric data frames, numeric vectors and dates. A complex vector is allowed for trim = 0, only.
      /// trim 	the fraction (0 to 0.5) of observations to be trimmed from each end of x before the mean is computed. Values outside that range are taken as the nearest endpoint.
      /// na.rm 	a logical value indicating whether NA values should be stripped before the computation proceeds.
      /// ... 	further arguments passed to or from other methods.

      /// Value
      /// For a data frame, a named vector with the appropriate method being applied column by column.
      /// If trim is zero (the default), the arithmetic mean of the values in x is computed, as a numeric or complex vector of length one. If x is not logical (coerced to numeric), integer, numeric or complex, NA is returned, with a warning.
      /// If trim is non-zero, a symmetrically trimmed mean is computed with a fraction of trim observations deleted from each end before the mean is computed.

      template < typename InputIterator >
      inline
      typename std::iterator_traits<InputIterator>::value_type
      mean(
          InputIterator begin, //!< [in]
          InputIterator end //!< [in]
          )
      {
        typedef typename std::iterator_traits<InputIterator>::value_type TReal;
        TReal size = static_cast<TReal>(std::distance(begin,end));
        TReal sum = std::accumulate(begin , end, 0. );
        return(sum/size);
      }

      /// mean
      template <typename TReal>
      inline TReal mean(const std::vector<TReal> & x )
      {
        TReal size = static_cast<TReal>(x.size());
        TReal sum = std::accumulate(x.begin() , x.end(), 0. );
        return ( sum / size ) ;
      }

      /// mean
      template <typename TReal>
      TReal mean(
          const std::vector<TReal> & x, //!< [in]
          TReal trim  //!< [in] trim 0 - 0.5
          )
      {
        if(trim >= 0.5)
        {
          trim = 0.4999999;
        }
        TReal size = static_cast<TReal>(x.size());
        std::vector<TReal> wc(x); //working copy
        std::sort(wc.begin(),wc.end());
        size_t nrelemstrim = static_cast<size_t>(round( size * trim )) ;
        size_t nrelems = std::distance(wc.begin() + nrelemstrim, wc.end() - nrelemstrim );
        TReal sum = std::accumulate(wc.begin() + nrelemstrim , wc.end() - nrelemstrim, 0. );
        return ( sum / static_cast<TReal>( nrelems ) ); //static_cast will be required with boost::math::round
      }

      /// computes the mean
      template<class Iter_T>
      typename std::iterator_traits<Iter_T>::value_type geometricMean(Iter_T first, Iter_T last)
      {
        typedef typename std::iterator_traits<Iter_T>::value_type TReal;
        size_t cnt = distance(first, last);
        std::vector<TReal> copyOfInput(first, last);

        // ln(x)
        typename std::vector<TReal>::iterator inputIt;

        for(inputIt = copyOfInput.begin(); inputIt != copyOfInput.end() ; ++inputIt)
        {
          *inputIt = std::log(*inputIt);
        }

        // sum(ln(x))
        TReal sum( std::accumulate(copyOfInput.begin(), copyOfInput.end(), TReal() ));

        // e^(sum(ln(x))/N)
        TReal geomean( std::exp(sum / cnt) );
        return geomean;
      }

      /// Range of Values
      /// range returns a std::pair containing minimum and maximum of all the given values.
      template<typename TReal>
      void Range(
          const std::vector<TReal> & values, //!< [in] data
          std::pair<TReal,TReal> & range //!< [out] range
          )
      {
        TReal min = * std::min_element(values.begin(),values.end());
        TReal max = * std::max_element(values.begin(),values.end());
        range.first = min;
        range.second = max;
      }

      ///  maximum of 3 numbers
      template<typename T>
      inline double max3(T a, T b, T c)
      {
        T max;
        if(a>b)
          max=a;
        else
          max=b;
        if(max<c)
          max=c;
        return(max);
      }

      /// log base 2
      template<typename TReal>
      inline TReal log2(TReal test)
      {
        if(test==0)
          return(0);
        else
          return( log10( test ) / log10( static_cast<TReal>(2.) ));
      }

    }//namespace BASE ends here
  }//namespace base ends here
}//namespace ralab ends here

#endif // BASE_H