/*
 * Copyright 2020 Ria Sonecha, Massachusetts Institute of Technology in Cambridge, MA, USA
 * Copyright 2020 Giuseppe Silano, University of Sannio in Benevento, Italy
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <thread>
#include <chrono>

#include "rotors_gazebo/Quaternion.h"
#include "rotors_gazebo/transform_datatypes.h"
#include "rotors_gazebo/parameters_ros.h"
#include <nav_msgs/Odometry.h>
#include "rotors_gazebo/Matrix3x3.h"
#include <ros/console.h>
#include <time.h>

#include <Eigen/Core>
#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>
#include <math.h>
#include "rotors_gazebo/spline_trajectory_generator.h"

#define SAMPLING_TIME  10e-3       /* SAMPLING CONTROLLER TIME [s] - 100Hz */

template<typename T> inline void GetRosParameterHovering(const ros::NodeHandle& nh,
                                                         const std::string& key,
                                                         const T& default_value,
                                                         T* value) {

  ROS_ASSERT(value != nullptr);
  bool have_parameter = nh.getParam(key, *value);
  if (!have_parameter) {
    ROS_WARN_STREAM("[rosparam]: could not find parameter " << nh.getNamespace()
                    << "/" << key << ", setting to default: " << default_value);
    *value = default_value;
  }
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "hovering_example_spline_swarm");
  ros::NodeHandle nh;
  // Create a private node handle for accessing node parameters.
  ros::NodeHandle nh_private("~");
  ros::Publisher trajectory_pub =
      nh.advertise<mav_msgs::DroneState>(
          mav_msgs::default_topics::DRONE_STATE, 10);
  ROS_INFO("[SWARM] Started hovering example with spline.");

  while (trajectory_pub.getNumSubscribers() == 0 && ros::ok()) {
    ROS_INFO("There is no subscriber available, trying again in 1 second.");
    ros::Duration(1.0).sleep();
  }

  // Trajectory message
  mav_msgs::DroneState trajectory_msg, trajectory_msg_pre;
  trajectory_msg.header.stamp = ros::Time::now();
  mav_msgs::EigenDroneState eigen_reference;
  rotors_gazebo::SplineTrajectoryGenerator hovering_example_spline_generator;

  Eigen::Vector3f check_position_final;
  GetRosParameterHovering(nh_private, "position_final/x", check_position_final.x(),
    &check_position_final.x());
  GetRosParameterHovering(nh_private, "position_final/y", check_position_final.y(),
    &check_position_final.y());
  GetRosParameterHovering(nh_private, "position_final/z", check_position_final.z(),
    &check_position_final.z());

  hovering_example_spline_generator.InitializeParams();

  // Reading the final time. This is stop condition for the spline generator
  double end_generation_time;
  GetRosParameterHovering(nh_private, "time_final/time", end_generation_time,
    &end_generation_time);

  double initial_time, final_time;
  initial_time = 0;

  // Publish the trajectory values until the final values is reached
  while(true){
    final_time = ros::Time::now().toSec();
    hovering_example_spline_generator.TrajectoryCallback(&eigen_reference,
      &final_time, &initial_time);

    mav_msgs::eigenDroneFromStateToMsg(&eigen_reference, trajectory_msg);

    // new message
    if(eigen_reference.position_W[0] <= check_position_final[0] &&
      eigen_reference.position_W[1] <= check_position_final[1] &&
      eigen_reference.position_W[2] <= check_position_final[2]){
      trajectory_pub.publish(trajectory_msg);
      trajectory_msg_pre = trajectory_msg;
    }

    ROS_DEBUG("Publishing waypoint from msg: [%f, %f, %f].",
      trajectory_msg.position.x, trajectory_msg.position.y, trajectory_msg.position.z);
    ROS_DEBUG("Publishing waypoint: [%f, %f, %f].",
      eigen_reference.position_W[0], eigen_reference.position_W[1], eigen_reference.position_W[2]);

    ros::Duration(SAMPLING_TIME).sleep();

    // Hold the message until the simulation ends
    if(eigen_reference.position_W[0] > check_position_final[0] &&
      eigen_reference.position_W[1] > check_position_final[1] &&
      eigen_reference.position_W[2] > check_position_final[2])
      trajectory_pub.publish(trajectory_msg_pre);

  }

  ros::spinOnce();
  ros::shutdown();

  return 0;
}


#include <string>
#include <boost/format.hpp>

using namespace std;

namespace disasm {
	namespace vector {
		string decode_vector_register(uint16_t _reg) {
			uint16_t cleaned = _reg & 0x03ff; // only 10 bits, be certain
			string rv;
			uint8_t x, y;
			uint8_t isV = (cleaned & 0x0040);
			switch (isV) {
			case 0: // H[XY](y, ??)
				rv += "H";
				x = 0;
				y = (cleaned & 0x003f);
				break;
			case 1: //  V[XY](y, ??+x)
				rv += "V";
				x = (cleaned & 0x000f);
				y = ((cleaned & 0x0030) << 4);
				break;
			}

			uint8_t xy = (cleaned & 0x03c0) >> 6;

			if (xy == 12 || xy == 13) rv += "Y";
			else if (xy >= 8 && xy <= 11) rv += "X";
			else if (xy == 14) return string( "-" );
			rv += "(";
			rv += std::to_string(y);
			rv += ", ";

			uint8_t szm = (cleaned & 0x0380) >> 7;
			switch (szm) {
			case 0:
			case 1:
			case 2:
			case 3:
				rv += std::to_string(szm * 16);
				break;
			case 4:
				rv += "0";
				break;
			case 5:
				rv += "32";
				break;
			case 6:
				rv += "0";
				break;
			case 7:
			default:
				// Hermans docs break down here and are incomplete
				rv += "???";
				break;
			}

			if (isV) rv += " + " + std::to_string(x);
			rv += ")";

			return rv;
		}
	}
}


/*!
 * @file
 * Defines `boost::mpl11::detail::logical_or::strict_is_same`.
 *
 *
 * @copyright Louis Dionne 2014
 * Distributed under the Boost Software License, Version 1.0.
 *         (See accompanying file LICENSE.md or copy at
 *             http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_MPL11_DETAIL_LOGICAL_OR_STRICT_IS_SAME_HPP
#define BOOST_MPL11_DETAIL_LOGICAL_OR_STRICT_IS_SAME_HPP

#include <boost/mpl11/bool.hpp>
#include <boost/mpl11/detail/std_is_same.hpp>

namespace boost { namespace mpl11 { namespace detail { namespace logical_or {
    template <bool ...>
    struct strict_is_same_impl;

    /*!
     * @ingroup details
     *
     * Strict (non short-circuiting) logical or implemented with `is_same`.
     *
     * All credit goes to Roland Bock for the technique.
     */
    template <typename ...xs>
    using strict_is_same = bool_<
        !std_is_same<
            strict_is_same_impl<(bool)xs::type::value...>,
            strict_is_same_impl<(xs::type::value, false)...>
        >::value
    >;
}}}} // end namespace boost::mpl11::detail::logical_or

#endif // !BOOST_MPL11_DETAIL_LOGICAL_OR_STRICT_IS_SAME_HPP


// Standard Librry
#include <iostream>
#include <tuple>
#include <vector>
#include <fstream>

// Eigen
#include <Eigen/Dense>
using Eigen::MatrixXd;
using Eigen::VectorXd;

// OMPL
#include <ompl/base/spaces/RealVectorStateSpace.h>

// Internal Libraries
#include "Sampler/RejectionSampler.h"
#include "Sampler/MonteCarloSampler.h"
#include "Sampler/RejectionSampler.h"
#include "Sampler/HitAndRunSampler.h"
#include "OmplWrappers/DimtStateSpace.h"
#include "OmplWrappers/OmplHelpers.h"
#include "Dimt/Params.h"
#include "Dimt/DoubleIntegratorMinimumTime.h"
#include "Dimt/ProblemGeneration.h"
#include "Benchmark/SampleBenchmark.h"
#include "Benchmark/TimeBenchmark.h"
#include "Benchmark/OptionParse.h"

std::tuple<bool, std::vector<int>> handleArguments(int argc, char *argv[])
{
    if (cmdOptionExists(argv, argv + argc, "-h"))
    {
        std::cout << "________________________________________________" << std::endl;
        std::cout << "Main function for sampling" << std::endl;
        std::cout << "________________________________________________" << std::endl;
        std::cout << "Arguments:" << std::endl;
        std::cout << "\t -batch - NUmber of batches to run" << std::endl;
        std::cout << "\t -samples - Number of samples to get" << std::endl;
        std::cout << "\t -filename - Filename to save the samples to" << std::endl;
        std::cout << "________________________________________________" << std::endl;
        return std::make_tuple(false, std::vector<int>{});
    }
    else
    {
        std::vector<int> args;

        // Get the number of samples
        if (cmdOptionExists(argv, argv + argc, "-samples"))
            args.push_back(atoi(getCmdOption(argv, argv + argc, "-samples")));
        else
            args.push_back(100);  // Default to 100 samples
                                  // Get the batch number
        if (cmdOptionExists(argv, argv + argc, "-batch"))
            args.push_back(atoi(getCmdOption(argv, argv + argc, "-batch")));
        else
            args.push_back(10);  // Default to 20 batches

        return std::make_tuple(true, args);
    }
}

