#include "objectfinder.h"
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>
#include <math.h>
void drawLine(cv::Mat & img, cv::Mat & img2, cv::Vec4f line, int thickness, CvScalar color, bool nightVision = false);

ObjectFinder::ObjectFinder()
{
	using boost::property_tree::ptree;
	try {

		ptree pt;
		read_ini("conf/camera.ini", pt);
		Vfov = pt.get<float>("Vfov");
		CamHeight = pt.get<float>("Height");
		CamAngleDev = pt.get<float>("AngleDev");
	}
	catch (...){
		ptree pt;
		pt.put("Height", CamHeight);
		pt.put("AngleDev", CamAngleDev);
		pt.put("Vfov", Vfov);
		write_ini("conf/camera.ini", pt);
	};
}
bool ObjectFinder::LocateCursor(cv::Mat &frameBGR, cv::Point2i cursor, OBJECT target, ObjectPosition &targetPos){

	cv::Scalar color(0, 0, 0);
	cv::circle(frameBGR, cursor, 8, color, -1);
	//std::cout << point << std::endl;
	targetPos = ConvertPixelToRealWorld(cursor, cv::Point2i(frameBGR.cols, frameBGR.rows));
	WriteInfoOnScreen(targetPos);
	return true;

}

bool ObjectFinder::Locate(ThresholdedImages &HSVRanges, cv::Mat &frameHSV, cv::Mat &frameBGR, OBJECT target, ObjectPosition &targetPos) {
	cv::Point2i point(-1, -1);
	cv::Scalar color(0, 0, 0);
	cv::Scalar color2(255, 0, 255);
	bool resetFilter = false;
	point = LocateOnScreen(HSVRanges, frameHSV, frameBGR, target);
	/*
	if (target == BALL){
		point = LocateBallOnScreen(HSVRanges, frameHSV, frameBGR, target);
		color = cv::Scalar(0, 225, 225);
	}
	else{
		point = LocateGateOnScreen(HSVRanges, frameHSV, frameBGR, target);
		cv::circle(frameBGR, point, 8, color2, -1);
	}
	*/
	if (point.x < -1 && point.y < -1){//If ball is not valid then no predicting
		lastPosition = point;
		resetFilter = true;
		return false;
	}
	else if (point.x < 0 && point.y < 0){//If ball is suddenly lost then predict where it could be
		point = filter->getPrediction();
		lastPosition = point;
		if (point.x < 0 && point.y < 0){
			resetFilter = true;
			return false;
		}
	}
	else {//Ball is in frame
		if (resetFilter){
			resetFilter = false;
			filter->reset(point);
		}
		point = filter->doFiltering(point);
		lastPosition = point;
	}
	//cv::circle(frameBGR, point, 8, color, -1);
	//std::cout << point << std::endl;
	targetPos = ConvertPixelToRealWorld(point, cv::Point2i(frameHSV.cols, frameHSV.rows));
	WriteInfoOnScreen(targetPos);
	return true;
}

void drawLine(cv::Mat & img, cv::Mat & img2, cv::Vec4f line, int thickness, CvScalar color, bool nightVision/* = false*/)
{
	double theMult = std::max(img.cols, img.rows);
	// calculate start point
	cv::Point startPoint;
	startPoint.x = line[2] - theMult*line[0];// x0
	startPoint.y = line[3] - theMult*line[1];// y0
	// calculate end point
	cv::Point endPoint;
	endPoint.x = line[2] + theMult*line[0];//x[1]
	endPoint.y = line[3] + theMult*line[1];//y[1]

	// draw overlay of bottom lines on image
	cv::clipLine(cv::Size(img.cols, img.rows), startPoint, endPoint);
	std::vector <cv::Point2i> points;
	if (startPoint.x == 0) {
		points.push_back(cv::Point2i(0, 0));
	}
	if (endPoint.x == img.cols - 1) {
		points.push_back(cv::Point2i(img.cols - 1, 0));
	}
	if (points.size() == 0) {
		if (startPoint.y == 0 && endPoint.y == img.rows - 1) {
			points.push_back(cv::Point2i(img.cols - 1, 0));
		}
		else if (startPoint.y == img.rows - 1 && endPoint.y == 0) {
			points.push_back(cv::Point2i(0, 0));
		}
	}
	points.push_back(endPoint);
	points.push_back(startPoint);

	if (points.size() > 2) {
		if (nightVision) {
			cv::fillConvexPoly(img, points, cv::Scalar(0, 0, 0));
		}
		cv::fillConvexPoly(img2, points, cv::Scalar(0, 0, 0));
	}
	else {
		std::cout << "unable to fill line: " << startPoint << ", " << endPoint << std::endl;
	}
	cv::line(img, startPoint, endPoint, color, thickness, 8, 0);
	return;

}
int ObjectFinder::IsolateField(ThresholdedImages &HSVRanges, cv::Mat &frameHSV, cv::Mat &frameBGR, bool detectBothBorders/* = false*/, bool nightVision/* = false*/) {

	std::vector<cv::Vec4i> lines;
	std::vector<cv::Vec4i> lines2;

	cv::Vec4f newLine1;
	cv::Vec4f newLine2;
	float distance = INT_MAX;
	//Canny(HSVRanges[OUTER_BORDER], HSVRanges[OUTER_BORDER], 50, 200, 3);
	//cv::imshow("aaa", HSVRanges[OUTER_BORDER]);
	cv::HoughLinesP(HSVRanges[OUTER_BORDER], lines2, 1, CV_PI / 180, 80, 30, 10);
	double last_angle = 999;
	if (!detectBothBorders){
		for (auto l2 : lines2){
			std::vector<cv::Point2i> points2;
			points2.push_back(cv::Point(l2[0], l2[1]));
			points2.push_back(cv::Point(l2[2], l2[3]));
			cv::fitLine(points2, newLine2, CV_DIST_L2, 0, 0.1, 0.1);
			if (last_angle != 999 && abs(last_angle - atan2(newLine2[1], newLine2[0]) * 180 / PI) < 15) continue; // same border
			last_angle = atan2(newLine2[1], newLine2[0]) * 180 / PI;
			drawLine(frameBGR, HSVRanges[BALL], newLine2, 1, cv::Scalar(255 * (1 + 0.3), 0, 0), nightVision);
			distance = std::min(distance, newLine2[3] - (frameHSV.cols / 2)*newLine2[1]);
		}
		return distance;
	}
	cv::HoughLinesP(HSVRanges[INNER_BORDER], lines, 1, CV_PI / 180, 50, 50, 10);
//	cv::HoughLinesP(HSVRanges[INNER_BORDER] + HSVRanges[FIELD], lines, 1, CV_PI / 180, 50, 50, 10);

	//std::cout << rect_points[j] << ", " << rect_points[(j + 1) % 4] << ": " << atan2(newLine[1], newLine[0])*180/PI << std::endl;

	for (auto l1 : lines2){
		std::vector<cv::Point2i> points;
		points.push_back(cv::Point(l1[0], l1[1]));
		points.push_back(cv::Point(l1[2], l1[3]));
		cv::fitLine(points, newLine1, CV_DIST_L2, 0, 0.1, 0.1);
		for (auto l2 : lines){
			std::vector<cv::Point2i> points2;
			points2.push_back(cv::Point(l2[0], l2[1]));
			points2.push_back(cv::Point(l2[2], l2[3]));
			cv::fitLine(points2, newLine2, CV_DIST_L2, 0, 0.1, 0.1);
			if (abs(atan2(newLine1[1], newLine1[0]) - atan2(newLine2[1], newLine2[0])) < 0.1) { // almost parallel
				//cv::line(frameBGR, cv::Point(l1[0], l1[1]), cv::Point(l1[2], l1[3]), cv::Scalar(255, 0, 255), 3, CV_AA);
				drawLine(frameBGR, HSVRanges[BALL], newLine1, 1, cv::Scalar(255 * (1 + 0.3), 0, 0), nightVision);

			}
		}
	}

	return distance;

}

void ObjectFinder::IsolateFieldOld(ThresholdedImages &HSVRanges, cv::Mat &frameHSV, cv::Mat &frameBGR) {

	cv::Mat innerThresholded = HSVRanges[INNER_BORDER];
	cv::Mat outerThresholded = HSVRanges[OUTER_BORDER];

	/*
	cv::Mat gate1Thresholded;
	inRange(frameHSV, cv::Scalar(gate1.hue.low, gate1.sat.low, gate1.val.low), cv::Scalar(gate1.hue.high, gate1.sat.high, gate1.val.high), gate1Thresholded); //Threshold the image
	cv::Mat gate2Thresholded;
	inRange(frameHSV, cv::Scalar(gate2.hue.low, gate2.sat.low, gate2.val.low), cv::Scalar(gate2.hue.high, gate2.sat.high, gate2.val.high), gate2Thresholded); //Threshold the image
	*/

	/*
	cv::Point2d orgin(frameBGR.cols / 2, frameBGR.rows *0.9);
	cv::circle(frameBGR, orgin, 40, cv::Scalar(40, 20, 100), 10);
	for (float a = -PI; a < 0 ; a += PI/10) {
		bool was_border_start = false;
		for (float d = 0; d < frameBGR.cols ; d += 10) {
			float x = d * std::cos(a);
			float y = d * std::sin(a);
			if (abs(x) > orgin.x) break;
			if (y < -orgin.y) break;

			cv::Point2i point = orgin + cv::Point2d(x, y);
			bool border = abs(x) > orgin.x * 0.9 || y < -orgin.y*0.9;
			if (!border) {
				bool is_border_start = innerThresholded.ptr<uchar>(point.y)[point.x] == 255;
				bool is_border_end = outerThresholded.ptr<uchar>(point.y)[point.x] == 255;

				border = !is_border_start && is_border_end && was_border_start;
				was_border_start = is_border_start;
			}
			cv::circle(frameBGR, point, 4, border? cv::Scalar(d, 255, 100 * a)  : cv::Scalar(d, 20, 100 * a), 10);
			if (border) break;
		}
		//break;
	}
	*/

//	cv::Mat gateThresholded = gate1Thresholded + gate2Thresholded;

	//dir: left -> right, right -> left, top -> down
	for (int dir = 0; dir < 2; dir++) {
		std::vector<cv::Point2i> points;
		int end = dir ? frameHSV.cols - 1 : 0;
		//int last_y = -1;
		cv::Point2i last_canditate(-1, -1);
		int dx = 0;
		cv::Point2i last_selected(-1, -1);
		for (int y = 0; y < frameHSV.rows; y += 5) {
			int outer_start = -1;
			int outer_end = -1;
			int inner_start = -1;
			int inner_end = -1;
			bool break2 = false;
			for (int x = 0; x < frameHSV.cols; x += 3) { // TODO: start near the last found piont
				//cv::circle(frameBGR, cv::Point2i(dir ? end - x : x, y), 1, dir ? cv::Scalar(255, 0, 0) : cv::Scalar(120, 0, 0), 10);
				if (inner_end > 0 && inner_end - inner_start > 1) {
					// we have detected new point on the border
					if (last_canditate.x != -1) {
						// check the distance form last candidate
						if (abs(y - last_canditate.y) < 40 && abs(x - last_canditate.x) < 40) { // close enough
							cv::Point2i new_point = cv::Point2i(dir ? end - last_selected.x : last_selected.x, last_selected.y);
							cv::circle(frameBGR, new_point, 4, dir ? cv::Scalar(255, 255, 100) : cv::Scalar(0, 20, 100), 10);
							points.push_back(new_point);
							dx = last_selected.x - outer_start;
							if (points.size() > 7) {
								break2 = true;
								break;
							}
						}
						else {
							// to big gap, either restart or stop
							if (points.size() < 3) {
								points.clear();
								cv::Point2i last_canditate(-1, -1);
								cv::Point2i last_selected(-1, -1);
								//break;
							}
							else {
								break2 = true;
								break;
							}
						}

					}
					{
						last_canditate = cv::Point2i(x, y);
						last_selected = cv::Point2i(inner_start, y);
						//cv::Point2i new_point = cv::Point2i(dir ? end - last_selected.x : last_selected.x, last_selected.y);
						//cv::circle(frameBGR, new_point, 1, !dir ? cv::Scalar(255, 255, 100) : cv::Scalar(0, 20, 100), 10);
					}
					break;
				}
				else if (inner_start > 0 && x - inner_start > 10 && innerThresholded.ptr<uchar>(y)[dir ? end -x : x] == 0) {
					inner_end = x;
				}
				else if (outer_end > 0 && x - outer_end < 150 && inner_start < 0 && innerThresholded.ptr<uchar>(y)[dir ? end -x : x] == 255) {
					inner_start = x;
				}
				else if (outer_end < 0 && outer_start > 0 && x - outer_start > 10 && outerThresholded.ptr<uchar>(y)[dir ? end -x : x] == 0) {
					outer_end = x;
				}
				else if (outer_start < 0 && outerThresholded.ptr<uchar>(y)[dir ? end -x : x] == 255) {
					outer_start = x;
				}
			}
			if (break2) break;
		}
		if (points.size() > 3) {
			cv::Vec4f newLine;
			cv::fitLine(points, newLine, CV_DIST_L2, 0, 0.1, 0.1);
			drawLine(frameBGR, frameHSV, newLine,1, cv::Scalar(0, 255*(dir+0.3), 0));
		}
		//break;
	}

	/*
	cv::imshow("io", innerThresholded + outerThresholded);
	cv::imshow("i", innerThresholded);
	cv::imshow("o", outerThresholded);
	*/

}

