#include "kondo.h"
#include "iostream_util.h"
#include <cstdio>
#include <sstream>
#include <boost/filesystem.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

using boost::property_tree::ptree;
using boost::property_tree::read_json;
//using boost::property_tree::write_json;

template <typename T>
std::vector<T> as_vector(ptree const& pt, ptree::key_type const& key)
{
    std::vector<T> r;
    for (auto& item : pt.get_child(key))
        r.push_back(item.second.get_value<T>());
    return r;
}

void triangular(int argc, char *argv[]) {
    auto engine = fkpm::mk_engine<cx_flt>();
    if (engine == nullptr) std::exit(EXIT_FAILURE);
    if (argc != 2) {
        cout << "Usage: " << argv[0] << " <base_dir>\n";
        std::exit(EXIT_SUCCESS);
    }

    std::string base_dir(argv[1]);
    auto input_name = base_dir + "/config.toml";
    std::cout << "using toml file `" << input_name << "`!\n";
    toml_ptr g = toml_from_file(input_name);

    auto m = SimpleModel::mk_triangular(toml_get<int64_t>(g, "model.w"), toml_get<int64_t>(g, "model.h"));
    m->J  = toml_get<double>(g, "model.J");
    m->t1 = toml_get(g, "model.t1", 0.0);
    m->t2 = toml_get(g, "model.t2", 0.0);
    m->t3 = toml_get(g, "model.t3", 0.0);
    m->kT_init  = toml_get<double>(g, "model.kT");
    m->kT_decay = toml_get(g, "model.kT_decay", 0.0);
    m->zeeman   = {toml_get(g, "model.zeeman_x", 0.0), toml_get(g, "model.zeeman_y", 0.0), toml_get(g, "model.zeeman_z", 0.0)};
    m->easy_z   = toml_get(g, "model.easy_z", 0.0);
    m->s1       = toml_get(g, "model.s1", 0.0);
    m->s2       = toml_get(g, "model.s2", 0.0);
    m->s3       = toml_get(g, "model.s3", 0.0);

    std::string json_name;
    std::cout << "Input dumpfile name:" << std::endl;
    std::cin >> json_name;
    std::cout << json_name << std::endl;
    json_name = base_dir + "/dump/" + json_name;
    std::ifstream json_file(json_name);
    if (!json_file.is_open()) {
        cerr << "Unable to open file `" << json_name << "`!\n";
        std::exit(EXIT_FAILURE);
    }
    cout << "Using json file `" << json_name << "`!\n";
    ptree pt_json;        // used for reading the json file
    std::string json_eachline, json_contents;
    while (std::getline(json_file, json_eachline)) {
        json_contents += json_eachline;
    }
    std::istringstream is (json_contents);
    read_json (is, pt_json);
    auto time    = pt_json.get<int>    ("time");
    auto action  = pt_json.get<double> ("action");
    auto filling = pt_json.get<double> ("filling");
    auto mu      = pt_json.get<double> ("mu");
    auto spin    = as_vector<double>(pt_json, "spin");

    std::cout << "lattice: " << toml_get<std::string>(g, "model.lattice") << std::endl;
    std::cout << "time:    " << time << std::endl;
    std::cout << "action:  " << action << std::endl;
    std::cout << "filling: " << filling << std::endl;
    std::cout << "mu:      " << mu << std::endl;
    std::cout << "J:       " << m->J << std::endl;
    std::cout << "t:       " << m->t1 << std::endl;
    std::cout << "kT:      " << m->kT() << std::endl;

    assert(spin.size() == m->n_sites * 3);              // build spin configuration from dump file
    for (int i = 0; i < m->n_sites; i++) {
        m->spin[i] = vec3(spin[3*i],spin[3*i+1],spin[3*i+2]);
    }
    m->set_hamiltonian(m->spin);

    int purpose, M, Mq, use_correlated, n_colors, seed;
    std::cout << "Inut 0/1/2 for calculating sigma_xx/sigma_xy/both: " << std::endl;
    std::cin >> purpose;
    fkpm::EnergyScale es;
    es = engine->energy_scale(m->H, 0.1);
    std::cout << "energyscale: [" << es.lo << ", " << es.hi << "]" << std::endl;
    std::cout << "Input M:" << std::endl;
    std::cin >> M;
    std::cout << M << std::endl;
    std::cout << "Input Mq:" << std::endl;
    std::cin >> Mq;
    std::cout << Mq << std::endl;
    std::cout << "Input 0/1 for using uncorrelated/correlated random numbers:" << std::endl;
    std::cin >> use_correlated;
    std::cout << use_correlated << std::endl;
    std::cout << "Input n_colors:" << std::endl;
    std::cin >> n_colors;
    std::cout << n_colors << std::endl;
    std::cout << "Input seed:" << std::endl;
    std::cin >> seed;
    std::cout << seed << std::endl;

    fkpm::RNG rng(seed);
    if (use_correlated) {
        engine->set_R_correlated(m->groups(n_colors), rng);
    } else {
        engine->set_R_uncorrelated(m->H.n_rows, 2*n_colors, rng);
    }

    auto kernel = fkpm::jackson_kernel(M);
    engine->set_H(m->H, es);

    auto jx = m->electric_current_operator(m->spin, {1,0,0});
    auto jy = m->electric_current_operator(m->spin, {0,1,0});

    json_file.close();

    auto mu_dos = engine->moments(M);
    auto gamma = fkpm::moment_transform(mu_dos, Mq);
    fkpm::Vec<double> mu_list, rho;
    fkpm::density_function(gamma, es, mu_list, rho);
    for (int i = 0; i < rho.size(); i++) {
        rho[i]/=m->n_sites;
    }

    std::cout << "calculating moments2... " << std::flush;
    fkpm::timer[0].reset();
    Vec<Vec<fkpm::cx_double>> mu_xx, mu_xy;
    if (purpose == 0) {
        mu_xx = engine->moments2_v1(M, jx, jx, 10, 16);
        mu_xy = mu_xx; // invalid
    } else if (purpose == 1){
        mu_xy = engine->moments2_v1(M, jx, jy, 10, 16);
        mu_xx = mu_xy; // invalid
    } else {
        mu_xx = engine->moments2_v1(M, jx, jx, 10, 16);
        mu_xy = engine->moments2_v1(M, jx, jy, 10, 16);
    }
    cout << " done. " << fkpm::timer[0].measure() << "s.\n";

    cout << "calculating dc conductivities... " << std::flush;
    std::ofstream fout2("full_time"+std::to_string(time)+"_M"+std::to_string(M)+"_corr"+std::to_string(use_correlated)+
                        "_color"+std::to_string(n_colors)+"_seed"+std::to_string(seed)+".dat", std::ios::out);
    fout2 << std::scientific << std::right;
    fout2 << std::setw(20) << "#(1)" << std::setw(20) << "(2)" << std::setw(20) << "(3)"
          << std::setw(20) << "(4)" << std::setw(20) << "(5)" << std::setw(20) << "(6)" << std::endl;
    fout2 << std::setw(20) << "M" << std::setw(20) << "kT" << std::setw(20) << "mu"
          << std::setw(20) << "rho" << std::setw(20) << "sigma_xx" << std::setw(20) << "sigma_xy" << std::endl;
    int interval = std::max(Mq/400,5);
    for (int i = 0; i < Mq; i+=interval) {
        auto cmn = electrical_conductivity_coefficients(M, Mq, m->kT(), mu_list[i], 0.0, es, kernel);
        auto sigma_xx = std::real(fkpm::moment_product(cmn, mu_xx));
        auto sigma_xy = std::real(fkpm::moment_product(cmn, mu_xy));
        fout2 << std::setw(20) << M << std::setw(20) << m->kT() << std::setw(20) << mu_list[i]
              << std::setw(20) << rho[i] << std::setw(20) << sigma_xx << std::setw(20) << sigma_xy << std::endl;
    }
    fout2.close();
    cout << " done. " << fkpm::timer[0].measure() << "s.\n";

    std::ifstream fin0("result_time"+std::to_string(time)+".dat");
    if (! fin0.good()) {
        fin0.close();
        std::ofstream fout3("result_time"+std::to_string(time)+".dat", std::ios::out | std::ios::app );
        fout3 << std::setw(20) << "#(1)" << std::setw(20) << "(2)" << std::setw(20) << "(3)"
              << std::setw(20) << "(4)" << std::setw(20) << "(5)" << std::setw(20) << "(6)"
              << std::setw(20) << "(7)" << std::setw(20) << "(8)" << std::endl;
        fout3 << std::setw(20) << "M" << std::setw(20) << "correlated" << std::setw(20) << "colors"
              << std::setw(20) << "seed" << std::setw(20) << "F" << std::setw(20) << "mu"
              << std::setw(20) << "sigma_xx" << std::setw(20) << "sigma_xy" << std::endl;
        fout3.close();
    }
    std::ofstream fout3("result_time"+std::to_string(time)+".dat", std::ios::out | std::ios::app );
    fout3 << std::scientific << std::right;
    auto cmn       = electrical_conductivity_coefficients(M, Mq, m->kT(), mu, 0.0, es, kernel);

    fout3 << std::setw(20) << M << std::setw(20) << use_correlated << std::setw(20) << n_colors
          << std::setw(20) << seed
          << std::setw(20) << fkpm::electronic_energy(gamma, es, m->kT(), filling, mu)/m->n_sites
          << std::setw(20) << mu
          << std::setw(20) << std::real(fkpm::moment_product(cmn, mu_xx))
          << std::setw(20) << std::real(fkpm::moment_product(cmn, mu_xy))
          << std::endl;
    fout3.close();
    std::cout << std::endl;
    std::remove("dump_device0.dat");
    std::remove("dump_device1.dat");
}

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