int main(int argc, char *argv[])
{
    //
    // Example for how to use the above sampler
    //
    bool run;
    std::vector<int> args;
    std::tie(run, args) = handleArguments(argc, argv);
    if (!run)
        return 0;

    int numSamples = args[0];
    int numbatch = args[1];

    std::string filename;
    bool save;
    std::tie(save, filename) = get_filename(argc, argv);

    // Create a problem definition
    int numDim = param.dimensions;
    double maxval = param.s_max;
    double minval = -param.s_max;
    UniformRealRandomGenerator uniRndGnr;

    // Initializations
    std::vector<double> maxVelocities(param.dof, param.v_max);
    std::vector<double> maxAccelerations(param.dof, param.a_max);
    //maxVelocities[1] = 0.1;
    //maxAccelerations[1] = 0.1;
    DIMTPtr dimt = std::make_shared<DIMT>(maxVelocities, maxAccelerations);

    ompl::base::SpaceInformationPtr si = createDimtSpaceInformation(dimt, minval, maxval);
    auto startState = si->allocState();
    auto goalState = si->allocState();

    const double levelSet = 1.4 * dimt->getMinTime(startState, goalState);

    std::ofstream logFile(filename + ".log");
    if(!logFile.is_open())
    {
        throw std::runtime_error("file not opened");
    }

    int numSamplers = 1;
    numbatch = 50;
    numSamplers = 500;
    for (int i = 0; i < numbatch; i++)
    {
        //std::cout << "BATCH " << i << std::flush;
        std::vector<std::chrono::high_resolution_clock::duration> times(numSamplers);
        std::vector<uint> sampleNums(numSamplers);
        std::vector<double> rejectionRatios(numSamplers, 1.0);

        // sample new start and goal
        for (int i = 0; i < numDim; i++)
        {
            startState->as<ompl::base::RealVectorStateSpace::StateType>()->values[i] = uniRndGnr.sample(minval, maxval);
            goalState->as<ompl::base::RealVectorStateSpace::StateType>()->values[i] = uniRndGnr.sample(minval, maxval);
        }
        // create a level set
        //double rndNum = uniRndGnr.sample(1.0, 2.0);
        //const double levelSet = rndNum * dimt->getMinTime(startState, goalState);

        const double levelSet = 1.51 - i * 0.01;
        //std::cout <<  " ratio " << rndNum << " " << std::flush;

        // create new problem definition
        ompl::base::ProblemDefinitionPtr pdef = createDimtProblem(startState, goalState, si, dimt);

        int curr=0;

        /*
        std::cout << " MCMC " << std::flush;
        {
            MatrixXd mcmcSamples;
            double alpha = 0.5;
            double L = 5;
            double epsilon = 0.1;
            double sigma = 1;
            int maxSteps = 50000000;
            ompl::base::MCMCSampler mcmcSampler(si, pdef, levelSet, 100, 100, epsilon, sigma, maxSteps);
            mcmcSamples = mcmcSampler.sample(numSamples, times[curr]);
            rejectionRatios[curr] = mcmcSampler.getAcceptanceRatio();
            sampleNums[curr] = mcmcSamples.rows();
        }
        printTime(times[curr], std::cout);
        curr++;

        std::cout << " HRS " << std::flush;
        {
            MatrixXd dimthrsSamples;
            ompl::base::DimtHierarchicalRejectionSampler dimthrsSampler(si, pdef, dimt,
                                                                        levelSet, 100, 100);
            dimthrsSamples = dimthrsSampler.sample(numSamples, times[curr]);
            rejectionRatios[curr] = dimthrsSampler.getAcceptanceRatio();
            sampleNums[curr] = dimthrsSamples.rows();

        }
        printTime(times[curr], std::cout);
        curr++;

        int numTrials = 5;
        std::cout << " H&R (" << numTrials << ") " << std::flush;
        {
            MatrixXd hitnrunSamples;
            ompl::base::HitAndRunSampler hitnrunSampler(si, pdef, levelSet, 100, 100, numTrials);
            hitnrunSamples = hitnrunSampler.sample(numSamples, times[curr]);
            rejectionRatios[curr] = hitnrunSampler.getAcceptanceRatio();
            sampleNums[curr] = hitnrunSamples.rows();
        }
        printTime(times[curr], std::cout);
        curr++;
        */
        //std::cout << " REJ " << std::flush;
        {
            std::cout << levelSet << " " << std::flush;
            MatrixXd rejSamples;
            ompl::base::RejectionSampler rejSampler(si, pdef, levelSet, 100, 100);
            rejSamples = rejSampler.sample(numSamples, times[curr]);
            rejectionRatios[curr] = rejSampler.getAcceptanceRatio();
            sampleNums[curr] = rejSamples.rows();

            std::cout << rejectionRatios[curr] << std::endl;
        }
        //printTime(times[curr], std::cout);
        //curr++;

        //std::cout << "        REJ RATIO[" << rejectionRatios[3] << "]" << std::flush;

        //appendTimeAndRatioToFile(times, rejectionRatios, sampleNums, logFile);

        //std::cout <<

        //std::cout << std::endl;
    }

    logFile.close();

}


/****************************************************************************
Copyright (c) 2013, Ryan Luna, Athanasios Krontiris, Kostas Bekris
All rights reserved.

The Software is available for download and use subject to the terms and
conditions of this License. Access or use of the Software constitutes
acceptance and agreement to the terms and conditions of this License.

Permission to use, copy, modify, and distribute this software and its
documentation for educational, research, and non-profit purposes, without fee,
and without a written agreement is hereby granted.  Permission to incorporate
this software into commercial products may be obtained by contacting the authors.

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 names of the authors 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 AUTHORS OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
****************************************************************************/

// Author: Ryan Luna

#include "Graph.h"
#include "Logger.h"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graphml.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/property_map/dynamic_property_map.hpp>

#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/trim_all.hpp>

#include <cassert>
#include <fstream>
#include <iostream>
#include <string>
#include <limits>

static const unsigned int NO_VERTEX = std::numeric_limits<unsigned int>::max();

enum vertex_location_t { vertex_location };
enum vertex_counter_t { vertex_counter};
enum vertex_type_t { vertex_type};

namespace boost
{
    BOOST_INSTALL_PROPERTY(vertex, location);
    BOOST_INSTALL_PROPERTY(vertex, counter);
    BOOST_INSTALL_PROPERTY(vertex, type);
}

struct Point
{
    Point() {}
    Point(double _x, double _y, double _z) : x(_x), y(_y), z(_x) {}
    double x,y,z;

    friend std::ostream& operator << (std::ostream &o, const Point& p);
    friend std::istream& operator >> (std::istream &i, const Point& p);
};

std::ostream& operator << (std::ostream& o, const Point& p)
{
    o << p.x << " " << p.y << " " << p.z;
    return o;
}

std::istream& operator >> (std::istream& i, Point& p)
{
    std::string str;
    std::getline(i, str);
    boost::trim_all(str);

    std::vector<std::string> strs;
    boost::split(strs, str, boost::is_space());

    assert(strs.size() == 3);
    p.x = atof(strs[0].c_str());
    p.y = atof(strs[1].c_str());
    p.z = atof(strs[2].c_str());
    return i;
}

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
                              boost::property<boost::vertex_index_t, unsigned int,
                              boost::property<vertex_location_t, Point,
                              boost::property<vertex_counter_t, int,
                              boost::property<vertex_type_t, std::string > > > >,
                              boost::property<boost::edge_weight_t, double> > GraphType;

typedef boost::graph_traits<GraphType>::vertex_descriptor  Vertex;
typedef boost::graph_traits<GraphType>::edge_descriptor    Edge;
typedef boost::graph_traits<GraphType>::vertex_iterator    VIterator;
typedef boost::graph_traits<GraphType>::edge_iterator      EIterator;
typedef boost::graph_traits<GraphType>::adjacency_iterator AdjIterator;

#define graph_ reinterpret_cast<GraphType*>(graphRaw_)

void Graph::print (){

  std::pair<VIterator, VIterator> vp;
  boost::property_map<GraphType, boost::vertex_index_t>::type vertices_indx = get(boost::vertex_index, *graph_);
  boost::property_map<GraphType, vertex_type_t >::type vertices_name = get(vertex_type, (*graph_));
  boost::property_map<GraphType, vertex_counter_t >::type vertices_counter = get(vertex_counter, (*graph_));
  std::cout << "vertices(g) = \n";
  std::pair<VIterator, VIterator> vp2;
  for (vp2 = vertices(*graph_); vp2.first != vp2.second; ++vp2.first)
    std::cout << "Index = " << vertices_indx [*vp2.first]
              << "\tType = " << vertices_name[*vp2.first]
              <<  "\tCounter = " << vertices_counter[*vp.first]
              << std::endl;
}

void Graph::set_counter (int index,int dis_time){
    boost::property_map<GraphType, vertex_counter_t >::type name = get(vertex_counter, (*graph_));
    boost::put(name,index,dis_time);
}

int Graph::get_counter(int index){
    boost::property_map<GraphType, vertex_counter_t >::type name = get(vertex_counter, (*graph_));
    return name[index];
}

void Graph::set_type (int index,std::string type){
    boost::property_map<GraphType, vertex_type_t >::type name = get(vertex_type, (*graph_));
    boost::put(name,index,type);
}

std::string Graph::get_type(int index){
    boost::property_map<GraphType, vertex_type_t >::type name =  get(vertex_type, *graph_);
    return name[index];
}

// Euclidean distance heuristic for A* search
// Returns square of distance between two vertices
template <class G, class CostType, class LocMap>
class DistanceHeuristic : public boost::astar_heuristic<G, CostType>
{
public:
    typedef typename boost::graph_traits<G>::vertex_descriptor Vertex;

    // Constructor.  Accepts a LocMap and the id of the goal of the search
    DistanceHeuristic (LocMap l, Vertex goal) : location_(l), goal_(goal) {}

    // Comparison operator.  Returns the estimated cost of traveling
    // between the goal and the vertex u
    CostType operator () (Vertex u)
    {
        CostType dx = location_[goal_].x - location_[u].x;
        CostType dy = location_[goal_].y - location_[u].y;
        CostType dz = location_[goal_].z - location_[u].z;
        return dx*dx + dy*dy + dz*dz;
    }

protected:

    LocMap location_;
    Vertex goal_;
};

// Empty structure.  Used to throw an exception indicating A* search termination.
struct foundAStarGoal {};

// A* search visitor.  Used during the A* search to check for the goal condition.
template <class Vertex>
class AStarGoalVisitor : public boost::default_astar_visitor
{
public:
    // Constructor.  Accepts the goal for use in termination condition.
    AStarGoalVisitor(Vertex goal) : goal_(goal) {}

    // Callback used during A* search when the vertex u is expanded.  If the
    template <class Graph>
    void examine_vertex(Vertex u, Graph& g)
    {
        if(u == goal_)
            throw foundAStarGoal ();
    }
private:
    Vertex goal_;
};

// Structure used to indicate the breadth first search has terminated.
template <class SearchData>
struct foundBrFSGoal
{
    /// Constructor.  Takes the goal node and related search data.
    foundBrFSGoal(unsigned int _g, const SearchData &d) : goal(_g), data(d) {}

    /// The goal node expanded
    unsigned int goal;
    // Search data computed during the brfs.  Useful for extracting the path from the start to the goal node.
    SearchData data;
};

// Breadth-first search visitor class.  Parameters: Graph (boost adjacency_list
// structure), SearchData (map used to store parent information), Goals (list
// of potential goal nodes).
template <class Graph, class SearchData, class Targets>
class brfsSearchVisitor : public boost::default_bfs_visitor
{
public:
    // Type describing a vertex in the graph
    typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
    // Type describing an edge in the graph
    typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;

    // Constructor.  Accepts a list of target nodes for the search.
    // If negative_targets, goals are states NOT in targets.
    brfsSearchVisitor(const Targets& _targets, bool _negative_targets) : boost::default_bfs_visitor(), targets(_targets), negative_targets(_negative_targets) {}

    // Initialization callback from breadth-first search.  Sets parent information to NO_VERTEX (invalid)
    void initialize_vertex(Vertex v, const Graph &g)
    {
        search_data[v] = NO_VERTEX;
    }

    // Edge expansion callback from breadth-first search.  Updates parent information in search_data.
    void tree_edge(Edge e, const Graph &g)
    {
        // Target vertices point to source in search_data
        search_data[boost::target (e, g)] = boost::source(e, g);
    }