ObjectPosition ObjectFinder::ConvertPixelToRealWorld(const cv::Point2i &point, const cv::Point2i &frame_size)
{
	if (point.y >= 0 && point.x >= 0 && point.y < frame_size.y && point.x < frame_size.x){//If there is no object found

	}

	const cv::Point2d center (frame_size.x / 2.0, frame_size.y / 2.0);
	//Calculating distance
	double angle = (Vfov * (point.y - center.y) / center.y) + CamAngleDev;
	double distance = CamHeight / tan(angle * PI / 180);
	//Calculating horizontal deviation
	double hor_space = tan(Hfov)*distance;
	double HorizontalDev = (hor_space * (point.x - center.x) / center.x);
	double Hor_angle = atan(HorizontalDev / distance)* 180/PI;
	/*
	if (Hor_angle > 0){
		Hor_angle = 360 - Hor_angle;
	}
	Hor_angle = abs(Hor_angle);
	*/
	return{ distance, HorizontalDev, Hor_angle };
}

void ObjectFinder::WriteInfoOnScreen(const ObjectPosition &info){
	cv::Mat infoWindow(100, 250, CV_8UC3, cv::Scalar::all(0));
	std::ostringstream oss;
	oss << "Distance :" << info.distance;
	cv::putText(infoWindow, oss.str(), cv::Point(20, 20), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(255, 255, 255));
	oss.str("");
	oss << "Horizontal Dev :" << info.horizontalDev;
	cv::putText(infoWindow, oss.str(), cv::Point(20, 50), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(255, 255, 255));
	oss.str("");
	oss << "Horizontal angle :" << info.horizontalAngle;
	cv::putText(infoWindow, oss.str(), cv::Point(20, 80), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(255, 255, 255));
	cv::namedWindow("Info Window");
	cv::imshow("Info Window", infoWindow);
	return;
}


#include <cstdio>
#include <cmath>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <map>
#include <cfloat>

// Eigen
#include <Eigen/Dense>

// Boost
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

/** \brief Class representing a point cloud in 3D. */
class PointCloud {
public:
  /** \brief Constructor. */
  PointCloud() {

  }

  /** \brief Copy constructor.
   * \param[in] point_cloud point cloud to copy
   */
  PointCloud(const PointCloud &point_cloud) {
    this->points.clear();

    for (unsigned int i = 0; i < point_cloud.points.size(); i++) {
      this->points.push_back(point_cloud.points[i]);
    }
  }

  /** \brief Destructor. */
  ~PointCloud() {

  }

  /** \brief Read point cloud from txt file.
   * \param[in] filepath path to file to read
   * \param[out] point_cloud
   * \return success
   */
  static bool from_txt(const std::string &filepath, PointCloud &point_cloud) {
    std::ifstream file(filepath.c_str());
    std::string line;
    std::stringstream ss;

    std::getline(file, line);
    ss << line;

    int n_points = 0;
    ss >> n_points;

    if (n_points < 0) {
      return false;
    }

    for (int i = 0; i < n_points; i++) {
      std::getline(file, line);

      ss.clear();
      ss.str("");
      ss << line;

      Eigen::Vector3f point(0, 0, 0);
      ss >> point(0);
      ss >> point(1);
      ss >> point(2);

      point_cloud.add_point(point);
    }

    return true;
  }

  /** \brief Add a point to the point cloud.
   * \param[in] point point to add
   */
  void add_point(const Eigen::Vector3f &point) {
    this->points.push_back(point);
  }

  /** \brief Get number of points.
   * \return number of points
   */
  unsigned int num_points() const {
    return this->points.size();
  }

private:
  /** \brief The points of the point cloud. */
  std::vector<Eigen::Vector3f> points;

};

int main(int argc, char** argv) {
  boost::program_options::options_description desc("Allowed options");
  desc.add_options()
      ("help", "produce help message")
      ("input",  boost::program_options::value<std::string>(), "path to input OFF file");

  boost::program_options::positional_options_description positionals;
  positionals.add("input", 1);

  boost::program_options::variables_map parameters;
  boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(desc).positional(positionals).run(), parameters);
  boost::program_options::notify(parameters);

  if (parameters.find("help") != parameters.end()) {
    std::cout << desc << std::endl;
    return 1;
  }

  boost::filesystem::path input(parameters["input"].as<std::string>());
  if (!boost::filesystem::is_regular_file(input)) {
    std::cout << "Input file does not exist." << std::endl;
    return 1;
  }

  PointCloud point_cloud;
  PointCloud::from_txt(input.string(), point_cloud);

  std::cout << "Read " << input << "." << std::endl;
  std::cout << point_cloud.num_points() << " points." << std::endl;

  return 0;
}


#include <interpreter/svg/reader.h>
#include <interpreter/svg/document.h>
#include <interpreter/svg/segment.h>

#include <boost/lexical_cast.hpp>
#include <boost/tokenizer.hpp>
#include <boost/algorithm/string.hpp>

#include <pugixml.hpp>

#include <functional>

namespace svg
{
	typedef double raw_coordinate;
	typedef raw_coordinate raw_coordinate_interval;

	struct path_element
	{
		virtual bool is_path_start()
		{
			return false;
		}

		virtual std::shared_ptr<svg::edge> make_edge(svg::point& start, svg::point& end) = 0;

		virtual svg::location endpoint() = 0;
	};

	struct path_starting_point : public path_element
	{
		bool is_path_start()
		{ return true; }

		svg::location endpoint()
		{
			return loc;
		}

		std::shared_ptr<svg::edge> make_edge(svg::point&, svg::point&)
		{
			assert(0);
			return nullptr;
		}

		svg::location loc;
	};

	struct straight_line : public path_element
	{
		svg::location endpoint()
		{
			return endpoint_;
		}

		std::shared_ptr<svg::edge> make_edge(svg::point& start, svg::point& end)
		{
			return std::make_shared<svg::segment>(start, end);
		}

		svg::location endpoint_;
	};
}

namespace
{
	void parse_segment(const pugi::xml_node& line_node, svg::document& doc)
	{
		std::string x_start = line_node.attribute("x1").value();
		std::string y_start = line_node.attribute("y1").value();
		std::string x_end = line_node.attribute("x2").value();
		std::string y_end = line_node.attribute("y2").value();

		svg::point p_start(svg::location(boost::lexical_cast<svg::raw_coordinate_interval>(x_start), (boost::lexical_cast<svg::raw_coordinate_interval>(y_start))));
		svg::point p_end(svg::location(boost::lexical_cast<svg::raw_coordinate_interval>(x_end), (boost::lexical_cast<svg::raw_coordinate_interval>(y_end))));

		doc.add_point(p_start);
		doc.add_point(p_end);

		auto edge = std::make_shared<svg::segment>(doc.points().at(p_start.loc()), doc.points().at(p_end.loc()));
		doc.add_segment(p_start.loc(), p_end.loc());
	}

	typedef boost::char_separator<char> svg_separator;
	typedef std::unique_ptr<svg::path_element> raw_path;
	typedef std::function<std::vector<raw_path>(std::string, const std::vector<raw_path>&)> path_element_creator;

	struct absolute_location
	{
		static svg::location get_starting_point(const std::vector<raw_path>&)
		{
			return svg::location(0, 0);
		}
	};

	struct relative_location
	{
		static svg::location get_starting_point(const std::vector<raw_path>& history)
		{
			if (history.empty())
				return svg::location(0, 0);

			return history.back()->endpoint();
		}
	};

	std::vector<svg::raw_coordinate> fetch_params(const std::string& serialized, std::size_t param_count, const std::string& symbols)
	{
		svg_separator separator((symbols + " ,;").c_str(), "", boost::drop_empty_tokens);
		boost::tokenizer<svg_separator> tokens(serialized, separator);

		std::vector<svg::raw_coordinate> coordinates;
		for (boost::tokenizer<svg_separator>::iterator it = tokens.begin(); it != tokens.end(); ++it)
			coordinates.push_back(boost::lexical_cast<svg::raw_coordinate>(*it));

		if (coordinates.size() % param_count != 0 && !coordinates.empty())
			throw std::logic_error("Invalid number of coordinates");

		return coordinates;
	}

	template <typename location_policy>
	std::vector<raw_path> move_to(const std::string& serialized, const std::vector<raw_path>& history)
	{
		auto coordinates = fetch_params(serialized, 2, "Mm");
		std::unique_ptr<svg::path_starting_point> path_start(std::make_unique<svg::path_starting_point>());
		svg::location start_point = location_policy::get_starting_point(history);
		path_start->loc.x = coordinates[0] + start_point.x;
		path_start->loc.y = coordinates[1] + start_point.y;

        std::vector<raw_path> res;
		res.push_back(std::move(path_start));
		for (std::size_t i = 2; i < coordinates.size(); i += 2)
		{
			auto next_line = std::make_unique<svg::straight_line>();
			next_line->endpoint_.x = coordinates[i] + res.back()->endpoint().x;
			next_line->endpoint_.y = coordinates[i + 1] + res.back()->endpoint().y;
			res.push_back(std::move(next_line));
		}

		return res;
	}

	template <typename location_policy>
	std::vector<raw_path> straight_line(std::string serialized, const std::vector<raw_path>& history)
	{
		std::vector<svg::raw_coordinate> coordinates = fetch_params(serialized, 2, "Ll");

		svg::location start_point = location_policy::get_starting_point(history);
		std::vector<raw_path> res;

		auto start = std::make_unique<svg::path_starting_point>();
		start->loc.x = start_point.x;
		start->loc.y = start_point.y;

		res.push_back(std::move(start));
		for (std::size_t i = 0; i < coordinates.size(); i += 2)
		{
			auto next_line = std::make_unique<svg::straight_line>();
			next_line->endpoint_.x = coordinates[i] + res.back()->endpoint().x;
			next_line->endpoint_.y = coordinates[i + 1] + res.back()->endpoint().y;
			res.push_back(std::move(next_line));
		}

		res.erase(res.begin());
		return res;
	}

	std::vector<raw_path> close_path(std::string, const std::vector<raw_path>& history)
	{
		if (history.size() < 2)
			throw std::logic_error("Too few elements for closing path");

		std::unique_ptr<svg::straight_line> line(std::make_unique<svg::straight_line>());
		line->endpoint_ = history.front()->endpoint();

		std::vector<raw_path> res;
		res.push_back(std::move(line));
		return res;
	}

	std::map<char, path_element_creator> get_path_parsers()
	{
		std::map<char, path_element_creator> parsers;
		parsers.insert(std::make_pair('M', move_to<absolute_location>));
		parsers.insert(std::make_pair('m', move_to<relative_location>));
    	parsers.insert(std::make_pair('L', straight_line<absolute_location>));
		parsers.insert(std::make_pair('l', straight_line<relative_location>));
		parsers.insert(std::make_pair('Z', close_path));
		parsers.insert(std::make_pair('z', close_path));

		return parsers;
	}