/*!
@file
Defines `boost::hana::Monad::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_MONAD_LAWS_HPP
#define BOOST_HANA_MONAD_LAWS_HPP

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

#include <boost/hana/applicative/applicative.hpp>
#include <boost/hana/comparable/comparable.hpp>
#include <boost/hana/core/datatype.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/functor/functor.hpp>
#include <boost/hana/logical/logical.hpp>

namespace boost { namespace hana {
    struct Monad::laws {
        template <typename Monad_, typename A, typename F, typename G>
        static constexpr auto check(Monad_ monad, A a, F f, G g) {
            auto lift_ = lift<datatype_t<decltype(monad)>>;
            return and_(
                equal(bind(lift_(a), f), f(a)),
                equal(bind(monad, lift_), monad),
                equal(bind(monad, [=](auto x) { return bind(f(x), g); }), bind(bind(monad, f), g)),
                equal(fmap(f, monad), bind(monad, compose(lift_, f)))
            );
        }
    };
}} // end namespace boost::hana

#endif // !BOOST_HANA_MONAD_LAWS_HPP


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE Subsample0InplaceTest

#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <vector>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/devicemanager.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ir.hpp>
#include <popart/ndarraywrapper.hpp>
#include <popart/tensor.hpp>
#include <popart/tensorinfo.hpp>
#include <popart/tensornames.hpp>
#include <popart/tensors.hpp>
#include <popart/testdevice.hpp>

using namespace popart;

BOOST_AUTO_TEST_CASE(Inplace_subsample0) {

  //       input of shape {2,3,4}
  //        |               \
  //       relu            scale-0.2
  //        |                   \
  //      subsample {2,1,2}    subsample {2,1,2}
  //            \                 /
  //             \               /
  //              \             /
  //               \           /
  //                \         /
  //                 \       /
  //                  \     /
  //                   \   /
  //                    add -> output of shape {1,3,2}

  // Build an onnx model
  auto builder = Builder::create();
  auto aiOnnx  = builder->aiOnnxOpset9();

  TensorInfo info0{"FLOAT", std::vector<int64_t>{2, 3, 4}};
  auto in0 = builder->addInputTensor(info0);

  auto rel0       = aiOnnx.relu({in0});
  auto subsample0 = builder->aiGraphcoreOpset1().subsample({rel0}, {2, 1, 2});

  float scaleFactor0 = 0.2;
  auto s0            = builder->aiGraphcoreOpset1().scale({in0}, scaleFactor0);
  auto subsample1    = builder->aiGraphcoreOpset1().subsample({s0}, {2, 1, 2});

  auto addOut = aiOnnx.add({subsample0, subsample1});
  auto dotOut = aiOnnx.sigmoid({addOut});
  builder->addOutputTensor(dotOut);

  builder->setInplacePreferences(s0, {{"ScaleInplace", 300.}});
  builder->setInplacePreferences(rel0, {{"ReluInplace", 900.}});
  builder->setInplacePreferences(subsample0, {{"SubsampleInplace", 800.0}});
  builder->setInplacePreferences(subsample1, {{"SubsampleInplace", 10.0}});
  builder->setInplacePreferences(addOut, {{"AddLhsInplace", 1000.0}});

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR
  auto dataFlow = DataFlow(1, {{dotOut, AnchorReturnType("All")}});
  auto device   = createTestDevice(TEST_TARGET);

  Ir ir;
  ir.prepare({modelProto,
              InputShapeInfo(),
              dataFlow,
              {},
              nullptr,
              *device,
              {},
              Patterns(PatternsLevel::NoPatterns)
                  .enableRuntimeAsserts(false)
                  .enableInPlace(true)});

  auto opsOfTypeSubsample = ir.opsOfType(Onnx::AiGraphcore::OpSet1::Subsample);
  BOOST_CHECK(opsOfTypeSubsample.size() == 0);

  auto opsOfTypeRelu = ir.opsOfType(Onnx::AiOnnx::OpSet9::Relu);
  BOOST_CHECK(opsOfTypeRelu.size() == 0);

  auto opsOfTypeScale = ir.opsOfType(Onnx::AiGraphcore::OpSet1::Scale);
  BOOST_CHECK(opsOfTypeScale.size() == 1);

  auto opsOfTypeAdd = ir.opsOfType(Onnx::AiOnnx::OpSet9::Add);
  BOOST_CHECK(opsOfTypeAdd.size() == 0);
}


#include "partition/recursive_bisection.hpp"
#include "partition/graph_generator.hpp"
#include "partition/recursive_bisection_state.hpp"

#include <algorithm>
#include <vector>

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

// make sure not to leak in recursive bisection
#include <tbb/task_scheduler_init.h>
tbb::task_scheduler_init init(2);

using namespace osrm::partition;
using namespace osrm::util;

BOOST_AUTO_TEST_SUITE(recursive_bisection)

BOOST_AUTO_TEST_CASE(dividing_four_grid_cells)
{
    // 40 entries of left/right edges
    const double step_size = 0.01;
    const int rows = 10;
    const int cols = 10;
    const int cut_edges = 4;

    auto graph = [&]() {
        std::vector<Coordinate> grid_coordinates;
        std::vector<EdgeWithSomeAdditionalData> grid_edges;

        const auto connect =
            [&grid_edges](int min_left, int max_left, int min_right, int max_right) {
                const NodeID source = (rand() % (max_left - min_left)) + min_left;
                const NodeID target = (rand() % (max_right - min_right)) + min_right;

                grid_edges.push_back({source, target, 1});
                grid_edges.push_back({target, source, 1});
            };

        // generate 10 big components
        for (int i = 0; i < 4; ++i)
        {
            // 10 rows of large components, interrupted by small disconnected components
            const auto coordinates = makeGridCoordinates(
                rows, cols, step_size, cols * (i % 2), (i * rows / 2) * step_size);
            grid_coordinates.insert(grid_coordinates.end(), coordinates.begin(), coordinates.end());

            // connect the grid edges, starting with i * (rows * cols + 1) as first id (0,11,22...)
            const auto edges = makeGridEdges(rows, cols, i * (rows * cols));
            grid_edges.insert(grid_edges.end(), edges.begin(), edges.end());
        }

        // add cut edges between neighboring cells
        int n = rows * cols;
        for (int i = 0; i < cut_edges; ++i)
        {
            // left/right
            connect(0, n, n, 2 * n);
            connect(2 * n, 3 * n, 3 * n, 4 * n);
            // top/bottom
            connect(0, n, 2 * n, 3 * n);
            connect(n, 2 * n, 3 * n, 4 * n);
        }
        groupEdgesBySource(grid_edges.begin(), grid_edges.end());
        return makeBisectionGraph(grid_coordinates, adaptToBisectionEdge(std::move(grid_edges)));
    }();

    RecursiveBisection bisection(graph, 120, 1.1, 0.25, 10, 1);

    const auto result = bisection.BisectionIDs();
    // all same IDs withing a group
    for (int i = 0; i < 4; ++i)
        for (int j = 0; j < rows * cols; ++j)
            BOOST_CHECK(result[i * (rows * cols)] == result[i * (rows * cols) + j]);

    // different IDs for all four groups
    for (int i = 0; i < 4; ++i)
        for (int j = 0; j < 4; ++j)
            BOOST_CHECK(i == j || result[i * (rows * cols)] != result[j * (rows * cols)]);
}

BOOST_AUTO_TEST_SUITE_END()


#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_ros/transforms.h>
#include <boost/foreach.hpp>
//#include "sensor_msgs/PointCloud2.h"
//#include "sensor_msgs/PointField.h"
#include "geometry_msgs/Point.h"
#include <vector>
#include <pcl_ros/impl/transforms.hpp>
#include <tf/transform_listener.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <stdlib.h>
#include <sensor_msgs/Image.h>
//pcl::toROSMsg
#include <pcl/io/pcd_io.h>
//stl stuff
#include <string>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/visualization/cloud_viewer.h>
#include <cmath>
#include <math.h>
#include <opencv2/core/core.hpp>
#include "slammin/pointVector3d.h"
#include "slammin/point3d.h"
#include "nav_msgs/OccupancyGrid.h"
#include "nav_msgs/GetMap.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include <tf/transform_datatypes.h>

#include <image_transport/image_transport.h>

#define PI 3.14159265

typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
float max_scanned_depth_;

ros::Publisher depthmap_pub;
image_transport::Publisher img_pub;
ros::Subscriber sub,map_sub,cam_sub,pose_sub;
ros::ServiceClient client;
nav_msgs::OccupancyGrid map_;
tf::TransformListener *tf_listener;
geometry_msgs::Pose pose_;
slammin::pointVector3d mapREC,mapC;
slammin::pointVector3d depthCamera_points;
std::vector<int> indexes_vec;

int recursive_func_iterations=0;
float max_z_point=0; // for height category clustering ..

bool visited(std::vector<int> ind_,int i_){
	for (int i = 0; i < ind_.size(); ++i)
	{
		if(ind_[i]==i_) return true;
	}
	return false;
}

//the recursive func..
void find_mapgrids_onRange(float angle,float pos_x,float pos_y){

	float newPos_x,newPos_y,x_fr_res,y_fr_res,x_lef_res,x_rig_res,y_lef_res,y_rig_res;
	float res=map_.info.resolution;
	slammin::point3d p_;
	//index->node id
	int index=(int)((pos_y-map_.info.origin.position.y)/map_.info.resolution)*map_.info.height+((pos_x-map_.info.origin.position.x)/map_.info.resolution);

	//calculate 3d point distance from the robot, to check if this measurement is over the limit..
	float dist=sqrt(pow(pose_.position.x-pos_x,2)+pow(pose_.position.y-pos_y,2));

	//Recursion break~
	if(dist > max_scanned_depth_ || visited(indexes_vec,index)|| angle!=angle){ //quit on invalid angle value (nan)
		// /if (visited(indexes_vec,index)) ROS_INFO("revisited");
		return;
	}
	recursive_func_iterations++; //debugin

	//mark as visited gridmap point
	indexes_vec.push_back(index);

	//mporei na ginei kai ektos anadromis den alazei~stable O()
	if(angle>22.5&&angle<=67.5){ //45 deg
		x_fr_res=res;y_fr_res=res;  x_lef_res=0;y_lef_res=res;  x_rig_res=res;y_rig_res=0;
	}else if(angle>67.5&&angle<=112.5){ //90
		x_fr_res=0;y_fr_res=res;  x_lef_res=-res;y_lef_res=res;  x_rig_res=res;y_rig_res=res;
	}else if(angle>112.5&&angle<=157.5){ //135
		x_fr_res=-res;y_fr_res=res;  x_lef_res=-res;y_lef_res=0;  x_rig_res=0;y_rig_res=res;
	}else if(angle>157.5&&angle<=202.5){ //180
		x_fr_res=-res;y_fr_res=0;  x_lef_res=-res;y_lef_res=-res;  x_rig_res=-res;y_rig_res=res;
	}else if(angle>202.5&&angle<=247.5){ //225
		x_fr_res=-res;y_fr_res=-res;  x_lef_res=0;y_lef_res=-res;  x_rig_res=-res;y_rig_res=0;
	}else if(angle>247.5&&angle<=292.5){ //270
		x_fr_res=0;y_fr_res=-res;  x_lef_res=res;y_lef_res=-res;  x_rig_res=-res;y_rig_res=-res;
	}else if(angle>292.5&&angle<=337.5){ //315
		x_fr_res=res;y_fr_res=-res;  x_lef_res=res;y_lef_res=0;  x_rig_res=0;y_rig_res=-res;
	}else if(angle>337.5||angle<=22.5){ //360-0
		x_fr_res=res;y_fr_res=0;  x_lef_res=res;y_lef_res=res;  x_rig_res=res;y_rig_res=-res;
	}

	//front gridBox
	// newPos_x=cos(angle*PI/180)*pose_.position.x-sin(angle*PI/180)*pose_.position.y+x_fr_res;
	// newPos_y=sin(angle*PI/180)*pose_.position.x+cos(angle*PI/180)*pose_.position.y+y_fr_res;
	newPos_x=pos_x+x_fr_res;
	newPos_y=pos_y+y_fr_res;
	index=(int)((newPos_y-map_.info.origin.position.y)/map_.info.resolution)*map_.info.height+((newPos_x-map_.info.origin.position.x)/map_.info.resolution);

	//ROS_INFO("the index is %d with coords %f %f me angle %f",index,newPos_x,newPos_y,angle);
	if(map_.data[index]==100){
		//ROS_INFO("found here! ");
		p_.x=newPos_x;
		p_.y=newPos_y;
		p_.z=0;
		p_.posIncloud=index;
		mapREC.vec3d.push_back(p_);
	}
	// for (int i = 0; i < mapC.vec3d.size(); ++i)
	// {
	// 	if (mapC.vec3d[i].posIncloud==index)
	// 	{
	// 		p_.x=newPos_x;
	// 		p_.y=newPos_y;
	// 		p_.z=0;
	// 		p_.posIncloud=index;
	// 		mapREC.vec3d.push_back(p_);
	// 		i==mapC.vec3d.size();
	// 	}
	// }

	find_mapgrids_onRange(angle,newPos_x,newPos_y);

	//front-left gridBox
	newPos_x=pos_x+x_lef_res;
	newPos_y=pos_y+y_lef_res;
	index=(int)((newPos_y-map_.info.origin.position.y)/map_.info.resolution)*map_.info.height+((newPos_x-map_.info.origin.position.x)/map_.info.resolution);

	//ROS_INFO("the index is %d with coords %f %f me angle %f",index,newPos_x,newPos_y,angle);
	if(map_.data[index]==100){
		//ROS_INFO("found here! ");
		p_.x=newPos_x;
		p_.y=newPos_y;
		p_.z=0;
		p_.posIncloud=index;
		mapREC.vec3d.push_back(p_);
	}
	// for (int i = 0; i < mapC.vec3d.size(); ++i)
	// {
	// 	if (mapC.vec3d[i].posIncloud==index)
	// 	{
	// 		p_.x=newPos_x;
	// 		p_.y=newPos_y;
	// 		p_.z=0;
	// 		p_.posIncloud=index;
	// 		mapREC.vec3d.push_back(p_);
	// 		i==mapC.vec3d.size();
	// 	}
	// }

	find_mapgrids_onRange(angle,newPos_x,newPos_y);

	//front-right gridBox
	newPos_x=pos_x+x_rig_res;
	newPos_y=pos_y+y_rig_res;
	index=(int)((newPos_y-map_.info.origin.position.y)/map_.info.resolution)*map_.info.height+((newPos_x-map_.info.origin.position.x)/map_.info.resolution);

	//ROS_INFO("the index is %d with coords %f %f me angle %f",index,newPos_x,newPos_y,angle);
	if(map_.data[index]==100){
		//ROS_INFO("found here! ");
		p_.x=newPos_x;
		p_.y=newPos_y;
		p_.z=0;
		p_.posIncloud=index;
		mapREC.vec3d.push_back(p_);
	}
	// for (int i = 0; i < mapC.vec3d.size(); ++i)
	// {
	// 	if (mapC.vec3d[i].posIncloud==index)
	// 	{
	// 		p_.x=newPos_x;
	// 		p_.y=newPos_y;
	// 		p_.z=0;
	// 		p_.posIncloud=index;
	// 		mapREC.vec3d.push_back(p_);
	// 		i==mapC.vec3d.size();
	// 	}
	// }

	find_mapgrids_onRange(angle,newPos_x,newPos_y);

//ROS_INFO("me cos %f sin %f einai sto %f %f kai tha paei sto %f %f ",cos(angle),sin(angle),pose_.position.x,pose_.position.y,mprostatoux,mprostatouy);

	// for (int i = 0; i <map_.info.height*map_.info.width ; ++i) //map_->info.height*map_->info.width
	// {
	// 	slammin::point3d p_;

	// 	//recostruction of the 2d map in world coordinates..
	// 	if(map_.data[i]==100){
	// 		// /ROS_INFO("%d",map_.data[i]);
	// 		p_.x=(div(i,map_.info.height).rem)*map_.info.resolution + map_.info.origin.position.x;
	// 		p_.y=(div(i,map_.info.height).quot)*map_.info.resolution + map_.info.origin.position.y;
	// 		float index=((p_.y-map_.info.origin.position.y)/map_.info.resolution)*map_.info.height+((p_.x-map_.info.origin.position.x)/map_.info.resolution);
	// 		float da=(float)(((float)(4-3)/(float)2)*(float)4);
	// 		ROS_INFO("see that %d %d %f",(int)index,i,da);
	// 		//p_.z=0;
	// 		//p_.posIncloud=0; //in mapREC is useless, so it will be used in the recursive func
	// 		//mapREC.vec3d.push_back(p_);
	// 	}
	// }
	return;
}

void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {
  	cv_bridge::CvImagePtr cv_ptr;

    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
	}
	// cv::imshow("OPENCV_WINDOW", cv_ptr->image);
	// cv::waitKey(3);

	slammin::pointVector3d matched_points_,new_v_forImage,new_v_forSlam;
	slammin::point3d p_;
	int iters=0;
	// for (int i = 0; i < depthCamera_points.vec3d.size(); ++i)
	// {
	// 	for (int j = 0; j < mapREC.vec3d.size(); ++j)
	// 	{
	// 		iters++;
	// 		if ((std::abs(mapREC.vec3d[j].x-depthCamera_points.vec3d[i].x)<=0.1) && (std::abs(mapREC.vec3d[j].y-depthCamera_points.vec3d[i].y)<=0.1)){
	// 			//p_=depthCamera_points.vec3d[i];
	// 			p_.x=div(depthCamera_points.vec3d[i].posIncloud,640).rem; //p_.x=depthCamera_points.vec3d[i].x;
	// 			p_.y=div(depthCamera_points.vec3d[i].posIncloud,640).quot;//p_.y=depthCamera_points.vec3d[i].y;
	// 			p_.z=depthCamera_points.vec3d[i].z; //height category
	// 			p_.posIncloud=depthCamera_points.vec3d[i].posIncloud;
	// 			matched_points_.vec3d.push_back(p_);

	// 			j=mapREC.vec3d.size();
	// 		}

	// 	}

	// }

// ...

	//transform drone's quat pose to euler..
	tf::Quaternion q(pose_.orientation.x,pose_.orientation.y,pose_.orientation.z,pose_.orientation.w);
	tf::Matrix3x3 m(q);
	double roll, pitch, yaw;
	m.getRPY(roll, pitch, yaw);
	//and calculate posing straffe..
	float angle;
	angle=(180*std::abs(yaw))/3.10;
	if(yaw<0){
		angle=angle-180;
		angle=180+std::abs(angle);
	}
	//ROS_INFO("angle %f",angle);

	recursive_func_iterations=0;
	//fetch map's occupied squares, in front of drone..
	if(map_.info.width>0)
		find_mapgrids_onRange(angle,pose_.position.x,pose_.position.y);
	else
		return;

	//ROS_INFO("its out and free %d %d",mapREC.vec3d.size(),recursive_func_iterations);
	//ROS_INFO("Exhastive search matches and Recursive %d",/*mapC.vec3d.size(),*/mapREC.vec3d.size());

	//find matched occupied points with Depth Camera's data.. and save any new occupied points (that slam node hasn't found)
	for (int i = 0; i < depthCamera_points.vec3d.size(); ++i)
	{
		bool found=false;
		for (int j = 0; j < mapREC.vec3d.size(); ++j){
			iters++;
			//if point exists as occupied in slam map..
			if ((std::abs(mapREC.vec3d[j].x-depthCamera_points.vec3d[i].x)<=map_.info.resolution) && (std::abs(mapREC.vec3d[j].y-depthCamera_points.vec3d[i].y)<=map_.info.resolution)){
				found=true;
				p_.x=div(depthCamera_points.vec3d[i].posIncloud,cv_ptr->image.cols).rem; //p_.x=depthCamera_points.vec3d[i].x;
				p_.y=div(depthCamera_points.vec3d[i].posIncloud,cv_ptr->image.cols).quot;//p_.y=depthCamera_points.vec3d[i].y;
				p_.z=depthCamera_points.vec3d[i].z; //height category
				p_.posIncloud=depthCamera_points.vec3d[i].posIncloud;

				//store highest scanned point
				if(p_.z>max_z_point){
					max_z_point=p_.z;
				}

				matched_points_.vec3d.push_back(p_);
				break;
			}
		}

		float dist=sqrt(pow(pose_.position.x-depthCamera_points.vec3d[i].x,2)+pow(pose_.position.y-depthCamera_points.vec3d[i].y,2));

		if(!found && dist<=max_scanned_depth_){
			p_.x=div(depthCamera_points.vec3d[i].posIncloud,cv_ptr->image.cols).rem; //p_.x=depthCamera_points.vec3d[i].x;
			p_.y=div(depthCamera_points.vec3d[i].posIncloud,cv_ptr->image.cols).quot;//p_.y=depthCamera_points.vec3d[i].y;
			p_.z=depthCamera_points.vec3d[i].z; //height category
			p_.posIncloud=depthCamera_points.vec3d[i].posIncloud;
			new_v_forImage.vec3d.push_back(p_);
			p_.x=depthCamera_points.vec3d[i].x;
			p_.y=depthCamera_points.vec3d[i].y;
			new_v_forSlam.vec3d.push_back(p_);
		}
	}

	ROS_INFO("Found extra %d ,total matched %d, doing iters %d",new_v_forImage.vec3d.size(),matched_points_.vec3d.size(),iters);

	// // try
	// //pcl::toROSMsg (pcl_out, image_); //in case we had a pointcloud..
	// //cv_ptr = cv_bridge::toCvCopy(image_, sensor_msgs::image_encodings::BGR8);

    //Colourize Matched points on Camera frame and post the new-founded points on cloudLIstener Node and then on Hector Slam
	cv::Mat image = cv_ptr->image;
	double alpha = 0.5;
	for (int i = 0; i < matched_points_.vec3d.size(); ++i)
	{
		if(matched_points_.vec3d[i].z>=0.8*max_z_point){
		  cv::Mat roi =  image(cv::Rect(matched_points_.vec3d[i].x,matched_points_.vec3d[i].y,1, 1));
		  cv::Mat color(roi.size(), CV_8UC3, cv::Scalar(0, 0, 125));
		  cv::addWeighted(color, alpha, roi, 1.0 - alpha , 0.0, roi);
		 // cv::circle(cv_ptr->image, cv::Point(obj_px[i].x, obj_px[i].y), 1, CV_RGB(255,0,0));
		}else if(matched_points_.vec3d[i].z>=0.6*max_z_point){
		  cv::Mat roi =  image(cv::Rect(matched_points_.vec3d[i].x,matched_points_.vec3d[i].y,1, 1));
		  cv::Mat color(roi.size(), CV_8UC3, cv::Scalar(0, 62.5, 125));
		  cv::addWeighted(color, alpha, roi, 1.0 - alpha , 0.0, roi);
		 // cv::circle(cv_ptr->image, cv::Point(obj_px[i].x, obj_px[i].y), 1, CV_RGB(255,0,0));
		}else if(matched_points_.vec3d[i].z>=0.4*max_z_point){
		  cv::Mat roi =  image(cv::Rect(matched_points_.vec3d[i].x,matched_points_.vec3d[i].y,1, 1));
		  cv::Mat color(roi.size(), CV_8UC3, cv::Scalar(0, 125, 125));
		  cv::addWeighted(color, alpha, roi, 1.0 - alpha , 0.0, roi);
		 // cv::circle(cv_ptr->image, cv::Point(obj_px[i].x, obj_px[i].y), 1, CV_RGB(255,0,0));
		}else{
		  cv::Mat roi =  image(cv::Rect(matched_points_.vec3d[i].x,matched_points_.vec3d[i].y,1, 1));
		  cv::Mat color(roi.size(), CV_8UC3, cv::Scalar(0, 125, 0));
		  cv::addWeighted(color, alpha, roi, 1.0 - alpha , 0.0, roi);
		  // cv::circle(cv_ptr->image, cv::Point(obj_px[i].x, obj_px[i].y), 1, CV_RGB(255,0,0));
		}
	}
	//colorize the new discovered 3d points-obstacles
	for (int i = 0; i < new_v_forImage.vec3d.size(); ++i)
	{
		if(new_v_forImage.vec3d[i].x>=0&&new_v_forImage.vec3d[i].y>=0){
			cv::Mat roi =  image(cv::Rect(new_v_forImage.vec3d[i].x,new_v_forImage.vec3d[i].y,1, 1));
			cv::Mat color(roi.size(), CV_8UC3, cv::Scalar(125, 0, 0));
			cv::addWeighted(color, alpha, roi, 1.0 - alpha , 0.0, roi);
		}
	}

	indexes_vec.clear();
	mapREC.vec3d.clear();

	cv_bridge::CvImage out_msg;
	out_msg.header   = msg->header;
	out_msg.encoding = sensor_msgs::image_encodings::BGR8;
	out_msg.image    =  cv_ptr->image;

	img_pub.publish(out_msg.toImageMsg());
	//cv::imshow( "Image window",  image);
	//cv::waitKey(3);