    // Vertex expansion callback from breadth-first search.
    void discover_vertex(Vertex v, const Graph& g)
    {
        bool found = targets.find(v) != targets.end();

        // Expanded a goal vertex
        if ((found && !negative_targets) || (!found && negative_targets))
        {
            // Throw the goal structure.  Populate it with this vertex, and the search_data computed to find this vertex.
            foundBrFSGoal<SearchData> the_goal(get(boost::vertex_index, g, v), search_data);
            throw the_goal;
        }
    }

protected:
    // Map containing parent information.  Used to extract shortest path.
    SearchData search_data;
    // Set containing candidate goal nodes
    Targets targets;
    bool negative_targets;
};

Graph::Graph(void)
{
    graphRaw_ = new GraphType();
}

Graph::~Graph(void)
{
    delete graph_;
    graphRaw_ = NULL;
}

void Graph::clear(void)
{
    graph_->clear();
}

void Graph::getNeighbors(unsigned int vtx, std::vector<unsigned int>& nbrs) const
{
    nbrs.clear();
    std::pair<AdjIterator, AdjIterator> iterators = boost::adjacent_vertices(boost::vertex(vtx, *graph_), *graph_);
    boost::property_map<GraphType, boost::vertex_index_t>::type vertices = get(boost::vertex_index, *graph_);
    for (AdjIterator iter = iterators.first; iter != iterators.second; ++iter)
        nbrs.push_back(vertices[*iter]);
}

unsigned int Graph::getNumNeighbors(unsigned int vtx) const
{
    return boost::degree(boost::vertex(vtx, *graph_), *graph_);
}

void Graph::readGraphML(const char* filename)
{
    clear();

    std::ifstream fin;
    fin.open(filename);
    if (!fin)
    {
        std::cout<<"Could not open"<<filename;
        return;
    }

    boost::dynamic_properties dp;
    dp.property("Coordinate", get(vertex_location, *graph_));
    dp.property("Weight", get(boost::edge_weight, *graph_));

    try
    {
        boost::read_graphml(fin, *(graph_), dp);
    }
    catch (boost::parse_error err)
    {
        std::cout << err.what() << std::endl;
    }
    fin.close();
}

void Graph::writeGraphML(const char* filename)
{
    std::ofstream fout;
    fout.open(filename);
    assert(filename);

    boost::dynamic_properties dp;
    dp.property("Coordinate", get(vertex_location, *graph_));
    dp.property("Weight", get(boost::edge_weight, *graph_));
    try
    {
        boost::write_graphml(fout, *(graph_), dp, true);
    }
    catch (boost::parse_error err)
    {
        std::cout << err.what() << std::endl;
    }
    fout.close();
}

double Graph::distanceHeuristic(unsigned int v1, unsigned int v2) const
{
    assert(numNodes() > v1 && numNodes() > v2);
    boost::property_map<GraphType, vertex_location_t>::type locs = get(vertex_location, *graph_);

    double dx = locs[v1].x - locs[v2].x;
    double dy = locs[v1].y - locs[v2].y;
    double dz = locs[v1].z - locs[v2].z;
    return sqrt(dx*dx + dy*dy + dz*dz);
}

unsigned int Graph::numEdges() const
{
    return boost::num_edges(*graph_);
}

unsigned int Graph::numNodes() const
{
    return boost::num_vertices(*graph_);
}

double Graph::edgeWeight(unsigned int v1, unsigned int v2) const
{
    boost::property_map<GraphType, boost::edge_weight_t>::type weights = get(boost::edge_weight, *graph_);

    Edge e;
    bool found;
    tie(e, found) = boost::edge(v1, v2, *graph_);

    if (found)
        return weights[e];

    std::cout<<"Edge %u => %u NOT FOUND"<< v1<< v2;
    return 0.0;
}

bool Graph::edgeExists(unsigned int v1, unsigned int v2) const
{
    Edge e;
    bool found;
    tie(e, found) = boost::edge(v1, v2, *graph_);

    return found;
}

void Graph::blockVertices(const std::vector <unsigned int>& blocked_vertices,
                        std::map<std::pair<unsigned int, unsigned int>, double>& cleared_edges)
{
    boost::property_map<GraphType, boost::edge_weight_t>::type edgeweights = get(boost::edge_weight, *graph_);

    for (size_t i = 0; i < blocked_vertices.size (); i++)
    {
        std::vector <unsigned int> nbrs;
        getNeighbors (blocked_vertices[i], nbrs);

        // Getting weights
        for (size_t j = 0; j < nbrs.size (); j++)
        {
            Edge e;
            bool found;
            tie(e, found) = boost::edge(blocked_vertices[i], nbrs[j], *graph_);

            cleared_edges[std::make_pair<unsigned int, unsigned int> (blocked_vertices[i], nbrs[j])] =
                edgeweights[e];
        }

        // Removing edges from blocked vertices in graph
        boost::clear_vertex (blocked_vertices[i], *graph_);
    }
}

void Graph::restoreVertices(std::map<std::pair<unsigned int, unsigned int>, double>& cleared_edges)
{
    std::map<std::pair<unsigned int, unsigned int>, double>::const_iterator iter = cleared_edges.begin ();

    for (; iter != cleared_edges.end (); ++iter)
    {
        boost::add_edge(boost::vertex(iter->first.first, *graph_), boost::vertex(iter->first.second, *graph_), iter->second, *graph_);
    }
}

bool Graph::AStar(unsigned int start, unsigned int goal, std::vector<unsigned int>& path) const
{
    std::vector<Vertex> p(numNodes());
    std::vector<double> d(numNodes());
    path.clear ();

    try
    {
        boost::astar_search (*graph_, boost::vertex(start, *graph_),
                             DistanceHeuristic <GraphType, double, boost::property_map<GraphType, vertex_location_t>::type>(get(vertex_location, *graph_), boost::vertex(goal, *graph_)),
                             boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(AStarGoalVisitor<Vertex>(boost::vertex(goal, *graph_))));
    }
    catch (foundAStarGoal fg)
    {
        boost::property_map<GraphType, boost::vertex_index_t>::type indices = get(boost::vertex_index, *graph_);
        for (Vertex v = boost::vertex(goal, *graph_); v != boost::vertex(start, *graph_); v = p[v])
            path.insert(path.begin(), indices[v]);
        path.insert(path.begin(), boost::vertex(start, *graph_));
        return true;
    }

    return false;
}

bool Graph::AStar(unsigned int start, unsigned int goal, std::vector<unsigned int>& path, const std::vector<unsigned int>& obstacles)
{
    std::map<std::pair<unsigned int, unsigned int>, double> blocked;
    blockVertices(obstacles, blocked);
    bool ret = AStar(start, goal, path);
    restoreVertices(blocked);

    return ret;
}

bool Graph::BrFS(unsigned int start, const std::set<unsigned int>& targets,
                  std::vector<unsigned int>& path, const std::vector<unsigned int>& obstacles, bool negative_target)
{
    std::map<std::pair<unsigned int, unsigned int>, double> blocked;
    blockVertices(obstacles, blocked);
    bool ret = BrFS(start, targets, path, negative_target);
    restoreVertices(blocked);

    return ret;
}

bool Graph::BrFS(unsigned int start, const std::set<unsigned int>& targets, std::vector<unsigned int>& path, bool negative_target) const
{
    typedef std::map<unsigned int, unsigned int> SearchData;
    path.clear();

    brfsSearchVisitor<GraphType, SearchData, std::set<unsigned int> >brfs_visitor(targets, negative_target);

    try
    {
        boost::breadth_first_search (*graph_, boost::vertex(start, *graph_), boost::visitor(brfs_visitor));
    }
    catch (foundBrFSGoal<SearchData> brfs_goal) // End of successful search.  If exception isn't thrown, no goal found
    {
        unsigned int goal = brfs_goal.goal;

        // Constructing shortest path.  Work backwards from goal, and make sure to add in start (not part of search)
        unsigned int index = goal;
        do
        {
            path.insert(path.begin (), index);
            index = brfs_goal.data[index];

            assert (index != brfs_goal.data[index]);

        } while (index != NO_VERTEX);

        return true;
    }
    return false;
}

void Graph::getMinSpanningTree(unsigned int root, std::vector<unsigned int>& predecessorList) const
{
    std::vector<Vertex> pred(numNodes());
    boost::prim_minimum_spanning_tree(*graph_, &pred[0], boost::weight_map(get(boost::edge_weight, *graph_)).
                                                         vertex_index_map(get(boost::vertex_index, *graph_)).
                                                         root_vertex(boost::vertex(root, *graph_)));

    boost::property_map<GraphType, boost::vertex_index_t>::type indices = get(boost::vertex_index, *graph_);

    predecessorList.clear();
    predecessorList.resize(numNodes());
    for (size_t i = 0; i < pred.size(); ++i)
    {
        predecessorList[i] = indices[pred[i]];
    }
}


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/miller/bins.h>
#include <scitbx/boost_python/utils.h>
#include <boost/python/class.hpp>
#include <boost/python/tuple.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/return_internal_reference.hpp>

namespace cctbx { namespace miller { namespace boost_python {

namespace {

  struct binning_wrappers
  {
    typedef binning w_t;

    static boost::python::tuple
    getinitargs(w_t const& o)
    {
      return boost::python::make_tuple(o.unit_cell(), o.limits());
    }

    static boost::python::object
    range_used(w_t const& o)
    {
      return scitbx::boost_python::range(1, o.n_bins_used());
    }

    static boost::python::object
    range_all(w_t const& o)
    {
      return scitbx::boost_python::range(o.n_bins_all());
    }

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<copy_const_reference> ccr;
      typedef return_internal_reference<> rir;
      class_<w_t>("binning", no_init)
        .enable_pickling()
        .def("__getinitargs__", getinitargs)
        .def(init<uctbx::unit_cell const&,
                  std::size_t,
                  double,
                  double,
                  optional<double> >())
        .def(init<uctbx::unit_cell const&,
                  std::size_t,
                  af::const_ref<index<> > const&,
                  optional<double, double, double> >())

        .def(init<uctbx::unit_cell const&,
                  af::const_ref<index<> > const&,
                  double,
                  double ,
                  double  > ()  )
        .def(init<uctbx::unit_cell const&,
                  af::const_ref<double> >())
        .def("unit_cell", &w_t::unit_cell, rir())
        .def("n_bins_used", &w_t::n_bins_used)
        .def("n_bins_all", &w_t::n_bins_all)
        .def("range_used", range_used)
        .def("range_all", range_all)
        .def("i_bin_d_too_large", &w_t::i_bin_d_too_large)
        .def("i_bin_d_too_small", &w_t::i_bin_d_too_small)
        .def("d_max", &w_t::d_max)
        .def("d_min", &w_t::d_min)
        .def("bin_d_range", &w_t::bin_d_range)
        .def("bin_d_min", &w_t::bin_d_min)
        .def("limits", &w_t::limits, ccr())
        .def("get_i_bin",
          (std::size_t(w_t::*)(double) const)
          &w_t::get_i_bin)
        .def("get_i_bin",
          (std::size_t(w_t::*)(index<> const&) const)
          &w_t::get_i_bin)
      ;
    }
  };