	bool is_next_path(const std::string& s)
	{
		return s[0] == 'm' || s[0] == 'M';
	}

	void parse_path(const pugi::xml_node& path_node, svg::document& doc)
	{
		auto path_parsers = get_path_parsers();

		std::string textual_representation = path_node.attribute("d").value();

		svg_separator separator("", "MmLlHhVvCcSsQqTtAaZz", boost::drop_empty_tokens);
		boost::tokenizer<svg_separator> tokens(textual_representation, separator);

		std::vector<std::string> splitted_into_tokens;
		for (boost::tokenizer<svg_separator>::iterator it = tokens.begin(); it != tokens.end(); ++it)
			splitted_into_tokens.push_back(*it);

		std::vector<std::vector<std::unique_ptr<svg::path_element> > > elements(1);
		std::size_t current_path_number = 0;
		for (std::size_t i = 0; i < splitted_into_tokens.size(); ++i)
		{
			std::string trimmed(boost::algorithm::trim_copy(splitted_into_tokens[i]));
			if (i < splitted_into_tokens.size() - 1)
				trimmed += splitted_into_tokens[++i];

			std::size_t k = 0;
			auto next_elements = path_parsers[trimmed[0]](trimmed, elements[current_path_number]);
			if (next_elements[0]->is_path_start())
			{
				elements.push_back(std::vector<std::unique_ptr<svg::path_element> >());
				++current_path_number;
				++k;
			}

			for(auto& elem : next_elements)
				elements[current_path_number].push_back(std::move(elem));
		}

		for (auto& single_path : elements)
		{
			svg::location previous_location;
			for (auto& elem : single_path)
			{
				if (elem->is_path_start())
					doc.add_point(svg::point(elem->endpoint()));

				else
					doc.add_segment(previous_location, elem->endpoint());

				previous_location = elem->endpoint();
			}

		}
	}

	const char* ignored_tags[] = { "metadata",
		"sodipodi:namedview",
		"defs",
		"" };

	bool ignored(const char* node_name)
	{
		for (const char** tag = ignored_tags; (**tag) != 0; ++tag)
		{
			if (std::strcmp(node_name, *tag) == 0)
				return true;
		}

		return false;
	}
}

namespace svg
{
	class reader::impl
	{
	public:
		impl()
		{
			parsers.insert(std::make_pair("line", parse_segment));
			parsers.insert(std::make_pair("path", parse_path));
		}

		void parse_all(const pugi::xml_node& root, svg::document& svg_doc)
		{
			for (auto node = root.first_child(); node; node = node.next_sibling())
			{
				if (std::strcmp(node.name(), "svg") == 0 || std::strcmp(node.name(), "g") == 0)
					parse_all(node, svg_doc);
				else if(!ignored(node.name()))
					parsers[node.name()](node, svg_doc);
			}
		}

		std::map < std::string, std::function<void(const pugi::xml_node&, document&)> > parsers;
	};

	reader::reader() : pimpl(std::make_unique<impl>())
	{}

	reader::~reader()
	{}

	std::unique_ptr<document> reader::parse(const std::string& str)
	{
		if (str.empty())
			return nullptr;

		pugi::xml_document xml_doc;

		if (!xml_doc.load(str.c_str()))
			throw std::runtime_error("Cannot load given svg file");

		auto width = xml_doc.child("svg").attribute("width");
		auto height = xml_doc.child("svg").attribute("height");
		auto svg_doc = std::make_unique<document>(width.as_double(), height.as_double());

		pimpl->parse_all(xml_doc.document_element(), *svg_doc);

		return svg_doc;
	}
}


#include "opcodes.h"

#include <fstream>
#include <iostream>
#include <sstream>
#include <vector>
#include <cmath>

#include <boost/multiprecision/cpp_int.hpp>

#define	OCT_DIGITS	"01234567"
#define	DEC_DIGITS	"0123456789"
#define	HEX_DIGITS	"0123456789ABCDEFabcdef"

using namespace std;
using namespace boost::multiprecision;

bool is_integer(const string& s)
{
	if (s.substr(0, 2) == "0x")
		return !s.empty() && s.substr(2).find_first_not_of(HEX_DIGITS) == string::npos;
	else if (s[0] == '0')
		return !s.empty() && s.find_first_not_of(OCT_DIGITS) == string::npos;
	else
		return !s.empty() && s.find_first_not_of(DEC_DIGITS) == string::npos;

}

int main()
{
	ifstream in("in", ios::binary);
	ofstream out("out");
	vector<string> t;

	cpp_int n;
	vector<unsigned char> nv;
	string ns;
	uint32_t nbytes;

	for (string ins; in >> ins;) {
		if (is_integer(ins)) {
			n = cpp_int(ins);
			export_bits(n, back_inserter(nv), 8);
			ns = string(nv.begin(), nv.end());
			nbytes = nv.size();

			if (n == 0)
				out << opcodes["OP_0"];
			else if (n <= 16)
				out << nv[0] + 0x50;
			else if (nbytes <= 0x4b)
				out << (char*) &nbytes << ns;
			else if (nbytes <= 0xff)
				out << opcodes["OP_PUSHDATA1"] << (char*) &nbytes << ns;
			else if (nbytes <= 0xffff)
				out << opcodes["OP_PUSHDATA2"] << (char*) &nbytes << ns;
			else if (nbytes <= 0xffffffff)
				out << opcodes["OP_PUSHDATA4"] << (char*) &nbytes << ns;
		} else {
			out << opcodes[ins];
		}
	}

	return (0);
}


#ifndef SIMPLE_SHORTEST_BASIS_HPP
#define SIMPLE_SHORTEST_BASIS_HPP 1
/**
 * @file simple_shortest_basis.hpp
 *
 * @brief calculate equidistribution property of the random number generator.
 *
 * calculate shortest basis of lattice. We can get the dimension of
 * equidistribution from the smallest norm of the vectors in lattice
 * basis.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2011 Mutsuo Saito, Makoto Matsumoto and
 * Hiroshima University. All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <tr1/memory>
#include <stdexcept>
#include <NTL/GF2.h>

namespace MTToolBox {
    /**
     * @class linear_generator_vector
     * @brief a pseudo random number generator as a vector
     *
     * This class is a pseudo random number
     * generator as a vector (and polynomial).  As a polynomial, Only
     * multiple by \b x is supported.
     *
     * @tparam G F2-linear generator
     * @tparam T the output type of \b G.
     */
    template<typename G, typename T> class linear_generator_vector {
    public:
        /**
         * constructor
         *
         * @param rand_ random number generator whose state transition
         * function is F2-linear.
         */
        linear_generator_vector<G, T>(const G& rand_) {
            using namespace std::tr1;
            shared_ptr<G> r(new G(rand_));
            rand = r;
            rand->seeding(1);
            count = 0;
            zero = false;
            next = 0;
        }

        /**
         * constructor of a vector in standard basis
         *
         * @param rand_ random number generator used as a template
         * @param bit_pos position of 1 for standard basis.
         */
        linear_generator_vector<G, T>(const G& rand_, int bit_pos) {
            using namespace std::tr1;
            shared_ptr<G> r(new G(rand_));
            rand = r;
            rand->make_zero_status();
            count = 0;
            zero = false;
            next = static_cast<T>(1) << (sizeof(T) * 8 - bit_pos - 1);
        }

        void add(const linear_generator_vector<G, T>& src);
        void next_state(int bit_len);
        void debug_print();

        /**
         * a pseudo random number generator whose state transition
         * function is F2-linear.
         */
        std::tr1::shared_ptr<G> rand;

        /**
         * The counter which shows how many times next_state() is called.
         * This is concerned with norm of vector and degree of polynomial.
         */
        int count;

        /**
         * This shows the vector is zero or not.
         */
        bool zero;

        /**
         * This is important.
         * v-bit MSBs of generator's recent output.
         * Or, the coefficent of highest degree term of polynomial.
         */
        T next;
    };

    /**
     * @class shotest_basis
     * @brief calculate shortest basis of lattice
     *
     * This class calculates shortest basis of lattice,
     * or equidistribution properties of random number generators.
     * This class is most complex part of this system.
     *
     * @tparam G a linear generator vector
     * @tparam T output type of the generator
     */
    template<typename G, typename T> class shortest_basis {
        typedef linear_generator_vector<G, T> linear_vec;
    public:
        /**
         * constructor
         *
         * @param rand a pseudo random number generator whose state
         * transition function is F2-linear.
         * @param bit_len_ from \b bit_len to 1 of dimensions of
         * equidistribution with v-bit accuracy are calculated.
         */
        shortest_basis(const G& rand, int bit_len_) {
            bit_len = bit_len_;
            size = bit_len + 1;
            basis = new linear_vec * [size];
            mexp = rand.get_mexp();
            for (int i = 0; i < bit_len; i++) {
                basis[i] = new linear_vec(rand, i);
            }
            basis[bit_len] = new linear_vec(rand);
            basis[bit_len]->next_state(bit_len);
        }
        /**
         * The destructor.
         */
        ~shortest_basis() {
            for (int i = 0; i < size; i++) {
                delete basis[i];
            }
            delete[] basis;
        }

        int get_all_equidist(int veq[]);
        int get_equidist(int *sum_equidist);
    private:
        int get_equidist_main(int bit_len);
        void adjust(int new_len);
        /** basis of lattice plus one vector */
        linear_vec **basis;
        /** bit lenght count from MSB */
        int bit_len;
        /** Mersenne Exponent, or max value of the dimension of
         * equidistribution. */
        int mexp;
        /** */
        int size;
    };

    /**
     * Adjust bit_len and recalculate the coefficient of the highest
     * degree term.
     *
     * @param new_len a bit length to be changed to.
     */
    template<typename G, typename T>
    void shortest_basis<G, T>::adjust(int new_len) {
        using namespace std;

        T mask = (~static_cast<T>(0)) << (sizeof(T) * 8 - new_len);
#if 0
        if (basis[basis.size() - 1]->zero) {
            basis.erase(basis.begin() + basis.size() - 1);
        } else {
            cerr << "no zero state" << endl;
            throw new logic_error("no zero state");
        }
#endif
        for (int i = 0; i < size; i++) {
            basis[i]->next = basis[i]->next & mask;
            if (basis[i]->next == 0) {
                basis[i]->next_state(new_len);
            }
        }
    }

    /**
     * print some information for debug.
     */
    template<typename G, typename T>
    void linear_generator_vector<G, T>::debug_print() {
        using namespace std;

        cout << "debug ====" << endl;
        cout << "count = " << dec << count << endl;
        cout << "zero = " << zero << endl;
        cout << "next = " << hex << next << endl;
        cout << "debug ====" << endl;
        //rand->debug_print();
    }

    /**
     * calculate the dimensions of equidistribution with v-bit
     * accuracy, where v is form 1 to \b bit_len
     *
     * @param[out] veq array of dimensions of equidistribution at v
     * @return sum of the differences between the theoretical
     * upper bounds and the dimensions.
     */
    template<typename G, typename T>
    int shortest_basis<G, T>::get_all_equidist(int veq[]) {
        using namespace std;

        int sum = 0;

        veq[bit_len - 1] = get_equidist_main(bit_len);
#ifdef DEBUG
        for (int i = 0; i < size; i++) {
            basis[i]->debug_print();
        }
#endif
        sum += mexp / bit_len - veq[bit_len - 1];
        bit_len--;
        for (; bit_len >= 1; bit_len--) {
            adjust(bit_len);
            veq[bit_len - 1] = get_equidist_main(bit_len);
            sum += mexp / bit_len - veq[bit_len - 1];
        }
        return sum;
    }

    /**
     * calculate the dimension of equidistribution with \b bit_len
     * accuracy, and additionally sum of the differences between the
     * theoretical upper bounds and veqs from veq is 1 to \b bit_len -1
     *
     * @param sum_equidist sum of the differences
     * @return the dimension of equidistribution at \b bit_len
     */
    template<typename G, typename T>
    int shortest_basis<G, T>::get_equidist(int *sum_equidist) {
        using namespace std;

        int veq = get_equidist_main(bit_len);
        int sum = 0;
        bit_len--;
        for (; bit_len >= 1; bit_len--) {
            adjust(bit_len);
            sum += mexp / bit_len - get_equidist_main(bit_len);
        }
        *sum_equidist = sum;
        return veq;
    }

    /**
     * addition of vectors
     * @param src a vector which is added to this.
     */
    template<typename G, typename T>
    void linear_generator_vector<G, T>::add(
        const linear_generator_vector<G, T>& src) {
        using namespace std;

        rand->add(*src.rand);
        next ^= src.next;
    }

    /**
     * transfer to the next state or n-th next state so that the
     * coeffcient of the maximum degree term should be non-zero. If internal
     * state is all zero, then set zero flag.
     *
     * @param bit_len bit length from MSB
     */
    template<typename G, typename T>
    void linear_generator_vector<G, T>::next_state(int bit_len) {
        using namespace std;

        if (zero) {
            return;
        }
        int zero_count = 0;
        next = rand->generate(bit_len);
        count++;
        while (next == 0) {
            zero_count++;
            if (zero_count > rand->get_mexp() * 2) {
                zero = true;
                if (rand->is_zero()) {
                    zero = true;
                }
                break;
            }
            next = rand->generate(bit_len);
            count++;
        }
    }

    /**
     * Calculate dimension of equidistirbution with v bit accuracy for
     * one v.
     *
     * In this function, \b pivot_index is a important variable.  \b
     * pivot_index shows the position of the first bit which is one in
     * \b next of the vector of the last element of \b basis. And all
     * vectors in \b basis but last are sorted by pivot_index, so the
     * pivot_index of a vector of \b basis[0] is zero.
     *
     * @param bit_len bit length from MSB, so bit_Len is v.
     */
    template<typename G, typename T>
    int shortest_basis<G, T>::get_equidist_main(int bit_len) {
        using namespace std;
        using namespace NTL;

        int pivot_index;
        int old_pivot = 0;

        pivot_index = calc_1pos(basis[bit_len]->next);
        while (!basis[bit_len]->zero) {
#ifdef DEBUG
            if (pivot_index != calc_1pos(basis[pivot_index]->next)) {
                cerr << "pivot error 1" << endl;
                cerr << "pivot_index:" << dec << pivot_index << endl;
                cerr << "calc_1pos:" << dec
                     << calc_1pos(basis[pivot_index]->next) << endl;
                cerr << "next:" << hex << basis[pivot_index]->next << endl;
                throw new std::logic_error("pivot error 1");
            }
#endif
            if (basis[bit_len]->count > basis[pivot_index]->count) {
                swap(basis[bit_len], basis[pivot_index]);
            }
            basis[bit_len]->add(*basis[pivot_index]);
            if (basis[bit_len]->next == 0) {
                basis[bit_len]->next_state(bit_len);
                pivot_index = calc_1pos(basis[bit_len]->next);
            } else {
                old_pivot = pivot_index;
                pivot_index = calc_1pos(basis[bit_len]->next);
                if (old_pivot <= pivot_index) {
                    cerr << "pivot error 2" << endl;
                    throw new std::logic_error("pivot error 2");
                }
            }
        }
        int min_count = basis[0]->count;
        for (int i = 1; i < bit_len; i++) {
            if (min_count > basis[i]->count) {
                min_count = basis[i]->count;
            }
        }
        if (min_count > mexp / bit_len) {
            cout << "over theoretical bound " << bit_len << endl;
            for(int i = 0; i < size; i++) {
                basis[i]->debug_print();
            }
            throw new std::logic_error("over theoretical bound");
        }
        return min_count;
    }
}
//  LocalWords:  equidistribution param endl NTL
#endif


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

  @copyright 2012-2015 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate the constant ten.

    @return The Ten constant for the proper type
  **/
  template<typename T> T Ten();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant ten.

      @return The Ten constant for the proper type
    **/
    const boost::dispatch::functor<tag::ten_> ten = {};
  }
} }
#endif

