#include <iostream>
#include <math.h>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <boost/filesystem.hpp>
#include <boost/format.hpp>
#include <boost/program_options.hpp>
#include <boost/lexical_cast.hpp>
#include "dataset_prepare.h"

using namespace std;
using cv::Mat;
using cv::Point;
using cv::Scalar;
using cv::Size;
using cv::Rect;
using namespace boost::filesystem;
using namespace boost::program_options;

variables_map opts;

void parse_options(int argc, char* argv[]) {
	options_description desc{ "Options" };
	desc.add_options()
		("help,h", "Help screen")
		("show", "Show intermediate view")
		("src", value<string>()->default_value("dataset"), "Source folder")
		("dst", value<string>()->default_value("dataset"), "Destination folder")
		("minsize", value<int>()->default_value(20), "Minimum marker size")
		("maxsize", value<int>()->default_value(416 / 2), "Maximum marker size")
		("stepsize", value<int>()->default_value(8), "Minimum marker size")
		;

	store(parse_command_line(argc, argv, desc), opts);

	if (opts.count("help")) {
		std::cout << desc << '\n';
		exit(0);
	}
}

namespace dp {

	struct context {
		int id;
		Rect selection;
	};

	class op {
	public:
		virtual void operator()(const cv::Mat& img, dp::context& context) const {}
	};

	class quad : public op {

		const op& next;

	public:
		quad(const op& next) :
			next(next) {
		}

		virtual void operator()(const cv::Mat& img, dp::context& context) const {

			int s = std::min(img.cols, img.rows);

			Mat quad(s, s, img.type(), Scalar(127, 127, 127, 0));

			cv::Rect rect((img.cols - s) / 2, (img.rows - s) / 2, s, s);
			img(rect).copyTo(quad);
			next(quad, context);
		}
	};

	class resize : public op {

		const int min_size, max_size, size_step;
		const op& next;

	public:
		resize(int min_size, int max_size, int size_step, const op& next) :
			min_size(min_size),
			max_size(max_size),
			size_step(size_step),
			next(next) {
		}

		virtual void operator()(const cv::Mat& img, dp::context& context) const {
			for (int size = min_size; size <= max_size; size += size_step) {
				cv::Mat resized;
				cv::resize(img, resized, cv::Size(size, size), 0, 0, cv::INTER_LANCZOS4);
				next(resized, context);
			}
		}
	};

	class shear : public op {

		const float shear_min, shear_max, shear_step;
		const op& next;

	public:
		shear(float smin/* = 0*/, float smax/* = 1.5*/, float sstep/* = 0.5*/, const op& next) :
			shear_min(smin),
			shear_max(smax),
			shear_step(sstep),
			next(next) {
		}

		virtual void operator()(const cv::Mat& img, dp::context& context) const {
			for (float s = shear_min; s <= shear_max; s += shear_step) {
				Mat M(2, 3, CV_32F);

				M.at<float>(0, 0) = 1;
				M.at<float>(0, 1) = 0;
				M.at<float>(0, 2) = 0;

				M.at<float>(1, 0) = s;
				M.at<float>(1, 1) = 1;
				M.at<float>(1, 2) = 0;

				vector<cv::Point2f> roi_points = {
					{0, 0},
					{(float)img.cols, 0},
					{(float)img.cols, (float)img.rows},
					{0, (float)img.rows}
				}, roi_points_sheared;

				cv::transform(roi_points, roi_points_sheared, M);

				auto bounding_sheared = cv::boundingRect(roi_points_sheared);

				Mat sheared;
				cv::warpAffine(img, sheared, M, bounding_sheared.size(), cv::INTER_LANCZOS4, cv::BORDER_CONSTANT, Scalar(127, 127, 127, 0));

				next(sheared, context);

			}
		}
	};

	class rotate : public op {

		const float min_angle, max_angle, angle_step;
		const op& next;

	public:
		rotate(float min_angle/* = -45*/, float max_angle/* = 45*/, float angle_step/* = 15*/, const op& next) :
			min_angle(min_angle),
			max_angle(max_angle),
			angle_step(angle_step),
			next(next) {
		}

		virtual void operator()(const cv::Mat& img, dp::context& context) const {
			for (float angle = min_angle; angle <= max_angle; angle += angle_step) {

				auto center = Point((img.cols - 1) / 2, (img.rows - 1) / 2);

				Mat matRotation = cv::getRotationMatrix2D(center, angle, 1);
				cv::Rect2f bbox = cv::RotatedRect(cv::Point2f(), img.size(), angle).boundingRect2f();
				matRotation.at<double>(0, 2) += bbox.width / 2.0 - img.cols / 2.0;
				matRotation.at<double>(1, 2) += bbox.height / 2.0 - img.rows / 2.0;

				Mat rotated;
				cv::warpAffine(img, rotated, matRotation, bbox.size(), cv::INTER_LANCZOS4, cv::BORDER_CONSTANT, Scalar(127, 127, 127, 0));

				next(rotated, context);
			}

		}
	};

	class brightness : public op {

		const float min_brightness, max_brightness, brightness_step;
		const op& next;

	public:
		brightness(float min_brightness/* = -16 * 14*/, float max_brightness/* = 16 * 14*/, float brightness_step/* = 16 * 4*/, const op& next) :
			min_brightness(min_brightness),
			max_brightness(max_brightness),
			brightness_step(brightness_step),
			next(next)
		{

		}

		virtual void operator()(const cv::Mat& img, dp::context& context) const {
			for (float brightness = min_brightness; brightness <= max_brightness; brightness += brightness_step) {
				Mat bright = img + cv::Scalar(brightness, brightness, brightness, 0);
				next(bright, context);
			}
		}
	};

	class blur : public op {

		const int min_blur, max_blur, blur_step;
		const op& next;

	public:
		blur(int min_blur/* = -16 * 14*/, int max_blur/* = 16 * 14*/, int blur_step/* = 16 * 4*/, const op& next) :
			min_blur(min_blur),
			max_blur(max_blur),
			blur_step(blur_step),
			next(next)
		{

		}

		virtual void operator()(const cv::Mat& img, dp::context& context) const {
			for (int blur = min_blur; blur <= max_blur; blur += blur_step) {

				if (0 == blur) {
					next(img, context);
				}
				else {
					Mat blurred;
					cv::GaussianBlur(img, blurred, cv::Size((blur - 1) * 2 + 1, (blur - 1) * 2 + 1), 0, 0);
					next(blurred, context);
				}
			}
		}
	};

	class noise : public op {

		const float mean, sigma;
		const op& next;

	public:
		noise(float mean/* = 50*/, float sigma/* = 50*/, const op& next) :
			mean(mean),
			sigma(sigma),
			next(next)
		{

		}

		virtual void operator()(const cv::Mat& img, dp::context& context) const {
			Mat noise = Mat::zeros(img.rows, img.cols, img.type());
			std::vector<float> m = { mean };
			std::vector<float> s = { sigma };
			cv::randn(noise, m, s);

			Mat noised = img + noise;
			next(noised, context);
		}
	};

	class pad : public op {

		const int width, height;
		const op& next;

	public:
		pad(int width/* = 416*/, int height/* = 416*/, const op& next) :
			width(width),
			height(height),
			next(next)
		{

		}

		virtual void operator()(const cv::Mat& img, dp::context& context) const {

			Mat padded(height, width, img.type(), Scalar(127, 127, 127, 0));

			if (img.cols <= width && img.rows <= height) {
				cv::Rect rect((width - img.cols) / 2, (height - img.rows) / 2, img.cols, img.rows);
				img.copyTo(padded(rect));
				context.selection = rect;
				next(padded, context);
			}
		}
	};

	class save : public op {

		const boost::filesystem::path folder;
		const bool positive;
		std::function<void(const std::string& /**/)> save_cb;
		const op& next;

	public:
		save(const boost::filesystem::path& folder, bool positive, std::function<void(const std::string& /**/)> save_cb, const op& next) :
			folder(folder),
			positive(positive),
			save_cb(save_cb),
			next(next)
		{

		}

		virtual void operator()(const cv::Mat& img, dp::context& context) const {

			boost::filesystem::create_directories(folder);

			volatile static int num = 0;
			auto path = folder / (boost::format("%1i.jpg") % num).str();
			auto path_txt = folder / (boost::format("%1i.txt") % num).str();

			num++;

			std::vector<int> params = { cv::IMWRITE_JPEG_QUALITY, 100 };
			cv::imwrite(path.string(), img, params);

			std::ofstream txt(path_txt.string());

			if (positive) {
				txt << context.id << " " << (float)(context.selection.x + context.selection.width / 2) / img.cols << " " << (float)(context.selection.y + context.selection.height / 2) / img.rows << " "
					<< (float)context.selection.width / img.cols << " " << (float)context.selection.height / img.rows;
			}

			txt.close();

			save_cb(path.string());

			next(img, context);
		}
	};

	class show : public op {

	public:
		show() {}

		virtual void operator()(const cv::Mat& img, dp::context& context) const {

			if (opts.count("show")) {
				Mat out = img.clone();
				cv::rectangle(out, context.selection, Scalar(0, 0, 255), 3, cv::LINE_AA);
				cv::imshow("img", out);
				cv::waitKey(1);
			}
		}
	};
}

void prepare_marker(const boost::filesystem::path& src, const boost::filesystem::path& dst, std::function<void(const std::string& /**/)> save_cb) {

	cout << src << endl;

	if (src.extension() != ".png") {
		cout << "skipping, not png..." << endl;
		return;
	}

	int min_size = opts["minsize"].as<int>();
	int max_size = opts["maxsize"].as<int>();
	int size_step = opts["stepsize"].as<int>();

	const int id = boost::lexical_cast<int>(src.stem().string());

	dp::context context;
	context.id = id;

	cout << id << endl;

	{
		boost::filesystem::path dst_dir(opts["dst"].as<std::string>());

		static bool first = true;

		std::ofstream names((dst_dir / "names.txt").string(), std::ios::out | (first ? std::ios::out : std::ios::app));

		first = false;

		names << id << endl;
	}

	cv::Mat img = cv::imread(src.string());
	cv::cvtColor(img, img, CV_32F);

	dp::resize(min_size, max_size, size_step,
		//dp::shear(0, 1.0, 0.5,
			//dp::rotate(-45, 45, 15,
				dp::pad(416, 416,
					dp::brightness(-16 * 2, 16 * 10, 16 * 4,
						dp::blur(0, 1, 1,
							//dp::noise(10, 10,
								dp::save(dst, true, save_cb,
									dp::show())))))(img, context);

}