/*	for (int i = 0; i < new_v_forSlam.vec3d.size(); ++i)
	{
		//ROS_INFO("another one %f %f",new_v_forImage.vec3d[i].x,new_v_forImage.vec3d[i].y);
		new_v_forSlam.vec3d[i].x=ceil((new_v_forSlam.vec3d[i].x+std::abs(map_.info.origin.position.x))*(1/map_.info.resolution));
		new_v_forSlam.vec3d[i].y=ceil((new_v_forSlam.vec3d[i].y+std::abs(map_.info.origin.position.y))*(1/map_.info.resolution));
	}*/
	depthmap_pub.publish(new_v_forSlam);

}

void vector_data(const slammin::pointVector3d::ConstPtr& data)
{
	//ROS_INFO("received points_v_ %d", data->vec3d.size());
	depthCamera_points=*data;
}

void get_map_(const nav_msgs::OccupancyGrid::ConstPtr& data)
{
/*	mapC.vec3d.clear();
	int coun=0;
	for (int i = 0; i <data->info.height*data->info.width ; ++i)
	{
		slammin::point3d p_;
		coun++;
		//recostruction of the 2d map in world coordinates..
		if(data->data[i]==100){
			// /ROS_INFO("%d",map_.data[i]);
			p_.x=(div(i,data->info.height).rem)*data->info.resolution + data->info.origin.position.x;
			p_.y=(div(i,data->info.height).quot)*data->info.resolution + data->info.origin.position.y;
			p_.z=0;
			p_.posIncloud=i; //in mapREC is useless, so it will be used in the recursive func
			mapC.vec3d.push_back(p_);
		}
	}*/
	map_=*data;
	//ROS_INFO("EKANE EXH %d",coun);
	//ROS_INFO("new map %d",mapC.vec3d.size());
}