#include <boost/simd/constant/definition/ten.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


#ifndef ODEINT_ANYODE_H_6D2AAAD4880011E6AC5C734FA77443A3
#define ODEINT_ANYODE_H_6D2AAAD4880011E6AC5C734FA77443A3

#include <limits>
#include <string>
#include <unordered_map>
#include <chrono>
#include <iostream>
#include <sstream>
#include <vector>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/odeint.hpp>

#include <anyode/anyode.hpp>

#if !defined(PYODEINT_NO_BOOST_CHECK)
  #if BOOST_VERSION / 100000 == 1
    #if BOOST_VERSION / 100 % 1000 == 60
      #error "Boost v 1.60 has a bug in rosenbrock stepper (see https://github.com/headmyshoulder/odeint-v2/issues/189) set PYODEINT_NO_BOOST_CHECK to ignore"
    #endif
    #if BOOST_VERSION / 100 % 1000 == 61
      #error "Boost v 1.61 has a bug in rosenbrock stepper (see https://github.com/headmyshoulder/odeint-v2/issues/189) set PYODEINT_NO_BOOST_CHECK to ignore"
    #endif
    #if BOOST_VERSION / 100 % 1000 == 62
      #error "Boost v 1.62 has a bug in rosenbrock stepper (see https://github.com/headmyshoulder/odeint-v2/issues/189) set PYODEINT_NO_BOOST_CHECK to ignore"
    #endif
  #endif
#endif

namespace {
    class StreamFmt
    {
        std::stringstream m_s;
    public:
        StreamFmt() {}
        ~StreamFmt() {}

        template <typename T>
        StreamFmt& operator << (const T& v) {
            this->m_s << v;
            return *this;
        }

        std::string str() const {
            return this->m_s.str();
        }
        operator std::string() const {
            return this->m_s.str();
        }

    };
}

namespace odeint_anyode{
    using namespace std::placeholders;

    using boost::numeric::odeint::integrate_adaptive;
    using boost::numeric::odeint::make_dense_output;
    using boost::numeric::odeint::rosenbrock4;
    using boost::numeric::odeint::runge_kutta_dopri5;
    using boost::numeric::odeint::bulirsch_stoer_dense_out;

    // value_type is hardcoded to double at the moment
    using value_type = double;
    using vector_type = boost::numeric::ublas::vector<value_type>;
    using matrix_type = boost::numeric::ublas::matrix<value_type>;

    // using OdeSys_t = AnyODE::OdeSysBase;

    enum class StepType : int { bulirsch_stoer, rosenbrock4, dopri5 };

    StepType styp_from_name(std::string name){
        if (name == "bulirsch_stoer")
            return StepType::bulirsch_stoer;
        else if (name == "rosenbrock4")
            return StepType::rosenbrock4;
        else if (name == "dopri5")
            return StepType::dopri5;
        else
            throw std::runtime_error(StreamFmt() << "Unknown stepper type name: " << name);
    }

    bool requires_jacobian(StepType styp){
        if (styp == StepType::rosenbrock4)
            return true;
        else
            return false;
    }

    vector_type vec_from_ptr(const value_type * const arr, std::size_t len){
        vector_type vec(len);
        for (std::size_t i=0; i<len; ++i)
            vec[i] = arr[i];
        return vec;
    }

    // Integr will be specialzed for: rosenbrock, dopri5 and bulrisch-stoer
    // adaptive and predefined cannot be put here since make_dense_output
    // is a function and bulirsch_stoer_dense_out is a class
    template<class OdeSys>
    struct Integr {
        OdeSys * m_odesys;
        double m_time_cpu = -1.0, m_time_wall = -1.0;
        value_type m_dx0, m_dx_max, m_atol, m_rtol;
        StepType m_styp;
        long int m_mxsteps;
        int m_autorestart;
        long int m_nsteps;
        bool m_return_on_error;

        void rhs(const vector_type &yarr, vector_type &dydx, value_type xval);
        void jac(const vector_type & yarr, matrix_type &Jmat,
                 const value_type & xval, vector_type &dfdx);
        Integr(OdeSys * odesys, value_type dx0, value_type dx_max, value_type atol, value_type rtol, StepType styp,
               long int mxsteps, int autorestart=0, bool return_on_error=false) :
            m_odesys(odesys), m_dx0(dx0), m_dx_max(dx_max), m_atol(atol), m_rtol(rtol), m_styp(styp),
            m_mxsteps(mxsteps), m_autorestart(autorestart), m_return_on_error(return_on_error) {}

        std::pair<std::vector<value_type>, std::vector<value_type> >
        adaptive(const value_type x0,
                 const value_type xend,
                 const value_type * const ANYODE_RESTRICT y0){
            std::time_t cputime0 = std::clock();
            auto t_start = std::chrono::high_resolution_clock::now();
            std::pair<std::vector<value_type>, std::vector<value_type> > result;
            try{
                if ( m_styp == StepType::bulirsch_stoer ) {
                    this->adaptive_bulirsch_stoer(x0, xend, y0);
                } else if ( m_styp == StepType::dopri5 ) {
                    this->adaptive_dopri5(x0, xend, y0);
                } else if ( m_styp == StepType::rosenbrock4 ) {
                    this->adaptive_rosenbrock4(x0, xend, y0);
                } else {
                    goto impossible_adaptive;
                }
            } catch (const std::exception& e) {
                if (m_autorestart > 0){
                    std::cerr << e.what() << std::endl;
                    if (this->m_xout.size() > 0){
                        std::cerr << "odeint_anyode.hpp:" << __LINE__ << ": Autorestart (" << m_autorestart
                                  << ") x=" << this->m_xout.back() << "\n";
                        m_autorestart--;
                        auto c_nsteps = this->m_nsteps;
                        auto c_xout = this->m_xout;
                        auto c_yout = this->m_yout;
                        adaptive(c_xout.back(), xend, &c_yout[c_yout.size() - m_odesys->get_ny()]);
                        c_xout.insert(c_xout.end(), m_xout.begin(), m_xout.end());
                        c_yout.insert(c_yout.end(), m_yout.begin(), m_yout.end());
                        m_xout = c_xout;
                        m_yout = c_yout;
                        m_nsteps += c_nsteps;
                    } else {
                        std::cerr << "odeint_anyode.hpp:" << __LINE__ << ": Autorestart failed." << "\n";
                        if (!m_return_on_error)
                            throw;
                    }
                } else {
                    if (!m_return_on_error)
                        throw;
                }
            }
            this->m_time_cpu = (std::clock() - cputime0) / (double)CLOCKS_PER_SEC;
            this->m_time_wall = std::chrono::duration<double>(
                std::chrono::high_resolution_clock::now() - t_start).count();
            return std::make_pair(this->m_xout, this->m_yout);
        impossible_adaptive:
            throw std::runtime_error("Impossible: unknown StepType!");
        }

        int predefined(const int nx,
                       const value_type * const ANYODE_RESTRICT xout,
                       const value_type * const ANYODE_RESTRICT y0,
                       value_type * const ANYODE_RESTRICT yout){
            int nreached;
            std::time_t cputime0 = std::clock();
            auto t_start = std::chrono::high_resolution_clock::now();
            std::copy(y0, y0 + (this->m_odesys->get_ny()), yout);
            try {
                if ( m_styp == StepType::bulirsch_stoer ) {
                    this->predefined_bulirsch_stoer(nx, xout, y0, yout, &nreached);
                } else if ( m_styp == StepType::dopri5 ) {
                    this->predefined_dopri5(nx, xout, y0, yout, &nreached);
                } else if ( m_styp == StepType::rosenbrock4 ) {
                    this->predefined_rosenbrock4(nx, xout, y0, yout, &nreached);
                } else {
                    goto impossible_predefined;
                }
            } catch (const std::exception& e) {
                if (m_autorestart > 0){
                    std::cerr << e.what() << std::endl;
                    if (this->m_xout.size() > 0) {
                        std::cerr << "odeint_anyode.hpp:" << __LINE__ << ": Autorestart (" << m_autorestart
                                  << ") x=" << this->m_xout.back() << "\n";
                        m_autorestart--;
                        auto c_nsteps = this->m_nsteps;
                        nreached += predefined(nx - nreached, xout + nreached,
                                               yout + nreached*m_odesys->get_ny(),
                                               yout + nreached*m_odesys->get_ny());
                        this->m_nsteps += c_nsteps;
                    } else {
                        std::cerr << "odeint_anyode.hpp:" << __LINE__ << ": Autorestart failed." << "\n";
                        if (!m_return_on_error)
                            throw;
                    }
                } else {
                    if (!m_return_on_error)
                        throw;
                }
            }
            this->m_time_cpu = (std::clock() - cputime0) / (double)CLOCKS_PER_SEC;
            this->m_time_wall = std::chrono::duration<double>(
                std::chrono::high_resolution_clock::now() - t_start).count();
            return nreached;
        impossible_predefined:
            throw std::runtime_error("Impossible: unknown StepType!");
        }
    private:
        std::vector<value_type> m_xout, m_yout;