void prepare_background(const boost::filesystem::path& src, const boost::filesystem::path& dst, std::function<void(const std::string&)> save_cb) {

	cout << src << endl;

	if (src.extension() != ".png") {
		cout << "skipping, not png..." << endl;
		return;
	}

	dp::context context;

	cv::Mat img = cv::imread(src.string());
	cv::cvtColor(img, img, CV_32F);

	dp::quad(
		dp::resize(416, 416, 1,
			//dp::shear(0, 1.0, 0.5,
			//	dp::rotate(-15, 15, 15,
			//		dp::brightness(-16 * 2, 16 * 10, 16 * 4,
						dp::save(dst, false, save_cb,
							dp::show())))(img, context);

}

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

	parse_options(argc, argv);

	boost::filesystem::path src_dir(opts["src"].as<std::string>());
	boost::filesystem::path dst_dir(opts["dst"].as<std::string>());
	boost::filesystem::path valid_path = dst_dir / "valid.txt";
	boost::filesystem::path train_path = dst_dir / "train.txt";

	std::ofstream valid(valid_path.string());
	std::ofstream train(train_path.string());

	std::vector<boost::filesystem::path> files;

	for (boost::filesystem::recursive_directory_iterator i(src_dir / "markers"), end; i != end; i++) {

		if (is_regular_file(*i) && i->path().has_stem()) {
			files.push_back(i->path());
		}
	}

	std::sort(files.begin(), files.end(), [](const boost::filesystem::path& p1, const boost::filesystem::path& p2) {
		return p1.stem() < p2.stem();
	});

	boost::system::error_code ec;
	boost::filesystem::remove_all(dst_dir / "positive", ec);
	boost::filesystem::remove_all(dst_dir / "negative", ec);

	for (const auto& i : files) {

		prepare_marker(i, dst_dir / "positive", [&valid, &train](const std::string& file_path) {
			static bool to_valid = true;
			if (to_valid) {
				valid << file_path << endl;
			}
			else {
				train << file_path << endl;
			}

			to_valid = !to_valid;
		});
	}

	for (boost::filesystem::recursive_directory_iterator i(src_dir / "backgrounds"), end; i != end; i++) {

		if (is_regular_file(*i)) {
			prepare_background(i->path(), dst_dir / "negative", [&valid, &train](const std::string& file_path) {
				static bool to_valid = true;
				if (to_valid) {
					valid << file_path << endl;
				}
				else {
					train << file_path << endl;
				}

				to_valid = !to_valid;
			});
		}
	}

	return 0;
}


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

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

//! [hello]
#include <iostream>
#include <list>
#include <numeric>
#include <vector>
//! [hello-include-pack]
#include <boost/simd/pack.hpp>
//! [hello-include-pack]

#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/splat.hpp>
#include <boost/simd/function/store.hpp>

#include <boost/simd/memory/allocator.hpp>

int main()
{
  //! [hello-namespace]
  namespace bs = boost::simd;
  //! [hello-namespace]
  //! [hello-pack]
  using pack_t = bs::pack<float>;
  //! [hello-pack]

  //! [hello-def-ctor]
  pack_t res;
  //! [hello-def-ctor]

  //! [hello-splat-ctor]
  pack_t tens{10};
  //! [hello-splat-ctor]

  pack_t elevens{11};

  //! [hello-ptr-iota]
  std::vector<float, bs::allocator<float>> values(1000);
  std::iota(values.begin(), values.end(), float(0));
  pack_t ptr_pack(values.data());
  //! [hello-ptr-iota]

  //! [hello-iter-con]
  std::vector<float, bs::allocator<float>> data(pack_t::static_size);
  std::iota(data.begin(), data.begin() + pack_t::static_size, float(0));
  pack_t iter_pack(data.begin(), data.end());
  //! [hello-iter-con]

  //! [hello-enum-con]
  pack_t enum_pack{10, 11, 12, 13, 14, 15, 16, 17};
  //! [hello-enum-con]

  //! [hello-ops]
  res = (tens + elevens) * 2;
  //! [hello-ops]

  //! [hello-store]
  std::vector<float, bs::allocator<float>> output(pack_t::static_size);
  bs::store(res, &output[0]);
  //! [hello-store]

  //! [hello-io]
  std::cout << res << std::endl;
  //! [hello-io]

  return 0;
}
//! [hello]


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file replace_test.cpp
/// \brief Add your file description here.

#include <iostream>
#include <string>
#include <set>
#include <boost/test/minimal.hpp>
#include "mcrl2/atermpp/container_utility.h"
#include "mcrl2/data/parse.h"
#include "mcrl2/data/data_specification.h"
#include "mcrl2/lps/parse.h"
#include "mcrl2/lps/replace.h"

using namespace mcrl2;
using namespace mcrl2::data;
using namespace mcrl2::lps;

const std::string SPEC =
  "act  a;                  \n"
  "                         \n"
  "proc P(b: Bool) =        \n"
  "       sum c: Bool.      \n"
  "         (b == c) ->     \n"
  "         a .             \n"
  "         P(c);           \n"
  "                         \n"
  "init P(true);            \n"
  ;

void test_replace()
{
  specification spec = parse_linear_process_specification(SPEC);
  action_summand s = spec.process().action_summands().front();
  variable b("b", sort_bool::bool_());
  variable c("c", sort_bool::bool_());
  variable d("d", sort_bool::bool_());
  assignment a(c, d);
  const action_summand& t = s;
}

std::string SPEC1a =
  "act  action: Nat;         \n"
  "                          \n"
  "proc P(s: Pos, i: Nat) =  \n"
  "       (s == 2) ->        \n"
  "         action(i) .      \n"
  "         P(1, i)          \n"
  "     + (s == 1) ->        \n"
  "         action(i) .      \n"
  "         P(2, i)          \n"
  "     + true ->            \n"
  "         delta;           \n"
  "                          \n"
  "init P(1, 0);             \n"
  ;

std::string SPEC1b =
  "act  action: Nat;         \n"
  "                          \n"
  "proc P(s: Pos, i: Nat) =  \n"
  "       (3 == 2) ->        \n"
  "         action(4) .      \n"
  "         P(1, 4)          \n"
  "     + (3 == 1) ->        \n"
  "         action(4) .      \n"
  "         P(2, 4)          \n"
  "     + true ->            \n"
  "         delta;           \n"
  "                          \n"
  "init P(1, 0);             \n"
  ;

void test_lps_substituter()
{
  specification spec1 = parse_linear_process_specification(SPEC1a);
  specification spec2 = parse_linear_process_specification(SPEC1b);
  data::mutable_map_substitution<> sigma;
  sigma[variable("s", sort_pos::pos())] = sort_pos::pos(3);
  sigma[variable("i", sort_nat::nat())] = sort_nat::nat(4);

  lps::replace_variables(spec1, sigma);
  std::cerr << lps::pp(spec1.process()) << std::endl;
  std::cerr << "-------------------------------------" << std::endl;
  std::cerr << lps::pp(spec2.process()) << std::endl;
  BOOST_CHECK(lps::pp(spec1.process()) == lps::pp(spec2.process()));
}

void test_lps_substitute()
{
  data::variable v("v", data::sort_bool::bool_());
  const data::data_expression& w = data::sort_bool::true_();
  data::mutable_map_substitution<> sigma;
  sigma[v] = w;
  data::data_expression e = v;
  e = lps::replace_free_variables(e, sigma);
}

void test_replace_process_parameters()
{
  std::string SPEC =
    "act a;                        \n"
    "proc P(b:Bool) = a.P(b = !b); \n"
    "init P(true);                 \n"
    ;
  specification spec = parse_linear_process_specification(SPEC);
  data::mutable_map_substitution<> sigma;
  data::variable b("b", data::sort_bool::bool_());
  data::variable b0("b0", data::sort_bool::bool_());
  sigma[b] = b0;
  lps::replace_process_parameters(spec, sigma);
  std::set<data::variable> variables = lps::find_all_variables(spec);
  BOOST_CHECK(variables.find(b) == variables.end());
}

void test_replace_summand_variables()
{
  std::string SPEC =
    "act a;                                   \n"
    "proc P(b:Bool) = sum c:Bool. a.P(b = c); \n"
    "init P(true);                            \n"
    ;
  specification spec = parse_linear_process_specification(SPEC);
  data::mutable_map_substitution<> sigma;
  data::variable c("c", data::sort_bool::bool_());
  data::variable c0("c0", data::sort_bool::bool_());
  sigma[c] = c0;
  lps::replace_summand_variables(spec, sigma);
  std::cout << lps::pp(spec) << std::endl;
  std::set<data::variable> variables = lps::find_all_variables(spec);
  BOOST_CHECK(variables.find(c) == variables.end());
}

void test_action_list()
{
  sort_expression_list s;
  s.push_front(sort_expression(sort_nat::nat()));
  process::action_label label(core::identifier_string("a"), s);

  variable b("b", data::sort_bool::bool_());
  variable c("c", data::sort_bool::bool_());
  data_expression_list e1;
  e1.push_front(data_expression(sort_bool::and_(b, c)));
  data_expression_list e2;
  e2.push_front(data_expression(sort_bool::and_(c, c)));

  process::action_list l1;
  process::action a1(label, e1);
  l1.push_front(a1);

  process::action_list l2;
  process::action a2(label, e2);
  l2.push_front(a2);

  data::mutable_map_substitution<> sigma;
  sigma[b] = c;

  l1 = process::replace_variables_capture_avoiding(l1, sigma, data::substitution_variables(sigma));
  BOOST_CHECK(l1 == l2);

  std::set<data::variable> v;
  l1 = process::replace_variables(l1, sigma);
  l1 = process::replace_variables_capture_avoiding(l1, sigma, v);
}

int test_main(int argc, char* argv[])
{
  test_replace();
  test_lps_substituter();
  test_lps_substitute();
  test_replace_process_parameters();
  test_replace_summand_variables();
  test_action_list();

  return 0;
}


#pragma once

#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <Eigen/Dense>
#include <dynkin/dynkin.hpp>

namespace py = pybind11;
using namespace pybind11::literals;
using namespace dynkin;
using namespace dynkin::rigidbody;

namespace dynkin{
    namespace python{

        inline void include_dynkin_pybind_wrappers(py::module& m){
            m.doc() = R"docs(
                A toolkit for 3D dynamics and kinematics of rigid bodies using the
                YPR euler angle convention.
            )docs";

            py::class_<_Frame, Frame>(m, "Frame")
                .def(py::init(
                    [](
                        const Eigen::Vector3d& position,
                        const Eigen::Vector3d& attitude,
                        const Eigen::Vector3d& linear_velocity,
                        const Eigen::Vector3d& angular_velocity,
                        Frame parent
                    ){
                        Frame out = create_frame(parent);
                        out->position() = position;
                        out->set_attitude(attitude);
                        out->linear_velocity() = linear_velocity;
                        out->angular_velocity() = angular_velocity;
                        return out;
                    }),
                    "position"_a = Eigen::Vector3d::Zero(),
                    "attitude"_a = Eigen::Vector3d::Zero(),
                    "linear_velocity"_a = Eigen::Vector3d::Zero(),
                    "angular_velocity"_a = Eigen::Vector3d::Zero(),
                    "parent"_a = nullptr
                )
                .def("align_child",
                    [](
                        _Frame& self,
                        const Eigen::Vector3d& position,
                        const Eigen::Vector3d& attitude,
                        const Eigen::Vector3d& linear_velocity,
                        const Eigen::Vector3d& angular_velocity
                    ){
                        Frame out = self.create_child();
                        out->position() = position;
                        out->set_attitude(attitude);
                        out->linear_velocity() = linear_velocity;
                        out->angular_velocity() = angular_velocity;
                        return out;
                    },
                    "position"_a = Eigen::Vector3d::Zero(),
                    "attitude"_a = Eigen::Vector3d::Zero(),
                    "linear_velocity"_a = Eigen::Vector3d::Zero(),
                    "angular_velocity"_a = Eigen::Vector3d::Zero()
                )
                .def("get_pose", &_Frame::get_pose)
                .def("get_twist", &_Frame::get_twist)
                .def_property("position",
                    &_Frame::position,
                    [](_Frame& self, const Eigen::Vector3d& value){
                        self.position() = value;
                    }
                )
                .def_property("attitude",
                    &_Frame::get_attitude,
                    &_Frame::set_attitude
                )
                .def_property("linear_velocity",
                    &_Frame::linear_velocity,
                    [](_Frame& self, const Eigen::Vector3d& value){
                        self.linear_velocity() = value;
                    }
                )
                .def_property("angular_velocity",
                    &_Frame::angular_velocity,
                    [](_Frame& self, const Eigen::Vector3d& value){
                        self.angular_velocity() = value;
                    }
                );