void get_pose_(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& ps)
{
	//ROS_INFO("Received pose..");
	pose_.position.x=ps->pose.pose.position.x;
	pose_.position.y=ps->pose.pose.position.y;
	pose_.orientation.z=ps->pose.pose.orientation.z;
	pose_.orientation.w=ps->pose.pose.orientation.w;
	 //2d hector
	pose_.position.z=0;
	pose_.orientation.x=0;
	pose_.orientation.y=0;

}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "slammin_matcher");
	ros::NodeHandle nh("~");
  	nh.getParam("max_scanned_depth", max_scanned_depth_);

	//subscribers
	sub= nh.subscribe<slammin::pointVector3d> ("/slammin_pointVector3d", 1, vector_data);
	pose_sub=nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/poseupdate", 1, get_pose_);
	map_sub= nh.subscribe<nav_msgs::OccupancyGrid> ("/dynamic_map", 1, get_map_);
	cam_sub= nh.subscribe<sensor_msgs::Image> ("/camera/rgb/image_raw", 1, imageCb);

	//publishers
	image_transport::ImageTransport it(nh);
	img_pub = it.advertise("/camera/rgb/image_ra2w", 1);
	depthmap_pub=nh.advertise<slammin::pointVector3d> ("/depthcam_scan", 1);
	//pV_pub = nh.advertise<slammin::pointVector3d> ("/slammin_pointVector3d", 1);

	ros::spin();

  return 0;
}


/*
 * Low level data bit manipulation
 */

#ifndef DATA_BIT_HPP_INCLUCED
#define DATA_BIT_HPP_INCLUCED

#include <boost/mpl/at.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/pair.hpp>
#include <boost/mpl/map.hpp>
#include <aero/aero_basic_type.hpp>

namespace aero
{
	namespace databit { namespace detail {

		struct underflow_traits
		{
			typedef underflow_traits this_traits;
			typedef this_traits value_type;

			typedef this_traits half_traits;
			typedef half_traits::value_type half_value_type;

			enum { size = -1 };
		};

		struct data8_traits
		{
			typedef data8_traits this_traits;
			typedef boost::uint8_t value_type;

			typedef underflow_traits half_traits;
			typedef half_traits::value_type half_value_type;

			enum { size = 8 * sizeof(value_type) };
		};

		struct data16_traits
		{
			typedef data16_traits this_traits;
			typedef boost::uint16_t value_type;

			typedef data8_traits half_traits;
			typedef half_traits::value_type half_value_type;

			enum { size = 8 * sizeof(value_type) };
		};

		struct data32_traits
		{
			typedef data32_traits this_traits;
			typedef boost::uint32_t value_type;

			typedef data16_traits half_traits;
			typedef half_traits::value_type half_value_type;

			enum { size = 8 * sizeof(value_type) };
		};

		struct data64_traits
		{
			typedef data64_traits this_traits;
			typedef boost::uint64_t value_type;

			typedef data32_traits::value_type half_value_type;
			typedef data32_traits half_traits;

			enum { size = 8 * sizeof(value_type) };
		};

		template <typename TypeTraits, unsigned int splitCount>
		struct split_type_worker
		{
			typedef typename split_type_worker
			<
				typename TypeTraits::half_traits,
				splitCount / 2
			>::type_traits type_traits;
		};

		template <typename TypeTraits>
		struct split_type_worker<TypeTraits, 1>
		{
			typedef typename TypeTraits type_traits;
		};

		template <typename TypeTraits>
		struct split_type_worker<TypeTraits, 0>
		{
			typedef typename TypeTraits type_traits;
		};