        void reset() {
            this->m_nsteps = 0;
            this->m_xout.clear();
            this->m_yout.clear();
        }

        void obs_adaptive(const vector_type &yarr, value_type xval){
            this->m_xout.push_back(xval);
            for(int i=0 ; i < this->m_odesys->get_ny() ; ++i)
                this->m_yout.push_back(yarr[i]);
            if (this->m_nsteps == this->m_mxsteps)
                throw std::runtime_error(StreamFmt() << "Maximum number of steps reached: " << this->m_nsteps);
            m_nsteps++;
        }

        void obs_predefined(const vector_type & /* yarr */, value_type /* xval */){
            if (this->m_nsteps == this->m_mxsteps)
                throw std::runtime_error(StreamFmt() << "Maximum number of steps reached: " << this->m_nsteps);
            m_nsteps++;
        }

        void adaptive_bulirsch_stoer(const value_type x0,
                                     const value_type xend,
                                     const value_type * const ANYODE_RESTRICT y0
                                     ){
            const int ny = this->m_odesys->get_ny();
            auto f = [&](const vector_type &yarr, vector_type &dydx, value_type xval) {
                this->m_odesys->rhs(xval, &(yarr.data()[0]), &(dydx.data()[0]));
            };
            auto stepper = bulirsch_stoer_dense_out< vector_type, value_type >(
                this->m_atol, this->m_rtol, 1.0, 1.0, this->m_dx_max);
            auto y_ = vec_from_ptr(y0, ny);
            this->reset();
            integrate_adaptive(stepper, f, y_, x0, xend, this->m_dx0,
                               std::bind(&Integr::obs_adaptive, this, _1, _2));
        }

        void predefined_bulirsch_stoer(const int nx,
                                       const value_type * const ANYODE_RESTRICT xout,
                                       const value_type * const ANYODE_RESTRICT y0,
                                       value_type * const ANYODE_RESTRICT yout,
                                       int * nreached){
            *nreached = 0;
            const auto ny = this->m_odesys->get_ny();
            vector_type y_ = vec_from_ptr(y0, ny);
            vector_type xout_ = vec_from_ptr(xout, nx);
            auto f = [&](const vector_type &yarr, vector_type &dydx, value_type xval) {
                this->m_odesys->rhs(xval, &(yarr.data()[0]), &(dydx.data()[0]));
            };
            try{
                auto stepper = bulirsch_stoer_dense_out< vector_type, value_type >(
                    this->m_atol, this->m_rtol, 1.0, 1.0, this->m_dx_max);
                for (*nreached=1; *nreached < nx; ++*nreached){
                    const int ix = *nreached;
                    this->reset();
                    integrate_adaptive(stepper, f, y_, xout[ix-1], xout[ix], this->m_dx0,
                                       std::bind(&Integr::obs_predefined, this, _1, _2));
                    for (int iy=0; iy < ny; ++iy)
                        yout[ix*ny + iy] = y_[iy];
                }
            } catch (const std::exception& e) {
                std::cerr << __FILE__ << ":" << __LINE__ << ":";
                std::cerr << e.what() << std::endl;
                nreached--;
                if (!m_return_on_error)
                    throw;
            }
        }

        void adaptive_dopri5(const value_type x0,
                             const value_type xend,
                             const value_type * const ANYODE_RESTRICT y0){
            const int ny = this->m_odesys->get_ny();
            auto f = [&](const vector_type &yarr, vector_type &dydx, value_type xval) {
                this->m_odesys->rhs(xval, &(yarr.data()[0]), &(dydx.data()[0]));
            };

            auto stepper = make_dense_output<runge_kutta_dopri5<vector_type, value_type> >(
                this->m_atol, this->m_rtol, this->m_dx_max);
            auto y_ = vec_from_ptr(y0, ny);
            this->reset();
            integrate_adaptive(stepper, f, y_, x0, xend, this->m_dx0,
                               std::bind(&Integr::obs_adaptive, this, _1, _2));
        }

        void predefined_dopri5(const int nx,
                               const value_type * const ANYODE_RESTRICT xout,
                               const value_type * const ANYODE_RESTRICT y0,
                              value_type * const ANYODE_RESTRICT yout,
                              int * nreached){
            *nreached = 0;
            const auto ny = this->m_odesys->get_ny();
            vector_type y_ = vec_from_ptr(y0, ny);
            vector_type xout_ = vec_from_ptr(xout, nx);
            auto f = [&](const vector_type &yarr, vector_type &dydx, value_type xval) {
                this->m_odesys->rhs(xval, &(yarr.data()[0]), &(dydx.data()[0]));
            };
            try {
                auto stepper = make_dense_output<runge_kutta_dopri5<vector_type, value_type> >(
                    this->m_atol, this->m_rtol, this->m_dx_max);
                for (*nreached=1; *nreached < nx; ++*nreached){
                    const int ix = *nreached;
                    this->reset();
                    integrate_adaptive(stepper, f, y_, xout[ix - 1], xout[ix], this->m_dx0,
                                       std::bind(&Integr::obs_predefined, this, _1, _2));
                    for (int iy=0; iy < ny; ++iy)
                        yout[ix*ny + iy] = y_[iy];
                }
            } catch (const std::exception& e) {
                std::cerr << __FILE__ << ":" << __LINE__ << ":";
                std::cerr << e.what() << std::endl;
                nreached--;
                if (!m_return_on_error)
                    throw;
            }
        }

        void adaptive_rosenbrock4(const value_type x0,
                                  const value_type xend,
                                  const value_type * const ANYODE_RESTRICT y0){
            const int ny = this->m_odesys->get_ny();
            auto f = [&](const vector_type &yarr, vector_type &dydx, value_type xval) {
                this->m_odesys->rhs(xval, &(yarr.data()[0]), &(dydx.data()[0]));
            };
            auto j = [&](const vector_type & yarr, matrix_type &Jmat,
                                     const value_type & xval, vector_type &dfdx) {
                this->m_odesys->dense_jac_rmaj(xval, &(yarr.data()[0]), nullptr, &(Jmat.data()[0]), ny, &(dfdx.data()[0]));
            };
            auto stepper = make_dense_output<rosenbrock4<value_type> >(this->m_atol, this->m_rtol, this->m_dx_max);
            auto y_ = vec_from_ptr(y0, ny);
            this->reset();
            integrate_adaptive(stepper, std::make_pair(f, j), y_, x0, xend, this->m_dx0,
                                              std::bind(&Integr::obs_adaptive, this, _1, _2));
        }

        void predefined_rosenbrock4(const int nx,
                                    const value_type * const ANYODE_RESTRICT xout,
                                    const value_type * const ANYODE_RESTRICT y0,
                                    value_type * const ANYODE_RESTRICT yout,
                                    int * nreached){
            *nreached = 0;
            const auto ny = this->m_odesys->get_ny();
            vector_type y_ = vec_from_ptr(y0, ny);
            vector_type xout_ = vec_from_ptr(xout, nx);
            auto f = [&](const vector_type &yarr, vector_type &dydx, value_type xval) {
                this->m_odesys->rhs(xval, &(yarr.data()[0]), &(dydx.data()[0]));
            };
            auto j = [&](const vector_type & yarr, matrix_type &Jmat,
                                     const value_type & xval, vector_type &dfdx) {
                this->m_odesys->dense_jac_rmaj(xval, &(yarr.data()[0]), nullptr, &(Jmat.data()[0]), ny, &(dfdx.data()[0]));
            };
            try {
                auto stepper = make_dense_output<rosenbrock4<value_type> >(this->m_atol, this->m_rtol, this->m_dx_max);
                for (*nreached=1; *nreached < nx; ++*nreached){
                    const int ix = *nreached;
                    this->reset();
                    integrate_adaptive(stepper, std::make_pair(f, j), y_, xout[ix - 1], xout[ix], this->m_dx0,
                                       std::bind(&Integr::obs_predefined, this, _1, _2));
                    for (int iy=0; iy < ny; ++iy)
                        yout[ix*ny + iy] = y_[iy];
                }
            } catch (const std::exception& e) {
                std::cerr << __FILE__ << ":" << __LINE__ << ":";
                std::cerr << e.what() << std::endl;
                nreached--;
                if (!m_return_on_error)
                    throw;
            }
        }

    };

    template <class OdeSys>
    void set_integration_info(OdeSys * odesys, const Integr<OdeSys>& integrator){
        odesys->current_info.nfo_int["n_steps"] = integrator.m_nsteps;
        odesys->current_info.nfo_int["nfev"] = odesys->nfev;
        odesys->current_info.nfo_int["njev"] = odesys->njev;
        odesys->current_info.nfo_dbl["time_wall"] = integrator.m_time_wall;
        odesys->current_info.nfo_dbl["time_cpu"] = integrator.m_time_cpu;
    }

    template <class OdeSys>
    std::pair<std::vector<double>, std::vector<double> >
    simple_adaptive(OdeSys * const odesys,
                    const double atol,
                    const double rtol,
                    const StepType styp,
                    const double * const y0,
                    const double x0,
                    const double xend,
                    long int mxsteps=0,
                    double dx0=0.0,
                    double dx_max=0.0,
                    int autorestart=0,
                    bool return_on_error=false
                    )
                    //,
                    // const double dx_min=0.0,

                    // long int mxsteps=0)
    {
        if (dx0 == 0.0)
            dx0 = odesys->get_dx0(x0, y0);
        if (dx0 == 0.0){
            if (x0 == 0)
                dx0 = std::numeric_limits<double>::epsilon() * 100;
            else
                dx0 = std::numeric_limits<double>::epsilon() * 100 * x0;
        }
        if (dx_max == 0.0)
            dx_max = odesys->get_dx_max(x0, y0);
        if (mxsteps == 0)
            mxsteps = 500;
        auto integr = Integr<OdeSys>(odesys, dx0, dx_max, atol, rtol, styp, mxsteps, autorestart, return_on_error);
        auto result = integr.adaptive(x0, xend, y0);
        odesys->current_info.clear();
        set_integration_info<OdeSys>(odesys, integr);
        return result;
    }

    template <class OdeSys>
    int simple_predefined(OdeSys * const odesys,
                          const double atol,
                          const double rtol,
                          const StepType styp,
                          const double * const y0,
                          const int nout,
                          const double * const xout,
                          double * const yout,
                          long int mxsteps=0,
                          double dx0=0.0,
                          double dx_max=0.0,
                          int autorestart=0,
                          bool return_on_error=false
                          )
    // const double dx_min=0.0,
    {
        if (dx0 == 0.0)
            dx0 = odesys->get_dx0(xout[0], y0);
        if (dx0 == 0.0){
            if (xout[0] == 0)
                dx0 = std::numeric_limits<double>::epsilon() * 100;
            else
                dx0 = std::numeric_limits<double>::epsilon() * 100 * xout[0];
        }
        if (dx_max == 0.0)
            dx_max = INFINITY;
        if (mxsteps == 0)
            mxsteps = 500;
        auto integr = Integr<OdeSys>(odesys, dx0, dx_max, atol, rtol, styp, mxsteps, autorestart, return_on_error);
        int nreached = integr.predefined(nout, xout, y0, yout);
        odesys->current_info.nfo_int.clear();
        odesys->current_info.nfo_dbl.clear();
        set_integration_info(odesys, integr);
        return nreached;
    }

}

#endif /* ODEINT_ANYODE_H_6D2AAAD4880011E6AC5C734FA77443A3 */