  struct binner_wrappers
  {
    typedef binner w_t;

    static boost::python::tuple
    getinitargs(w_t const& o)
    {
      return boost::python::make_tuple(
        *static_cast<const binning*>(&o),
        o.miller_indices());
    }

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<copy_const_reference> ccr;
      class_<w_t, bases<binning> >("binner", no_init)
        .enable_pickling()
        .def("__getinitargs__", getinitargs)
        .def(init<binning const&,
                  af::shared<index<> > const&>())
        .def("miller_indices", &w_t::miller_indices, ccr())
        .def("bin_indices", &w_t::bin_indices, ccr())
        .def("count", &w_t::count)
        .def("counts", &w_t::counts)
        .def("selection", &w_t::selection)
        .def("array_indices", &w_t::array_indices)
        .def("bin_centers",
          (af::shared<double>(w_t::*)(
            double const&) const)
          &w_t::bin_centers)
        .def("interpolate",
          (af::shared<double>(w_t::*)(
            af::const_ref<double> const&,
            double const&) const)
          &w_t::interpolate)
      ;
    }
  };

} // namespace <anoymous>

  void wrap_bins()
  {
    binning_wrappers::wrap();
    binner_wrappers::wrap();
  }

}}} // namespace cctbx::miller::boost_python


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/*
 * @file testOrientedPlane3.cpp
 * @date Dec 19, 2012
 * @author Alex Trevor
 * @brief Tests the OrientedPlane3Factor class
 */

#include <gtsam/slam/OrientedPlane3Factor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/numericalDerivative.h>

#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <boost/assign/std/vector.hpp>

using namespace boost::assign;
using namespace gtsam;
using namespace std;

GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)

// *************************************************************************
TEST (OrientedPlane3Factor, lm_translation_error) {
  // Tests one pose, two measurements of the landmark that differ in range only.
  // Normal along -x, 3m away
  Symbol lm_sym('p', 0);
  OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);

  ISAM2 isam2;
  Values new_values;
  NonlinearFactorGraph new_graph;

  // Init pose and prior.  Pose Prior is needed since a single plane measurement does not fully constrain the pose
  Symbol init_sym('x', 0);
  Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
  Vector sigmas(6);
  sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001;
  new_graph.addPrior(
      init_sym, init_pose, noiseModel::Diagonal::Sigmas(sigmas));
  new_values.insert(init_sym, init_pose);

  // Add two landmark measurements, differing in range
  new_values.insert(lm_sym, test_lm0);
  Vector sigmas3(3);
  sigmas3 << 0.1, 0.1, 0.1;
  Vector test_meas0_mean(4);
  test_meas0_mean << -1.0, 0.0, 0.0, 3.0;
  OrientedPlane3Factor test_meas0(test_meas0_mean,
      noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym);
  new_graph.add(test_meas0);
  Vector test_meas1_mean(4);
  test_meas1_mean << -1.0, 0.0, 0.0, 1.0;
  OrientedPlane3Factor test_meas1(test_meas1_mean,
      noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym);
  new_graph.add(test_meas1);

  // Optimize
  ISAM2Result result = isam2.update(new_graph, new_values);
  Values result_values = isam2.calculateEstimate();
  OrientedPlane3 optimized_plane_landmark = result_values.at<OrientedPlane3>(
      lm_sym);

  // Given two noisy measurements of equal weight, expect result between the two
  OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0);
  EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
}

// *************************************************************************
TEST (OrientedPlane3Factor, lm_rotation_error) {
  // Tests one pose, two measurements of the landmark that differ in angle only.
  // Normal along -x, 3m away
  Symbol lm_sym('p', 0);
  OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);

  ISAM2 isam2;
  Values new_values;
  NonlinearFactorGraph new_graph;

  // Init pose and prior.  Pose Prior is needed since a single plane measurement does not fully constrain the pose
  Symbol init_sym('x', 0);
  Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
  new_graph.addPrior(init_sym, init_pose,
      noiseModel::Diagonal::Sigmas(
          (Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished()));
  new_values.insert(init_sym, init_pose);

//  // Add two landmark measurements, differing in angle
  new_values.insert(lm_sym, test_lm0);
  Vector test_meas0_mean(4);
  test_meas0_mean << -1.0, 0.0, 0.0, 3.0;
  OrientedPlane3Factor test_meas0(test_meas0_mean,
      noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, lm_sym);
  new_graph.add(test_meas0);
  Vector test_meas1_mean(4);
  test_meas1_mean << 0.0, -1.0, 0.0, 3.0;
  OrientedPlane3Factor test_meas1(test_meas1_mean,
      noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, lm_sym);
  new_graph.add(test_meas1);

  // Optimize
  ISAM2Result result = isam2.update(new_graph, new_values);
  Values result_values = isam2.calculateEstimate();
  OrientedPlane3 optimized_plane_landmark = result_values.at<OrientedPlane3>(
      lm_sym);

  // Given two noisy measurements of equal weight, expect result between the two
  OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0,
      0.0, 3.0);
  EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
}

// *************************************************************************
TEST( OrientedPlane3DirectionPrior, Constructor ) {

  // Example: pitch and roll of aircraft in an ENU Cartesian frame.
  // If pitch and roll are zero for an aerospace frame,
  // that means Z is pointing down, i.e., direction of Z = (0,0,-1)

  Vector planeOrientation = (Vector(4) << 0.0, 0.0, -1.0, 10.0).finished(); // all vertical planes directly facing the origin

  // Factor
  Key key(1);
  SharedGaussian model = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 10.0));
  OrientedPlane3DirectionPrior factor(key, planeOrientation, model);

  // Create a linearization point at the zero-error point
  Vector theta1 = Vector4(0.0, 0.02, -1.2, 10.0);
  Vector theta2 = Vector4(0.0, 0.1, -0.8, 10.0);
  Vector theta3 = Vector4(0.0, 0.2, -0.9, 10.0);

  OrientedPlane3 T1(theta1);
  OrientedPlane3 T2(theta2);
  OrientedPlane3 T3(theta3);

  // Calculate numerical derivatives
  Matrix expectedH1 = numericalDerivative11<Vector, OrientedPlane3>(
      boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
          boost::none), T1);

  Matrix expectedH2 = numericalDerivative11<Vector, OrientedPlane3>(
      boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
          boost::none), T2);

  Matrix expectedH3 = numericalDerivative11<Vector, OrientedPlane3>(
      boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
          boost::none), T3);

  // Use the factor to calculate the derivative
  Matrix actualH1, actualH2, actualH3;
  factor.evaluateError(T1, actualH1);
  factor.evaluateError(T2, actualH2);
  factor.evaluateError(T3, actualH3);

  // Verify we get the expected error
  EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
  EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
  EXPECT(assert_equal(expectedH3, actualH3, 1e-8));
}

/* ************************************************************************* */
int main() {
  srand(time(nullptr));
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


//  boost run_timer_test.cpp  -----------------------------------------------------//

//  Copyright Beman Dawes 2006, 2008
//  Copyright 2009 Vicente J. Botet Escriba

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

//  See http://www.boost.org/libs/chrono for documentation.

#include <boost/chrono/chrono.hpp>
#include <boost/chrono/process_cpu_clocks.hpp>
#include <boost/chrono/thread_clock.hpp>
#include "boost/chrono/stopwatches/simple_stopwatch.hpp"
#include <cstdlib> // for atol()
#include <iostream>
#include <sstream>
#include <locale>
#include <ctime>
#include <cmath>  // for sqrt(), used to burn time

//using boost::chrono::run_timer;
using boost::system::error_code;

#include <boost/detail/lightweight_test.hpp>

namespace
{
  typedef boost::chrono::nanoseconds ns;

  // accuracy test
  void accuracy_test( int argc, char * argv[] )
  {
    long timeout_in_secs = 1;
    if ( argc > 1 ) timeout_in_secs = std::atol( argv[1] );
    std::cout << "accuracy test for " << timeout_in_secs << " second(s)...";

    std::clock_t timeout_in_clock_t = std::clock();
    std::cout << "accuracy test. Now=" << timeout_in_clock_t << " ticks...";
    timeout_in_clock_t += (timeout_in_secs * CLOCKS_PER_SEC);
    std::cout << "accuracy test. Timeout=" << timeout_in_clock_t << " ticks...";

    boost::chrono::simple_stopwatch<boost::chrono::system_clock>          sys;
#ifdef BOOST_CHRONO_HAS_CLOCK_STEADY
    boost::chrono::simple_stopwatch<boost::chrono::steady_clock>          steady;
#endif
    boost::chrono::simple_stopwatch<>  hires;
    boost::chrono::simple_stopwatch<boost::chrono::process_cpu_clock>          process;

    std::clock_t now;
    do
    {
      now = std::clock();
    } while ( now < timeout_in_clock_t );

    boost::chrono::simple_stopwatch<boost::chrono::system_clock>::duration sys_dur = sys.elapsed();
#ifdef BOOST_CHRONO_HAS_CLOCK_STEADY
    boost::chrono::simple_stopwatch<boost::chrono::steady_clock>::duration steady_dur = steady.elapsed();
#endif
    boost::chrono::simple_stopwatch<>::duration hires_dur = hires.elapsed();
    boost::chrono::simple_stopwatch<boost::chrono::process_cpu_clock>::duration times;
    times = process.elapsed();
    std::cout << "accuracy test. Now=" << now << " ticks...";

    std::cout << std::endl;

    ns timeout_in_nanoseconds( static_cast<long long>(timeout_in_secs) * 1000000000LL );

    //  Allow 20% leeway. Particularly on Linux, there seems to be a large discrepancy
    //  between std::clock() and higher resolution clocks.
    ns maximum_delta ( static_cast<long long>(timeout_in_nanoseconds.count() * 0.20 ) );

    std::cout << timeout_in_nanoseconds.count() << " timeout_in_nanoseconds\n";
    std::cout << maximum_delta.count() << " maximum_delta\n";

    std::cout << sys_dur.count() << " sys_dur\n";

    BOOST_TEST( sys_dur > timeout_in_nanoseconds - maximum_delta
      && sys_dur < timeout_in_nanoseconds + maximum_delta );

#ifdef BOOST_CHRONO_HAS_CLOCK_STEADY
    std::cout << steady_dur.count() << " steady_dur\n";

    BOOST_TEST( steady_dur > timeout_in_nanoseconds - maximum_delta
      && steady_dur < timeout_in_nanoseconds + maximum_delta );
#endif

    std::cout << hires_dur.count() << " hires_dur\n";

    BOOST_TEST( hires_dur > timeout_in_nanoseconds - maximum_delta
      && hires_dur < timeout_in_nanoseconds + maximum_delta );

    std::cout << times.count().real << " times.real\n";

    BOOST_TEST( ns(times.count().real) > timeout_in_nanoseconds - maximum_delta
      && ns(times.count().real) < timeout_in_nanoseconds + maximum_delta );
  }

}

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

  return boost::report_errors();
}