		typedef boost::mpl::map
		<
#if defined(UCHAR_MAX) && UCHAR_MAX == 0xFF
			boost::mpl::pair<char, data8_traits>,
			boost::mpl::pair<unsigned char, data8_traits>,
#elif defined(UCHAR_MAX) && UCHAR_MAX == 0xFFFF
			boost::mpl::pair<char, data16_traits>,
			boost::mpl::pair<unsigned char, data16_traits>,
#elif defined(UCHAR_MAX) && UCHAR_MAX == 0xFFFFFFFF
			boost::mpl::pair<char, data32_traits>,
			boost::mpl::pair<unsigned char, data32_traits>,
#elif defined(UCHAR_MAX) && UCHAR_MAX == 0xFFFFFFFFFFFFFFFF
			boost::mpl::pair<char, data64_traits>,
			boost::mpl::pair<unsigned char, data64_traits>,
#else
			boost::mpl::pair<char, data8_traits>,
			boost::mpl::pair<unsigned char, data8_traits>,
#endif

#if defined(USHRT_MAX) && USHRT_MAX == 0xFF
			boost::mpl::pair<short, data8_traits>,
			boost::mpl::pair<unsigned short, data8_traits>,
#elif defined(USHRT_MAX) && USHRT_MAX == 0xFFFF
			boost::mpl::pair<short, data16_traits>,
			boost::mpl::pair<unsigned short, data16_traits>,
#elif defined(USHRT_MAX) && USHRT_MAX == 0xFFFFFFFF
			boost::mpl::pair<short, data32_traits>,
			boost::mpl::pair<unsigned short, data32_traits>,
#elif defined(USHRT_MAX) && USHRT_MAX == 0xFFFFFFFFFFFFFFFF
			boost::mpl::pair<short, data64_traits>,
			boost::mpl::pair<unsigned short, data64_traits>,
#else
			boost::mpl::pair<short, data16_traits>,
			boost::mpl::pair<unsigned short, data16_traits>,
#endif

#if defined(UINT_MAX) && UINT_MAX == 0xFF
			boost::mpl::pair<int, data8_traits>,
			boost::mpl::pair<unsigned int, data8_traits>,
#elif defined(UINT_MAX) && UINT_MAX == 0xFFFF
			boost::mpl::pair<int, data16_traits>,
			boost::mpl::pair<unsigned int, data16_traits>,
#elif defined(UINT_MAX) && UINT_MAX == 0xFFFFFFFF
			boost::mpl::pair<int, data32_traits>,
			boost::mpl::pair<unsigned int, data32_traits>,
#elif defined(UINT_MAX) && UINT_MAX == 0xFFFFFFFFFFFFFFFF
			boost::mpl::pair<int, data64_traits>,
			boost::mpl::pair<unsigned int, data64_traits>,
#else
			boost::mpl::pair<int, data32_traits>,
			boost::mpl::pair<unsigned int, data32_traits>,
#endif

#if defined(ULONG_MAX) && ULONG_MAX == 0xFF
			boost::mpl::pair<long, data8_traits>,
			boost::mpl::pair<unsigned long, data8_traits>,
#elif defined(ULONG_MAX) && ULONG_MAX == 0xFFFF
			boost::mpl::pair<long, data16_traits>,
			boost::mpl::pair<unsigned long, data16_traits>,
#elif defined(ULONG_MAX) && ULONG_MAX == 0xFFFFFFFF
			boost::mpl::pair<long, data32_traits>,
			boost::mpl::pair<unsigned long, data32_traits>,
#elif defined(ULONG_MAX) && ULONG_MAX == 0xFFFFFFFFFFFFFFFF
			boost::mpl::pair<long, data64_traits>,
			boost::mpl::pair<unsigned long, data64_traits>,
#else
			boost::mpl::pair<long, data32_traits>,
			boost::mpl::pair<unsigned long, data32_traits>,
#endif

#ifdef _MSC_VER
			boost::mpl::pair<__int64, data64_traits>,
			boost::mpl::pair<unsigned __int64, data64_traits>,
#endif
			boost::mpl::pair<long long, data64_traits>,
			boost::mpl::pair<unsigned long long, data64_traits>
		> type_traits_map;

		template <typename IntType, unsigned int i>
		struct split_type
		{
			typedef IntType int_type;
			typedef typename boost::mpl::if_
			<
				boost::is_same
				<
					typename boost::mpl::at<type_traits_map, int_type>::type,
					boost::mpl::void_
				>,
				boost::mpl::void_,
				typename boost::mpl::at<type_traits_map, int_type>::type
			>::type original_type_traits;
			typedef typename original_type_traits::value_type original_value_type;  // Change from signed to unsigned, if any.

			typedef typename split_type_worker<original_type_traits, i>::type_traits type_traits;
			typedef typename type_traits::value_type value_type;
		};
	} }

	template <unsigned int splitCount, typename Type>
	inline
	typename databit::detail::split_type<Type, splitCount>::value_type number_split(Type n, unsigned int splitIndex)
	{
		typedef typename databit::detail::split_type<Type, splitCount> split_type;

		return (split_type::value_type)
		(
			((split_type::original_value_type)((split_type::value_type)-1)) /* mask*/ &
			(
				(split_type::original_type_traits::size * splitIndex / splitCount) ?
					(((split_type::original_value_type)n) >>
						(split_type::original_type_traits::size * splitIndex / splitCount))
					: ((split_type::original_value_type)n)
			)
		);
	}

	template <unsigned int splitCount, unsigned int splitIndex, typename Type>
	inline
	typename databit::detail::split_type<Type, splitCount>::value_type number_split(Type n)
	{
		typedef typename databit::detail::split_type<Type, splitCount> split_type;

		return (split_type::value_type)
		(
			((split_type::original_value_type)((split_type::value_type)-1)) /* mask*/ &
			(
				(split_type::original_type_traits::size * splitIndex / splitCount) ?
					(((split_type::original_value_type)n) >>
						(split_type::original_type_traits::size * splitIndex / splitCount))
					: ((split_type::original_value_type)n)
			)
		);
	}
}

#endif // DATA_BIT_HPP_INCLUCED


/*
 * StreetsDatabaseAPI.cpp
 *
 *  Created on: Dec 17, 2015
 *      Author: jcassidy
 */

#include "StreetsDatabaseAPI.h"
#include "StreetsDatabase.h"

#include <string>
#include <fstream>
#include <iostream>
#include <sstream>

#include <boost/graph/adj_list_serialize.hpp>
#include <boost/archive/binary_iarchive.hpp>

StreetsDatabase streetsDB;

using namespace std;

// load the layer-2 streets database

bool loadStreetsDatabaseBIN(const std::string fn) {
    ifstream is(fn.c_str());
    if (!is.good())
        return false;

    boost::archive::binary_iarchive ia(is);

    ia & streetsDB;

    return true;
}

void closeStreetDatabase() {
    streetsDB = StreetsDatabase();
}

// aggregate queries

unsigned getNumberOfStreets() {
    return streetsDB.streets().size();
}

unsigned getNumberOfStreetSegments() {
    return num_edges(streetsDB.roads());
}

unsigned getNumberOfIntersections() {
    return num_vertices(streetsDB.roads());
}

unsigned getNumberOfPointsOfInterest() {
    return streetsDB.getNumberOfPOIs();
}

unsigned getNumberOfFeatures() {
    return streetsDB.getNumberOfFeatures();
}

//------------------------------------------------
// Intersection information

std::string getIntersectionName(unsigned intersectionID) {
    std::set<unsigned> streetIDs;

    const auto& G = streetsDB.roads();
    const auto v = streetsDB.intersection(intersectionID);

    for (const auto e : out_edges(v, G))
        streetIDs.insert(G[e].streetVectorIndex);

    auto it = streetIDs.begin();
    stringstream ss;

    if (it != streetIDs.end())
        ss << streetsDB.streets().at(*(it++));

    for (; it != streetIDs.end(); ++it)
        ss << " & " << streetsDB.streets().at(*it);

    return ss.str();
}

LatLon getIntersectionPosition(unsigned intersectionID) {
    return (streetsDB.roads())[streetsDB.intersection(intersectionID)].latlon;
}

OSMID getIntersectionOSMNodeID(unsigned intersectionID) {
    return (streetsDB.roads())[streetsDB.intersection(intersectionID)].osmid;
}

//number of street segments at an intersection

unsigned getIntersectionStreetSegmentCount(unsigned intersectionID) {
    return out_degree(streetsDB.intersection(intersectionID), streetsDB.roads());
}

// find the street segments at an intersection. idx is from
// 0..streetSegmentCount-1 (at this intersection)

unsigned getIntersectionStreetSegment(unsigned intersectionID, unsigned idx) {
    const auto& G = streetsDB.roads();
    const auto u = streetsDB.intersection(intersectionID);

    const auto Es = out_edges(u, G);

    if (idx >= out_degree(u, G))
        throw std::out_of_range("getIntersectionStreetSegment: idx");

    const auto e = *(begin(Es) + idx);

    return G[e].streetSegmentVectorIndex;
}

//------------------------------------------------
// Street segment information

// return info struct for the requested street segment

StreetSegmentInfo getStreetSegmentInfo(unsigned streetSegmentID) {
    StreetSegmentInfo info;

    const PathNetwork& G = streetsDB.roads();
    const auto e = streetsDB.streetSegment(streetSegmentID);

    info.from = source(e, G);
    info.to = target(e, G);

    info.oneWay = G[e].oneWay != EdgeProperties::Bidir;

    // if should be going to greater vertex number (T) but to < from (F) then swap
    // also if should be going to lesser vertex number (F) but to > from (T) also swap

    if (info.oneWay && ((G[e].oneWay == EdgeProperties::ToGreaterVertexNumber) ^ (info.from < info.to)))
        std::swap(info.from, info.to);

    info.wayOSMID = G[e].wayOSMID;
    info.streetID = G[e].streetVectorIndex;
    info.speedLimit = G[e].maxspeed;
    info.curvePointCount = G[e].curvePoints.size();

    return info;
}

//fetch the latlon of the idx'th curve point

LatLon getStreetSegmentCurvePoint(unsigned streetSegmentID, unsigned idx) {
    const PathNetwork& G = streetsDB.roads();
    const auto e = streetsDB.streetSegment(streetSegmentID);

    return G[e].curvePoints.at(idx);
}

//------------------------------------------------
// Street information

std::string getStreetName(unsigned streetID) {
    return streetsDB.streets().at(streetID); // throws exception if out of bounds
}

//------------------------------------------------
// Points of interest

std::string getPointOfInterestType(unsigned pointOfInterestID) {
    return streetsDB.poi(pointOfInterestID).type();
}

std::string getPointOfInterestName(unsigned pointOfInterestID) {
    return streetsDB.poi(pointOfInterestID).name();

}

LatLon getPointOfInterestPosition(unsigned pointOfInterestID) {
    return streetsDB.poi(pointOfInterestID).pos();

}

OSMID getPointOfInterestOSMNodeID(unsigned pointOfInterestID) {
    return streetsDB.poi(pointOfInterestID).osmNodeID();
}

//------------------------------------------------
// Natural features

FeatureType getFeatureType(unsigned featureID) {
    return streetsDB.feature(featureID).type();
}

const string& getFeatureName(unsigned featureID) {
    return streetsDB.feature(featureID).name();
}

OSMID getFeatureOSMID(unsigned featureID) {
    return streetsDB.feature(featureID).id().first;

}

OSMEntityType getFeatureOSMEntityType(unsigned featureID) {
    return streetsDB.feature(featureID).id().second;
}

unsigned getFeaturePointCount(unsigned featureID) {
    return streetsDB.feature(featureID).pointCount();

}

LatLon getFeaturePoint(unsigned featureID, unsigned idx) {
    return streetsDB.feature(featureID).point(idx);
}