//==================================================================================================
/**
  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_LITERALS_HPP_INCLUDED
#define BOOST_SIMD_LITERALS_HPP_INCLUDED

#include <boost/simd/detail/nsm.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    // Turn '4' into 4
    constexpr int to_int(char c) { return static_cast<int>(c) - 48; }

    // Terminal case for char recursion
    template< std::size_t N>
    constexpr std::uint64_t parse(int,const char (&)[N], nsm::uint64_t<0> const&)
    {
      return 0;
    }

    // Extract a char, get its value, multiply by current base, repeat
    template< std::size_t N, std::size_t I>
    constexpr std::uint64_t parse(int base, const char (&arr)[N], nsm::uint64_t<I> const&)
    {
      return to_int(arr[I - 1]) * base + parse<N>(base*10, arr,nsm::uint64_t<I-1>{});
    }

    // Intermediate trampoline so MSVC doesn't cry
    template<std::size_t N,char... c> constexpr std::uint64_t parse(int base)
    {
      return parse<N>(base, {c...}, nsm::uint64_t<N>{});
    }
  }

  namespace literal
  {
    // Enables xxx_c to be turned into usable integral constant int_<xxx>
    template <char ...c>
    constexpr nsm::uint64_t< boost::simd::detail::parse<sizeof...(c),c...>(1)> operator"" _c()
    {
      return {};
    }
  }
} }

#endif


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_IS_NLEZ_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_IS_NLEZ_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/simd/is_gtz.hpp>
#include <boost/simd/function/simd/is_not_less_equal.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(is_nlez_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE bs::as_logical_t<A0>  operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return is_not_less_equal(a0, Zero<A0>());
      }
   };

   BOOST_DISPATCH_OVERLOAD(is_nlez_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE bs::as_logical_t<A0>  operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return is_gtz(a0);
      }
   };

} } }
#endif


#pragma once

#include <vector>
#include <algorithm>
#include <random>
#include <Eigen/Dense>

namespace cilantro {
    // CRTP base class
    template <class ModelEstimatorT, class ModelT, typename ResidualScalarT, typename IndexT = size_t>
    class RandomSampleConsensusBase {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef ModelT Model;
        typedef ResidualScalarT ResidualScalar;
        typedef std::vector<ResidualScalarT> ResidualVector;
        typedef IndexT Index;
        typedef std::vector<IndexT> IndexVector;

        RandomSampleConsensusBase(size_t sample_size,
                                  size_t inlier_count_thresh,
                                  size_t max_iter,
                                  ResidualScalar inlier_dist_thresh,
                                  bool re_estimate = true)
                : sample_size_(sample_size),
                  inlier_count_thresh_(inlier_count_thresh),
                  max_iter_(max_iter),
                  inlier_dist_thresh_(inlier_dist_thresh),
                  re_estimate_(re_estimate),
                  iteration_count_(0)
        {}

        inline size_t getSampleSize() const { return sample_size_; }

        inline ModelEstimatorT& setSampleSize(size_t sample_size) {
            sample_size_ = sample_size;
            return *static_cast<ModelEstimatorT*>(this);
        }

        inline size_t getTargetInlierCount() const { return inlier_count_thresh_; }

        inline ModelEstimatorT& setTargetInlierCount(size_t inlier_count_thres) {
            inlier_count_thresh_ = inlier_count_thres;
            return *static_cast<ModelEstimatorT*>(this);
        }

        inline size_t getMaxNumberOfIterations() const { return max_iter_; }

        inline ModelEstimatorT& setMaxNumberOfIterations(size_t max_iter) {
            max_iter_ = max_iter;
            return *static_cast<ModelEstimatorT*>(this);
        }

        inline ResidualScalar getMaxInlierResidual() const { return inlier_dist_thresh_; }

        inline ModelEstimatorT& setMaxInlierResidual(ResidualScalar inlier_dist_thresh) {
            inlier_dist_thresh_ = inlier_dist_thresh;
            return *static_cast<ModelEstimatorT*>(this);
        }

        inline bool getReEstimationStep() const { return re_estimate_; }

        inline ModelEstimatorT& setReEstimationStep(bool re_estimate) {
            re_estimate_ = re_estimate;
            return *static_cast<ModelEstimatorT*>(this);
        }

        ModelEstimatorT& estimate() {
            ModelEstimatorT& estimator = *static_cast<ModelEstimatorT*>(this);
            const size_t num_points = estimator.getDataPointsCount();
            if (num_points < sample_size_) sample_size_ = num_points;
            if (inlier_count_thresh_ > num_points) inlier_count_thresh_ = num_points;

            // Initialize index permutation and random engine
            IndexVector perm(num_points);
            for (size_t i = 0; i < num_points; i++) perm[i] = static_cast<IndexT>(i);
            std::mt19937 rng(std::random_device{}());

            // Random sample results
            Model curr_params;
            ResidualVector curr_residuals;
            IndexVector curr_inliers;

            iteration_count_ = 0;
            while (iteration_count_ < max_iter_) {
                // Pick a random sample
                IndexVector sample_ind(sample_size_);
                size_t prev_size = num_points;
                for (size_t i = 0; i < sample_size_; i++) {
                    std::uniform_int_distribution<size_t> dist(0, prev_size - 1);
                    size_t rand_ind = dist(rng);
                    sample_ind[i] = perm[rand_ind];
                    prev_size--;
                    std::swap(perm[rand_ind], perm[prev_size]);
                }

                // Fit model to sample and get its inliers
                estimator.estimateModel(sample_ind, curr_params);
                estimator.computeResiduals(curr_params, curr_residuals);
                curr_inliers.resize(num_points);
                size_t k = 0;
                for (size_t i = 0; i < num_points; i++) {
                    if (curr_residuals[i] <= inlier_dist_thresh_) curr_inliers[k++] = static_cast<IndexT>(i);
                }
                curr_inliers.resize(k);

                iteration_count_++;
                if (curr_inliers.size() < sample_size_) continue;

                // Update best found
                if (curr_inliers.size() > model_inliers_.size()) {
                    model_params_ = curr_params;
                    model_residuals_ = std::move(curr_residuals);
                    model_inliers_ = std::move(curr_inliers);
                }

                // Check if target inlier count was reached
                if (model_inliers_.size() >= inlier_count_thresh_) break;
            }

            // Re-estimate
            if (re_estimate_) {
                estimator.estimateModel(model_inliers_, model_params_);
                estimator.computeResiduals(model_params_, model_residuals_);
                model_inliers_.resize(num_points);
                size_t k = 0;
                for (size_t i = 0; i < num_points; i++){
                    if (model_residuals_[i] <= inlier_dist_thresh_) model_inliers_[k++] = static_cast<IndexT>(i);
                }
                model_inliers_.resize(k);
            }

            return estimator;
        }

        inline ModelEstimatorT& estimate(ResidualScalar max_residual,
                                         size_t target_inlier_count,
                                         size_t max_iter)
        {
            inlier_count_thresh_ = target_inlier_count;
            max_iter_ = max_iter;
            inlier_dist_thresh_ = max_residual;
            return estimate();
        }

        inline const ModelEstimatorT& getEstimationResults(Model &model_params,
                                                           ResidualVector &model_residuals,
                                                           IndexVector &model_inliers) const
        {
            model_params = model_params_;
            model_residuals = model_residuals_;
            model_inliers = model_inliers_;
            return *static_cast<ModelEstimatorT*>(this);
        }

        inline const Model& getModel() const { return model_params_; }

        inline const ModelEstimatorT& getModel(Model &model_params) const {
            model_params = model_params_;
            return *static_cast<ModelEstimatorT*>(this);
        }

        inline const ResidualVector& getModelResiduals() const { return model_residuals_; }

        inline const ModelEstimatorT& getModelResiduals(ResidualVector &model_residuals) const {
            model_residuals = model_residuals_;
            return *static_cast<ModelEstimatorT*>(this);
        }

        inline const IndexVector& getModelInliers() const { return model_inliers_; }

        inline const ModelEstimatorT& getModelInliers(IndexVector &model_inliers) const {
            model_inliers = model_inliers_;
            return *static_cast<ModelEstimatorT*>(this);
        }

        inline bool targetInlierCountAchieved() const { return model_inliers_.size() >= inlier_count_thresh_; }

        inline size_t getNumberOfPerformedIterations() const { return iteration_count_; }

        inline size_t getNumberOfInliers() const { return model_inliers_.size(); }

    protected:
        // Parameters
        size_t sample_size_;
        size_t inlier_count_thresh_;
        size_t max_iter_;
        ResidualScalar inlier_dist_thresh_;
        bool re_estimate_;

        // Object state and results
        size_t iteration_count_;
        Model model_params_;
        ResidualVector model_residuals_;
        IndexVector model_inliers_;
    };
}


/****************************************************************************
 *  Copyright (C) 2019 RoboMaster.
 *
 *  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/>.
 ***************************************************************************/

#include <tf/tf.h>
#include <tf/transform_listener.h>

#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>

#include <Eigen/Eigen>
#include <chrono>

#include "cubic_spline/cubic_spline_ros.h"
#include "utility.h"

namespace robomaster{

class LocalPlanner{
 public:
  LocalPlanner(ros::NodeHandle& given_nh):nh(given_nh),plan_(false), prune_index_(0){

    nh.param<double>("max_speed", max_speed_, 2.0);
    double max_angle_diff;
    nh.param<double>("max_angle_diff", max_angle_diff, 60);
    max_angle_diff_ = max_angle_diff * M_PI / 180;
    nh.param<double>("p_coeff", p_coeff_, 10.0);
    nh.param<int>("plan_frequency", plan_freq_, 50);
    nh.param<double>("goal_tolerance", goal_tolerance_, 0.1);
    nh.param<double>("prune_ahead_distance", prune_ahead_dist_, 0.3);
    nh.param<std::string>("global_frame", global_frame_, "odom");

    tf_listener_ = std::make_shared<tf::TransformListener>();

    local_path_pub_= nh.advertise<nav_msgs::Path>("path", 5);

    cmd_vel_pub_ = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);

    global_path_sub_ = nh.subscribe("/global_planner/path", 5, &LocalPlanner::GlobalPathCallback,this);

    plan_timer_ = nh.createTimer(ros::Duration(1.0/plan_freq_),&LocalPlanner::Plan,this);

  }
  ~LocalPlanner()= default;
  void GlobalPathCallback(const nav_msgs::PathConstPtr & msg){
    if (!msg->poses.empty()){
      global_path_ = *msg;
      prune_index_ = 0;
      plan_ = true;
    }
  }
 private:

  void Plan(const ros::TimerEvent& event){

    if (plan_){

      auto begin = std::chrono::steady_clock::now();

      // 1. Update the transform from global path frame to local planner frame
      UpdateTransform(tf_listener_, global_frame_,
                      global_path_.header.frame_id, global_path_.header.stamp,
                      global2path_transform_);//source_time needs decided

      // 2. Get current robot pose in global path frame
      geometry_msgs::PoseStamped robot_pose;
      GetGlobalRobotPose(tf_listener_, global_path_.header.frame_id, robot_pose);

      // 3. Check if robot has already arrived with given distance tolerance
      if (GetEuclideanDistance(robot_pose,global_path_.poses.back())<= goal_tolerance_
          && prune_index_ == global_path_.poses.size() - 1){
        plan_ = false;
        geometry_msgs::Twist cmd_vel;
        cmd_vel.linear.x = 0;
        cmd_vel.linear.y = 0;
        cmd_vel.angular.z = 0;
        cmd_vel_pub_.publish(cmd_vel);
        ROS_INFO("Planning Success!");
        return;
      }

      // 4. Get prune index from given global path
      FindNearstPose(robot_pose, global_path_, prune_index_, prune_ahead_dist_);// TODO: double direct prune index is needed later!

      // 5. Generate the prune path and transform it into local planner frame
      nav_msgs::Path prune_path, local_path;

      local_path.header.frame_id = global_frame_;
      prune_path.header.frame_id = global_frame_;

      geometry_msgs::PoseStamped tmp_pose;
      tmp_pose.header.frame_id = global_frame_;

      TransformPose(global2path_transform_, robot_pose, tmp_pose);
      prune_path.poses.push_back(tmp_pose);

      for (int i = prune_index_; i < global_path_.poses.size(); i++){
        TransformPose(global2path_transform_, global_path_.poses[i], tmp_pose);
        prune_path.poses.push_back(tmp_pose);

      }

      // 6. Generate the cubic spline trajectory from above prune path
      GenTraj(prune_path, local_path);
      local_path_pub_.publish(local_path);

      // 7. Follow the trajectory and calculate the velocity
      geometry_msgs::Twist cmd_vel;
      //FollowTraj(prune_path.poses.front(), local_path, cmd_vel);
      //cmd_vel_pub_.publish(cmd_vel);

      auto plan_time = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() - begin);
      ROS_INFO("Planning takes %f ms and passed %d/%d.",
               plan_time.count()/1000.,
               prune_index_,
               static_cast<int>(global_path_.poses.size()));
    }

  }

  void FollowTraj(const geometry_msgs::PoseStamped& robot_pose,
                  const nav_msgs::Path& traj,
                  geometry_msgs::Twist& cmd_vel){
    double diff_yaw = GetYawFromOrientation(traj.poses[0].pose.orientation)
        - GetYawFromOrientation(robot_pose.pose.orientation);

    double yaw = GetYawFromOrientation(robot_pose.pose.orientation);

    double diff_x = traj.poses[0].pose.position.x
        - robot_pose.pose.position.x;
    double diff_y = traj.poses[0].pose.position.y
        - robot_pose.pose.position.y;

    double vx_map = 0.5 * diff_x;
    double vy_map = 0.5 * diff_y;

    cmd_vel.linear.x = cos(yaw) * vx_map + sin(yaw) * vy_map;
    cmd_vel.linear.y = -sin(yaw) * vx_map + cos(yaw) * vy_map;

    // // set it from -PI to PI
    // if(diff_yaw > M_PI){
    //   diff_yaw -= 2*M_PI;
    // } else if(diff_yaw < -M_PI){
    //   diff_yaw += 2*M_PI;
    // }

    // // calculate velocity

    // if( diff_yaw > 0 ){
    //   cmd_vel.angular.z = std::min(p_coeff_*diff_yaw, 2.0);
    // }else{
    //   cmd_vel.angular.z = std::max(p_coeff_*diff_yaw, -2.0);
    // }

    // cmd_vel.linear.x = max_speed_*(1.0-std::abs(diff_yaw)/(max_angle_diff_));
    // cmd_vel.linear.y = 0;
    // if(std::abs(diff_yaw) > max_angle_diff_){
    //   cmd_vel.linear.x = 0;
    // }
  }

 private:

  void FindNearstPose(geometry_msgs::PoseStamped& robot_pose,nav_msgs::Path& path, int& prune_index, double prune_ahead_dist = 0.3){

    double dist_threshold = 10;// threshold is 10 meters (basically never over 10m i suppose)
    double sq_dist_threshold = dist_threshold * dist_threshold;
    double sq_dist;
    if(prune_index!=0){
      sq_dist = GetEuclideanDistance(robot_pose,path.poses[prune_index-1]);
    }else{
      sq_dist = 1e10;
    }

    double new_sq_dist = 0;
    while (prune_index < (int)path.poses.size()) {
      new_sq_dist = GetEuclideanDistance(robot_pose,path.poses[prune_index]);
      if (new_sq_dist > sq_dist && sq_dist < sq_dist_threshold) {

        //Judge if it is in the same direction and sq_dist is further than 0.3 meters
        if ((path.poses[prune_index].pose.position.x - robot_pose.pose.position.x) *
            (path.poses[prune_index-1].pose.position.x - robot_pose.pose.position.x) +
            (path.poses[prune_index].pose.position.y - robot_pose.pose.position.y) *
                (path.poses[prune_index-1].pose.position.y - robot_pose.pose.position.y) > 0
            && sq_dist > prune_ahead_dist) {
          prune_index--;
        }else{
          sq_dist = new_sq_dist;
        }

        break;
      }
      sq_dist = new_sq_dist;
      ++prune_index;
    }

    prune_index = std::min(prune_index, (int)(path.poses.size()-1));

  }

  ros::NodeHandle nh;
  std::shared_ptr<tf::TransformListener> tf_listener_;
  tf::StampedTransform global2path_transform_;

  std::string global_frame_;
  ros::Timer plan_timer_;

  ros::Subscriber global_path_sub_;
  ros::Publisher local_path_pub_;
  ros::Publisher cmd_vel_pub_;

  bool plan_;
  int prune_index_;
  nav_msgs::Path global_path_;

  double max_speed_;
  double max_angle_diff_;
  double p_coeff_;
  double goal_tolerance_;
  double prune_ahead_dist_;
  int plan_freq_;

};
}

using namespace robomaster;
int main(int argc, char **argv)
{
  ros::init(argc, argv, "local_planner");
  ros::NodeHandle nh("~");
  LocalPlanner local_planner(nh);
  ros::spin();
  return 0;
}


#pragma once
#include <boost/multiprecision/cpp_int.hpp>
#include <cryptopp/osrng.h>

namespace rai
{
extern thread_local CryptoPP::AutoSeededRandomPool random_pool;

using uint128_t = boost::multiprecision::uint128_t;
using uint256_t = boost::multiprecision::uint256_t;
using uint512_t = boost::multiprecision::uint512_t;

const rai::uint128_t Colin = rai::uint128_t("1");
const rai::uint128_t uRAI = 1000 * Colin;      // 10^3 Colin
const rai::uint128_t mRAI = 1000000 * Colin;   // 10^6 Colin
const rai::uint128_t RAI = 1000000000 * Colin;  // 10^9 Colin

union uint128_union
{
public:
    uint128_union();
    uint128_union(const rai::uint128_union&) = default;
    uint128_union(uint64_t);
    uint128_union(const rai::uint128_t&);
    bool operator==(const rai::uint128_union&) const;
    bool operator!=(const rai::uint128_union&) const;
    bool operator<(const rai::uint128_union&) const;
    bool operator>(const rai::uint128_union&) const;
    bool operator<=(const rai::uint128_union&) const;
    bool operator>=(const rai::uint128_union&) const;
    rai::uint128_union operator+(const rai::uint128_union&) const;
    rai::uint128_union operator-(const rai::uint128_union&) const;
    rai::uint128_union& operator+=(const rai::uint128_union&);
    rai::uint128_union& operator-=(const rai::uint128_union&);
    rai::uint128_t Number() const;
    void Clear();
    bool IsZero() const;
    void EncodeHex(std::string&) const;
    bool DecodeHex(const std::string&);
    void EncodeDec(std::string&) const;
    bool DecodeDec(const std::string&);
    void EncodeBalance(const rai::uint128_t&, std::string&) const;
    bool DecodeBalance(const rai::uint128_t&, const std::string&);
    std::string StringHex() const;
    std::string StringDec() const;
    std::string StringBalance(const rai::uint128_t&) const;

    std::array<char, 16> chars;
    std::array<uint8_t, 16> bytes;
    std::array<uint32_t, 4> dwords;
    std::array<uint64_t, 2> qwords;
};
// Balances are 128 bit.
using Amount = uint128_union;

class RawKey;
union uint256_union
{
    uint256_union();
    uint256_union(const rai::uint256_union&) = default;
    uint256_union(uint64_t);
    uint256_union(const rai::uint256_t&);
    bool operator==(const rai::uint256_union&) const;
    bool operator!=(const rai::uint256_union&) const;
    bool operator<(const rai::uint256_union&) const;
    bool operator>(const rai::uint256_union&) const;
    bool operator<=(const rai::uint256_union&) const;
    bool operator>=(const rai::uint256_union&) const;
    rai::uint256_union operator^(const rai::uint256_union&) const;
    rai::uint256_union& operator^=(const rai::uint256_union&);
    rai::uint256_union operator+(const rai::uint256_union&) const;
    rai::uint256_union operator-(const rai::uint256_union&) const;
    rai::uint256_union& operator+=(const rai::uint256_union&);
    rai::uint256_union& operator-=(const rai::uint256_union&);
    rai::uint256_union operator~() const;
    rai::uint256_t Number() const;
    void Clear();
    void SecureClear();
    void Encrypt(const rai::RawKey&, const rai::RawKey&,
                 const rai::uint128_union&);
    bool IsZero() const;
    void EncodeHex(std::string&) const;
    bool DecodeHex(const std::string&);
    void EncodeDec(std::string&) const;
    bool DecodeDec(const std::string&);
    void EncodeAccount(std::string&) const;
    bool DecodeAccount(const std::string&);
    std::string StringHex() const;
    std::string StringDec() const;
    std::string StringAccount() const;
    void Random();
    bool ValidPublicKey() const;
    void ClampKey();

    std::array<uint8_t, 32> bytes;
    std::array<char, 32> chars;
    std::array<uint32_t, 8> dwords;
    std::array<uint64_t, 4> qwords;
    std::array<uint128_union, 2> owords;
};
using BlockHash     = uint256_union;
using Account       = uint256_union;
using PublicKey     = uint256_union;
using PrivateKey    = uint256_union;
using UniqueId      = uint256_union;
using TokenValue    = uint256_union;
using TokenAddress  = uint256_union;

class AccountParser
{
public:
    AccountParser(const std::string&);
    rai::Account Account() const;
    std::string SubAccount() const;
    bool Error() const;

private:
    bool error_;
    rai::Account account_;
    std::string sub_account_;
};

class AccountHeight
{
public:
    rai::Account account_;
    uint64_t height_;

    bool operator<(const AccountHeight& other) const;
    rai::AccountHeight& operator++();
};

class RawKey
{
public:
    RawKey() = default;
    ~RawKey();
    bool operator==(const rai::RawKey&) const;
    bool operator!=(const rai::RawKey&) const;
    void Decrypt(const rai::uint256_union&, const rai::RawKey&,
                 const uint128_union&);

    rai::uint256_union data_;
};

union uint512_union
{
    uint512_union();
    uint512_union(const rai::uint512_union&) = default;
    uint512_union(uint64_t);
    uint512_union(const rai::uint512_t&);
    uint512_union(const std::vector<uint8_t>&);
    uint512_union(uint8_t, size_t);
    uint512_union(const std::string&, bool = true);
    uint512_union(const rai::uint256_union&, const rai::uint256_union&);
    bool operator==(const rai::uint512_union&) const;
    bool operator!=(const rai::uint512_union&) const;
    rai::uint512_union operator+(const rai::uint512_union&) const;
    rai::uint512_union& operator+=(const rai::uint512_union&);
    rai::uint512_t Number() const;
    void Clear();
    bool IsZero() const;
    void EncodeHex(std::string&) const;
    bool DecodeHex(const std::string&);
    void ToLower();
    std::string StringHex() const;

    std::array<uint8_t, 64> bytes;
    std::array<uint32_t, 16> dwords;
    std::array<uint64_t, 8> qwords;
    std::array<uint256_union, 2> uint256s;
};
using Signature = uint512_union;
using Prefix = uint512_union;
using SwapRate = uint512_union;

rai::uint512_union SignMessage(const rai::RawKey&, const rai::PublicKey&,
                               const rai::uint256_union&);

bool ValidateMessage(const rai::PublicKey&, const rai::uint256_union&,
                     const rai::uint512_union&);

rai::PublicKey GeneratePublicKey(const rai::PrivateKey&);
uint64_t Random(uint64_t, uint64_t);
}  // namespace rai

namespace boost
{
template <>
struct hash<rai::uint256_union>
{
    size_t operator()(const rai::uint256_union& data) const
    {
        return *reinterpret_cast<const size_t*>(data.bytes.data());
    }
};
}  // namespace boost

namespace std
{
template <>
struct hash<rai::uint256_union>
{
    size_t operator()(const rai::uint256_union& data) const
    {
        return *reinterpret_cast<const size_t*>(data.bytes.data());
    }
};
}  // namespace std


#include <armadillo>
#include <sstream>

#include <gtest/gtest.h>

#include <besiq/io/covariates.hpp>

using namespace arma;