#ifndef NETWORKS_HPP
#define NETWORKS_HPP

#include <string>
// For External Library
#include <torch/torch.h>
#include <boost/program_options.hpp>

// Define Namespace
namespace nn = torch::nn;
namespace po = boost::program_options;

// Function Prototype
void weights_init(nn::Module &m);
void DownSampling(nn::Sequential &sq, const size_t in_nc, const size_t out_nc, const bool BN, const bool ReLU, const bool bias=false);
void UpSampling(nn::Sequential &sq, const size_t in_nc, const size_t out_nc, const bool BN, const bool ReLU, const bool bias=false);
torch::Tensor AbsoluteEuclideanDistance(torch::Tensor x1, torch::Tensor x2, const bool rec_detach=true);
torch::Tensor RelativeEuclideanDistance(torch::Tensor x1, torch::Tensor x2, const bool rec_detach=true);
torch::Tensor CosineSimilarity(torch::Tensor x1, torch::Tensor x2, const bool rec_detach=true);
void save_params(const std::string path, torch::Tensor mu, torch::Tensor sigma, torch::Tensor phi);
void load_params(const std::string path, torch::Tensor &mu, torch::Tensor &sigma, torch::Tensor &phi);

// -------------------------------------------------
// struct{EncoderImpl}(nn::Module)
// -------------------------------------------------
struct EncoderImpl : nn::Module{
private:
    nn::Sequential model;
public:
    EncoderImpl(){}
    EncoderImpl(po::variables_map &vm);
    torch::Tensor forward(torch::Tensor x);
};

// -------------------------------------------------
// struct{DecoderImpl}(nn::Module)
// -------------------------------------------------
struct DecoderImpl : nn::Module{
private:
    nn::Sequential model;
public:
    DecoderImpl(){}
    DecoderImpl(po::variables_map &vm);
    torch::Tensor forward(torch::Tensor z_c);
};

// -------------------------------------------------
// struct{EstimationNetworkImpl}(nn::Module)
// -------------------------------------------------
struct EstimationNetworkImpl : nn::Module{
private:
    float eps;
    size_t N;
    torch::Tensor gamma_sum_keep, mu_sum_keep, sigma_sum_keep;
    torch::Tensor energy_keep, NVI_keep, precision_keep;
    nn::Sequential model;
public:
    EstimationNetworkImpl(){}
    EstimationNetworkImpl(po::variables_map &vm, const float eps_=1e-12);
    torch::Tensor forward(torch::Tensor z);
    void estimation(torch::Tensor z, torch::Tensor gamma_ap);
    void estimationNVI(torch::Tensor z, torch::Tensor gamma_ap);
    void resetGMP(torch::Device device);
    void estimationGMP(torch::Tensor z, torch::Tensor gamma_ap);
    torch::Tensor estimated_mu();
    torch::Tensor estimated_sigma();
    torch::Tensor estimated_phi();
    torch::Tensor energy_just_before();
    torch::Tensor NVI_just_before();
    torch::Tensor precision_just_before();
    torch::Tensor anomaly_score(torch::Tensor z, torch::Tensor mu, torch::Tensor sigma, torch::Tensor phi);
};

// -------------------------------------------------
// struct{LinearResBlockImpl}(nn::Module)
// -------------------------------------------------
struct LinearResBlockImpl : nn::Module{
private:
    nn::Sequential model;
public:
    LinearResBlockImpl(){}
    LinearResBlockImpl(const size_t nr, bool use_dropout=false);
    torch::Tensor forward(torch::Tensor x);
};

TORCH_MODULE(Encoder);
TORCH_MODULE(Decoder);
TORCH_MODULE(EstimationNetwork);
TORCH_MODULE(LinearResBlock);

#endif


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

// Copyright (c) 2012-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2015.
// Modifications copyright (c) 2015 Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, 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)

#if defined(_MSC_VER)
#  pragma warning( disable : 4244 )
#  pragma warning( disable : 4267 )
#endif

#include <fstream>
#include <sstream>

#include <boost/foreach.hpp>
#include <boost/program_options.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/timer.hpp>

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

#include <boost/geometry/algorithms/detail/buffer/buffer_inserter.hpp>

#include <boost/geometry/strategies/buffer.hpp>

#include <common/common_settings.hpp>
#include <common/make_square_polygon.hpp>

struct buffer_settings : public common_settings
{
    int join_code;
    double distance;
};

namespace bg = boost::geometry;