//           Copyright Jeroen Habraken 2010 - 2012.
//
// 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)

#if !defined(BOOST_COERCE_RESERVE_HPP)
#define BOOST_COERCE_RESERVE_HPP

#if defined(_MSC_VER) && _MSC_VER >= 1200
    #pragma once
#endif  // defined(_MSC_VER) && _MSC_VER >= 1200

#include <boost/coerce/detail/precision.hpp>
#include <boost/coerce/detail/reserve.hpp>
#include <boost/coerce/tag_fwd.hpp>

#include <boost/config.hpp>
#include <boost/integer/static_log2.hpp>
#include <boost/limits.hpp>
#include <boost/optional.hpp>

#include <cstddef>  // std::size_t

namespace boost { namespace coerce { namespace traits {

    template <typename T, typename Tag>
    struct reserve_size_impl {
        BOOST_STATIC_CONSTANT(std::size_t, value = 0);
    };

    template <typename Tag>
    struct reserve_size_impl<char, Tag> {
        BOOST_STATIC_CONSTANT(std::size_t, value = 1);
    };

#if !defined(BOOST_NO_INTRINSIC_WCHAR_T)
    template <typename Tag>
    struct reserve_size_impl<wchar_t, Tag> {
        BOOST_STATIC_CONSTANT(std::size_t, value = 1);
    };
#endif  // !defined(BOOST_NO_INTRINSIC_WCHAR_T)

    template <typename T, typename Tag>
    struct reserve_size_impl_integral {
        BOOST_STATIC_CONSTANT(std::size_t, value =
            std::numeric_limits<T>::is_signed +
            1 +
            std::numeric_limits<T>::digits10);
    };

    template <typename T, unsigned Radix>
    struct reserve_size_impl_integral<T, tag::base<Radix> > {
        BOOST_STATIC_CONSTANT(std::size_t, value =
        1 +
        std::numeric_limits<T>::digits / static_log2<Radix>::value);
    };

    template <typename T>
    struct reserve_size_impl_integral<T, tag::bin> {
        BOOST_STATIC_CONSTANT(std::size_t, value =
            1 +
            std::numeric_limits<T>::digits);
    };

    template <typename T>
    struct reserve_size_impl_integral<T, tag::oct> {
        BOOST_STATIC_CONSTANT(std::size_t, value =
            1 +
            std::numeric_limits<T>::digits / 3);
    };

    template <typename T>
    struct reserve_size_impl_integral<T, tag::hex> {
        BOOST_STATIC_CONSTANT(std::size_t, value =
            1 +
            std::numeric_limits<T>::digits / 4);
    };

    template <typename Tag>
    struct reserve_size_impl<int, Tag>
        : reserve_size_impl_integral<int, Tag> { };

    template <typename Tag>
    struct reserve_size_impl<short, Tag>
        : reserve_size_impl_integral<short, Tag> { };

    template <typename Tag>
    struct reserve_size_impl<long, Tag>
        : reserve_size_impl_integral<long, Tag> { };

    template <typename Tag>
    struct reserve_size_impl<unsigned int, Tag>
        : reserve_size_impl_integral<unsigned int, Tag> { };

    template <typename Tag>
    struct reserve_size_impl<unsigned short, Tag>
        : reserve_size_impl_integral<unsigned short, Tag> { };

    template <typename Tag>
    struct reserve_size_impl<unsigned long, Tag>
        : reserve_size_impl_integral<unsigned long, Tag> { };

#if defined(BOOST_HAS_LONG_LONG)
    template <typename Tag>
    struct reserve_size_impl<boost::long_long_type, Tag>
        : reserve_size_impl_integral<boost::long_long_type, Tag> { };

    template <typename Tag>
    struct reserve_size_impl<boost::ulong_long_type, Tag>
        : reserve_size_impl_integral<boost::ulong_long_type, Tag> { };
#endif  // defined(BOOST_HAS_LONG_LONG)

    template <typename T, typename Tag>
    struct reserve_size_impl_floating_point {
        BOOST_STATIC_CONSTANT(std::size_t, value =
            std::numeric_limits<T>::is_signed +
            8 +
            detail::precision<T>::value);
    };

    template <typename Tag>
    struct reserve_size_impl<float, Tag>
        : reserve_size_impl_floating_point<float, Tag> { };

    template <typename Tag>
    struct reserve_size_impl<double, Tag>
        : reserve_size_impl_floating_point<double, Tag> { };

    template <typename Tag>
    struct reserve_size_impl<long double, Tag>
        : reserve_size_impl_floating_point<long double, Tag> { };

    template <typename Tag>
    struct reserve_size_impl<bool, Tag> {
        BOOST_STATIC_CONSTANT(std::size_t, value = 5);
    };

    template <typename T, typename Tag>
    struct reserve_size_impl<boost::optional<T>, Tag>
        : reserve_size_impl<T, Tag> { };

    template <typename T, typename Tag, typename Enable = void>
    struct reserve_size {
        typedef std::size_t type;

        static inline type
        call(T const &, Tag const &) {
            return reserve_size_impl<T, Tag>::value;
        }
    };

} } }  // namespace boost::coerce::traits

#endif  // !defined(BOOST_COERCE_RESERVE_HPP)


/**
 * $Id$
 *
 * Software License Agreement (GNU General Public License)
 *
 *  Copyright (C) 2015:
 *
 *    Johann Prankl, prankl@acin.tuwien.ac.at
 *    Aitor Aldoma, aldoma@acin.tuwien.ac.at
 *
 *      Automation and Control Institute
 *      Vienna University of Technology
 *      Gusshausstraße 25-29
 *      1170 Vienn, Austria
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * @author Johann Prankl
 *
 */

#ifndef KP_TSF_POSE_TRACKER_KLT_HH
#define KP_TSF_POSE_TRACKER_KLT_HH

#include <iostream>
#include <fstream>
#include <float.h>
#include <Eigen/Dense>
#include <opencv2/core/core.hpp>
#include "opencv2/video/tracking.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <boost/thread/mutex.hpp>
#include <boost/thread.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/io.h>
#include <boost/shared_ptr.hpp>
#include <v4r/recognition/RansacSolvePnPdepth.h>
#include <v4r/common/impl/DataMatrix2D.hpp>
#include <v4r/camera_tracking_and_mapping/TSFData.h>
#include <v4r/core/macros.h>

namespace v4r
{

/**
 * TSFPoseTrackerKLT
 */
class V4R_EXPORTS TSFPoseTrackerKLT
{
public:

  /**
   * Parameter
   */
  class Parameter
  {
  public:
    cv::TermCriteria termcrit;
    cv::Size win_size;
    cv::Size subpix_win_size;
    int max_count;
    double pcent_reinit;
    double conf_tracked_points_norm;
    v4r::RansacSolvePnPdepth::Parameter rt;
    Parameter()
      : termcrit(cv::TermCriteria(cv::TermCriteria::COUNT|cv::TermCriteria::EPS,20,0.03)), win_size(cv::Size(31,31)),
        subpix_win_size(cv::Size(10,10)), max_count(500), pcent_reinit(0.5), conf_tracked_points_norm(250),
        rt(v4r::RansacSolvePnPdepth::Parameter(1.5, 0.01, 5000, INT_MIN, 4, 0.015)){}
  };

private:
  Parameter param;

  cv::Mat_<double> intrinsic;

  std::vector<uchar> status;
  std::vector<float> err;
  std::vector<int> inliers;

  bool run, have_thread;

  boost::thread th_obectmanagement;
  boost::thread th_init;

  TSFData *data;

  v4r::RansacSolvePnPdepth::Ptr pnp;
  std::vector<int> plk_converged;
  std::vector<float> depth;

  double kal_dt;
  cv::Mat_<double> measurement;
  cv::KalmanFilter kalmanFilter;

  void operate();

  void getImage(const v4r::DataMatrix2D<Surfel> &cloud, cv::Mat &im);
  bool needReinit(const std::vector<cv::Point2f> &points);
  bool trackCamera(double &conf_ransac_iter, double &conf_tracked_points);
  void getPoints3D(const pcl::PointCloud<pcl::PointXYZRGB> &cloud, const std::vector<cv::Point2f> &points, std::vector<Eigen::Vector3f> &points3d);
  void filterValidPoints3D(std::vector<cv::Point2f> &points, std::vector<Eigen::Vector3f> &points3d);
  void filterValidPoints3D(std::vector<cv::Point2f> &pts1, std::vector<Eigen::Vector3f> &pts3d1, std::vector<cv::Point2f> &pts2, std::vector<Eigen::Vector3f> &pts3d2);
  void filterInliers(std::vector<cv::Point2f> &pts1, std::vector<Eigen::Vector3f> &pts3d1, std::vector<cv::Point2f> &pts2, std::vector<Eigen::Vector3f> &pts3d2, std::vector<int> &inliers);
  void filterConverged(std::vector<cv::Point2f> &pts1, std::vector<Eigen::Vector3f> &pts3d1, std::vector<cv::Point2f> &pts2, std::vector<Eigen::Vector3f> &pts3d2, std::vector<int> &converged);

  void initKalmanFilter( cv::KalmanFilter &kf, double dt);
  void updateKalmanFilter( cv::KalmanFilter &kf, const Eigen::Matrix4f &pose, Eigen::Matrix4f &kal_pose, bool have_pose );

  cv::Mat euler2rot(const cv::Mat & euler);
  cv::Mat rot2euler(const cv::Mat & rotationMatrix);

  inline float getInterpolated(const v4r::DataMatrix2D<v4r::Surfel> &cloud, const cv::Point2f &pt);
  inline float getInterpolated(const pcl::PointCloud<pcl::PointXYZRGB> &cloud, const cv::Point2f &pt);
  inline float sqr(const float &d) {return d*d;}

public:
  cv::Mat dbg;

  TSFPoseTrackerKLT(const Parameter &p=Parameter());
  ~TSFPoseTrackerKLT();

  void start();
  void stop();

  inline bool isStarted() {return have_thread;}

  void reset();

  void setData(TSFData *_data) { data = _data; }
  void track(double &conf_ransac_iter, double &conf_tracked_points);

  void setCameraParameter(const cv::Mat &_intrinsic);
  void setParameter(const Parameter &p);