TEST(CovariateTest, Simple)
{
    std::stringstream cov_file;
    cov_file << "FID IID gender age\n";
    cov_file << "1 1 0 15\n";
    cov_file << "2 2 NA 35\n";
    cov_file << "3 3 1 20\n";
    cov_file << "5 5 15 100\n";

    std::vector<std::string> order;
    order.push_back( "1" );
    order.push_back( "2" );
    order.push_back( "3" );
    order.push_back( "4" );

    arma::uvec missing = arma::zeros<arma::uvec>( 4 );

    arma::mat cov = parse_covariate_matrix( cov_file, missing, order );

    ASSERT_EQ( cov.n_rows, 4 );
    ASSERT_EQ( cov.n_cols, 2 );

    ASSERT_NEAR( cov( 0, 0 ), 0.0, 0.0001 );
    ASSERT_NEAR( cov( 0, 1 ), 15.0, 0.0001 );
    ASSERT_NEAR( cov( 1, 0 ), 0.0, 0.0001 );
    ASSERT_NEAR( cov( 1, 1 ), 35.0, 0.0001 );
    ASSERT_NEAR( cov( 2, 0 ), 1.0, 0.0001 );
    ASSERT_NEAR( cov( 2, 1 ), 20.0, 0.0001 );
    ASSERT_NEAR( cov( 3, 0 ), 0.0, 0.0001 );
    ASSERT_NEAR( cov( 3, 1 ), 0.0, 0.0001 );

    ASSERT_EQ( missing[ 0 ], 0 );
    ASSERT_EQ( missing[ 1 ], 1 );
    ASSERT_EQ( missing[ 2 ], 0 );
    ASSERT_EQ( missing[ 3 ], 1 );
}

TEST(CovariateTest, HeaderFail)
{
    std::stringstream cov_file;
    cov_file << "FID random gender age\n";
    cov_file << "1 1 0 15\n";

    std::vector<std::string> order( 1, "1" );
    arma::uvec missing = arma::zeros<arma::uvec>( 1 );

    ASSERT_THROW( parse_covariate_matrix( cov_file, missing, order ), std::runtime_error );
}

TEST(CovariateTest, ColumnFail)
{
    std::stringstream cov_file;
    cov_file << "FID IID gender age\n";
    cov_file << "1 1 0\n";

    std::vector<std::string> order( 1, "1" );
    arma::uvec missing = arma::zeros<arma::uvec>( 1 );

    ASSERT_THROW( parse_covariate_matrix( cov_file, missing, order ), std::runtime_error );
}

TEST(CovariateTest, ParseFail)
{
    std::stringstream cov_file;
    cov_file << "FID IID gender age\n";
    cov_file << "1 1 0 hej\n";

    std::vector<std::string> order( 1, "1" );
    arma::uvec missing = arma::zeros<arma::uvec>( 1 );

    ASSERT_THROW( parse_covariate_matrix( cov_file, missing, order ), std::runtime_error );
}

TEST(CovariateTest, ParsePhenos)
{
    std::stringstream cov_file;
    cov_file << "FID IID pheno\n";
    cov_file << "1 1 1\n";
    cov_file << "2 2 2\n";
    cov_file << "3 3 2\n";
    cov_file << "4 4 1\n";

    std::vector<std::string> order;
    order.push_back( "1" );
    order.push_back( "2" );
    order.push_back( "3" );
    order.push_back( "4" );

    arma::uvec missing = arma::zeros<arma::uvec>( 4 );

    arma::vec pheno = parse_phenotypes( cov_file, missing, order );

    ASSERT_EQ( pheno.n_elem, 4 );
    ASSERT_NEAR( pheno[ 0 ], 1.0, 0.0001 );
    ASSERT_NEAR( pheno[ 1 ], 2.0, 0.0001 );
    ASSERT_NEAR( pheno[ 2 ], 2.0, 0.0001 );
    ASSERT_NEAR( pheno[ 3 ], 1.0, 0.0001 );
}


/*
 * WaveEquation.cc
 *
 *  Created on: 05.05.2017
 *      Author: thies
 */

/*
 * based on step23.cc from the deal.II tutorials
 */

#include <deal.II/base/exceptions.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/thread_management.h>
#include <deal.II/base/timer.h>
#include <deal.II/base/types.h>
#include <deal.II/base/utilities.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/solver_control.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/vector_tools.h>

#include <forward/WaveEquation.h>

#include <stddef.h>
#include <iostream>
#include <map>
#include <string>

#include <deal.II/base/data_out_base.h>
#include <deal.II/numerics/data_out.h>
#include <fstream>

namespace wavepi {
namespace forward {

using namespace dealii;
using namespace wavepi::base;

template<int dim>
WaveEquation<dim>::WaveEquation(std::shared_ptr<SpaceTimeMesh<dim>> mesh)
      : AbstractEquation<dim>(mesh), initial_values_u(std::make_shared<Functions::ZeroFunction<dim>>(1)),
            initial_values_v(std::make_shared<Functions::ZeroFunction<dim>>(1)),
            boundary_values_u(std::make_shared<Functions::ZeroFunction<dim>>(1)),
            boundary_values_v(std::make_shared<Functions::ZeroFunction<dim>>(1)) {
}

template<int dim>
WaveEquation<dim>::WaveEquation(const WaveEquation<dim> &wave)
      : AbstractEquation<dim>(wave.get_mesh()), initial_values_u(wave.get_initial_values_u()),
            initial_values_v(wave.get_initial_values_v()), boundary_values_u(wave.get_boundary_values_u()),
            boundary_values_v(wave.get_boundary_values_v()) {

   this->set_param_c(wave.get_param_c());
   this->set_param_nu(wave.get_param_nu());
   this->set_param_rho(wave.get_param_rho());
   this->set_param_q(wave.get_param_q());
   this->set_rho_time_dependent(wave.is_rho_time_dependent());

   this->set_theta(wave.get_theta());
   this->set_solver_tolerance(wave.get_solver_tolerance());
   this->set_solver_max_iter(wave.get_solver_max_iter());
}

template<int dim>
void WaveEquation<dim>::apply_boundary_conditions_u(double time) {
   boundary_values_u->set_time(time);
   boundary_values_u->set_time(time);

   std::map<types::global_dof_index, double> boundary_values;
   VectorTools::interpolate_boundary_values(*dof_handler, 0, *boundary_values_u, boundary_values);
   MatrixTools::apply_boundary_values(boundary_values, system_matrix, solution_u, system_rhs);
}

template<int dim>
void WaveEquation<dim>::apply_boundary_conditions_v(double time) {
   boundary_values_v->set_time(time);

   std::map<types::global_dof_index, double> boundary_values;
   VectorTools::interpolate_boundary_values(*dof_handler, 0, *boundary_values_v, boundary_values);
   MatrixTools::apply_boundary_values(boundary_values, system_matrix, solution_v, system_rhs);
}

template<int dim>
void WaveEquation<dim>::initial_values(double time) {
   initial_values_u->set_time(time);
   initial_values_v->set_time(time);

   /* projecting might make more sense, but VectorTools::project
    leads to a mutex error (deadlock) on my laptop (Core i5 6267U) */
   //   VectorTools::project(*dof_handler, constraints, QGauss<dim>(3), *initial_values_u, old_solution_u);
   //   VectorTools::project(*dof_handler, constraints, QGauss<dim>(3), *initial_values_v, old_solution_v);
   VectorTools::interpolate(*dof_handler, *initial_values_u, solution_u);
   constraints->distribute(solution_u);

   VectorTools::interpolate(*dof_handler, *initial_values_v, solution_v);
   constraints->distribute(solution_v);
}

template<int dim>
void WaveEquation<dim>::assemble_matrices(size_t time_idx) {
   LogStream::Prefix p("assemble_matrices");

   this->fill_matrices(mesh, time_idx, matrix_A, matrix_B, matrix_C);
}

template<int dim>
DiscretizedFunction<dim> WaveEquation<dim>::run(std::shared_ptr<RightHandSide<dim>> right_hand_side,
      typename AbstractEquation<dim>::Direction direction) {
   {
      LogStream::Prefix p("WaveEq");
      LogStream::Prefix pp("BoundChecking");

      // bound checking for ρ and c (if possible)
      // (should not take long compared to the rest and can be very tricky to find out otherwise
      //    -> do it even in release mode)
      if (this->param_c_disc) {
         double c_min, c_max;
         this->param_c_disc->min_max_value(&c_min, &c_max);

         std::stringstream bound_str;
         bound_str << c_min << " ≤ c ≤ " << c_max;

         AssertThrow(c_min > 0, ExcMessage("C is not positive, " + bound_str.str()));
         AssertThrow(1.0 / (c_max * c_max) >= 1e-4, ExcMessage("C is not coercive, " + bound_str.str()));

         deallog << bound_str.str() << std::endl;
      }

      if (this->param_rho_disc) {
         double rho_min, rho_max;
         this->param_rho_disc->min_max_value(&rho_min, &rho_max);

         std::stringstream bound_str;
         bound_str << rho_min << " ≤ ρ ≤ " << rho_max;

         AssertThrow(rho_min > 0, ExcMessage("A and D are not positive, " + bound_str.str()));
         AssertThrow(1.0 / rho_max >= 1e-4, ExcMessage("A and D are not coercive, " + bound_str.str()));

         deallog << bound_str.str() << std::endl;
      }
   }

   return AbstractEquation<dim>::run(right_hand_side, direction);
}

template class WaveEquation<1> ;
template class WaveEquation<2> ;
template class WaveEquation<3> ;

} /* namespace forward */
} /* namespace wavepi */


#ifndef GIFS_CONVERSION_H
#define GIFS_CONVERSION_H

#include <algorithm>
#include <armadillo>

/* Direct conversion! */
class Conversion
{
public:
    //
    explicit Conversion(double mass, double length, double time);
    Conversion(const Conversion& rhs) = default;
    Conversion(Conversion&& rhs) = default;
    Conversion& operator=(const Conversion& rhs) = default;
    Conversion& operator=(Conversion&& rhs) = default;

    //
    template<typename itr1, typename itr2>
    inline
    void
    transform_crd_md2au(itr1 in_begin, itr1 in_end, itr2 result_begin) {
        std::transform(in_begin, in_end, result_begin, [this](double value){ return this->crd_md2au(value); });
    };
    //
    template<typename itr1, typename itr2>
    inline
    void
    transform_veloc_md2au(itr1 in_begin, itr1 in_end, itr2 result_begin) {
        std::transform(in_begin, in_end, result_begin, [this](double value){ return this->veloc_md2au(value); });
    }
    //
    template<typename itr1, typename itr2>
    inline
    void
    transform_veloc_au2md(itr1 in_begin, itr1 in_end, itr2 result_begin) {
        std::transform(in_begin, in_end, result_begin, [this](double value){ return this->veloc_au2md(value); });
    }
    //
    template<typename itr1, typename itr2>
    inline
    void
    transform_gradient_au2md(itr1 in_begin, itr1 in_end, itr2 result_begin) {
        std::transform(in_begin, in_end, result_begin, [this](double value){ return this->grd_au2md(value); });
    }
    //
    template<typename itr1, typename itr2>
    inline
    void
    transform_gradient_md2au(itr1 in_begin, itr1 in_end, itr2 result_begin) {
        std::transform(in_begin, in_end, result_begin, [this](double value){ return this->grd_md2au(value); });
    }
    //
    template<typename itr1, typename itr2>
    inline
    void
    transform_masses_md2au(itr1 in_begin, itr1 in_end, itr2 result_begin) {
        std::transform(in_begin, in_end, result_begin, [this](double value){ return this->mass_md2au(value); });
    }
    //
    inline double energy_au2md(double value) const noexcept {return _energy_au2md*value;}
    inline double energy_md2au(double value) const noexcept {return _energy_md2au*value;}
    //
    inline double crd_au2md(double value) const noexcept {return _crd_au2md*value;}
    inline double crd_md2au(double value) const noexcept {return _crd_md2au*value;}
    //
    inline double veloc_au2md(double value) const noexcept {return _veloc_au2md*value;}
    inline double veloc_md2au(double value) const noexcept {return _veloc_md2au*value;}
    //
    inline double grd_au2md(double value) const noexcept {return _grd_au2md*value;}
    inline double grd_md2au(double value) const noexcept {return _grd_md2au*value;}
    //
    inline double mass_au2md(double value) const noexcept {return _mass_au2md*value;}
    inline double mass_md2au(double value) const noexcept {return _mass_md2au*value;}

private:
    // energies
    double _energy_au2md;
    double _energy_md2au;
    // coordinates
    double _crd_au2md;
    double _crd_md2au;
    // velocities
    double _veloc_au2md;
    double _veloc_md2au;
    // gradient
    double _grd_au2md;
    double _grd_md2au;
    // mass
    double _mass_au2md;
    double _mass_md2au;
    //
};

#endif