            m.def("transform", &transform, "Create a Transform between two Frames");

            py::class_<Transform>(m, "Transform")
                .def("apply_vector", &Transform::apply_vector)
                .def("apply_position", &Transform::apply_position)
                .def("apply_wrench", &Transform::apply_wrench)
                .def("inverse", &Transform::inverse);

            py::module rb = m.def_submodule("rigidbody");

            py::class_<RigidBody>(rb, "RigidBody")
                .def(py::init(
                    [](
                        const double& mass,
                        const Eigen::Vector3d& gyradii,
                        const Eigen::Vector3d& cog
                    ){
                        return RigidBody(
                            generalized_inertia_matrix(mass, gyradii),
                            cog
                        );
                    }),
                    "mass"_a,
                    "gyradii"_a,
                    "cog"_a = Eigen::Vector3d::Zero()
                )
                .def_readonly("origin", &RigidBody::origin)
                .def_readonly("CoG", &RigidBody::CoG)
                .def("generalized_coordinates", &RigidBody::generalized_coordinates)
                .def("generalized_velocities", &RigidBody::generalized_velocities)
                .def("acceleration", &RigidBody::acceleration,
                    "wrench"_a,
                    "additional_inertia"_a = Eigen::Matrix6d::Zero())
                .def("wrench", &RigidBody::wrench,
                    "acceleration"_a,
                    "additional_inertia"_a = Eigen::Matrix6d::Zero()
                );
        }

    }
}


#pragma once

#include <Eigen/Dense>

#include "pnc/draco_pnc/draco_state_provider.hpp"
#include "pnc/whole_body_controllers/task.hpp"

class DracoAngularMomentumTask : public Task {
public:
  DracoAngularMomentumTask(RobotSystem *_robot);

  virtual ~DracoAngularMomentumTask(){};

private:
  void update_cmd();
  void update_jacobian();

  DracoStateProvider *sp_;
};


#include <iostream>
#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <limits>
#include <vector>
#include "MathMorph.cpp"

using namespace std;

void waitForViewer(boost::shared_ptr<pcl::visualization::PCLVisualizer>  viewer0)
{
	while(!viewer0->wasStopped())
	{
		viewer0->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
}

boost::shared_ptr<pcl::visualization::PCLVisualizer> visualizePC(pcl::PointCloud<PointType>::ConstPtr cloud_ptr, string id)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer0 (new pcl::visualization::PCLVisualizer (id));
	waitForViewer(viewer0);
	viewer0->setBackgroundColor (0, 0, 0);
    pcl::visualization::PointCloudColorHandlerRandom<PointType> rgb(cloud_ptr);
	viewer0->addPointCloud<PointType>(cloud_ptr, "Nube");
	viewer0->initCameraParameters ();
	return viewer0;
}

boost::shared_ptr<pcl::visualization::PCLVisualizer> visualizeNormal(pcl::PointCloud<PointType>::ConstPtr cloud_ptr, pcl::PointCloud<pcl::Normal>::ConstPtr normals, string id)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer0 (new pcl::visualization::PCLVisualizer (id));
	viewer0->setBackgroundColor (0, 0, 0);
    viewer0->addPointCloudNormals<PointType, pcl::Normal> (cloud_ptr, normals, 20, 0.01, "normals", 0);
	viewer0->initCameraParameters ();
	return viewer0;
}

pcl::PointCloud<PointType>::Ptr
hardCopy (pcl::PointCloud<PointType> cloud_ptr) {
    pcl::PointCloud<PointType>::Ptr cloud_out (new pcl::PointCloud<PointType>);
    PointType point;

    for (int i=0; i<cloud_ptr.size(); i++) {
        PointType point;
        point.x=cloud_ptr.points[i].x;
        point.y=cloud_ptr.points[i].y;
        point.z=cloud_ptr.points[i].z;
        cloud_out->points.push_back(point);
    }
    return cloud_out;
}

int main (int argc, char **argv)
{
    if (argc==4)
   	{
	    pcl::PointCloud<PointType> cloud;
	    pcl::PointCloud<PointType>::Ptr cloud_ptr, dilate, dilate2;
	    pcl::PointCloud<pcl::Normal>::Ptr normals;
	    double size, leafSize;

	    size = atof(argv[1]);
	    leafSize = atof(argv[2]);

		// Lee los puntos de un fichero ply
		pcl::PLYReader ply_reader;
		if(ply_reader.read(argv[3], cloud) < 0) {
		   cerr << "Error while reading the .ply file" << std::endl;
		   return (0);
		}
		// Hacemos esta copia dura por los problemas de visualización
	    cloud_ptr=hardCopy(cloud);

	    MathMorph mm(size, leafSize);

	    dilate = mm.dilate(cloud_ptr);
	    dilate2 = mm.dilate2(cloud_ptr);
	    normals = mm.getNormals();

		boost::shared_ptr<pcl::visualization::PCLVisualizer> visu0 = visualizePC(cloud_ptr, "pointcloud");
		boost::shared_ptr<pcl::visualization::PCLVisualizer> visu1 = visualizePC(dilate, "dilate");
		boost::shared_ptr<pcl::visualization::PCLVisualizer> visu3 = visualizePC(dilate2, "dilate2");
		boost::shared_ptr<pcl::visualization::PCLVisualizer> visu5 = visualizePC(dilate2, "dilate2");
		boost::shared_ptr<pcl::visualization::PCLVisualizer> visu2 = visualizeNormal(cloud_ptr, normals, "mathmorph");
		boost::shared_ptr<pcl::visualization::PCLVisualizer> visu4 = visualizeNormal(cloud_ptr, normals, "mathmorph2");
		waitForViewer(visu0);
	}
	else
		cerr << "It needs two values: size_of_structured_element size_of_the_leaf model \n";

	return (0);
}


#include <iostream>
#include <boost/numeric/conversion/cast.hpp>

int main()
{
// this is fine
    const auto s1 = boost::numeric_cast<unsigned short>(42);
    std::cout << "boost::numeric_cast<unsigned short>(42) = " << s1 << "\n";

// this will throw
    try{
        const auto s2 = boost::numeric_cast<unsigned>(-1);
        (void)s2;
    }
    catch(const boost::numeric::bad_numeric_cast& e){
        std::cout << e.what() << "\n";
    }

// so will this
    try{
        const auto s3 = boost::numeric_cast<unsigned short>(100000);
        (void)s3;
    }
    catch(const boost::numeric::positive_overflow& e){
        std::cout << e.what() << "\n";
    }

// exception types:
    // boost::numeric::positive_overflow;
    // boost::numeric::negative_overflow;
    // boost::numeric::bad_numeric_cast;
}


/**
 * IMT Atlantique
 * Author: Xavier CORBILLON
 *
 * Virtual class that represents the scenario interface
 */
#pragma once

#include "adaptationSet.hpp"
#include "user.hpp"
#include "bandwidth.hpp"
#include "SolutionWritter.hpp"
#include "serializationTuple.hpp"

#include <string>
#include <vector>
#include <map>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/map.hpp>

#include <ilcplex/ilocplex.h>

namespace IMT {
class Scenario
{
    public:
	struct SolutionInfo
	{
        SolutionInfo(void):
            m_objectiveValue(INVALIDE_FLOAT_VALUE), m_solutionGap(INVALIDE_FLOAT_VALUE),
            m_solutionStatus("Not Solved Yet"), m_nbStall(), m_displayedViewpoint(), m_downloadRatio() {};
        SolutionInfo(int nbProcessedChunk, int nbLagChunk, int nbViewpoint, int nbTiles, int nbQuality):
            m_objectiveValue(INVALIDE_FLOAT_VALUE), m_solutionGap(INVALIDE_FLOAT_VALUE),
            m_solutionStatus("Not Solved Yet"), m_nbStall(nbProcessedChunk+nbLagChunk,0),
            m_displayedViewpoint(nbProcessedChunk, nbViewpoint+1),
            m_downloadRatio(nbProcessedChunk*(nbProcessedChunk+nbLagChunk)*nbViewpoint*nbTiles*nbQuality+(nbProcessedChunk+nbLagChunk)*nbViewpoint*nbTiles*nbQuality+ nbViewpoint*nbTiles*nbQuality + nbTiles*nbQuality + nbQuality, INVALIDE_FLOAT_VALUE) {}
        float m_objectiveValue;
        float m_solutionGap;
        std::string m_solutionStatus;
        std::vector<float> m_nbStall;
        std::vector<int> m_displayedViewpoint;
        std::vector<float> m_downloadRatio;
        template<class Archive>
        void serialize(Archive& ar, const unsigned int version)
        {
            ar & BOOST_SERIALIZATION_NVP(m_objectiveValue);
            ar & BOOST_SERIALIZATION_NVP(m_solutionGap);
            ar & BOOST_SERIALIZATION_NVP(m_solutionStatus);
            ar & BOOST_SERIALIZATION_NVP(m_nbStall);
            ar & BOOST_SERIALIZATION_NVP(m_displayedViewpoint);
            ar & BOOST_SERIALIZATION_NVP(m_downloadRatio);
        }
	};
        Scenario(int nbUsers, int nbBandwidth, int nbChunk, int nbProcessedChunk, int nbLagChunk, int nbViewpoint, int nbTiles, int nbQuality, float optimalGap, int nbThread):
            m_as(nbViewpoint, nbTiles, nbQuality, nbChunk),
            m_users(nbUsers, User(nbViewpoint, nbTiles, nbChunk)), m_bandwidths(nbBandwidth, Bandwidth(nbChunk, nbLagChunk)),
            m_optimalGap(optimalGap), m_nbThread(nbThread),
            m_solInfo(),
            m_nbChunk(nbChunk), m_nbProcessedChunk(nbProcessedChunk), m_nbLagChunk(nbLagChunk), m_nbViewpoint(nbViewpoint), m_nbTiles(nbTiles),
            m_nbQuality(nbQuality)
            //m_downloadRatio()
            {
                for (int chunkOffset = 0; chunkOffset < nbChunk; chunkOffset += nbProcessedChunk)
                {
                    for (int u = 0; u < m_users.size(); ++u)
                    {
                        for(int b = 0; b < m_bandwidths.size(); ++b)
                        {
                            m_solInfo.emplace_back(m_nbProcessedChunk, m_nbLagChunk, m_nbViewpoint, m_nbTiles, m_nbQuality);
                        }
                    }
                }
            }
        virtual ~Scenario(void) {}