  typedef boost::shared_ptr< ::v4r::TSFPoseTrackerKLT> Ptr;
  typedef boost::shared_ptr< ::v4r::TSFPoseTrackerKLT const> ConstPtr;
};

/*************************** INLINE METHODES **************************/

/**
 * @brief TSFPoseTrackerKLT::getInterpolated
 * @param cloud
 * @param pt
 * @return
 */
inline float TSFPoseTrackerKLT::getInterpolated(const v4r::DataMatrix2D<v4r::Surfel> &cloud, const cv::Point2f &pt)
{
  int xt = (int) pt.x;
  int yt = (int) pt.y;
  float ax = pt.x - xt;
  float ay = pt.y - yt;
  float d=0;
  float sn=0, n;

  if (!std::isnan(cloud(yt,xt).pt[0]))
  {
    n = (1.-ax) * (1.-ay);
    d += n * cloud(yt,xt).pt[2];
    sn += n;
  }
  if (!std::isnan(cloud(yt,xt+1).pt[0]))
  {
    n = ax * (1.-ay);
    d += n * cloud(yt,xt+1).pt[2];
    sn += n;
  }
  if (!std::isnan(cloud(yt+1,xt).pt[0]))
  {
    n = (1.-ax) * ay;
    d += n * cloud(yt+1,xt).pt[2];
    sn += n;
  }
  if (!std::isnan(cloud(yt+1,xt+1).pt[0]))
  {
    n = ax * ay;
    d += n * cloud(yt+1,xt+1).pt[2];
    sn += n;
  }

  return (d>0.?d/sn:std::numeric_limits<float>::quiet_NaN());
}

/**
 * @brief TSFPoseTrackerKLT::getInterpolated
 * @param cloud
 * @param pt
 * @return
 */
inline float TSFPoseTrackerKLT::getInterpolated(const pcl::PointCloud<pcl::PointXYZRGB> &cloud, const cv::Point2f &pt)
{
  int xt = (int) pt.x;
  int yt = (int) pt.y;
  float ax = pt.x - xt;
  float ay = pt.y - yt;
  float d=0;
  float sn=0, n;

  if (!std::isnan(cloud(xt,yt).x))
  {
    n = (1.-ax) * (1.-ay);
    d += n * cloud(xt,yt).z;
    sn += n;
  }
  if (!std::isnan(cloud(xt+1,yt).x))
  {
    n = ax * (1.-ay);
    d += n * cloud(xt+1, yt).z;
    sn += n;
  }
  if (!std::isnan(cloud(xt, yt+1).x))
  {
    n = (1.-ax) * ay;
    d += n * cloud(xt, yt+1).z;
    sn += n;
  }
  if (!std::isnan(cloud(xt+1, yt+1).x))
  {
    n = ax * ay;
    d += n * cloud(xt+1,yt+1).z;
    sn += n;
  }

  return (d>0.?d/sn:std::numeric_limits<float>::quiet_NaN());
}

} //--END--

#endif


#ifndef CUAUV_SIM_THRUSTERS_H
#define CUAUV_SIM_THRUSTERS_H

#include <functional>

#include <Eigen/Geometry>

#include <libshm/c/shm.h>
#include <conf/vehicle.hpp>

#include "engine.hpp"
#include "pid.hpp"
#include "screw.hpp"

namespace cuauv {
namespace fishbowl {

class ciw_thrusters : public engine {
public:
    ciw_thrusters(const Eigen::Quaterniond& btom_rq);
    screw on();
    void step(double delta);

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
    const Eigen::Quaterniond& btom_rq;
    control_internal_wrench shm_wrench;
};

typedef std::function<screw()> passive_forces_func;

class des_thrusters : public engine {
public:
    des_thrusters(const Eigen::Quaterniond& q, const Eigen::Vector3d& v,
            const Eigen::Vector3d& x, const Eigen::Quaterniond& btom_rq,
            const cuauv::conf::vector6d& axes, const cuauv::conf::vector6d& mins,
            const cuauv::conf::vector6d& maxs, passive_forces_func pff, const Eigen::Matrix3d& I, double m);
    screw on() override;
    void step(double delta) override;

    // All these values are in the model frame.

    epid hp {0, 0, 0, 0}; //<! Heading Euler angle PID controller.
    epid pp {0, 0, 0, 0}; //<! Pitch Euler angle PID controller.
    epid rp {0, 0, 0, 0}; //<! Roll Euler angle PID controller.

    pid xp {0, 0, 0, 0}; //<! X-velocity PID controller.
    pid yp {0, 0, 0, 0}; //<! Y-velocity PID controller.
    pid dp {0, 0, 0, 0}; //<! Depth PID controller.

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
    const Eigen::Quaterniond& q;
    const Eigen::Vector3d& v;
    const Eigen::Vector3d& x;

    Eigen::Matrix3d btom_rm;
    Eigen::Quaterniond mtob_rq;
    Eigen::Matrix3d mtob_rm;
    cuauv::conf::vector6d axes;
    double axesm; //<! The squared magnitude of [axes].

    // Force and torque limits, [a, b].
    Eigen::Vector3d fa;
    Eigen::Vector3d fb;
    Eigen::Vector3d ta;
    Eigen::Vector3d tb;

    passive_forces_func pff;

    Eigen::Matrix3d I;
    double m;

    settings_control shm_settings;

    desires shm_desires;
    settings_heading shm_hp;
    settings_pitch shm_pp;
    settings_roll shm_rp;
    settings_velx shm_xp;
    settings_vely shm_yp;
    settings_depth shm_dp;

    Eigen::Vector3d f {0, 0, 0};
    Eigen::Vector3d t {0, 0, 0};
};

} // namespace fishbowl
} // namespace cuauv

#endif // CUAUV_SIM_THRUSTERS_H


#pragma once
#ifndef Method_Solver_H
#define Method_Solver_H

#include "Spirit_Defines.h"
#include <Spirit/Simulation.h>
#include <data/Parameters_Method.hpp>
#include <data/Spin_System_Chain.hpp>
#include <engine/Manifoldmath.hpp>
#include <engine/Method.hpp>
#include <engine/Solver_Kernels.hpp>
#include <engine/Vectormath.hpp>
#include <utility/Constants.hpp>
#include <utility/Logging.hpp>
#include <utility/Timing.hpp>

#include <deque>
#include <fstream>
#include <iomanip>
#include <map>
#include <sstream>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <fmt/format.h>

namespace Engine
{
enum class Solver
{
    None        = -1,
    SIB         = Solver_SIB,
    Heun        = Solver_Heun,
    Depondt     = Solver_Depondt,
    RungeKutta4 = Solver_RungeKutta4,
    LBFGS_OSO   = Solver_LBFGS_OSO,
    LBFGS_Atlas = Solver_LBFGS_Atlas,
    VP          = Solver_VP,
    VP_OSO      = Solver_VP_OSO
};

/*
    Base Class for Solver-based Simulation/Calculation Methods.
    It is templated to allow a flexible choice of Solver to iterate the systems.
*/
template<Solver solver>
class Method_Solver : public Method
{
public:
    // Constructor to be used in derived classes
    Method_Solver( std::shared_ptr<Data::Parameters_Method> parameters, int idx_img, int idx_chain )
            : Method( parameters, idx_img, idx_chain )
    {
    }

    // // `Iterate` uses the `Solver_Iteration` function to evolve given systems according to the
    // // `Calculate_Force` implementation of the Method-Subclass.
    // //      It iterates until: the maximum number of iterations is reached or the maximum
    // //      walltime is reaches or the force has converged or a file called `STOP` is found
    // //      or the calculation is stopped externally (via the API).
    // virtual void Iterate() override;

    // Solver name as string
    virtual std::string SolverName() override;
    virtual std::string SolverFullName() override;

    // Iteration represents one iteration of a certain Solver
    virtual void Iteration() override;

protected:
    // Prepare random numbers for thermal fields, if needed
    virtual void Prepare_Thermal_Field() {}

    // Calculate Forces onto Systems
    //      This is currently overridden by methods to specify how the forces on a set of configurations should be
    //      calculated. This function is used in `the Solver_...` functions.
    // TODO: maybe rename to separate from deterministic and stochastic force functions
    virtual void Calculate_Force(
        const std::vector<std::shared_ptr<vectorfield>> & configurations, std::vector<vectorfield> & forces )
    {
        Log( Utility::Log_Level::Error, Utility::Log_Sender::All,
             "Tried to use Method_Solver::Calculate_Force() of the Method_Solver class!", this->idx_image,
             this->idx_chain );
    }

    // Calculate virtual Forces onto Systems (can be precession and damping forces, correctly scaled)
    // Calculate the effective force on a configuration. It is a combination of
    //      precession and damping terms for the Hamiltonian, spin currents and
    //      temperature. This function is used in `the Solver_...` functions.
    // Default implementation: direct minimization
    virtual void Calculate_Force_Virtual(
        const std::vector<std::shared_ptr<vectorfield>> & configurations, const std::vector<vectorfield> & forces,
        std::vector<vectorfield> & forces_virtual )
    {
        // Not Implemented!
        Log( Utility::Log_Level::Error, Utility::Log_Sender::All,
             "Tried to use Method_Solver::Calculate_Force_Virtual() of the Method_Solver class!", this->idx_image,
             this->idx_chain );
    }

    // Calculate maximum of absolute values of force components for a spin configuration
    virtual scalar Force_on_Image_MaxAbsComponent( const vectorfield & image, vectorfield & force ) final;

    // Calculate maximum torque for a spin configuration
    virtual scalar MaxTorque_on_Image( const vectorfield & image, vectorfield & force ) final;

    // ...
    // virtual bool Iterations_Allowed() override;
    // Check if the forces are converged
    virtual bool Converged();

    // Check if any stop criteria were encountered
    bool ContinueIterating() override
    {
        return Method::ContinueIterating() && !this->Converged();
    }

    // Initialise contains the initialisations of arrays etc. for a certain solver
    virtual void Initialize() override;
    virtual void Finalize() override;

    // Log message blocks
    virtual void Message_Start() override;
    virtual void Message_Step() override;
    virtual void Message_End() override;

    //////////// DEPONDT ////////////////////////////////////////////////////////////
    // Temporaries for virtual forces
    std::vector<vectorfield> rotationaxis;
    std::vector<scalarfield> forces_virtual_norm;
    // Preccession angle
    scalarfield angle;

    //////////// LBFGS ////////////////////////////////////////////////////////////

    // General
    int n_lbfgs_memory;
    int local_iter;
    scalar maxmove;
    scalarfield rho;
    scalarfield alpha;