template <typename Geometry1, typename Geometry2>
void create_svg(std::string const& filename
                , Geometry1 const& mp
                , Geometry2 const& buffer
                )
{
    typedef typename boost::geometry::point_type<Geometry1>::type point_type;

    std::ofstream svg(filename.c_str());
    boost::geometry::svg_mapper<point_type> mapper(svg, 800, 800);

    boost::geometry::model::box<point_type> box;
    bg::envelope(mp, box);
    bg::buffer(box, box, 1.0);
    mapper.add(box);

    if (! bg::is_empty(buffer))
    {
        bg::envelope(buffer, box);
        bg::buffer(box, box, 1.0);
        mapper.add(box);
    }

    mapper.map(mp, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:3");
    mapper.map(buffer, "stroke-opacity:0.9;stroke:rgb(0,0,0);fill:none;stroke-width:1");

    //mapper.map(intersection,"opacity:0.6;stroke:rgb(0,128,0);stroke-width:5");
}

template <typename MultiPolygon, typename Settings>
bool verify(std::string const& caseid, MultiPolygon const& mp, MultiPolygon const& buffer, Settings const& settings)
{
    bool result = true;

    // Area of buffer must be larger than of original polygon
    BOOST_AUTO(area_mp, bg::area(mp));
    BOOST_AUTO(area_buf, bg::area(buffer));

    if (area_buf < area_mp)
    {
        result = false;
    }

    if (result)
    {
        typedef typename boost::range_value<MultiPolygon const>::type polygon_type;
        BOOST_FOREACH(polygon_type const& polygon, mp)
        {
            typename bg::point_type<polygon_type>::type point;
            bg::point_on_border(point, polygon);
            if (! bg::within(point, buffer))
            {
                result = false;
            }
        }
    }

    bool svg = settings.svg;
    bool wkt = settings.wkt;
    if (! result)
    {
        std::cout << "ERROR " << caseid << std::endl;
        //std::cout << bg::wkt(mp) << std::endl;
        //std::cout << bg::wkt(buffer) << std::endl;
        svg = true;
        wkt = true;
    }

    if (svg || wkt)
    {
        //std::cout << caseid << std::endl;
    }

    if (svg)
    {
        std::ostringstream filename;
        filename << caseid << "_"
            << typeid(typename bg::coordinate_type<MultiPolygon>::type).name()
            << ".svg";
        create_svg(filename.str(), mp, buffer);
    }

    if (wkt)
    {
        std::ostringstream filename;
        filename << caseid << "_"
            << typeid(typename bg::coordinate_type<MultiPolygon>::type).name()
            << ".wkt";
        std::ofstream stream(filename.str().c_str());
        stream << bg::wkt(mp) << std::endl;
        stream << bg::wkt(buffer) << std::endl;
    }

    return result;
}

template <typename MultiPolygon, typename Generator, typename Settings>
bool test_buffer(MultiPolygon& result, int& index,
            Generator& generator,
            int level, Settings const& settings)
{
    MultiPolygon p, q;

    // Generate two boxes
    if (level == 0)
    {
        p.resize(1);
        q.resize(1);
        make_square_polygon(p.front(), generator, settings);
        make_square_polygon(q.front(), generator, settings);
        bg::correct(p);
        bg::correct(q);
    }
    else
    {
        bg::correct(p);
        bg::correct(q);
        if (! test_buffer(p, index, generator, level - 1, settings)
            || ! test_buffer(q, index, generator, level - 1, settings))
        {
            return false;
        }
    }

    typedef typename boost::range_value<MultiPolygon>::type polygon;

    MultiPolygon mp;
    bg::detail::union_::union_insert
        <
            polygon
        >(p, q, std::back_inserter(mp));

    bg::unique(mp);
    bg::unique(mp);
    bg::correct(mp);
    result = mp;

    typedef typename bg::coordinate_type<MultiPolygon>::type coordinate_type;
    typedef typename bg::point_type<MultiPolygon>::type point_type;
    typedef bg::strategy::buffer::distance_asymmetric<coordinate_type> distance_strategy_type;
    distance_strategy_type distance_strategy(settings.distance, settings.distance);

    typedef typename boost::range_value<MultiPolygon>::type polygon_type;
    MultiPolygon buffered;

    std::ostringstream out;
    out << "recursive_polygons_buffer_" << index++ << "_" << level;

    bg::strategy::buffer::end_round end_strategy;
    bg::strategy::buffer::point_circle point_strategy;
    bg::strategy::buffer::side_straight side_strategy;
    bg::strategy::buffer::join_round join_round_strategy(100); // Compatible with unit tests
    bg::strategy::buffer::join_miter join_miter_strategy;

    try
    {
        switch(settings.join_code)
        {
            case 1 :
                bg::buffer(mp, buffered,
                                distance_strategy, side_strategy,
                                join_round_strategy,
                                end_strategy, point_strategy);
                break;
            case 2 :
                bg::buffer(mp, buffered,
                                distance_strategy, side_strategy,
                                join_miter_strategy,
                                end_strategy, point_strategy);
                break;
            default :
                return false;
        }
    }
    catch(std::exception const& e)
    {
        MultiPolygon empty;
        std::cout << out.str() << std::endl;
        std::cout << "Exception " << e.what() << std::endl;
        verify(out.str(), mp, empty, settings);
        return false;
    }

    return verify(out.str(), mp, buffered, settings);
}

template <typename T, bool Clockwise, bool Closed, typename Settings>
void test_all(int seed, int count, int level, Settings const& settings)
{
    boost::timer t;

    typedef boost::minstd_rand base_generator_type;

    base_generator_type generator(seed);

    boost::uniform_int<> random_coordinate(0, settings.field_size - 1);
    boost::variate_generator<base_generator_type&, boost::uniform_int<> >
        coordinate_generator(generator, random_coordinate);

    typedef bg::model::polygon
        <
            bg::model::d2::point_xy<T>, Clockwise, Closed
        > polygon;
    typedef bg::model::multi_polygon<polygon> mp;

    int index = 0;
    for(int i = 0; i < count; i++)
    {
        mp p;
        test_buffer<mp>(p, index, coordinate_generator, level, settings);
    }
    std::cout
        << "geometries: " << index
        << " type: " << typeid(T).name()
        << " time: " << t.elapsed()  << std::endl;
}

int main(int argc, char** argv)
{
    try
    {
        namespace po = boost::program_options;
        po::options_description description("=== recursive_polygons_linear_areal ===\nAllowed options");

        int count = 1;
        int seed = static_cast<unsigned int>(std::time(0));
        int level = 3;
        bool ccw = false;
        bool open = false;
        buffer_settings settings;
        std::string form = "box";
        std::string join = "round";

        description.add_options()
            ("help", "Help message")
            ("seed", po::value<int>(&seed), "Initialization seed for random generator")
            ("count", po::value<int>(&count)->default_value(1), "Number of tests")
            ("level", po::value<int>(&level)->default_value(3), "Level to reach (higher->slower)")
            ("distance", po::value<double>(&settings.distance)->default_value(1.0), "Distance (1.0)")
            ("form", po::value<std::string>(&form)->default_value("box"), "Form of the polygons (box, triangle)")
            ("join", po::value<std::string>(&join)->default_value("round"), "Form of the joins (round, miter)")
            ("ccw", po::value<bool>(&ccw)->default_value(false), "Counter clockwise polygons")
            ("open", po::value<bool>(&open)->default_value(false), "Open polygons")
            ("size", po::value<int>(&settings.field_size)->default_value(10), "Size of the field")
            ("wkt", po::value<bool>(&settings.wkt)->default_value(false), "Create a WKT of the inputs, for all tests")
            ("svg", po::value<bool>(&settings.svg)->default_value(false), "Create a SVG for all tests")
        ;

        po::variables_map varmap;
        po::store(po::parse_command_line(argc, argv, description), varmap);
        po::notify(varmap);

        if (varmap.count("help")
            || (form != "box" && form != "triangle")
            || (join != "round" && join != "miter")
            )
        {
            std::cout << description << std::endl;
            return 1;
        }

        settings.triangular = form != "box";
        settings.join_code = join == "round" ? 1 : 2;

        if (ccw && open)
        {
            test_all<double, false, false>(seed, count, level, settings);
        }
        else if (ccw)
        {
            test_all<double, false, true>(seed, count, level, settings);
        }
        else if (open)
        {
            test_all<double, true, false>(seed, count, level, settings);
        }
        else
        {
            test_all<double, true, true>(seed, count, level, settings);
        }

#if defined(HAVE_TTMATH)
        // test_all<ttmath_big, true, true>(seed, count, max, svg, level);
#endif
    }
    catch(std::exception const& e)
    {
        std::cout << "Exception " << e.what() << std::endl;
    }
    catch(...)
    {
        std::cout << "Other exception" << std::endl;
    }

    return 0;
}


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

 [begin_description]
 Implementation of the Adams-Moulton method. This is method is not a real stepper, it is more a helper class
 which computes the corrector step in the Adams-Bashforth-Moulton method.
 [end_description]

 Copyright 2011-2012 Karsten Ahnert
 Copyright 2011-2013 Mario Mulansky
 Copyright 2012 Christoph Koke

 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_ADAMS_MOULTON_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_ADAMS_MOULTON_HPP_INCLUDED

#include <boost/numeric/odeint/util/bind.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/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>

#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4_classic.hpp>

#include <boost/numeric/odeint/stepper/detail/adams_moulton_call_algebra.hpp>
#include <boost/numeric/odeint/stepper/detail/adams_moulton_coefficients.hpp>
#include <boost/numeric/odeint/stepper/detail/rotating_buffer.hpp>

namespace boost {
namespace numeric {
namespace odeint {

/*
 * Static implicit Adams-Moulton multistep-solver without step size control and without dense output.
 */
template<
size_t Steps ,
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 adams_moulton
{
private:

public :

    typedef State state_type;
    typedef state_wrapper< state_type > wrapped_state_type;
    typedef Value value_type;
    typedef Deriv deriv_type;
    typedef state_wrapper< deriv_type > wrapped_deriv_type;
    typedef Time time_type;
    typedef Algebra algebra_type;
    typedef Operations operations_type;
    typedef Resizer resizer_type;
    typedef stepper_tag stepper_category;

    typedef adams_moulton< Steps , State , Value , Deriv , Time , Algebra , Operations , Resizer > stepper_type;

    static const size_t steps = Steps;

    typedef unsigned short order_type;
    static const order_type order_value = steps + 1;

    typedef detail::rotating_buffer< wrapped_deriv_type , steps > step_storage_type;

    adams_moulton( )
    : m_coefficients() , m_dxdt() , m_resizer() ,
      m_algebra_instance() , m_algebra( m_algebra_instance )
    { }

    adams_moulton( algebra_type &algebra )
    : m_coefficients() , m_dxdt() , m_resizer() ,
      m_algebra_instance() , m_algebra( algebra )
    { }

    adams_moulton& operator=( const adams_moulton &stepper )
    {
        m_dxdt = stepper.m_dxdt;
        m_resizer = stepper.m_resizer;
        m_algebra = stepper.m_algebra;
        return *this;
    }

    order_type order( void ) const { return order_value; }

    /*
     * Version 1 : do_step( system , x , t , dt , buf );
     *
     * solves the forwarding problem
     */
    template< class System , class StateInOut , class StateIn , class ABBuf >
    void do_step( System system , StateInOut &x , StateIn const & pred , time_type t , time_type dt , const ABBuf &buf )
    {
        do_step( system , x , pred , t , x , dt , buf );
    }

    template< class System , class StateInOut , class StateIn , class ABBuf >
    void do_step( System system , const StateInOut &x , StateIn const & pred , time_type t , time_type dt , const ABBuf &buf )
    {
        do_step( system , x , pred , t , x , dt , buf );
    }

    /*
     * Version 2 : do_step( system , in , t , out , dt , buf );
     *
     * solves the forwarding problem
     */
    template< class System , class StateIn , class PredIn , class StateOut , class ABBuf >
    void do_step( System system , const StateIn &in , const PredIn &pred , time_type t , StateOut &out , time_type dt , const ABBuf &buf )
    {
        do_step_impl( system , in , pred , t , out , dt , buf );
    }

    template< class System , class StateIn , class PredIn , class StateOut , class ABBuf >
    void do_step( System system , const StateIn &in , const PredIn &pred , time_type t , const StateOut &out , time_type dt , const ABBuf &buf )
    {
        do_step_impl( system , in , pred , t , out , dt , buf );
    }

    template< class StateType >
    void adjust_size( const StateType &x )
    {
        resize_impl( x );
    }

    algebra_type& algebra()
    {   return m_algebra; }

    const algebra_type& algebra() const
    {   return m_algebra; }

private:

    template< class System , class StateIn , class PredIn , class StateOut , class ABBuf >
    void do_step_impl( System system , const StateIn &in , const PredIn &pred , time_type t , StateOut &out , time_type dt , const ABBuf &buf )
    {
        typename odeint::unwrap_reference< System >::type &sys = system;
        m_resizer.adjust_size( in , detail::bind( &stepper_type::template resize_impl<StateIn> , detail::ref( *this ) , detail::_1 ) );
        sys( pred , m_dxdt.m_v , t );
        detail::adams_moulton_call_algebra< steps , algebra_type , operations_type >()( m_algebra , in , out , m_dxdt.m_v , buf , m_coefficients , dt );
    }

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

    const detail::adams_moulton_coefficients< value_type , steps > m_coefficients;
    wrapped_deriv_type m_dxdt;
    resizer_type m_resizer;

protected:

    algebra_type m_algebra_instance;
    algebra_type &m_algebra;
};

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

#endif // BOOST_NUMERIC_ODEINT_STEPPER_ADAMS_MOULTON_HPP_INCLUDED


#include <iostream>
#include <Transformer.h>
#include <boost/test/unit_test.hpp>

using namespace esrocos::transformer;

typedef AcyclicTransformer<20,20> atf20_20;
typedef atf20_20::Transformation tsf;
typedef atf20_20::Frame frame;

BOOST_AUTO_TEST_SUITE(transformer_tests)

/*
The following tree is modelled for testing.

[Frame]
(dynamic transformation)
<static transformation>

      [world]
        |
    (odometry)
       |
  [base_link]
    |      \
 <link_1>  (x_gimbal)
   |         \
[lidar_1]    [camera_1]
*/

struct Matrices{
  Eigen::Matrix4d m0;
  Eigen::Matrix4d m1;
  Eigen::Matrix4d m2;

  Matrices(){
    m0 << 1, 0,   0,   2,
          0, 0.3, 0,   0,
          0, 0,   2.3, 2,
          0, 0,   0,   1;

    m1 << 1, 0, 0, 1,
          0, 1, 0, 3,
          0, 0, 1, 0,
          0, 0, 0, 1;

    m2 << 2, 0, 0, 0,
          0, 1, 0, 0,
          0, 0, 1, 0,
          0, 0, 0, 1;
  }
};

struct Transformations {
  atf20_20::Transformation odom;
  atf20_20::Transformation link_1;
  atf20_20::Transformation x_gimbal;

  Transformations(){
    odom = atf20_20::Transformation("base_link","world","odometry");
    link_1 = atf20_20::Transformation("lidar_1", "base_link", "link_1");
    x_gimbal = atf20_20::Transformation("camera_1", "base_link", "x_gimbal");
  }
};

struct Frames {
  atf20_20::Frame base_link;
  atf20_20::Frame lidar_1;
  atf20_20::Frame camera_1;

  Frames(){
    base_link = atf20_20::Frame("base_link");
    lidar_1 = atf20_20::Frame("lidar_1");
    camera_1 = atf20_20::Frame("camera_1");
  }
};

struct TestTree {
 atf20_20 tf;

 TestTree(){
  std::cout << "load fixtures" << std::endl;

  tf = atf20_20("world");

  Frames fs;
  Transformations ts;
  Matrices ms;

  std::cout << "build test tree" << std::endl;

  ts.odom.atob(ms.m0);
  fs.base_link.transformToParent = ts.odom;

  ts.link_1.atob(ms.m1);
  fs.lidar_1.transformToParent = ts.link_1;

  ts.x_gimbal.atob(ms.m2);
  fs.camera_1.transformToParent = ts.x_gimbal;

  tf.addFrame(fs.base_link);
  tf.addFrame(fs.lidar_1);
  tf.addFrame(fs.camera_1);

  std::cout << "built test tree" << std::endl;
 }
};

BOOST_AUTO_TEST_CASE(transformer_construction)
{
  atf20_20 tf("world");
}

BOOST_AUTO_TEST_CASE(transformer_get_transform)
{
  Frames fs;
  Transformations ts;
  Matrices ms;
  TestTree tree;
  atf20_20 tf = tree.tf;

  Eigen::Matrix4d t,t0;

  t0 << 1,0,0,0,
        0,1,0,0,
	0,0,1,0,
	0,0,0,1;
  bool approx;

  // transformation from a to a should be identity
  tf.getTransform("lidar_1","lidar_1",t);
  approx = t0.isApprox(t);
  BOOST_CHECK_EQUAL(true, approx);

  t0 << 0,0,0,0,
        0,0,0,0,
	0,0,0,0,
	0,0,0,0;

  // transformation between nonexisting frames should return all zeros matrix
  tf.getTransform("lidar_1","nonsense",t);
  approx = t0.isApprox(t);
  BOOST_CHECK_EQUAL(true,approx);

  tf.getTransform("nonesense","lidar_1",t);
  approx = t0.isApprox(t);
  BOOST_CHECK_EQUAL(true,approx);

  tf.getTransform("nonesense","nonsense",t);
  approx = t0.isApprox(t);
  BOOST_CHECK_EQUAL(true,approx);

  tf.getTransform("lidar_1","world",t);
  approx = (ms.m1*ms.m0).isApprox(t);
  BOOST_CHECK_EQUAL(true, approx);

  tf.getTransform("world","lidar_1",t);
  approx = (ms.m1.inverse()*ms.m0.inverse()).isApprox(t);
  BOOST_CHECK_EQUAL(true, approx);

  tf.getTransform("lidar_1","camera_1",t);
  approx = (ms.m1 * ms.m2.inverse()).isApprox(t);
  BOOST_CHECK_EQUAL(true, approx);

  tf.getTransform("camera_1","lidar_1",t);
  approx = (ms.m2 * ms.m1.inverse()).isApprox(t);
  BOOST_CHECK_EQUAL(true, approx);

}

BOOST_AUTO_TEST_CASE(update_tf){

  Frames fs;
  Transformations ts;
  Matrices ms;
  TestTree tree;
  atf20_20 tf = tree.tf;

  Eigen::Matrix4d u0,u1,u2,u3;

  u0 << 1,0,0,0,
        0,1,0,0,
	0,0,1,0,
	0,0,0,1;

  u1 << 1,0,0,0,
        0,1,0,0,
        0,0,1,1,
	0,0,0,1;

  u2 << 1,0,0,0,
        0,1,0,0,
        0,0,1,2,
	0,0,0,1;

  u3 << 1,0,0,0,
        0,1,0,0,
        0,0,1,3,
	0,0,0,1;

  tsf t;
  bool result;
  // update, then check
  tf.updateTransform(ts.odom.id(),u0);
  tf.getTransform(ts.odom.id(),t);
  result = t.atob().isApprox(u0);
  BOOST_CHECK_EQUAL(true,result);
  // update, then check
  tf.updateTransform(ts.odom.id(),u1);
  tf.getTransform(ts.odom.id(),t);
  result = t.atob().isApprox(u1);
  BOOST_CHECK_EQUAL(true,result);
  // update, then check
  tf.updateTransform(ts.odom.id(),u2);
  tf.getTransform(ts.odom.id(),t);
  result = t.atob().isApprox(u2);
  BOOST_CHECK_EQUAL(true,result);
  // update, then check
  tf.updateTransform(ts.odom.id(),u3);
  tf.getTransform(ts.odom.id(),t);
  result = t.atob().isApprox(u3);
  BOOST_CHECK_EQUAL(true,result);

  //should be false
  std::cout << "try to update nonexisting transform" << std::endl;
  result = tf.updateTransform("somerandomname", u0);
  std::cout << "just to be sure" << std::endl;
  BOOST_CHECK_EQUAL(false, result);
}

BOOST_AUTO_TEST_CASE(get_tf){

  Frames fs;
  Transformations ts;
  Matrices ms;
  TestTree tree;
  atf20_20 tf = tree.tf;

  tsf t;
  bool result;

  tf.getTransform(ts.odom.id(),t);

  result = ms.m0.isApprox(t.atob());
  BOOST_CHECK_EQUAL(true,result);

  // should be false
  result = tf.getTransform("somerandomnonexistingid",t);
  BOOST_CHECK_EQUAL(false,result);
}

BOOST_AUTO_TEST_CASE(transformer_getFrame){

  TestTree tree;
  atf20_20 tf = tree.tf;

  frame f;
  bool result;

  result = tf.getFrame("world",f);
  BOOST_CHECK_EQUAL(true,result);

  result = tf.getFrame("nonsense",f);
  BOOST_CHECK_EQUAL(false,result);
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!
  @file

    @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_FUNCTION_IS_GEZ_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_GEZ_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-predicates
    This function object returns @ref True or @ref False according x
    is greater or equal to @ref Zero or not.

    @par Header <boost/simd/function/is_gez.hpp>

    @par Note

      Using `is_gez(x)` is equivalent to `x >= 0`

    @par Example:

      @snippet is_gez.cpp is_gez

    @par Possible output:

      @snippet is_gez.txt is_gez

  **/
  as_logical_t<Value> is_gez(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/is_gez.hpp>
#include <boost/simd/function/simd/is_gez.hpp>

#endif


/*
 * Copyright 2018 Vladimir Balun
 *
 * 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.
 */

#pragma once

#include <boost/geometry.hpp>
#include <boost/filesystem.hpp>
#include <boost/polygon/polygon.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>

namespace boost::geometry
{

    template<typename Type>
    using point2_t = boost::geometry::model::point<Type, 2, boost::geometry::cs::cartesian>;

    using point2i_t = point2_t<int>;
    using point2f_t = point2_t<float>;
    using point2d_dt = point2_t<double>;

    template<typename Type>
    using point3_t = boost::geometry::model::point<Type, 3, boost::geometry::cs::cartesian>;

    using point3i_t = point3_t<int>;
    using point3f_t = point3_t<float>;
    using point3d_t = point3_t<double>;

    template<typename Type>
    using box2_t = boost::geometry::model::box<point2_t<Type>>;

    using box2i_t = box2_t<int>;
    using box2f_t = box2_t<float>;
    using box2d_t = box2_t<double>;

    template<typename Type>
    using box3_t = boost::geometry::model::box<point3_t<Type>>;

    using box3i_t = box3_t<int>;
    using box3f_t = box3_t<float>;
    using box3d_t = box3_t<double>;

}

namespace bp = boost::polygon;
namespace bg = boost::geometry;
namespace bf = boost::filesystem;
namespace bpt = boost::property_tree;


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

  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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/divides.hpp>
#include <boost/simd/function/saturated.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/zero.hpp>
#include <scalar_test.hpp>

#ifdef BOOST_MSVC
  #pragma warning(push)
  #pragma warning(disable: 4723) // potential divide by 0
#endif

STF_CASE_TPL( "Check divides behavior with floating", STF_IEEE_TYPES )
{
  namespace bs = boost::simd;
  using bs::divides;
  using r_t = decltype(divides(T(), T()));
  STF_TYPE_IS(r_t, T);

#ifndef BOOST_SIMD_NO_INVALIDS
  STF_IEEE_EQUAL(divides(bs::Inf<T>(),  bs::Inf<T>()), bs::Nan<r_t>());
  STF_IEEE_EQUAL(divides(bs::Minf<T>(), bs::Minf<T>()), bs::Nan<r_t>());
  STF_IEEE_EQUAL(divides(bs::Nan<T>(),  bs::Nan<T>()), bs::Nan<r_t>());
#endif
  STF_EQUAL(divides(T(1), T(0)), bs::Inf<r_t>());
  STF_IEEE_EQUAL(divides(T(0), T(0)), bs::Nan<r_t>());
  STF_EQUAL(divides(T(1), T(1)), bs::One<r_t>());
}

STF_CASE_TPL( "Check divides saturated behavior", STF_NUMERIC_TYPES )
{
  namespace bs = boost::simd;
  using bs::divides;
  using r_t = decltype(bs::saturated_(divides)(T(), T()));
  STF_TYPE_IS(r_t, T);

  STF_EQUAL(bs::saturated_(divides)(bs::One<T>(),bs::One<T>()), bs::One<r_t>());
  STF_EQUAL(bs::saturated_(divides)(bs::One<T>(), bs::Zero<T>()), bs::Inf<r_t>());
}

#ifdef BOOST_MSVC
  #pragma warning(pop)
#endif


/*
BSD 3-Clause License

Copyright (c) 2021 Jack Miles Hunt
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder 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 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.
*/

#define BOOST_TEST_MODULE PathFindingTests

#include <map>
#include <vector>

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

#include <CPPUtils/Algorithms/PathFinding.hpp>

using namespace CPPUtils::Algorithms;
using namespace CPPUtils::DataStructures::Graphs;

BOOST_AUTO_TEST_SUITE(PathFindingTestSuite)

BOOST_AUTO_TEST_CASE(VertexCostPriorityTest) {
    RankedVertex<int> a(0.1, 1);
    RankedVertex<int> b(0.2, 1);

    BOOST_CHECK(VertexCostPriority<int>()(a, b));
    BOOST_CHECK(!VertexCostPriority<int>()(b, a));
}

BOOST_AUTO_TEST_CASE(GetPathTest) {
    const std::map<int, int> traversals = {
        {4, 3},
        {3, 2},
        {2, 1}
    };

    const auto path = getPath(traversals, 1, 4);
    const std::vector<int> true_path = { 1, 2, 3, 4 };
    BOOST_CHECK_EQUAL(path.size(), true_path.size());
    BOOST_CHECK_EQUAL_COLLECTIONS(path.cbegin(), path.cend(),
                                  true_path.cbegin(), true_path.cend());
}

BOOST_AUTO_TEST_CASE(GetPathTestEmpty) {
    const std::map<int, int> traversals = {
        {4, 3},
        {3, 3},
        {2, 1}
    };

    const auto path = getPath(traversals, 1, 4);
    BOOST_CHECK(path.empty());
}

BOOST_AUTO_TEST_CASE(GetPathInvalidSourceTest) {
    const std::map<int, int> traversals = {
        {4, 3},
        {3, 2},
        {2, 1}
    };

    const auto path = getPath(traversals, 5, 4);
    BOOST_CHECK(path.empty());
}

BOOST_AUTO_TEST_CASE(GetPathInvalidSinkTest) {
    const std::map<int, int> traversals = {
        {4, 3},
        {3, 2},
        {2, 1}
    };

    const auto path = getPath(traversals, 1, 5);
    BOOST_CHECK(path.empty());
}

BOOST_AUTO_TEST_CASE(AStarSearchTest) {
    DirectedGraph<int> G;

    constexpr double w = 0.1;
    G.addEdge(1, 2, w);
    G.addEdge(2, 3, w);
    G.addEdge(3, 4, w);
    G.addEdge(4, 5, w);

    G.addEdge(2, 4, w);

    auto heuristic = [](int a, int b) -> double {
        return a % b == 0 ? 0.0 : 1.0;
    };

    auto goal = [](int v) {
        return v == 5;
    };

    const auto path = AStarSearch<int, double, double>(G, 1, goal, heuristic);
    const std::vector<int> true_path = { 1, 2, 4, 5 };
    BOOST_CHECK_EQUAL(path.size(), true_path.size());

    BOOST_CHECK_EQUAL_COLLECTIONS(path.cbegin(), path.cend(),
                                  true_path.cbegin(), true_path.cend());
}

BOOST_AUTO_TEST_CASE(AStarSearchNeverReachGoalTest) {
    DirectedGraph<int> G;

    constexpr double w = 0.1;
    G.addEdge(1, 2, w);
    G.addEdge(2, 3, w);
    G.addEdge(3, 4, w);
    G.addEdge(4, 5, w);

    G.addEdge(2, 4, w);

    auto heuristic = [](int a, int b) -> double {
        return 0;
    };

    auto goal = [](int v) {
        return false;
    };

    const auto path = AStarSearch<int, double, double>(G, 1, goal, heuristic);
    BOOST_CHECK(path.empty());
}

BOOST_AUTO_TEST_CASE(AStarSearchInvalidStartTest) {
    DirectedGraph<int> G;

    constexpr double w = 0.1;
    G.addEdge(1, 2, w);
    G.addEdge(2, 3, w);
    G.addEdge(3, 4, w);
    G.addEdge(4, 5, w);

    G.addEdge(2, 4, w);

    auto heuristic = [](int a, int b) -> double {
        return a % b == 0 ? 0.0 : 1.0;
    };

    auto goal = [](int v) {
        return v == 5;
    };

    const auto path = AStarSearch<int, double, double>(G, 6, goal, heuristic);
    BOOST_CHECK(path.empty());
}

BOOST_AUTO_TEST_CASE(DijkstraTest) {
    DirectedGraph<int> G;

    G.addEdge(1, 2, 0.1);
    G.addEdge(2, 3, 0.1);
    G.addEdge(3, 4, 0.1);
    G.addEdge(4, 5, 0.1);

    G.addEdge(1, 4, 1.5); // Shorter, but higher cost.

    const auto path = dijkstra<int, double>(G, 1, 5);

    const std::vector<int> true_path = { 1, 2, 3, 4, 5 };
    BOOST_CHECK_EQUAL(path.size(), true_path.size());

    BOOST_CHECK_EQUAL_COLLECTIONS(path.cbegin(), path.cend(),
                                  true_path.cbegin(), true_path.cend());
}

BOOST_AUTO_TEST_CASE(DijkstraInvalidStartTest) {
    DirectedGraph<int> G;

    constexpr double w = 0.1;
    G.addEdge(1, 2, w);
    G.addEdge(2, 3, w);
    G.addEdge(3, 4, w);
    G.addEdge(4, 5, w);

    const auto path = dijkstra<int, double>(G, 6, 5);
    BOOST_CHECK(path.empty());
}

BOOST_AUTO_TEST_CASE(DijkstraInvalidEndTest) {
    DirectedGraph<int> G;

    constexpr double w = 0.1;
    G.addEdge(1, 2, w);
    G.addEdge(2, 3, w);
    G.addEdge(3, 4, w);
    G.addEdge(4, 5, w);

    const auto path = dijkstra<int, double>(G, 1, 6);
    BOOST_CHECK(path.empty());
}

BOOST_AUTO_TEST_SUITE_END()


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Hauke Heibel <hauke.heibel@gmail.com>
//
// 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 "main.h"

#include <Eigen/Core>

using namespace Eigen;

template <typename Scalar, int Storage>
void run_matrix_tests()
{
  typedef Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Storage> MatrixType;
  typedef typename MatrixType::Index Index;

  MatrixType m, n;

  // boundary cases ...
  m = n = MatrixType::Random(50,50);
  m.conservativeResize(1,50);
  VERIFY_IS_APPROX(m, n.block(0,0,1,50));

  m = n = MatrixType::Random(50,50);
  m.conservativeResize(50,1);
  VERIFY_IS_APPROX(m, n.block(0,0,50,1));

  m = n = MatrixType::Random(50,50);
  m.conservativeResize(50,50);
  VERIFY_IS_APPROX(m, n.block(0,0,50,50));

  // random shrinking ...
  for (int i=0; i<25; ++i)
  {
    const Index rows = internal::random<Index>(1,50);
    const Index cols = internal::random<Index>(1,50);
    m = n = MatrixType::Random(50,50);
    m.conservativeResize(rows,cols);
    VERIFY_IS_APPROX(m, n.block(0,0,rows,cols));
  }

  // random growing with zeroing ...
  for (int i=0; i<25; ++i)
  {
    const Index rows = internal::random<Index>(50,75);
    const Index cols = internal::random<Index>(50,75);
    m = n = MatrixType::Random(50,50);
    m.conservativeResizeLike(MatrixType::Zero(rows,cols));
    VERIFY_IS_APPROX(m.block(0,0,n.rows(),n.cols()), n);
    VERIFY( rows<=50 || m.block(50,0,rows-50,cols).sum() == Scalar(0) );
    VERIFY( cols<=50 || m.block(0,50,rows,cols-50).sum() == Scalar(0) );
  }
}

template <typename Scalar>
void run_vector_tests()
{
  typedef Matrix<Scalar, 1, Eigen::Dynamic> MatrixType;

  MatrixType m, n;

  // boundary cases ...
  m = n = MatrixType::Random(50);
  m.conservativeResize(1);
  VERIFY_IS_APPROX(m, n.segment(0,1));

  m = n = MatrixType::Random(50);
  m.conservativeResize(50);
  VERIFY_IS_APPROX(m, n.segment(0,50));

  // random shrinking ...
  for (int i=0; i<50; ++i)
  {
    const int size = internal::random<int>(1,50);
    m = n = MatrixType::Random(50);
    m.conservativeResize(size);
    VERIFY_IS_APPROX(m, n.segment(0,size));
  }

  // random growing with zeroing ...
  for (int i=0; i<50; ++i)
  {
    const int size = internal::random<int>(50,100);
    m = n = MatrixType::Random(50);
    m.conservativeResizeLike(MatrixType::Zero(size));
    VERIFY_IS_APPROX(m.segment(0,50), n);
    VERIFY( size<=50 || m.segment(50,size-50).sum() == Scalar(0) );
  }
}

void test_conservative_resize()
{
  for(int i=0; i<g_repeat; ++i)
  {
    CALL_SUBTEST_1((run_matrix_tests<int, Eigen::RowMajor>()));
    CALL_SUBTEST_1((run_matrix_tests<int, Eigen::ColMajor>()));
    CALL_SUBTEST_2((run_matrix_tests<float, Eigen::RowMajor>()));
    CALL_SUBTEST_2((run_matrix_tests<float, Eigen::ColMajor>()));
    CALL_SUBTEST_3((run_matrix_tests<double, Eigen::RowMajor>()));
    CALL_SUBTEST_3((run_matrix_tests<double, Eigen::ColMajor>()));
    CALL_SUBTEST_4((run_matrix_tests<std::complex<float>, Eigen::RowMajor>()));
    CALL_SUBTEST_4((run_matrix_tests<std::complex<float>, Eigen::ColMajor>()));
    CALL_SUBTEST_5((run_matrix_tests<std::complex<double>, Eigen::RowMajor>()));
    CALL_SUBTEST_6((run_matrix_tests<std::complex<double>, Eigen::ColMajor>()));

    CALL_SUBTEST_1((run_vector_tests<int>()));
    CALL_SUBTEST_2((run_vector_tests<float>()));
    CALL_SUBTEST_3((run_vector_tests<double>()));
    CALL_SUBTEST_4((run_vector_tests<std::complex<float> >()));
    CALL_SUBTEST_5((run_vector_tests<std::complex<double> >()));
  }
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

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

#include <algorithm>
#include <list>
#include <sstream>
#include <string>
#include <vector>

#include <geometry_test_common.hpp>

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/extensions/iterators/segment_returning_iterator.hpp>
#include <boost/geometry/io/wkt/read.hpp>

template <typename C>
void test_linestring(std::string const& wkt, std::string const& expected)
{
    typedef C point_list;
    typedef typename C::value_type point;
    typedef typename C::iterator base_iterator;
    typedef bg::segment_returning_iterator<base_iterator, point> segment_returning_iterator;
    typedef typename segment_returning_iterator::value_type segment;
    typedef bg::model::linestring<point> linestring;

    linestring g;
    bg::read_wkt(wkt, g);

    point_list v;
    std::copy(g.begin(), g.end(), std::back_insert_iterator<point_list>(v));
    BOOST_CHECK_EQUAL(g.size(), v.size());

    segment_returning_iterator it(v.begin(), v.end());
    segment_returning_iterator end(v.end());

    std::ostringstream oss;
    while (it != end)
    {
        segment const& s = *it;
        oss << bg::get<0>(s.first) << bg::get<1>(s.first)
            << bg::get<0>(s.second) << bg::get<1>(s.second);
        ++it;
    }
    BOOST_CHECK_EQUAL(oss.str(), expected);
}

int test_main(int, char* [])
{
    // Test std::vector
    typedef std::vector<bg::model::d2::point_xy<double> > points_v;
    test_linestring<points_v>("linestring empty", "");
    test_linestring<points_v>("linestring ()", "");
    test_linestring<points_v>("linestring (1 1)", "");
    test_linestring<points_v>("linestring (1 1, 2 2, 3 3)", "11222233");
    test_linestring<points_v>("linestring (1 1, 2 2, 3 3, 4 4)", "112222333344");
    test_linestring<points_v>("linestring (1 1, 2 2, 3 3, 4 4, 5 5, 6 6)", "11222233334444555566");

    // Test std::list
    typedef std::list<bg::model::d2::point_xy<double> > points_l;
    test_linestring<points_l>("linestring empty", "");
    test_linestring<points_l>("linestring ()", "");
    test_linestring<points_l>("linestring (1 1)", "");
    test_linestring<points_l>("linestring (1 1, 2 2, 3 3)", "11222233");
    test_linestring<points_l>("linestring (1 1, 2 2, 3 3, 4 4)", "112222333344");
    test_linestring<points_l>("linestring (1 1, 2 2, 3 3, 4 4, 5 5, 6 6)", "11222233334444555566");

    return 0;
}


#ifndef GRAVITYFIELD_H
#define GRAVITYFIELD_H

#include <Eigen/Dense>

#include "forceField.hpp"

namespace SnowSimulator {

class GravityField : public ForceField {
public:
  GravityField(const Eigen::Vector3f &gravity);
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  Eigen::Vector3f force(const Eigen::Vector3f &pos, float mass) const;
  Eigen::Matrix3f gradForce(const Eigen::Vector3f &pos, float mass) const;

private:
  Eigen::Vector3f m_gravity;
};

} // namespace SnowSimulator

#endif // GRAVITYFIELD_H