        template<class Archive>
        void serialize(Archive& ar, const unsigned int version)
        {
            ar & BOOST_SERIALIZATION_NVP(m_users);
            ar & BOOST_SERIALIZATION_NVP(m_as);
            ar & BOOST_SERIALIZATION_NVP(m_bandwidths);
            ar & BOOST_SERIALIZATION_NVP(m_optimalGap);
            ar & BOOST_SERIALIZATION_NVP(m_solInfo);
            ar & BOOST_SERIALIZATION_NVP(m_nbChunk);
            ar & BOOST_SERIALIZATION_NVP(m_nbLagChunk);
            ar & BOOST_SERIALIZATION_NVP(m_nbViewpoint);
            ar & BOOST_SERIALIZATION_NVP(m_nbTiles);
            ar & BOOST_SERIALIZATION_NVP(m_nbQuality);
        }

        void Init(const std::string& pathToAdaptationSetConf, const std::vector<std::string>& pathToUserVisibilityConf, const std::vector<std::string>& pathToBandwidthConf)
        {
            m_as.Init(pathToAdaptationSetConf);
            for (int u = 0; u < m_users.size(); ++u)
            {
                m_users[u].Init(pathToUserVisibilityConf[u]);
            }
            for (int b = 0; b < m_bandwidths.size(); ++b)
            {
                m_bandwidths[b].Init(pathToBandwidthConf[b]);
            }
        }

        void Run(std::string outputScenarioId, std::string printableScenarioName, double scenariosAverageDuration, int scenarioId, int nbScenarios);
        Solution PrintResults(void) const;

        int GetNbChunk(void) const {return m_nbChunk;}
        int GetNbProcessedChunk(void) const {return m_nbProcessedChunk;}
        int GetNbLagChunk(void) const {return m_nbLagChunk;}
        int GetNbViewpoint(void) const {return m_nbViewpoint;}
        int GetNbTile(void) const {return m_nbTiles;}
        int GetNbQuality(void) const {return m_nbQuality;}
        float GetTotalDownloadRatio(int chunkId, int viewpointId, int tilesId, int qualityId, int k) const
        {
            float total(0);
            for (int lagChunkId = -m_nbLagChunk; lagChunkId < int(m_nbChunk); ++lagChunkId)
            {
                total += m_solInfo[k].m_downloadRatio.at(ToDownloadRatioVecId(chunkId, lagChunkId, viewpointId, tilesId, qualityId));
            }
            return total;
        }
    protected:
        AdaptationSet m_as;
        std::vector<User> m_users;
        std::vector<Bandwidth> m_bandwidths;
        float m_optimalGap;
        int m_nbThread;
        std::vector<SolutionInfo> m_solInfo;
        //variables related to the Cplex model
        IloEnv m_env;
        IloModel m_model;
        IloIntVarArray m_x; //selected representation
        IloIntVarArray m_y; //selected viewpoint
        IloNumVarArray m_d; //downloaded segments
        IloIntVarArray m_ds; //downloaded chunk
        IloIntVarArray m_stall; //stall duration
        IloIntVarArray m_stallEvent; //assess a stall event
        IloNumVarArray m_varr; //qualityVariance
        IloNumVarArray m_displayedQuality; //qualityVariance
        IloNumVarArray m_stallQad; //qualityVariance

        int GetSolId(int ProcessId, int userId, int bandwidthId) const {return ProcessId*m_users.size()*m_bandwidths.size() + userId*m_bandwidths.size() + bandwidthId;}
        int GetXId(int chunkId, int viewpointId, int tilesId, int qualityId) const
        {return chunkId*m_nbViewpoint*m_nbTiles*(1+m_nbQuality)+viewpointId*m_nbTiles*(1+m_nbQuality)+tilesId*(1+m_nbQuality)+qualityId+1;}
        int GetYId(int chunkId, int viewpointId, int switchId, int u, int k) const {return chunkId*m_nbViewpoint*(m_users[u].GetNbSwitch(k, GetNbProcessedChunk())+1)+viewpointId*(m_users[u].GetNbSwitch(k, GetNbProcessedChunk())+1)+switchId;}
        int GetDId(int chunkId, int lagChunkId, int viewpointId, int tilesId, int qualityId) const
        {return ToDownloadRatioVecId(chunkId, lagChunkId, viewpointId, tilesId, qualityId);}
        int GetDsId(int chunkId, int lagChunkId) const
        {return chunkId*(m_nbProcessedChunk+m_nbLagChunk)+ lagChunkId+m_nbLagChunk;}
        void SetDownloadRatio(int chunkId, int lagChunkId, int viewpointId, int tilesId, int qualityId, int k, float downloadRatio)
        {m_solInfo[k].m_downloadRatio[ToDownloadRatioVecId(chunkId, lagChunkId, viewpointId, tilesId, qualityId)] = downloadRatio;}
        void SetBandwidthConstraint(int k, int u, int b);
        void SetTransmissionDelayConstraint(int k, int u, int b);
        void SetDownloadOnceConstraint(int k, int u, int b);
        void SetDownloadDeadlineConstraint(int k, int u, int b);
        virtual void SetSelectAndDownloadedConstraint(int k, int u, int b);
        void SetSelectOneIfVisibleConstraint(int k, int u, int b);
        void SetDisplayOneAndOnlyOneViewpointContraint(int k, int u, int b);
        void SetSelectIfDisplayedViewpointConstraint(int k, int u, int b);
        void SetViewpointSwitchingConstraint(int k, int u, int b);
        void SetDownloadedChunkIndicator(int k, int u, int b);
        void SetInOrderSelection(int k, int u, int b);
        virtual void Presolve(int k, int u, int b);
        void SetObjective(int k, int u, int b);
        void GetResults(IloCplex& cplex, int k, int u, int b);
    //private:
        int m_nbChunk;
        int m_nbProcessedChunk;
        int m_nbLagChunk;
        int m_nbViewpoint;
        int m_nbTiles;
        int m_nbQuality;
        //std::map<std::tuple<int, int, int, int ,int>,int> m_downloadRatio;

        int ToDownloadRatioVecId(int chunkId, int lagChunkId, int viewpointId, int tilesId, int qualityId) const
        {return chunkId*(m_nbProcessedChunk+m_nbLagChunk)*m_nbViewpoint*m_nbTiles*m_nbQuality+(lagChunkId+m_nbLagChunk)*m_nbViewpoint*m_nbTiles*m_nbQuality+
            viewpointId*m_nbTiles*m_nbQuality + tilesId*m_nbQuality + qualityId;}

        void InitCplexVariables(int k, int u, int b);
        void ClearCplexVariables(IloCplex& cplex, int k, int u, int b);
        virtual void ClearCplexVariablesImpl(int k, int u, int b) {};
        virtual void RunImpl(int k, int u, int b) {};
        int GetNbK(void) const { return std::min(7, GetNbChunk()/GetNbProcessedChunk());};
};
}


//
//  Copyright (c) 2000-2002
//  Joerg Walter, Mathias Koch
//
//  Permission to use, copy, modify, distribute and sell this software
//  and its documentation for any purpose is hereby granted without fee,
//  provided that the above copyright notice appear in all copies and
//  that both that copyright notice and this permission notice appear
//  in supporting documentation.  The authors make no representations
//  about the suitability of this software for any purpose.
//  It is provided "as is" without express or implied warranty.
//
//  The authors gratefully acknowledge the support of
//  GeNeSys mbH & Co. KG in producing this work.
//

#ifndef BOOST_UBLAS_TRAITS_H
#define BOOST_UBLAS_TRAITS_H

#include <algorithm>
#include <cmath>
#include <complex>

#include <boost/numeric/ublas/config.hpp>

// Promote traits borrowed from Todd Veldhuizen

namespace boost { namespace numeric { namespace ublas {

    template<class T>
    struct type_traits {
        typedef type_traits<T> self_type;
        typedef T value_type;
        typedef const T &const_reference;
        typedef T &reference;
        typedef T real_type;
        typedef T precision_type;

        BOOST_STATIC_CONSTANT (std::size_t, plus_complexity = 0);
        BOOST_STATIC_CONSTANT (std::size_t, multiplies_complexity = 0);

        static
        BOOST_UBLAS_INLINE
        real_type real (const_reference t);
        static
        BOOST_UBLAS_INLINE
        real_type imag (const_reference t);
        static
        BOOST_UBLAS_INLINE
        value_type conj (const_reference t);

        static
        BOOST_UBLAS_INLINE
        real_type abs (const_reference t);
        static
        BOOST_UBLAS_INLINE
        value_type sqrt (const_reference t);