    // Atlas coords
    std::vector<field<vector2field>> atlas_updates;
    std::vector<field<vector2field>> grad_atlas_updates;
    std::vector<scalarfield> atlas_coords3;
    std::vector<vector2field> atlas_directions;
    std::vector<vector2field> atlas_residuals;
    std::vector<vector2field> atlas_residuals_last;
    std::vector<vector2field> atlas_q_vec;

    // OSO
    std::vector<field<vectorfield>> delta_a;
    std::vector<field<vectorfield>> delta_grad;
    std::vector<vectorfield> searchdir;
    std::vector<vectorfield> grad;
    std::vector<vectorfield> grad_pr;
    std::vector<vectorfield> q_vec;

    // buffer variables for checking convergence for solver and Newton-Raphson
    // std::vector<scalarfield> r_dot_d, dda2;

    //////////// VP ///////////////////////////////////////////////////////////////
    // "Mass of our particle" which we accelerate
    scalar m = 1.0;

    // Force in previous step [noi][nos]
    std::vector<vectorfield> forces_previous;
    // Velocity in previous step [noi][nos]
    std::vector<vectorfield> velocities_previous;
    // Velocity used in the Steps [noi][nos]
    std::vector<vectorfield> velocities;
    // Projection of velocities onto the forces [noi]
    std::vector<scalar> projection;
    // |force|^2
    std::vector<scalar> force_norm2;

    // Temporary Spins arrays
    vectorfield temp1, temp2;

    // Actual Forces on the configurations
    std::vector<vectorfield> forces;
    std::vector<vectorfield> forces_predictor;
    // Virtual Forces used in the Steps
    std::vector<vectorfield> forces_virtual;
    std::vector<vectorfield> forces_virtual_predictor;

    // RK 4
    std::vector<std::shared_ptr<vectorfield>> configurations_k1;
    std::vector<std::shared_ptr<vectorfield>> configurations_k2;
    std::vector<std::shared_ptr<vectorfield>> configurations_k3;
    std::vector<std::shared_ptr<vectorfield>> configurations_k4;

    // Random vector array
    vectorfield xi;

    // Pointers to Configurations (for Solver methods)
    std::vector<std::shared_ptr<vectorfield>> configurations;
    std::vector<std::shared_ptr<vectorfield>> configurations_predictor;
    std::vector<std::shared_ptr<vectorfield>> configurations_temp;
};

// Return the maximum of absolute values of force components for an image
template<Solver solver>
scalar Method_Solver<solver>::Force_on_Image_MaxAbsComponent( const vectorfield & image, vectorfield & force )
{
    // Take out component in direction of v2
    Manifoldmath::project_tangential( force, image );

    // We want the Maximum of Absolute Values of all force components on all images
    return Vectormath::max_abs_component( force );
}

// Return the maximum norm of the torque for an image
template<Solver solver>
scalar Method_Solver<solver>::MaxTorque_on_Image( const vectorfield & image, vectorfield & force )
{
    // Take out component in direction of v2
    Manifoldmath::project_tangential( force, image );
    return Vectormath::max_norm( force );
}

template<Solver solver>
bool Method_Solver<solver>::Converged()
{
    bool converged = false;
    if( this->max_torque < this->parameters->force_convergence )
        converged = true;
    return converged;
}

// Default implementation: do nothing
template<Solver solver>
void Method_Solver<solver>::Initialize(){};

// Default implementation: do nothing
template<Solver solver>
void Method_Solver<solver>::Finalize(){};

// Default implementation: do nothing
template<Solver solver>
void Method_Solver<solver>::Iteration(){};

template<Solver solver>
void Method_Solver<solver>::Message_Start()
{
    using namespace Utility;

    //---- Log messages
    std::vector<std::string> block;
    block.push_back( fmt::format( "------------  Started  {} Calculation  ------------", this->Name() ) );
    block.push_back( fmt::format( "    Going to iterate {} step(s)", this->n_log ) );
    block.push_back( fmt::format( "                with {} iterations per step", this->n_iterations_log ) );
    block.push_back( fmt::format(
        "    Force convergence parameter: {:." + fmt::format( "{}", this->print_precision ) + "f}",
        this->parameters->force_convergence ) );
    block.push_back( fmt::format(
        "    Maximum torque:              {:." + fmt::format( "{}", this->print_precision ) + "f}",
        this->max_torque ) );
    block.push_back( fmt::format( "    Solver: {}", this->SolverFullName() ) );
    if( this->Name() == "GNEB" )
    {
        scalar length = Manifoldmath::dist_geodesic( *this->configurations[0], *this->configurations[this->noi - 1] );
        block.push_back( fmt::format( "    Total path length: {}", length ) );
    }
    block.push_back( "-----------------------------------------------------" );
    Log.SendBlock( Log_Level::All, this->SenderName, block, this->idx_image, this->idx_chain );
}

template<Solver solver>
void Method_Solver<solver>::Message_Step()
{
    using namespace Utility;

    std::string percentage = fmt::format( "{:.2f}%:", 100 * double( this->iteration ) / double( this->n_iterations ) );
    bool llg_dynamics
        = this->Name() == "LLG"
          && !(
              this->systems[0]->llg_parameters->direct_minimization || solver == Solver::VP || solver == Solver::VP_OSO
              || solver == Solver::LBFGS_OSO || solver == Solver::LBFGS_Atlas );

    // Update time of current step
    auto t_current = system_clock::now();

    // Send log message
    std::vector<std::string> block;
    block.push_back( fmt::format(
        "----- {} Calculation ({} Solver): {}", this->Name(), this->SolverName(),
        Timing::DateTimePassed( t_current - this->t_start ) ) );
    block.push_back(
        fmt::format( "    Time since last step: {}", Timing::DateTimePassed( t_current - this->t_last ) ) );
    block.push_back(
        fmt::format( "    Completed {:>8}    {} / {} iterations", percentage, this->iteration, this->n_iterations ) );
    block.push_back( fmt::format(
        "    Iterations / sec:     {:.2f}",
        this->n_iterations_log / Timing::SecondsPassed( t_current - this->t_last ) ) );
    if( llg_dynamics )
        block.push_back( fmt::format( "    Simulated time:       {} ps", this->get_simulated_time() ) );
    if( this->Name() == "GNEB" )
    {
        scalar length = Manifoldmath::dist_geodesic( *this->configurations[0], *this->configurations[this->noi - 1] );
        block.push_back( fmt::format( "    Total path length:    {}", length ) );
    }
    block.push_back( fmt::format(
        "    Force convergence parameter: {:." + fmt::format( "{}", this->print_precision ) + "f}",
        this->parameters->force_convergence ) );
    block.push_back( fmt::format(
        "    Maximum torque:              {:." + fmt::format( "{}", this->print_precision ) + "f}",
        this->max_torque ) );
    Log.SendBlock( Log_Level::All, this->SenderName, block, this->idx_image, this->idx_chain );

    // Update time of last step
    this->t_last = t_current;
}

template<Solver solver>
void Method_Solver<solver>::Message_End()
{
    using namespace Utility;

    std::string percentage = fmt::format( "{:.2f}%:", 100 * double( this->iteration ) / double( this->n_iterations ) );
    bool llg_dynamics
        = this->Name() == "LLG"
          && !(
              this->systems[0]->llg_parameters->direct_minimization || solver == Solver::VP || solver == Solver::VP_OSO
              || solver == Solver::LBFGS_OSO || solver == Solver::LBFGS_Atlas );

    //---- End timings
    auto t_end = system_clock::now();

    //---- Termination reason
    std::string reason = "";
    if( this->StopFile_Present() )
        reason = "A STOP file has been found";
    else if( this->Converged() )
        reason = "The force converged";
    else if( this->Walltime_Expired( t_end - this->t_start ) )
        reason = "The maximum walltime has been reached";

    //---- Log messages
    std::vector<std::string> block;
    block.push_back( fmt::format( "------------ Terminated {} Calculation ------------", this->Name() ) );
    if( reason.length() > 0 )
        block.push_back( fmt::format( "------- Reason: {}", reason ) );
    block.push_back( fmt::format( "    Total duration:    {}", Timing::DateTimePassed( t_end - this->t_start ) ) );
    block.push_back(
        fmt::format( "    Completed {:>8} {} / {} iterations", percentage, this->iteration, this->n_iterations ) );
    block.push_back( fmt::format(
        "    Iterations / sec:  {:.2f}", this->iteration / Timing::SecondsPassed( t_end - this->t_start ) ) );
    if( llg_dynamics )
        block.push_back( fmt::format( "    Simulated time:    {} ps", this->get_simulated_time() ) );
    if( this->Name() == "GNEB" )
    {
        scalar length = Manifoldmath::dist_geodesic( *this->configurations[0], *this->configurations[this->noi - 1] );
        block.push_back( fmt::format( "    Total path length: {}", length ) );
    }
    block.push_back( fmt::format(
        "    Force convergence parameter: {:." + fmt::format( "{}", this->print_precision ) + "f}",
        this->parameters->force_convergence ) );
    block.push_back( fmt::format(
        "    Maximum torque:              {:." + fmt::format( "{}", this->print_precision ) + "f}",
        this->max_torque ) );
    block.push_back( fmt::format( "    Solver: {}", this->SolverFullName() ) );
    block.push_back( "-----------------------------------------------------" );
    Log.SendBlock( Log_Level::All, this->SenderName, block, this->idx_image, this->idx_chain );
}

template<>
inline std::string Method_Solver<Solver::None>::SolverName()
{
    return "None";
};

template<>
inline std::string Method_Solver<Solver::None>::SolverFullName()
{
    return "None";
};

// Include headers which specialize the Solver functions
#include <engine/Solver_Depondt.hpp>
#include <engine/Solver_Heun.hpp>
#include <engine/Solver_LBFGS_Atlas.hpp>
#include <engine/Solver_LBFGS_OSO.hpp>
#include <engine/Solver_RK4.hpp>
#include <engine/Solver_SIB.hpp>
#include <engine/Solver_VP.hpp>
#include <engine/Solver_VP_OSO.hpp>
} // namespace Engine

#endif


//==================================================================================================
/**
  Copyright 2017 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_SIMD_ASINPI_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SIMD_ASINPI_HPP_INCLUDED

#include <boost/simd/function/scalar/asinpi.hpp>
#include <boost/simd/arch/common/generic/function/autodispatcher.hpp>
#include <boost/simd/arch/common/simd/function/asinpi.hpp>

#endif