        static
        BOOST_UBLAS_INLINE
        real_type norm_1 (const_reference t) {
            // Oops, should have known that!
            return type_traits<real_type>::abs (self_type::real (t)) +
                   type_traits<real_type>::abs (self_type::imag (t));
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_2 (const_reference t) {
            return self_type::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_inf (const_reference t) {
            // Oops, should have known that!
            return std::max (type_traits<real_type>::abs (self_type::real (t)),
                             type_traits<real_type>::abs (self_type::imag (t)));
        }

        static
        BOOST_UBLAS_INLINE
        bool equals (const_reference t1, const_reference t2) {
            // Check, that the values match at least half.
            static real_type sqrt_epsilon (type_traits<real_type>::sqrt (std::numeric_limits<real_type>::epsilon ()));
            return self_type::norm_inf (t1 - t2) < sqrt_epsilon *
                   std::max (std::max (self_type::norm_inf (t1),
                                       self_type::norm_inf (t2)),
                             std::numeric_limits<real_type>::min ());
        }
    };

    template<>
    struct type_traits<float> {
        typedef type_traits<float> self_type;
        typedef float value_type;
        typedef float const_reference;
        typedef float &reference;
        typedef float real_type;
        typedef double precision_type;

        BOOST_STATIC_CONSTANT (std::size_t, plus_complexity = 1);
        BOOST_STATIC_CONSTANT (std::size_t, multiplies_complexity = 1);

        static
        BOOST_UBLAS_INLINE
        real_type real (const_reference t) {
                return t;
        }
        static
        BOOST_UBLAS_INLINE
        real_type imag (const_reference t) {
                return 0;
        }
        static
        BOOST_UBLAS_INLINE
        value_type conj (const_reference t) {
                return t;
        }

        static
        BOOST_UBLAS_INLINE
        real_type abs (const_reference t) {
#if defined (BOOST_NO_STDC_NAMESPACE) || defined (BOOST_UBLAS_NO_CMATH)
            return ::fabsf (t);
#else
            return std::abs (t);
#endif
        }
        static
        BOOST_UBLAS_INLINE
        value_type sqrt (const_reference t) {
#if defined (BOOST_NO_STDC_NAMESPACE) || defined (BOOST_UBLAS_NO_CMATH)
            return ::sqrtf (t);
#else
            return std::sqrt (t);
#endif
        }

        static
        BOOST_UBLAS_INLINE
        real_type norm_1 (const_reference t) {
            return self_type::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_2 (const_reference t) {
            return self_type::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_inf (const_reference t) {
            return self_type::abs (t);
        }

        static
        BOOST_UBLAS_INLINE
        bool equals (const_reference t1, const_reference t2) {
            // Check, that the values match at least half.
            static real_type sqrt_epsilon (type_traits<real_type>::sqrt (std::numeric_limits<real_type>::epsilon ()));
            return self_type::norm_inf (t1 - t2) < sqrt_epsilon *
                   std::max (std::max (self_type::norm_inf (t1),
                                       self_type::norm_inf (t2)),
                             std::numeric_limits<real_type>::min ());
        }
    };
    template<>
    struct type_traits<double> {
        typedef type_traits<double> self_type;
        typedef double value_type;
        typedef double const_reference;
        typedef double &reference;
        typedef double real_type;
#ifndef BOOST_UBLAS_USE_LONG_DOUBLE
        typedef double precision_type;
#else
        typedef long double precision_type;
#endif

        BOOST_STATIC_CONSTANT (std::size_t, plus_complexity = 1);
        BOOST_STATIC_CONSTANT (std::size_t, multiplies_complexity = 1);

        static
        BOOST_UBLAS_INLINE
        real_type real (const_reference t) {
                return t;
        }
        static
        BOOST_UBLAS_INLINE
        real_type imag (const_reference t) {
                return 0;
        }
        static
        BOOST_UBLAS_INLINE
        value_type conj (const_reference t) {
                return t;
        }

        static
        BOOST_UBLAS_INLINE
        real_type abs (const_reference t) {
#if defined (BOOST_NO_STDC_NAMESPACE) || defined (BOOST_UBLAS_NO_CMATH)
            return ::fabs (t);
#else
            return std::abs (t);
#endif
        }
        static
        BOOST_UBLAS_INLINE
        value_type sqrt (const_reference t) {
#if defined (BOOST_NO_STDC_NAMESPACE) || defined (BOOST_UBLAS_NO_CMATH)
            return ::sqrt (t);
#else
            return std::sqrt (t);
#endif
        }

        static
        BOOST_UBLAS_INLINE
        real_type norm_1 (const_reference t) {
            return self_type::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_2 (const_reference t) {
            return self_type::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_inf (const_reference t) {
            return self_type::abs (t);
        }

        static
        BOOST_UBLAS_INLINE
        bool equals (const_reference t1, const_reference t2) {
            // Check, that the values match at least half.
            static real_type sqrt_epsilon (type_traits<real_type>::sqrt (std::numeric_limits<real_type>::epsilon ()));
            return self_type::norm_inf (t1 - t2) < sqrt_epsilon *
                   std::max (std::max (self_type::norm_inf (t1),
                                       self_type::norm_inf (t2)),
                             std::numeric_limits<real_type>::min ());
        }
    };
#ifdef BOOST_UBLAS_USE_LONG_DOUBLE
    template<>
    struct type_traits<long double> {
        typedef type_traits<long double> self_type;
        typedef long double value_type;
        typedef long double const_reference;
        typedef long double &reference;
        typedef long double real_type;
        typedef long double precision_type;

        BOOST_STATIC_CONSTANT (std::size_t, plus_complexity = 1);
        BOOST_STATIC_CONSTANT (std::size_t, multiplies_complexity = 1);

        static
        BOOST_UBLAS_INLINE
        real_type real (const_reference t) {
                return t;
        }
        static
        BOOST_UBLAS_INLINE
        real_type imag (const_reference t) {
                return 0;
        }
        static
        BOOST_UBLAS_INLINE
        value_type conj (const_reference t) {
                return t;
        }

        static
        BOOST_UBLAS_INLINE
        real_type abs (const_reference t) {
#if defined (BOOST_NO_STDC_NAMESPACE) || defined (BOOST_UBLAS_NO_CMATH)
            return ::fabsl (t);
#else
            return std::abs (t);
#endif
        }
        static
        BOOST_UBLAS_INLINE
        value_type sqrt (const_reference t) {
#if defined (BOOST_NO_STDC_NAMESPACE) || defined (BOOST_UBLAS_NO_CMATH)
            return ::sqrtl (t);
#else
            return std::sqrt (t);
#endif
        }

        static
        BOOST_UBLAS_INLINE
        real_type norm_1 (const_reference t) {
            return self_type::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_2 (const_reference t) {
            return self_type::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_inf (const_reference t) {
            return self_type::abs (t);
        }

        static
        BOOST_UBLAS_INLINE
        bool equals (const_reference t1, const_reference t2) {
            // Check, that the values match at least half.
            static real_type sqrt_epsilon (type_traits<real_type>::sqrt (std::numeric_limits<real_type>::epsilon ()));
            return self_type::norm_inf (t1 - t2) < sqrt_epsilon *
                   std::max (std::max (self_type::norm_inf (t1),
                                       self_type::norm_inf (t2)),
                             std::numeric_limits<real_type>::min ());
        }
    };
#endif

    template<>
    struct type_traits<std::complex<float> > {
        typedef type_traits<std::complex<float> > self_type;
        typedef std::complex<float> value_type;
        typedef const std::complex<float> &const_reference;
        typedef std::complex<float> &reference;
        typedef float real_type;
        typedef std::complex<double> precision_type;

        BOOST_STATIC_CONSTANT (std::size_t, plus_complexity = 2);
        BOOST_STATIC_CONSTANT (std::size_t, multiplies_complexity = 6);

        static
        BOOST_UBLAS_INLINE
        real_type real (const_reference t) {
                // return t.real ();
                return std::real (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type imag (const_reference t) {
                // return t.imag ();
                return std::imag (t);
        }
        static
        BOOST_UBLAS_INLINE
        value_type conj (const_reference t) {
                // return t.conj ();
                return std::conj (t);
        }

        static
        BOOST_UBLAS_INLINE
        real_type abs (const_reference t) {
                return std::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        value_type sqrt (const_reference t) {
                return std::sqrt (t);
        }

        static
        BOOST_UBLAS_INLINE
        real_type norm_1 (const_reference t) {
            // Oops, should have known that!
            return type_traits<real_type>::abs (self_type::real (t)) +
                   type_traits<real_type>::abs (self_type::imag (t));
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_2 (const_reference t) {
            return self_type::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_inf (const_reference t) {
            // Oops, should have known that!
            return std::max (type_traits<real_type>::abs (self_type::real (t)),
                             type_traits<real_type>::abs (self_type::imag (t)));
        }

        static
        BOOST_UBLAS_INLINE
        bool equals (const_reference t1, const_reference t2) {
            // Check, that the values match at least half.
            static real_type sqrt_epsilon (type_traits<real_type>::sqrt (std::numeric_limits<real_type>::epsilon ()));
            return self_type::norm_inf (t1 - t2) < sqrt_epsilon *
                   std::max (std::max (self_type::norm_inf (t1),
                                       self_type::norm_inf (t2)),
                             std::numeric_limits<real_type>::min ());
        }
    };
    template<>
    struct type_traits<std::complex<double> > {
        typedef type_traits<std::complex<double> > self_type;
        typedef std::complex<double> value_type;
        typedef const std::complex<double> &const_reference;
        typedef std::complex<double> &reference;
        typedef double real_type;
#ifndef BOOST_UBLAS_USE_LONG_DOUBLE
        typedef std::complex<double> precision_type;
#else
        typedef std::complex<long double> precision_type;
#endif

        BOOST_STATIC_CONSTANT (std::size_t, plus_complexity = 2);
        BOOST_STATIC_CONSTANT (std::size_t, multiplies_complexity = 6);

        static
        BOOST_UBLAS_INLINE
        real_type real (const_reference t) {
                // return t.real ();
                return std::real (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type imag (const_reference t) {
                // return t.imag ();
                return std::imag (t);
        }
        static
        BOOST_UBLAS_INLINE
        value_type conj (const_reference t) {
                // return t.conj ();
                return std::conj (t);
        }

        static
        BOOST_UBLAS_INLINE
        real_type abs (const_reference t) {
                return std::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        value_type sqrt (const_reference t) {
                return std::sqrt (t);
        }

        static
        BOOST_UBLAS_INLINE
        real_type norm_1 (const_reference t) {
            // Oops, should have known that!
            return type_traits<real_type>::abs (self_type::real (t)) +
                   type_traits<real_type>::abs (self_type::imag (t));
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_2 (const_reference t) {
            return self_type::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_inf (const_reference t) {
            // Oops, should have known that!
            return std::max (type_traits<real_type>::abs (self_type::real (t)),
                             type_traits<real_type>::abs (self_type::imag (t)));
        }

        static
        BOOST_UBLAS_INLINE
        bool equals (const_reference t1, const_reference t2) {
            // Check, that the values match at least half.
            static real_type sqrt_epsilon (type_traits<real_type>::sqrt (std::numeric_limits<real_type>::epsilon ()));
            return self_type::norm_inf (t1 - t2) < sqrt_epsilon *
                   std::max (std::max (self_type::norm_inf (t1),
                                       self_type::norm_inf (t2)),
                             std::numeric_limits<real_type>::min ());
        }
    };
#ifdef BOOST_UBLAS_USE_LONG_DOUBLE
    template<>
    struct type_traits<std::complex<long double> > {
        typedef type_traits<std::complex<long double> > self_type;
        typedef std::complex<long double> value_type;
        typedef const std::complex<long double> &const_reference;
        typedef std::complex<long double> &reference;
        typedef long double real_type;
        typedef std::complex<long double> precision_type;

        BOOST_STATIC_CONSTANT (std::size_t, plus_complexity = 2);
        BOOST_STATIC_CONSTANT (std::size_t, multiplies_complexity = 6);

        static
        BOOST_UBLAS_INLINE
        real_type real (const_reference t) {
                // return t.real ();
                return std::real (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type imag (const_reference t) {
                // return t.imag ();
                return std::imag (t);
        }
        static
        BOOST_UBLAS_INLINE
        value_type conj (const_reference t) {
                // return t.conj ();
                return std::conj (t);
        }

        static
        BOOST_UBLAS_INLINE
        real_type abs (const_reference t) {
                return std::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        value_type sqrt (const_reference t) {
                return std::sqrt (t);
        }

        static
        BOOST_UBLAS_INLINE
        real_type norm_1 (const_reference t) {
            // Oops, should have known that!
            return type_traits<real_type>::abs (self_type::real (t)) +
                   type_traits<real_type>::abs (self_type::imag (t));
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_2 (const_reference t) {
            return self_type::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_inf (const_reference t) {
            // Oops, should have known that!
            return std::max (type_traits<real_type>::abs (self_type::real (t)),
                             type_traits<real_type>::abs (self_type::imag (t)));
        }

        static
        BOOST_UBLAS_INLINE
        bool equals (const_reference t1, const_reference t2) {
            // Check, that the values match at least half.
            static real_type sqrt_epsilon (type_traits<real_type>::sqrt (std::numeric_limits<real_type>::epsilon ()));
            return self_type::norm_inf (t1 - t2) < sqrt_epsilon *
                   std::max (std::max (self_type::norm_inf (t1),
                                       self_type::norm_inf (t2)),
                             std::numeric_limits<real_type>::min ());
        }
    };
#endif

    template<class T1, class T2>
    struct promote_traits {
        // Default promotion will badly fail, if the types are different.
        // Thanks to Kresimir Fresl for spotting this.
        BOOST_STATIC_ASSERT ((boost::is_same<T1, T2>::value));
        typedef T1 promote_type;
    };

    template<>
    struct promote_traits<float, double> {
        typedef double promote_type;
    };
    template<>
    struct promote_traits<double, float> {
        typedef double promote_type;
    };
#ifdef BOOST_UBLAS_USE_LONG_DOUBLE
    template<>
    struct promote_traits<float, long double> {
        typedef long double promote_type;
    };
    template<>
    struct promote_traits<long double, float> {
        typedef long double promote_type;
    };
    template<>
    struct promote_traits<double, long double> {
        typedef long double promote_type;
    };
    template<>
    struct promote_traits<long double, double> {
        typedef long double promote_type;
    };
#endif

    template<>
    struct promote_traits<float, std::complex<float> > {
        typedef std::complex<float> promote_type;
    };
    template<>
    struct promote_traits<std::complex<float>, float> {
        typedef std::complex<float> promote_type;
    };
    template<>
    struct promote_traits<float, std::complex<double> > {
        typedef std::complex<double> promote_type;
    };
    template<>
    struct promote_traits<std::complex<double>, float> {
        typedef std::complex<double> promote_type;
    };
#ifdef BOOST_UBLAS_USE_LONG_DOUBLE
    template<>
    struct promote_traits<float, std::complex<long double> > {
        typedef std::complex<long double> promote_type;
    };
    template<>
    struct promote_traits<std::complex<long double>, float> {
        typedef std::complex<long double> promote_type;
    };
#endif

    template<>
    struct promote_traits<double, std::complex<float> > {
        // Here we'd better go the conservative way.
        // typedef std::complex<float> promote_type;
        typedef std::complex<double> promote_type;
    };
    template<>
    struct promote_traits<std::complex<float>, double> {
        // Here we'd better go the conservative way.
        // typedef std::complex<float> promote_type;
        typedef std::complex<double> promote_type;
    };
    template<>
    struct promote_traits<double, std::complex<double> > {
        typedef std::complex<double> promote_type;
    };
    template<>
    struct promote_traits<std::complex<double>, double> {
        typedef std::complex<double> promote_type;
    };
#ifdef BOOST_UBLAS_USE_LONG_DOUBLE
    template<>
    struct promote_traits<double, std::complex<long double> > {
        typedef std::complex<long double> promote_type;
    };
    template<>
    struct promote_traits<std::complex<long double>, double> {
        typedef std::complex<long double> promote_type;
    };
#endif

#ifdef BOOST_UBLAS_USE_LONG_DOUBLE
    template<>
    struct promote_traits<long double, std::complex<float> > {
        // Here we'd better go the conservative way.
        // typedef std::complex<float> promote_type;
        typedef std::complex<long double> promote_type;
    };
    template<>
    struct promote_traits<std::complex<float>, long double> {
        // Here we'd better go the conservative way.
        // typedef std::complex<float> promote_type;
        typedef std::complex<long double> promote_type;
    };
    template<>
    struct promote_traits<long double, std::complex<double> > {
        // Here we'd better go the conservative way.
        // typedef std::complex<double> promote_type;
        typedef std::complex<long double> promote_type;
    };
    template<>
    struct promote_traits<std::complex<double>, long double> {
        // Here we'd better go the conservative way.
        // typedef std::complex<double> promote_type;
        typedef std::complex<long double> promote_type;
    };
    template<>
    struct promote_traits<long double, std::complex<long double> > {
        typedef std::complex<long double> promote_type;
    };
    template<>
    struct promote_traits<std::complex<long double>, long double> {
        typedef std::complex<long double> promote_type;
    };
#endif

    template<>
    struct promote_traits<std::complex<float>, std::complex<double> > {
        typedef std::complex<double> promote_type;
    };
    template<>
    struct promote_traits<std::complex<double>, std::complex<float> > {
        typedef std::complex<double> promote_type;
    };
#ifdef BOOST_UBLAS_USE_LONG_DOUBLE
    template<>
    struct promote_traits<std::complex<float>, std::complex<long double> > {
        typedef std::complex<long double> promote_type;
    };
    template<>
    struct promote_traits<std::complex<long double>, std::complex<float> > {
        typedef std::complex<long double> promote_type;
    };
    template<>
    struct promote_traits<std::complex<double>, std::complex<long double> > {
        typedef std::complex<long double> promote_type;
    };
    template<>
    struct promote_traits<std::complex<long double>, std::complex<double> > {
        typedef std::complex<long double> promote_type;
    };
#endif

#ifdef BOOST_UBLAS_USE_INTERVAL
    template<>
    struct type_traits<boost::numeric::interval<float> > {
        typedef type_traits<boost::numeric::interval<float> > self_type;
        typedef boost::numeric::interval<float> value_type;
        typedef const boost::numeric::interval<float> &const_reference;
        typedef boost::numeric::interval<float> &reference;
        typedef boost::numeric::interval<float> real_type;
        typedef boost::numeric::interval<double> precision_type;

        BOOST_STATIC_CONSTANT (std::size_t, plus_complexity = 1);
        BOOST_STATIC_CONSTANT (std::size_t, multiplies_complexity = 1);

        static
        BOOST_UBLAS_INLINE
        real_type real (const_reference t) {
                return t;
        }
        static
        BOOST_UBLAS_INLINE
        real_type imag (const_reference t) {
                return 0;
        }
        static
        BOOST_UBLAS_INLINE
        value_type conj (const_reference t) {
                return t;
        }

        static
        BOOST_UBLAS_INLINE
        real_type abs (const_reference t) {
            return boost::numeric::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        value_type sqrt (const_reference t) {
            return boost::numeric::sqrt (t);
        }

        static
        BOOST_UBLAS_INLINE
        real_type norm_1 (const_reference t) {
            return self_type::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_2 (const_reference t) {
            return self_type::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_inf (const_reference t) {
            return self_type::abs (t);
        }

        static
        BOOST_UBLAS_INLINE
        bool equals (const_reference t1, const_reference t2) {
            // Check, that the values match at least half.
            static real_type sqrt_epsilon (type_traits<real_type>::sqrt (std::numeric_limits<real_type>::epsilon ()));
            return self_type::norm_inf (t1 - t2) < sqrt_epsilon *
                   std::max (std::max (self_type::norm_inf (t1),
                                       self_type::norm_inf (t2)),
                             std::numeric_limits<real_type>::min ());
        }
    };
    template<>
    struct type_traits<boost::numeric::interval<double> > {
        typedef type_traits<boost::numeric::interval<double> > self_type;
        typedef boost::numeric::interval<double> value_type;
        typedef const boost::numeric::interval<double> &const_reference;
        typedef boost::numeric::interval<double> &reference;
        typedef boost::numeric::interval<double> real_type;
#ifndef BOOST_UBLAS_USE_LONG_DOUBLE
        typedef boost::numeric::interval<double> precision_type;
#else
        typedef boost::numeric::interval<boost::numeric::interval<long double> > precision_type;
#endif

        BOOST_STATIC_CONSTANT (std::size_t, plus_complexity = 1);
        BOOST_STATIC_CONSTANT (std::size_t, multiplies_complexity = 1);

        static
        BOOST_UBLAS_INLINE
        real_type real (const_reference t) {
                return t;
        }
        static
        BOOST_UBLAS_INLINE
        real_type imag (const_reference t) {
                return 0;
        }
        static
        BOOST_UBLAS_INLINE
        value_type conj (const_reference t) {
                return t;
        }

        static
        BOOST_UBLAS_INLINE
        real_type abs (const_reference t) {
            return boost::numeric::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        value_type sqrt (const_reference t) {
            return boost::numeric::sqrt (t);
        }

        static
        BOOST_UBLAS_INLINE
        real_type norm_1 (const_reference t) {
            return self_type::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_2 (const_reference t) {
            return self_type::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_inf (const_reference t) {
            return self_type::abs (t);
        }

        static
        BOOST_UBLAS_INLINE
        bool equals (const_reference t1, const_reference t2) {
            // Check, that the values match at least half.
            static real_type sqrt_epsilon (type_traits<real_type>::sqrt (std::numeric_limits<real_type>::epsilon ()));
            return self_type::norm_inf (t1 - t2) < sqrt_epsilon *
                   std::max (std::max (self_type::norm_inf (t1),
                                       self_type::norm_inf (t2)),
                             std::numeric_limits<real_type>::min ());
        }
    };
#ifdef BOOST_UBLAS_USE_LONG_DOUBLE
    template<>
    struct type_traits<boost::numeric::interval<long double> > {
        typedef type_traits<boost::numeric::interval<long double> > self_type;
        typedef boost::numeric::interval<long double> value_type;
        typedef const boost::numeric::interval<long double> &const_reference;
        typedef boost::numeric::interval<long double> &reference;
        typedef boost::numeric::interval<long double> real_type;
        typedef boost::numeric::interval<long double> precision_type;

        BOOST_STATIC_CONSTANT (std::size_t, plus_complexity = 1);
        BOOST_STATIC_CONSTANT (std::size_t, multiplies_complexity = 1);

        static
        BOOST_UBLAS_INLINE
        real_type real (const_reference t) {
                return t;
        }
        static
        BOOST_UBLAS_INLINE
        real_type imag (const_reference t) {
                return 0;
        }
        static
        BOOST_UBLAS_INLINE
        value_type conj (const_reference t) {
                return t;
        }

        static
        BOOST_UBLAS_INLINE
        real_type abs (const_reference t) {
            return boost::numeric::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        value_type sqrt (const_reference t) {
            return boost::numeric::sqrt (t);
        }

        static
        BOOST_UBLAS_INLINE
        real_type norm_1 (const_reference t) {
            return self_type::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_2 (const_reference t) {
            return self_type::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_inf (const_reference t) {
            return self_type::abs (t);
        }

        static
        BOOST_UBLAS_INLINE
        bool equals (const_reference t1, const_reference t2) {
            // Check, that the values match at least half.
            static real_type sqrt_epsilon (type_traits<real_type>::sqrt (std::numeric_limits<real_type>::epsilon ()));
            return self_type::norm_inf (t1 - t2) < sqrt_epsilon *
                   std::max (std::max (self_type::norm_inf (t1),
                                       self_type::norm_inf (t2)),
                             std::numeric_limits<real_type>::min ());
        }
    };
#endif

#ifdef BOOST_UBLAS_USE_BOOST_COMPLEX
    template<>
    struct type_traits<boost::complex<boost::numeric::interval<float> > > {
        typedef type_traits<boost::complex<boost::numeric::interval<float> > > self_type;
        typedef boost::complex<boost::numeric::interval<float> > value_type;
        typedef const boost::complex<boost::numeric::interval<float> > &const_reference;
        typedef boost::complex<boost::numeric::interval<float> > &reference;
        typedef boost::numeric::interval<float> real_type;
        typedef boost::complex<double> precision_type;

        BOOST_STATIC_CONSTANT (std::size_t, plus_complexity = 2);
        BOOST_STATIC_CONSTANT (std::size_t, multiplies_complexity = 6);

        static
        BOOST_UBLAS_INLINE
        real_type real (const_reference t) {
                // return t.real ();
                return std::real (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type imag (const_reference t) {
                // return t.imag ();
                return std::imag (t);
        }
        static
        BOOST_UBLAS_INLINE
        value_type conj (const_reference t) {
                // return t.conj ();
                return std::conj (t);
        }

        static
        BOOST_UBLAS_INLINE
        real_type abs (const_reference t) {
                return boost::numeric::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        value_type sqrt (const_reference t) {
                return boost::numeric::sqrt (t);
        }

        static
        BOOST_UBLAS_INLINE
        real_type norm_1 (const_reference t) {
            // Oops, should have known that!
            return type_traits<real_type>::abs (self_type::real (t)) +
                   type_traits<real_type>::abs (self_type::imag (t));
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_2 (const_reference t) {
            return self_type::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_inf (const_reference t) {
            // Oops, should have known that!
            return std::max (type_traits<real_type>::abs (self_type::real (t)),
                             type_traits<real_type>::abs (self_type::imag (t)));
        }

        static
        BOOST_UBLAS_INLINE
        bool equals (const_reference t1, const_reference t2) {
            // Check, that the values match at least half.
            static real_type sqrt_epsilon (type_traits<real_type>::sqrt (std::numeric_limits<real_type>::epsilon ()));
            return self_type::norm_inf (t1 - t2) < sqrt_epsilon *
                   std::max (std::max (self_type::norm_inf (t1),
                                       self_type::norm_inf (t2)),
                             std::numeric_limits<real_type>::min ());
        }
    };
    template<>
    struct type_traits<boost::complex<double> > {
        typedef type_traits<boost::complex<double> > self_type;
        typedef boost::complex<double> value_type;
        typedef const boost::complex<double> &const_reference;
        typedef boost::complex<double> &reference;
        typedef double real_type;
#ifndef BOOST_UBLAS_USE_LONG_DOUBLE
        typedef boost::complex<double> precision_type;
#else
        typedef boost::complex<boost::numeric::interval<long double> > precision_type;
#endif

        BOOST_STATIC_CONSTANT (std::size_t, plus_complexity = 2);
        BOOST_STATIC_CONSTANT (std::size_t, multiplies_complexity = 6);

        static
        BOOST_UBLAS_INLINE
        real_type real (const_reference t) {
                // return t.real ();
                return std::real (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type imag (const_reference t) {
                // return t.imag ();
                return std::imag (t);
        }
        static
        BOOST_UBLAS_INLINE
        value_type conj (const_reference t) {
                // return t.conj ();
                return std::conj (t);
        }

        static
        BOOST_UBLAS_INLINE
        real_type abs (const_reference t) {
                return boost::numeric::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        value_type sqrt (const_reference t) {
                return boost::numeric::sqrt (t);
        }

        static
        BOOST_UBLAS_INLINE
        real_type norm_1 (const_reference t) {
            // Oops, should have known that!
            return type_traits<real_type>::abs (self_type::real (t)) +
                   type_traits<real_type>::abs (self_type::imag (t));
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_2 (const_reference t) {
            return self_type::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_inf (const_reference t) {
            // Oops, should have known that!
            return std::max (type_traits<real_type>::abs (self_type::real (t)),
                             type_traits<real_type>::abs (self_type::imag (t)));
        }

        static
        BOOST_UBLAS_INLINE
        bool equals (const_reference t1, const_reference t2) {
            // Check, that the values match at least half.
            static real_type sqrt_epsilon (type_traits<real_type>::sqrt (std::numeric_limits<real_type>::epsilon ()));
            return self_type::norm_inf (t1 - t2) < sqrt_epsilon *
                   std::max (std::max (self_type::norm_inf (t1),
                                       self_type::norm_inf (t2)),
                             std::numeric_limits<real_type>::min ());
        }
    };
#ifdef BOOST_UBLAS_USE_LONG_DOUBLE
    template<>
    struct type_traits<boost::complex<boost::numeric::interval<long double> > > {
        typedef type_traits<boost::complex<boost::numeric::interval<long double> > > self_type;
        typedef boost::complex<boost::numeric::interval<long double> > value_type;
        typedef const boost::complex<boost::numeric::interval<long double> > &const_reference;
        typedef boost::complex<boost::numeric::interval<long double> > &reference;
        typedef boost::numeric::interval<long double> real_type;
        typedef boost::complex<boost::numeric::interval<long double> > precision_type;

        BOOST_STATIC_CONSTANT (std::size_t, plus_complexity = 2);
        BOOST_STATIC_CONSTANT (std::size_t, multiplies_complexity = 6);

        static
        BOOST_UBLAS_INLINE
        real_type real (const_reference t) {
                // return t.real ();
                return std::real (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type imag (const_reference t) {
                // return t.imag ();
                return std::imag (t);
        }
        static
        BOOST_UBLAS_INLINE
        value_type conj (const_reference t) {
                // return t.conj ();
                return std::conj (t);
        }

        static
        BOOST_UBLAS_INLINE
        real_type abs (const_reference t) {
                return boost::numeric::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        value_type sqrt (const_reference t) {
                return boost::numeric::sqrt (t);
        }

        static
        BOOST_UBLAS_INLINE
        real_type norm_1 (const_reference t) {
            // Oops, should have known that!
            return type_traits<real_type>::abs (self_type::real (t)) +
                   type_traits<real_type>::abs (self_type::imag (t));
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_2 (const_reference t) {
            return self_type::abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_inf (const_reference t) {
            // Oops, should have known that!
            return std::max (type_traits<real_type>::abs (self_type::real (t)),
                             type_traits<real_type>::abs (self_type::imag (t)));
        }

        static
        BOOST_UBLAS_INLINE
        bool equals (const_reference t1, const_reference t2) {
            // Check, that the values match at least half.
            static real_type sqrt_epsilon (type_traits<real_type>::sqrt (std::numeric_limits<real_type>::epsilon ()));
            return self_type::norm_inf (t1 - t2) < sqrt_epsilon *
                   std::max (std::max (self_type::norm_inf (t1),
                                       self_type::norm_inf (t2)),
                             std::numeric_limits<real_type>::min ());
        }
    };
#endif
#endif

    template<>
    struct promote_traits<boost::numeric::interval<float>, boost::numeric::interval<double> > {
        typedef boost::numeric::interval<double> promote_type;
    };
    template<>
    struct promote_traits<boost::numeric::interval<double>, boost::numeric::interval<float> > {
        typedef boost::numeric::interval<double> promote_type;
    };
#ifdef BOOST_UBLAS_USE_LONG_DOUBLE
    template<>
    struct promote_traits<boost::numeric::interval<float>, boost::numeric::interval<long double> > {
        typedef boost::numeric::interval<long double> promote_type;
    };
    template<>
    struct promote_traits<boost::numeric::interval<long double>, boost::numeric::interval<float> > {
        typedef boost::numeric::interval<long double> promote_type;
    };
    template<>
    struct promote_traits<boost::numeric::interval<double>, boost::numeric::interval<long double> > {
        typedef boost::numeric::interval<long double> promote_type;
    };
    template<>
    struct promote_traits<boost::numeric::interval<long double>, boost::numeric::interval<double> > {
        typedef boost::numeric::interval<long double> promote_type;
    };
#endif

#ifdef BOOST_UBLAS_USE_BOOST_COMPLEX
    template<>
    struct promote_traits<boost::numeric::interval<float>, boost::complex<boost::numeric::interval<float> > > {
        typedef boost::complex<boost::numeric::interval<float> > promote_type;
    };
    template<>
    struct promote_traits<boost::complex<boost::numeric::interval<float> >, boost::numeric::interval<float> > {
        typedef boost::complex<boost::numeric::interval<float> > promote_type;
    };
    template<>
    struct promote_traits<boost::numeric::interval<float>, boost::complex<boost::numeric::interval<double> > > {
        typedef boost::complex<boost::numeric::interval<double> > promote_type;
    };
    template<>
    struct promote_traits<boost::complex<boost::numeric::interval<double> >, boost::numeric::interval<float> > {
        typedef boost::complex<boost::numeric::interval<double> > promote_type;
    };
#ifdef BOOST_UBLAS_USE_LONG_DOUBLE
    template<>
    struct promote_traits<boost::numeric::interval<float>, boost::complex<boost::numeric::interval<long double> > > {
        typedef boost::complex<boost::numeric::interval<long double> > promote_type;
    };
    template<>
    struct promote_traits<boost::complex<boost::numeric::interval<long double> >, boost::numeric::interval<float> > {
        typedef boost::complex<boost::numeric::interval<long double> > promote_type;
    };
#endif

    template<>
    struct promote_traits<boost::numeric::interval<double>, boost::complex<boost::numeric::interval<float> > > {
        // Here we'd better go the conservative way.
        // typedef boost::complex<boost::numeric::interval<float> > promote_type;
        typedef boost::complex<boost::numeric::interval<double> > promote_type;
    };
    template<>
    struct promote_traits<boost::complex<boost::numeric::interval<float> >, boost::numeric::interval<double> > {
        // Here we'd better go the conservative way.
        // typedef boost::complex<boost::numeric::interval<float> > promote_type;
        typedef boost::complex<boost::numeric::interval<double> > promote_type;
    };
    template<>
    struct promote_traits<boost::numeric::interval<double>, boost::complex<boost::numeric::interval<double> > > {
        typedef boost::complex<boost::numeric::interval<double> > promote_type;
    };
    template<>
    struct promote_traits<boost::complex<boost::numeric::interval<double> >, boost::numeric::interval<double> > {
        typedef boost::complex<boost::numeric::interval<double> > promote_type;
    };
#ifdef BOOST_UBLAS_USE_LONG_DOUBLE
    template<>
    struct promote_traits<boost::numeric::interval<double>, boost::complex<boost::numeric::interval<long double> > > {
        typedef boost::complex<boost::numeric::interval<long double> > promote_type;
    };
    template<>
    struct promote_traits<boost::complex<boost::numeric::interval<long double> >, boost::numeric::interval<double> > {
        typedef boost::complex<boost::numeric::interval<long double> > promote_type;
    };
#endif

#ifdef BOOST_UBLAS_USE_LONG_DOUBLE
    template<>
    struct promote_traits<boost::numeric::interval<long double>, boost::complex<boost::numeric::interval<float> > > {
        // Here we'd better go the conservative way.
        // typedef boost::complex<boost::numeric::interval<float> > promote_type;
        typedef boost::complex<boost::numeric::interval<long double> > promote_type;
    };
    template<>
    struct promote_traits<boost::complex<boost::numeric::interval<float> >, boost::numeric::interval<long double> > {
        // Here we'd better go the conservative way.
        // typedef boost::complex<boost::numeric::interval<float> > promote_type;
        typedef boost::complex<boost::numeric::interval<long double> > promote_type;
    };
    template<>
    struct promote_traits<boost::numeric::interval<long double>, boost::complex<boost::numeric::interval<double> > > {
        // Here we'd better go the conservative way.
        // typedef boost::complex<boost::numeric::interval<double> > promote_type;
        typedef boost::complex<boost::numeric::interval<long double> > promote_type;
    };
    template<>
    struct promote_traits<boost::complex<boost::numeric::interval<double> >, boost::numeric::interval<long double> > {
        // Here we'd better go the conservative way.
        // typedef boost::complex<boost::numeric::interval<double> > promote_type;
        typedef boost::complex<boost::numeric::interval<long double> > promote_type;
    };
    template<>
    struct promote_traits<boost::numeric::interval<long double>, boost::complex<boost::numeric::interval<long double> > > {
        typedef boost::complex<boost::numeric::interval<long double> > promote_type;
    };
    template<>
    struct promote_traits<boost::complex<boost::numeric::interval<long double> >, boost::numeric::interval<long double> > {
        typedef boost::complex<boost::numeric::interval<long double> > promote_type;
    };
#endif

    template<>
    struct promote_traits<boost::complex<boost::numeric::interval<float> >, boost::complex<boost::numeric::interval<double> > > {
        typedef boost::complex<boost::numeric::interval<double> > promote_type;
    };
    template<>
    struct promote_traits<boost::complex<boost::numeric::interval<double> >, boost::complex<boost::numeric::interval<float> > > {
        typedef boost::complex<boost::numeric::interval<double> > promote_type;
    };
#ifdef BOOST_UBLAS_USE_LONG_DOUBLE
    template<>
    struct promote_traits<boost::complex<boost::numeric::interval<float> >, boost::complex<boost::numeric::interval<long double> > > {
        typedef boost::complex<boost::numeric::interval<long double> > promote_type;
    };
    template<>
    struct promote_traits<boost::complex<boost::numeric::interval<long double> >, boost::complex<boost::numeric::interval<float> > > {
        typedef boost::complex<boost::numeric::interval<long double> > promote_type;
    };
    template<>
    struct promote_traits<boost::complex<boost::numeric::interval<double> >, boost::complex<boost::numeric::interval<long double> > > {
        typedef boost::complex<boost::numeric::interval<long double> > promote_type;
    };
    template<>
    struct promote_traits<boost::complex<boost::numeric::interval<long double> >, boost::complex<boost::numeric::interval<double> > > {
        typedef boost::complex<boost::numeric::interval<long double> > promote_type;
    };
#endif
#endif
#endif

    struct unknown_storage_tag {};
    struct sparse_proxy_tag: public unknown_storage_tag {};
    struct sparse_tag: public sparse_proxy_tag {};
    struct packed_proxy_tag: public sparse_proxy_tag {};
    struct packed_tag: public packed_proxy_tag {};
    struct dense_proxy_tag: public packed_proxy_tag {};
    struct dense_tag: public dense_proxy_tag {};

    template<class S1, class S2>
    struct storage_restrict_traits {
        typedef S1 storage_category;
    };

    template<>
    struct storage_restrict_traits<sparse_tag, dense_proxy_tag> {
        typedef sparse_proxy_tag storage_category;
    };
    template<>
    struct storage_restrict_traits<sparse_tag, packed_proxy_tag> {
        typedef sparse_proxy_tag storage_category;
    };
    template<>
    struct storage_restrict_traits<sparse_tag, sparse_proxy_tag> {
        typedef sparse_proxy_tag storage_category;
    };

    template<>
    struct storage_restrict_traits<packed_tag, dense_proxy_tag> {
        typedef packed_proxy_tag storage_category;
    };
    template<>
    struct storage_restrict_traits<packed_tag, packed_proxy_tag> {
        typedef packed_proxy_tag storage_category;
    };
    template<>
    struct storage_restrict_traits<packed_tag, sparse_proxy_tag> {
        typedef sparse_proxy_tag storage_category;
    };

    template<>
    struct storage_restrict_traits<packed_proxy_tag, sparse_proxy_tag> {
        typedef sparse_proxy_tag storage_category;
    };

    template<>
    struct storage_restrict_traits<dense_tag, dense_proxy_tag> {
        typedef dense_proxy_tag storage_category;
    };
    template<>
    struct storage_restrict_traits<dense_tag, packed_proxy_tag> {
        typedef packed_proxy_tag storage_category;
    };
    template<>
    struct storage_restrict_traits<dense_tag, sparse_proxy_tag> {
        typedef sparse_proxy_tag storage_category;
    };

    template<>
    struct storage_restrict_traits<dense_proxy_tag, packed_proxy_tag> {
        typedef packed_proxy_tag storage_category;
    };
    template<>
    struct storage_restrict_traits<dense_proxy_tag, sparse_proxy_tag> {
        typedef sparse_proxy_tag storage_category;
    };

    struct sparse_bidirectional_iterator_tag : public std::bidirectional_iterator_tag {};
    struct packed_random_access_iterator_tag : public std::random_access_iterator_tag {};
    struct dense_random_access_iterator_tag : public packed_random_access_iterator_tag {};

    // Thanks to Kresimir Fresl for convincing Comeau with iterator_base_traits ;-)
    template<class IC>
    struct iterator_base_traits {};

    template<>
    struct iterator_base_traits<std::forward_iterator_tag> {
        template<class I, class T>
        struct iterator_base {
            typedef forward_iterator_base<std::forward_iterator_tag, I, T> type;
        };
    };

    template<>
    struct iterator_base_traits<std::bidirectional_iterator_tag> {
        template<class I, class T>
        struct iterator_base {
            typedef bidirectional_iterator_base<std::bidirectional_iterator_tag, I, T> type;
        };
    };
    template<>
    struct iterator_base_traits<sparse_bidirectional_iterator_tag> {
        template<class I, class T>
        struct iterator_base {
            typedef bidirectional_iterator_base<sparse_bidirectional_iterator_tag, I, T> type;
        };
    };

    template<>
    struct iterator_base_traits<std::random_access_iterator_tag> {
        template<class I, class T>
        struct iterator_base {
            typedef random_access_iterator_base<std::bidirectional_iterator_tag, I, T> type;
        };
    };
    template<>
    struct iterator_base_traits<packed_random_access_iterator_tag> {
        template<class I, class T>
        struct iterator_base {
            typedef random_access_iterator_base<packed_random_access_iterator_tag, I, T> type;
        };
    };
    template<>
    struct iterator_base_traits<dense_random_access_iterator_tag> {
        template<class I, class T>
        struct iterator_base {
            typedef random_access_iterator_base<dense_random_access_iterator_tag, I, T> type;
        };
    };

    template<class I1, class I2>
    struct iterator_restrict_traits {
        typedef I1 iterator_category;
    };

    template<>
    struct iterator_restrict_traits<packed_random_access_iterator_tag, sparse_bidirectional_iterator_tag> {
        typedef sparse_bidirectional_iterator_tag iterator_category;
    };
    template<>
    struct iterator_restrict_traits<sparse_bidirectional_iterator_tag, packed_random_access_iterator_tag> {
        typedef sparse_bidirectional_iterator_tag iterator_category;
    };

    template<>
    struct iterator_restrict_traits<dense_random_access_iterator_tag, sparse_bidirectional_iterator_tag> {
        typedef sparse_bidirectional_iterator_tag iterator_category;
    };
    template<>
    struct iterator_restrict_traits<sparse_bidirectional_iterator_tag, dense_random_access_iterator_tag> {
        typedef sparse_bidirectional_iterator_tag iterator_category;
    };

    template<>
    struct iterator_restrict_traits<dense_random_access_iterator_tag, packed_random_access_iterator_tag> {
        typedef packed_random_access_iterator_tag iterator_category;
    };
    template<>
    struct iterator_restrict_traits<packed_random_access_iterator_tag, dense_random_access_iterator_tag> {
        typedef packed_random_access_iterator_tag iterator_category;
    };

}}}

#endif


/*
 * Copyright (c) 2019 Opticks Team. All Rights Reserved.
 *
 * This file is part of Opticks
 * (see https://bitbucket.org/simoncblyth/opticks).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "NPY_FLAGS.hh"

#include <cstdlib>
#include <cstdio>
#include <cstring>
#include <string>
#include <vector>

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

#include "NSlice.hpp"

NSlice::NSlice(unsigned int low_, unsigned int high_, unsigned int step_)
    :
    low(low_),
    high(high_),
    step(step_),
    _description(0)
{
}

const char* NSlice::description()
{
    if(!_description)
    {
        char desc[128];
        snprintf(desc, 128, "NSlice  %5d : %5d : %5d ", low, high, step );
        _description = strdup(desc) ;
    }
    return _description ;
}

unsigned int NSlice::count()
{
    unsigned int n = 0 ;
    for(unsigned int i=low ; i < high ; i+=step ) n+=1 ;
    return n ;
}

bool NSlice::isTail(unsigned index, unsigned window)
{
    int h = high ;
    int i = index ;
    int o = i - h ;
    int w = window ;
    return o < 0 && -o <= w ;
}

bool NSlice::isHead(unsigned index, unsigned window)
{
    int l = low ;
    int i = index ;
    int o = i - l ;
    int w = window ;
    return o >= 0 && o < w ;
}

bool NSlice::isMargin(unsigned index, unsigned window)
{
    return isTail(index, window) || isHead(index, window) ;
}

NSlice::NSlice(const char* slice, const char* delim)
{
    // defaults
    low = 0 ;
    high = 1 ;
    step = 1 ;
    _description = 0 ;

    std::vector<std::string> elem ;
    boost::split(elem, slice, boost::is_any_of(delim));
    unsigned int size = elem.size();

    if(size > 0) low = boost::lexical_cast<unsigned int>(elem[0]);
    if(size > 1) high = boost::lexical_cast<unsigned int>(elem[1]);
    if(size > 2) step = boost::lexical_cast<unsigned int>(elem[2]);

    if(size == 1) high = low + 1 ;  // only provided low

/*
   // strsep has portability issues
    unsigned int i = 0 ;
    char* str = strdup(slice);
    char* token;
    while ((token = strsep(&str, delim)))
    {
       switch(i)
       {
          case 0:  low = atoi(token) ; break ;
          case 1: high = atoi(token) ; break ;
          case 2: step = atoi(token) ; break ;
       }
       i++ ;
    }
    if(i == 1) high = low+1 ;  // when only single int provided
*/

}


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

    @copyright 2016 NumScale SAS

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

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

  /*!

    @ingroup group-operator
    This function object returns @ref True or @ref False
    according x and y are equal or not.

    Infix notation can be used with operator '=='

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

    @par Note

      Using `is_equal(x,y)` is equivalent to `x == y`

    @see  is_not_equal, is_eqz, is_equal_with_equal_nans

    @par Example:

      @snippet is_equal.cpp is_equal

    @par Possible output:

      @snippet is_equal.txt is_equal

  **/
  as_logical_t<Value> is_equal(Value const& x, Value const& y);
} }
#endif

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

#endif