#include <vector>
#include <iostream>
#include <fstream>
#include <random>
#include <string>
#include <algorithm>

#include <boost/algorithm/string.hpp>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/program_options/parsers.hpp>

namespace po = boost::program_options;

typedef float float_t;

struct Point {
	float_t x, y;

	Point() {}
	Point(float_t nx, float_t ny): x(nx), y(ny) {}

	Point operator - (const Point &p) const {
		return Point(x - p.x, y - p.y);
	}

	Point operator + (const Point &p) const {
		return Point(x + p.x, y + p.y);
	}

	float_t operator * (const Point &p) const {
		return x * p.x + y * p.y;
	}

	Point operator * (const float_t k) const {
		return Point(x * k, y * k);
	}
};

struct GraphLayout {
	int n;
	std::vector< std::vector<int> > adj;
	std::vector<Point> layout;
	float_t W, H;

	GraphLayout(
			int n,
			float_t nodeDistW, float_t borderW, float_t edgeW,
			float_t crossW, float_t crossT,
			float_t canvasW, float_t canvasH
	) {
		this->n = n;
		_nw = nodeDistW;
		_bw = borderW;
		_ew = edgeW;
		_cw = crossW;
		_mind = crossT;
		W = canvasW;
		H = canvasH;
		adj.assign(n, std::vector<int>());
	}

	void addEdge(int u, int v) {
		adj[u].push_back(v);
		adj[v].push_back(u);
	}

	template<typename RNG> void initialize_positions(const std::vector<Point> &start, RNG &rng) {
		layout = start;
		if (layout.empty()) {
			layout.resize(n);
			std::uniform_real_distribution<float_t> wdis(0, W);
			std::uniform_real_distribution<float_t> hdis(0, H);
			for (int i = 0; i < n; ++i) {
				layout[i].x = wdis(rng);
				layout[i].y = hdis(rng);
			}
		}
		initEnergy();
	}

	void move(int v, Point d) {
		_tmpne = _ne;
		_tmpbe = _be;
		_tmpee = _ee;
		_tmpce = _ce;
		_tmpPoint = layout[v];
		addEnergy(v, -1);
		layout[v].x = std::clamp<float_t>(
				layout[v].x + d.x, W / 1000, static_cast<const float_t &>(W * 0.999)
		);
		layout[v].y = std::clamp<float_t>(
				layout[v].y + d.y, H / 1000, static_cast<const float_t &>(H * 0.999)
		);
		addEnergy(v, 1);
	}

	// Take care to call this only straight after move
	void undoMove(int v, Point d) {
		layout[v] = _tmpPoint;
		_ne = _tmpne;
		_be = _tmpbe;
		_ee = _tmpee;
		_ce = _tmpce;
	}

	float_t getEnergy() const {
		return _ne + _be + _ee + _ce;
	}

	float_t getNodeDistEnergy() const {
		return _ne;
	}

	float_t getBorderEnergy() const {
		return _be;
	}

	float_t getEdgesEnergy() const {
		return _ee;
	}

	float_t getCrossingsEnergy() const {
		return _ce;
	};

private:
	float_t _mind;
	float_t _ne, _be, _ee, _ce;
	float_t _tmpne, _tmpbe, _tmpee, _tmpce;
	Point _tmpPoint;
	float_t _nw, _bw, _ew, _cw;

	float_t _sqr(float_t x) {
		return x * x;
	}

	float_t _sqdist(int i, int j) {
		return _sqr(layout[i].x - layout[j].x) + _sqr(layout[i].y - layout[j].y);
	}

	float_t _segsqdist(int u, int v, int w) {
		float_t ans = fmin(_sqdist(w, u), _sqdist(w, v));
		Point uw = layout[w] - layout[u];
		Point uv = layout[v] - layout[u];
		float_t cp = uw * uv;
		if (cp < 0) return ans;
		if ((layout[u] - layout[v]) * (layout[w] - layout[v]) < 0) return ans;
		cp /= uv * uv;
		Point h = layout[u] + (uv * cp);
		return _sqr(layout[w].x - h.x) + _sqr(layout[w].y - h.y);
	}

	void initEnergy() {
		_ne = _be = _ee = _ce = 0;
		if (_nw != 0) {
			float_t dE = 0;
			for (int i = 0; i < n; ++i) {
				for (int j = 0; j < i; ++j) {
					dE += 1 / _sqdist(i, j);
				}
			}
			_ne += dE * _nw;
		}
		if (_bw != 0) {
			float_t dE = 0;
			for (int i = 0; i < n; ++i) {
				dE +=
						1 / _sqr(layout[i].x) + 1 / _sqr(W - layout[i].x) +
						1 / _sqr(layout[i].y) + 1 / _sqr(H - layout[i].y);
			}
			_be += _bw * dE;
		}
		if (_ew != 0) {
			float_t dE = 0;
			for (int v = 0; v < n; ++v) {
				for (const auto &u : adj[v]) {
					if (v < u) {
						dE += _sqdist(v, u);
					}
				}
			}
			_ee += _ew * dE;
		}
		if (_cw != 0) {
			float_t dE = 0;
			for (int v = 0; v < n; ++v) {
				for (int u : adj[v]) {
					if (u < v) continue;
					for (int w = 0; w < n; ++w) {
						if (v == w || u == w) continue;
						dE += 1 / fmax(_mind, _segsqdist(u, v, w));
					}
				}
			}
			_ce += _cw * dE;
		}
	}

	void addEnergy(int v, double c) {
		if (_nw != 0) {  // exact equality is fine here
			float_t dE = 0;
			for (int i = 0; i < n; ++i) {
				if (i != v) dE += 1 / _sqdist(v, i);
			}
			_ne += c * _nw * dE;
		}
		if (_bw != 0) {
			float_t dE =
					1 / _sqr(layout[v].x) + 1 / _sqr(W - layout[v].x) +
					1 / _sqr(layout[v].y) + 1 / _sqr(H - layout[v].y);
			_be += c * _cw * dE;
		}
		if (_ew != 0) {
			float_t dE = 0;
			for (const auto &u: adj[v]) {
				dE += _sqdist(v, u);
			}
			_ee += c * _ew * dE;
		}
		if (_cw != 0) {
			float_t dE = 0;
			for (int u = 0; u < n; ++u) {
				for (int w : adj[u]) {
					if (u == v || w == v || w < u) continue;
					dE += 1 / fmax(_mind, _segsqdist(u, w, v));
				}
			}
			for (int u : adj[v]) {
				for (int w = 0; w < n; ++w) {
					if (v == w || u == w) continue;
					dE += 1 / fmax(_mind, _segsqdist(v, u, w));
//					std::cerr << "DEBUG " << _segsqdist(v, u, w) << "\n";
//					std::cerr << "DEBUG " << _segsqdist(u, v, w) << "\n";
//					std::cerr << "====\n";
				}
			}
			_ce += c * _cw * dE;
		}
	}
};

float_t transition_probability(float_t dE, float_t T) {
	// here dE must be positive
	return exp(-dE / T);
}

void print_layout(const GraphLayout &layout) {
	for (int i = 0; i < layout.n; ++i) {
		std::cout << layout.layout[i].x << " " << layout.layout[i].y << ";";
	}
	std::cout
		<< layout.getNodeDistEnergy() << ";" << layout.getBorderEnergy()
		<< ";" << layout.getEdgesEnergy() << ";" << layout.getCrossingsEnergy() << "\n";
}

template<typename RNG>
void simulate_annealing(
		GraphLayout &layout, int iters, float_t startT,
		float_t dT, bool fine_tune, RNG &rng, bool verbose
) {
	std::uniform_int_distribution<int> vdis(0, layout.n - 1);
	std::uniform_real_distribution<float_t> pdis(0, 1);

	auto curS = static_cast<float_t>(hypotl(layout.W, layout.H) / 2);
	auto dS = static_cast<float_t>(powl(0.001, (float_t) 1.0 / iters));
	float_t temp = startT;

	if (fine_tune) {
		curS = static_cast<float_t>(hypotl(layout.W, layout.H) / 300);
		dS = 1.0;
	}

	print_layout(layout);
	for (int i = 0; i < iters; ++i) {
		int v = vdis(rng);
		Point d{};
		d.x = (2 * std::generate_canonical<float_t, 1, RNG>(rng) - 1) * curS;
		d.y = (2 * std::generate_canonical<float_t, 1, RNG>(rng) - 1) * curS;
		float_t was_energy = layout.getEnergy();
		if (verbose) {
			std::cerr << "Iteration #" << i << ": E=" << was_energy << " T=" << temp << std::endl;
			std::cerr << "ne=" << layout.getNodeDistEnergy();
			std::cerr << " be=" << layout.getBorderEnergy();
			std::cerr << " ee=" << layout.getEdgesEnergy();
			std::cerr << " ce=" << layout.getCrossingsEnergy() << std::endl;
		}
		layout.move(v, d);
		float_t new_energy = layout.getEnergy();
		if (
				(fine_tune && new_energy > was_energy) ||
				(new_energy > was_energy &&
					pdis(rng) > transition_probability(new_energy - was_energy, temp)
				)
		) {
			layout.undoMove(v, d);
			if (verbose) {
				std::cerr
					<< "undo occured (dE=" << new_energy - was_energy
					<< ", T=" << temp << ") prob="
					<< 1 - transition_probability(new_energy - was_energy, temp) << ")\n";
			}
		}
		temp *= dT;
		curS *= dS;
		print_layout(layout);
	}
}

int main(int argc, char **argv) {
	freopen("graph.txt", "r", stdin);
	po::options_description desc("Allowed options");
	desc.add_options()
		("help", "show usage")
		("nodes", po::value<float_t>(), "weight of even node distribution in energy")
		("edges", po::value<float_t>(), "weight of length of edges in energy")
		("border", po::value<float_t>(), "weight of proximity to borderline in energy")
		("cross", po::value<float_t>(), "weight of crossings in energy")
		("cross-threshold", po::value<float_t>(), "minimum distance value for node edge interaction")
		("startT", po::value<float_t>(), "statring temperature")
		("cooling", po::value<float_t>(), "schedule of temperature reduction")
		("rounds", po::value<int>(), "number of rounds of annealing")
		("width", po::value<float_t>(), "width of canvas")
		("height", po::value<float_t>(), "height of canvas")
		("starting-layout", po::value<std::string>(), "path to starting layout file (if omitted, random one is used)")
		("seed", po::value<int>(), "random seed")
		("fine-tune", "flag to enable local search (used for fine tuning)")
		("verbose", "verbose mode");
	po::variables_map varmap;
	po::store(po::parse_command_line(argc, argv, desc), varmap);
	po::notify(varmap);

	if (varmap.count("help")) {
		std::cout << desc << std::endl;
		return 0;
	}

	bool failed = false;
	for (const auto &s: {"nodes", "edges", "border", "startT", "cooling", "rounds", "width", "height"}) {
		if (!varmap.count(s)) {
			std::cerr << "Option --" << s << " is required" << std::endl;
			failed = true;
		}
	}
	if (failed) {
		std::cout << desc << std::endl;
		return 1;
	}

	int n, m;
	std::cin >> n >> m;
	GraphLayout layout(
			n,
			varmap["nodes"].as<float_t>(),
			varmap["border"].as<float_t>(),
			varmap["edges"].as<float_t>(),
			(varmap.count("cross") ? varmap["cross"].as<float_t>() : 0),
			(varmap.count("cross-threshold") ? varmap["cross-threshold"].as<float_t>() : 1),
			varmap["width"].as<float_t>(),
			varmap["height"].as<float_t>()
	);
	for (int i = 0; i < m; ++i) {
		int from, to;
		std::cin >> from >> to;
		layout.addEdge(from, to);
	}

	std::vector<Point> start;
	if (varmap.count("starting-layout")) {
		std::ifstream start_file(varmap["starting-layout"].as<std::string>());
		std::string s, last;
		while (std::getline(start_file, s)) {
			last = s;
		}
		std::istringstream ss(last);
		start.resize(static_cast<unsigned long>(n));
		for (int i = 0; i < n; ++i) {
			char sep;
			ss >> start[i].x >> start[i].y >> sep;
		}
	}
	std::mt19937 rng(varmap.count("seed") ? varmap["seed"].as<int>() : 0);
	layout.initialize_positions(start, rng);

	simulate_annealing(
			layout, varmap["rounds"].as<int>(),
			varmap["startT"].as<float_t>(), varmap["cooling"].as<float_t>(),
			static_cast<bool>(varmap.count("fine-tune")), rng,
			static_cast<bool>(varmap.count("verbose"))
	);
}


/* ModelReduction.cpp
 * -*- coding: utf-8 -*-
 *
 */

#include <boost/test/unit_test.hpp>

#include <string>
#include <stdexcept>

#include <boost/filesystem.hpp>

#include <Eigen/Dense>

// local includes ==============================================================

#include "../src/FileFactory.hpp"
#include "../src/ModelReduction.hpp"
#include "../src/ReductionFile.hpp"
#include "../src/NormalModeMeanSquareFluctuationCalculator.hpp"
#include "../src/Atom.hpp"
#include "../src/Residuum.hpp"

#include "utils/test.hpp"
#include "utils/file.hpp"
#include "utils/definitions.hpp"
#include "utils/log.hpp"

class MockedNormalModeMeanSquareFluctuationCalculator : public NormalModeMeanSquareFluctuationCalculator {
public:
    MockedNormalModeMeanSquareFluctuationCalculator(const std::vector<Residuum> & residues) : NormalModeMeanSquareFluctuationCalculator(residues) {

    };
    void reduce_model(const ModelReduction & model_reduction, const Eigen::MatrixXd & hessian_matrix) {
        this->hessian_matrix = hessian_matrix;
        calculate_model_reduction(model_reduction);
    };

    double get_mass() const {
        double sum_of_mass = 0;
        for(int i = 0; i < this->mass.size(); ++i) {
            sum_of_mass += this->mass(i);
        }
        return sum_of_mass;
    };
};

BOOST_AUTO_TEST_SUITE(reduce_matrix)

    struct Setup {
        Setup() { factory = ReductionFileFactory(); }

        ReductionFileFactory factory;

        std::string test_file_dir = "test/resources/mat_reduce/";
    };

    int get_dimension_from_path(std::string path) {
        std::string filename = boost::filesystem::path(path).filename().string();

        size_t first_  = filename.find('_') + 1;
        size_t second_ = filename.find('_', first_);

        try {
            return std::stoi(filename.substr(first_, second_ - first_)) * 3;
        } catch (std::invalid_argument & e) {
            TEST_MESSAGE("ERROR: failed to get dimension from filename: " + filename);
            BOOST_REQUIRE(false);
        }
    }

    BOOST_AUTO_TEST_CASE(calculate_model_reduction_test) {
        TEST_MESSAGE("calculate_model_reduction_test");

        Setup s;

        std::vector<Residuum> residues(0);
        std::vector<Atom> atoms_in_residuum_1;
        atoms_in_residuum_1.push_back(Atom(0, 0, 0, "C", 0, 0));
        atoms_in_residuum_1.push_back(Atom(0, 0, 0, "O", 0, 0));
         // mass = 28.009999999999998
        Atom c_alpha_residuum_1(0, 0, 0, "C", 0, 0);
        residues.push_back(Residuum(c_alpha_residuum_1, atoms_in_residuum_1));

        std::vector<Atom> atoms_in_residuum_2;
        atoms_in_residuum_2.push_back(Atom(0, 0, 0, "C", 0, 0));
        atoms_in_residuum_2.push_back(Atom(0, 0, 0, "O", 0, 0));
        atoms_in_residuum_2.push_back(Atom(0, 0, 0, "N", 0, 0));
        //mass = 42.086999999999996
        Atom c_alpha_residuum_2(0, 0, 0, "C", 0, 0);
        residues.push_back(Residuum(c_alpha_residuum_2, atoms_in_residuum_2));

        std::vector<Atom> atoms_in_residuum_3;
        atoms_in_residuum_2.push_back(Atom(0, 0, 0, "C", 0, 0));
        atoms_in_residuum_2.push_back(Atom(0, 0, 0, "O", 0, 0));
        atoms_in_residuum_2.push_back(Atom(0, 0, 0, "N", 0, 0));
        atoms_in_residuum_2.push_back(Atom(0, 0, 0, "S", 0, 0));
        //mass = 74.14699999999999
        Atom c_alpha_residuum_3(0, 0, 0, "C", 0, 0);
        residues.push_back(Residuum(c_alpha_residuum_3, atoms_in_residuum_3));

        Eigen::MatrixXd hessian9 = get_sym_matrix_from_file(s.test_file_dir + "hessian_input_9.mat", 9, ' ');

        std::string path(s.test_file_dir + "selection_2_9");
        MockedNormalModeMeanSquareFluctuationCalculator nmsf_calculator(residues);
        ModelReduction reduction(s.factory.create(path, 3));
        nmsf_calculator.reduce_model(reduction, hessian9);
        BOOST_CHECK_EQUAL(nmsf_calculator.get_mass(), 28.009999999999998 + 42.086999999999996);
        BOOST_CHECK_EQUAL(nmsf_calculator.get_mean_square_fluctuation().size(), 3);
        BOOST_CHECK_EQUAL(nmsf_calculator.get_weighted_eigenvalues().size(), 6);
        BOOST_CHECK_EQUAL(nmsf_calculator.get_hessian_matrix().size(), 6*6);
        BOOST_CHECK_EQUAL(nmsf_calculator.get_eigenvalues().size(), 6);
        BOOST_CHECK_EQUAL(nmsf_calculator.get_weighted_eigenvector().size(), 6);
        BOOST_CHECK_EQUAL(nmsf_calculator.get_eigenvectors().size(), 6*6);

    }

    BOOST_AUTO_TEST_CASE(reduce_matrix_test) {
        TEST_MESSAGE("reduce_matrix_test");

        Setup s;

        // test 90x90 matrix
        Eigen::MatrixXd hessian90 = get_sym_matrix_from_file(s.test_file_dir + "hessian_input_90.mat", 90, ' ');

        for (std::string & path : glob(s.test_file_dir + "selection_*_90")) {
            ModelReduction reduction(s.factory.create(path, 30));

            // Eigen::MatrixXd expected = get_sym_matrix_from_file(path + ".hessian", n * 3, ' ');
            Eigen::MatrixXd expected = get_sym_matrix_from_file(path + ".hessian",
                                                                get_dimension_from_path(path),
                                                                ' ');

            Eigen::MatrixXd actual = reduction.reduce_hessian_matrix(hessian90);

            compare_two_sym_matrices(actual, expected, PRECISION);
        }
    }

    BOOST_AUTO_TEST_CASE(empty_reductionfile_should_leave_matrix_untouched) {
        TEST_MESSAGE("empty_reductionfile_should_leave_matrix_untouched");

        Setup s;

        Eigen::MatrixXd hessian90 = get_sym_matrix_from_file(s.test_file_dir + "hessian_input_90.mat", 90, ' ');

        ModelReduction reduction;

        Eigen::MatrixXd reduced_matrix = reduction.reduce_hessian_matrix(hessian90);

        BOOST_REQUIRE_EQUAL(reduced_matrix, hessian90);
    }

BOOST_AUTO_TEST_SUITE_END()

// vim: tabstop=8 expandtab shiftwidth=4 softtabstop=4 fenc=utf-8


#ifndef MI_OPEN_GL_UTILITY_HPP
#define MI_OPEN_GL_UTILITY_HPP 1
#include <cassert>
#include <vector>
#include <Eigen/Dense>
#if defined(__APPLE__)
#include <OpenGL/gl.h>
#else
#if defined(WIN32) || defined(_WIN32) || defined(__WIN32__)
#include <windows.h>
#pragma comment(lib, "opengl32.lib")
#endif
#include <GL/gl.h>
#endif

namespace mi4
{
        class OpenGlUtility
        {
        public:
                static void lookAt ( const  Eigen::Vector3d& eye, const Eigen::Vector3d& center, const Eigen::Vector3d& up )
                {
                        const auto forward = ( center - eye ).normalized();
                        const auto side = forward.cross ( up ).normalized();
                        const auto upv  = side.cross ( forward ).normalized();
                        std::vector<double> m ( 16, 0 );

                        for ( int i = 0  ; i < 3 ; ++i ) {
                                m[i * 4 + 0] = side[i];
                                m[i * 4 + 1] = upv[i];
                                m[i * 4 + 2] = -forward[i];
                        }

                        m[3 * 4 + 3] = 1.0;
                        ::glMultMatrixd ( m.data() );
                        ::glTranslated ( -eye.x(), -eye.y(), -eye.z() );
                        return;
                }
                static void perspective ( const double fov, const double aspect, const double znear, const double zfar )
                {
                        assert ( znear - zfar != 0 );
                        std::vector< double > m(16, 0); //std::array

                        const double f = 1.0 / std::tan(0.5 * fov * M_PI / 180.0);
                        m[0] = f * 1.0 / aspect;
                        m[1 * 4 + 1] = f;
                        m[2 * 4 + 2] = ( zfar + znear ) * 1.0 / ( znear - zfar ) ;
                        m[2 * 4 + 3] = -1.0;
                        m[3 * 4 + 2] =  2.0 * zfar * znear / ( znear - zfar ) ;
                        ::glMultMatrixd ( m.data() );
                }
                static void ortho ( const double left, const double  right, const double  bottom, const double  top, const double znear, const double zfar )
                {
                        ::glOrtho ( left, right, bottom, top, znear, zfar );
                        return;
                }
                static void ortho2d ( const double  left, const double  right, const double  bottom, const double  top )
                {
                        ::glOrtho ( left, right, bottom, top, -1, 1 );
                        return;
                }
        };
}
#endif


#include <istat/istattime.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <boost/cstdint.hpp>
#include <string.h>
#include <time.h>

void usage()
{
    fprintf(stderr, "usage: random_numbers [-t] [count [average]]\n");
    exit(1);
}

int main(int argc, char const *argv[])
{
    bool timestamp = false;

    if (argc > 1 && !strcmp(argv[1], "-t"))
    {
        timestamp = true;
        ++argv;
        --argc;
    }

    int count = 100;
    if (argc > 1)
    {
        count = atoi(argv[1]);
        if (count < 1 || count > 1000000)
        {
            fprintf(stderr, "count out of range\n");
            usage();
        }
        argc--;
        argv++;
    }

    int center = 100;
    int swing = 0;
    if (argc > 1)
    {
        center = atoi(argv[1]);
        if ((center == 0 && !strcmp(argv[0], "0")) || (center < -1000000) || (center > 1000000))
        {
            fprintf(stderr, "center is out of range\n");
            usage();
        }
        argc--;
        argv++;
    }
    if (center >= -100 && center < 100)
    {
        swing = 100;
    }
    else
    {
        swing = abs(center);
    }
    center -= swing/2;

    if (argc > 1)
    {
        fprintf(stderr, "unknown argument: %s\n", argv[1]);
        usage();
    }

    time_t now;
    istat::istattime(&now);

    while (count > 0)
    {
        if (timestamp)
        {
            long long int i64 = (int64_t)now;
            fprintf(stdout, "%lld ", i64);
            while (rand() > RAND_MAX / 2)
            {
                now += 1;
            }
        }
        fprintf(stdout, "%ld\n", (long)((double)rand() * swing / RAND_MAX + center));
        --count;
    }
    return 0;
}


#include "o3.hpp"
#include "sco/expr_ops.hpp"
#include "sco/modeling_utils.hpp"
#include "osgviewer/osgviewer.hpp"
#include "trajopt/collision_checker.hpp"
#include "trajopt/collision_terms.hpp"
#include "trajopt/common.hpp"
#include "trajopt/plot_callback.hpp"
#include "trajopt/problem_description.hpp"
#include "trajopt/rave_utils.hpp"
#include "trajopt/trajectory_costs.hpp"
#include "utils/clock.hpp"
#include "utils/config.hpp"
#include "utils/eigen_conversions.hpp"
#include "utils/stl_to_string.hpp"
#include <boost/assign.hpp>
#include <boost/foreach.hpp>
#include <ctime>
#include <openrave-core.h>
#include <openrave/openrave.h>

using namespace trajopt;
using namespace std;
using namespace OpenRAVE;
using namespace util;
using namespace boost::assign;
using namespace Eigen;

struct NeedleError : public VectorOfVector {
  ConfigurationPtr cfg0, cfg1;
  double radius;
  KinBodyPtr body;
  NeedleError(ConfigurationPtr cfg0, ConfigurationPtr cfg1, double radius) : cfg0(cfg0), cfg1(cfg1), radius(radius), body(cfg0->GetBodies()[0]) {}
  VectorXd operator()(const VectorXd& a) const {
    cfg0->SetDOFValues(toDblVec(a.topRows(6)));
    OR::Transform Tw0 = body->GetTransform();
    cfg1->SetDOFValues(toDblVec(a.middleRows(6,6)));
    OR::Transform Tw1 = body->GetTransform();
    double theta = a(12);
    OR::Transform Ttarg0(OR::geometry::quatFromAxisAngle(OR::Vector(0,0,1), theta),
        OR::Vector(radius*sin(theta), radius*(1-cos(theta)),0));

    OR::Transform Ttargw = Tw0 * Ttarg0;
    OR::Vector position_errA = Ttargw.trans - Tw1.trans;
    OR::Vector ori_err = Ttargw * OR::Vector(1,0,0) - Tw1 * OR::Vector(1,0,0);
    return concat(toVector3d(position_errA), toVector3d(ori_err));

  }
};

struct TrajPlotter1 {
  vector<IncrementalRBPtr> rbs;
  VarArray vars;
  OSGViewerPtr viewer;
  TrajPlotter1(const vector<IncrementalRBPtr>& rbs, const VarArray& vars);
  void OptimizerCallback(OptProb*, DblVec& x);

};

TrajPlotter1::TrajPlotter1(const vector<IncrementalRBPtr>& rbs, const VarArray& vars) : rbs(rbs), vars(vars) {
  viewer = OSGViewer::GetOrCreate(rbs[0]->GetEnv());
}
void TrajPlotter1::OptimizerCallback(OptProb*, DblVec& x) {
  vector<GraphHandlePtr> handles;
  vector<KinBodyPtr> bodies = rbs[0]->GetBodies();
  MatrixXd traj = getTraj(x,vars);
  for (int i=0; i < traj.rows(); ++i) {
    rbs[i]->SetDOFValues(toDblVec(traj.row(i)));
    BOOST_FOREACH(const KinBodyPtr& body, bodies) {
      handles.push_back(viewer->PlotKinBody(body));
      SetTransparency(handles.back(), .35);
    }
  }
  viewer->Idle();
}

int main(int argc, char** argv)
{
  bool plotting=false, verbose=false;

  {
    Config config;
    config.add(new Parameter<bool>("plotting", &plotting, "plotting"));
    config.add(new Parameter<bool>("verbose", &verbose, "verbose"));
    CommandParser parser(config);
    parser.read(argc, argv);
  }

  RaveInitialize(false, verbose ? Level_Debug : Level_Info);
  EnvironmentBasePtr env = RaveCreateEnvironment();
  env->StopSimulation();
  OSGViewerPtr viewer = OSGViewer::GetOrCreate(env);
  assert(viewer);

  env->Load(string(DATA_DIR) + "/needleprob.env.xml");
  RobotBasePtr robot = GetRobot(*env);

  int n_steps = 19;
  int n_dof = 6;

  OptProbPtr prob(new OptProb());
  VarArray trajvars;
  AddVarArray(*prob, n_steps, n_dof, "j", trajvars);

  O3Helper helper(robot, trajvars.block(0,3,n_steps,3));

  double radius = 1; // turning radius for needle
  VectorXd start(n_dof); start << 0,0,0,0,0,0;
  VectorXd goal(n_dof); goal <<  4,.25,0, 0,0,0;

  VectorXd vel_coeffs = VectorXd::Ones(3);
  prob->addCost(CostPtr(new JointVelCost(trajvars.block(0,0,n_steps, 3), vel_coeffs)));

  helper.AddAngVelCosts(*prob, vel_coeffs[0]);

  double dtheta_lb = (goal.topRows(3) - start.topRows(3)).norm() / (n_steps-1)/radius;
  Var dthetavar = prob->createVariables(singleton<string>("speed"), singleton<double>(dtheta_lb),singleton<double>(INFINITY))[0];

  for (int i=0; i < n_steps-1; ++i) {
    VarVector vars0 = trajvars.row(i), vars1 = trajvars.row(i+1);
    VectorOfVectorPtr f(new NeedleError(helper.m_rbs[i], helper.m_rbs[i+1], radius));
    VectorXd coeffs = VectorXd::Ones(6);
    VarVector vars = concat(vars0, vars1);
    vars.push_back(dthetavar);
    prob->addConstraint(ConstraintPtr(new ConstraintFromFunc(f, vars, coeffs, EQ, (boost::format("needle%i")%i).str())));
  }

  for (int j=0; j < n_dof; ++j) {
    prob->addLinearConstraint(exprSub(AffExpr(trajvars(0,j)), start[j]), EQ);
  }
  for (int j=0; j < 3; ++j) { // NO orientation constraint
    prob->addLinearConstraint(exprSub(AffExpr(trajvars(n_steps-1,j)), goal[j]), EQ);
  }

  BasicTrustRegionSQP opt(prob);
  helper.ConfigureOptimizer(opt);
  opt.max_iter_ = 500;

  boost::shared_ptr<TrajPlotter1> plotter;
  if (plotting) {
    plotter.reset(new TrajPlotter1(helper.m_rbs, trajvars));
    opt.addCallback(boost::bind(&TrajPlotter1::OptimizerCallback, boost::ref(plotter), _1, _2));
  }

  MatrixXd initTraj(n_steps, n_dof);
  for (int idof = 0; idof < n_dof; ++idof) {
    initTraj.col(idof) = VectorXd::LinSpaced(n_steps, start[idof], goal[idof]);
  }
  DblVec initVec = trajToDblVec(initTraj);
  initVec.push_back(dtheta_lb);
  opt.initialize(initVec);

  opt.optimize();

  RaveDestroy();

}


#include <gtest/gtest.h>
#include <qi/anymodule.hpp>
#include <qi/geometry/geometry.hpp>
#include <qi/application.hpp>
#include <boost/math/constants/constants.hpp>

using namespace qi::geometry;

class GeometryModuleTest : public ::testing::Test{
public:
  qi::AnyModule gm = qi::import("geometry_module");
};

TEST_F(GeometryModuleTest, makeVector3) {
  auto v3 = gm.call<Vector3>("makeVector3", 1., 2., 3.);
  EXPECT_EQ(1., v3.x);
  EXPECT_EQ(2., v3.y);
  EXPECT_EQ(3., v3.z);
}

TEST_F(GeometryModuleTest, makeQuaternion) {
  auto q = gm.call<Quaternion>("makeQuaternion", 1., 2., 3., 4.);
  EXPECT_EQ(1., q.x);
  EXPECT_EQ(2., q.y);
  EXPECT_EQ(3., q.z);
  EXPECT_EQ(4., q.w);

  auto n = gm.call<double>("norm", q);
  EXPECT_DOUBLE_EQ(sqrt(1. + 4. + 9. + 16.), n);

  auto qn = gm.call<Quaternion>("normalized", q);
  EXPECT_DOUBLE_EQ(q.x/n, qn.x);
  EXPECT_DOUBLE_EQ(q.y/n, qn.y);
  EXPECT_DOUBLE_EQ(q.z/n, qn.z);
  EXPECT_DOUBLE_EQ(q.w/n, qn.w);
}

TEST_F(GeometryModuleTest, makeQuaternionFromAngleAxis) {
  auto axis = gm.call<Vector3>("makeVector3", 1., 1., 1.);
  auto norm = sqrt(3.);

  auto angle = 2 * boost::math::constants::pi<double>()/3;
  auto sin_ha = sin(angle/2);
  auto cos_ha = cos(angle/2);
  auto q = gm.call<Quaternion>("makeQuaternionFromAngleAxis", angle, axis);
  EXPECT_DOUBLE_EQ(sin_ha * axis.x/norm, q.x);
  EXPECT_DOUBLE_EQ(sin_ha * axis.y/norm, q.y);
  EXPECT_DOUBLE_EQ(sin_ha * axis.z/norm, q.z);
  EXPECT_DOUBLE_EQ(cos_ha, q.w);

  EXPECT_DOUBLE_EQ(1., gm.call<double>("norm", q));
}

// TODO: duplicate this test using quaternion which is not normalized
TEST_F(GeometryModuleTest, transform) {

  auto rotation = gm.call<Quaternion>("makeQuaternion", 0.5, 0.5, 0.5, 0.5);
  auto translation = gm.call<Vector3>("makeVector3", 1., 2., 3.);
  auto transform = gm.call<Transform>("makeTransform", rotation, translation);

  EXPECT_EQ(rotation.x, transform.rotation.x);
  EXPECT_EQ(rotation.y, transform.rotation.y);
  EXPECT_EQ(rotation.z, transform.rotation.z);
  EXPECT_EQ(rotation.w, transform.rotation.w);
  EXPECT_EQ(translation.x, transform.translation.x);
  EXPECT_EQ(translation.y, transform.translation.y);
  EXPECT_EQ(translation.z, transform.translation.z);

  auto inv_transform = gm.call<Transform>("inverse", transform);
  auto id_transform = gm.call<Transform>("multiply", transform, inv_transform);

  // id_transform should be the identity
  EXPECT_DOUBLE_EQ(0., id_transform.rotation.x);
  EXPECT_DOUBLE_EQ(0., id_transform.rotation.y);
  EXPECT_DOUBLE_EQ(0., id_transform.rotation.z);
  EXPECT_TRUE((id_transform.rotation.w == 1.) ||
              (id_transform.rotation.w == -1.));
  EXPECT_DOUBLE_EQ(0., id_transform.translation.x);
  EXPECT_DOUBLE_EQ(0., id_transform.translation.y);
  EXPECT_DOUBLE_EQ(0., id_transform.translation.z);
}

int main(int argc, char** argv) {
  ::testing::InitGoogleTest(&argc, argv);
  qi::Application app(argc, argv);
  return RUN_ALL_TESTS();
}


/*
 * Copyright (c) 2020 Intel Corporation
 * 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.
 */

/**
 * @file scurve_planner.hpp
 *
 * Maintainer: Yu Yan <yu.yan@intel.com>
 *
 */

#pragma once

#include <RTmotion/global.hpp>
#include <stdlib.h>
#include <math.h>
#include <algorithm>
#include <Eigen/Core>
#include <map>
#include <RTmotion/algorithm/math_utils.hpp>

using RTmotion::MC_ERROR_CODE;

typedef enum {
  POSITION_ID = 0,
  SPEED_ID = 1,
  ACCELERATION_ID = 2,
  JERK_ID = 3
} WaypointIndex;

namespace trajectory_processing
{
struct ScurveCondition
{
  // Condition inputs for planner
  double q0 = 0.0;
  double q1 = 0.0;
  double v0 = 0.0;
  double v1 = 0.0;
  double a0 = 0.0;
  double a1 = 0.0;
  double v_max = 0.0;
  double a_max = 0.0;
  double j_max = 0.0;
};

struct ScurveProfile
{
  // Time domains for offline scurve planning results
  double Tj1 = 0.0;
  double Ta = 0.0;
  double Tj2 = 0.0;
  double Tv = 0.0;
  // Time domains for online scurve planning results
  double Tj2a = 0.0;
  double Tj2c = 0.0;
  double Tj2b = 0.0;
  double Td = 0.0;
  double Th = 0.0;
};

class ScurvePlanner
{
public:
  ScurvePlanner() = default;
  ~ScurvePlanner() = default;

  /**
   * @brief Sign transforms for being able to calculate trajectory with q1 < q0.
   *        Look at 'Trajectory planning for automatic machines and
   * robots(2008)'
   * @param condition  The input scurve condition, i.e. q0, q1, v0, v1, v_max,
   * a_max, j_max
   */
  void signTransforms(ScurveCondition& condition);

  /**
   * @brief Transforms point back to the original sign.
   * @param p Reference to Point, i.e. acc, vel, pos
   */
  void pointSignTransform(Eigen::Vector4d& p);

  virtual MC_ERROR_CODE plan() = 0;
  virtual Eigen::Vector4d getWaypoint(double t) = 0;

  ScurveProfile profile_;
  ScurveCondition condition_;
  double s_ = 0.0;  // Sign transform flag parameter
};

}  // namespace trajectory_processing


// ~/xlin/gsl/testminsky.cc
// 150924 003 Task now -> grid
// Basert på 150921 001 testminskyall.cc
// Unified source for (74) and (77) variants
//
// Todo:  Endre kappa def til arctan ? (se:'destabilizing..')
//

// GSL Manual 26.6
//  http://stackoverflow.com/questions/27913858/how-to-consult-gsl-ode-with-many-parameters-and-harmonic-functions
// http://www.tutorialspoint.com/matlab/matlab_differential.htm
// Løser differentialene vha octave-symbolic se ~/xoct/minsky/differentiate.
// Transformerer octave-løsningene til c++ (e)^ -> exp() og x^y -> pow(x,y)
// se testdiff.cpp i ~/xlin/cpp/scratch
// rk8 bruker ikke jac fjernet f.o.m versjon 014
//
#include <stdio.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv2.h>

#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <fstream>
#include <iomanip> // setiosflags(ios::fixed)
#include <limits>
#include <map>
#include <string>
#include <sstream>
#include <vector>

#include <boost/array.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/foreach.hpp>
#include <unistd.h>

using namespace std;

// Warn about use of deprecated functions.
#define GNUPLOT_DEPRECATE_WARN
#include "gnuplot-iostream.h"

#ifndef M_PI
#       define M_PI 3.14159265358979323846
#endif

inline void mysleep(unsigned millis){
  ::usleep(millis * 1000);
}

//http://stackoverflow.com/questions/7248627/setting-width-in-c-output-stream:
class formatted_output
{
private:
  int width, precision;
  std::ostream& stream_obj;
public:
  formatted_output(std::ostream& obj, int w, int p): width(w), precision(p),stream_obj(obj) {}
  template<typename T>
  formatted_output& operator<<(const T& output)  {
    stream_obj << std::fixed << std::setw(width) << std::setprecision(precision) << output;
    return *this;
  }
  formatted_output& operator<<(std::ostream& (*func)(std::ostream&))  {
    func(stream_obj);
    return *this;
  }
};

struct param_type {
  int eqset;
  double alfa;
  double beta;
  double delta;
  double nu;
  double r;
  double k1;
  double k2;
  double k3;
  double k4;
  double k5;
  double k6;
  double k7;
  double k8;
} ;

double kappa(double k1, double k2, double k3, double profit){
  double response;
  response = k1 + exp(k2) * exp( k3 * profit);
  return response;
}

double phi(double k4, double empshare){
  double response;
  response = (pow(k4,3)/(1.0-pow(k4,2)))/pow((1.0 - empshare),2) - k4/(1.0-pow(k4,2));
  return response;
}

double psi(double k5, double k6, double k7, double k8, double growthrate){
  double response;
  response = k5 + k6* exp(k7)*exp(k8 * growthrate);
  return response;
}

int func (double t, const double y[], double f[],
           void *params)
     {
       struct param_type *my_params_pointer = (param_type*) params;
        int eqset     = my_params_pointer->eqset;
	double alfa   = my_params_pointer->alfa;
	double beta   = my_params_pointer->beta;
	double delta  = my_params_pointer->delta;
	double nu     = my_params_pointer->nu;
	double r      = my_params_pointer->r;
	double k1     = my_params_pointer->k1;
	double k2     = my_params_pointer->k2;
	double k3     = my_params_pointer->k3;
	double k4     = my_params_pointer->k4;
	double k5     = my_params_pointer->k5;
	double k6     = my_params_pointer->k6;
	double k7     = my_params_pointer->k7;
	double k8     = my_params_pointer->k8;

        if (eqset == 74) {
	  f[0] =  y[0] * ( phi(k4, y[1]) - alfa) ;
	  f[1] =  y[1] * ( kappa(k1, k2, k3, 1 - y[0] - r * y[2]) /nu  - alfa - beta -delta);
	  f[2] =  y[2] * ( r -  kappa(k1, k2, k3, 1 - y[0] - r* y[2])/nu + delta) + kappa(k1, k2, k3,1 - y[0] - r * y[2]) - (1 - y[0]) + y[3];
	  f[3] =  y[3] * ( psi(k5, k6, k7, k8, kappa(k1, k2, k3, 1 - y[0] -r*y[2]) / nu  - delta) + kappa(k1, k2, k3, 1 - y[0] - r * y[2]) / nu - delta );
	}

	if (eqset == 77) {
	  f[0] =  y[0] * ( phi(k4, y[1]) - alfa) ;
	  f[1] =  y[1] * (( kappa(k1, k2, k3, 1 - y[0] - r/( y[2] * y[3] )) /nu ) - alfa - beta -delta);
	  f[2] =  y[2] * ( psi(k5, k6, k7, k8, kappa(k1, k2, k3, 1 - y[0] -r/(y[2] * y[3] )) / nu - delta) - r) - ( y[2] * y[2] * ( y[3] * kappa(k1, k2, k3,1 - y[0] - r/(y[2] * y[3])) - y[3] * (1 - y[0]) + 1 ));
	  f[3] =  y[3] * ( -psi(k5, k6, k7, k8, ( kappa(k1, k2, k3, 1 - y[0] -r/(y[2] * y[3])) / nu ) - delta) + (kappa(k1, k2, k3, 1 - y[0] - r/( y[2] * y[3] )) / nu) - delta );
	}

        return GSL_SUCCESS;
     }

     // fjernet - finnes i minsky.jac.cc = minsky.013.cc
     /*
     int jac (double t, const double y[], double *dfdy,
          double dfdt[], void *params)
     {
      return GSL_SUCCESS;
     }
     */

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

       if (argc < 2) {
         std::cout << "Usage: minsky eqset (74|77) [omega lambda d|upsilon  p|x num_steps])" << endl;
         exit(0);
       }

       int eqset = 74;
       if (argc > 1) eqset   = atoi(argv[1]);
       cout << eqset << endl; // CONTROL
       if (eqset != 74 && eqset != 77) {
         std::cout << "eqset has to be either 74 or 77" << endl;
         exit(0);
       }
       double y[4] = { 0.0, 0.0, 0.0, 0.0 } ;
       if (eqset == 74){
	 //y[4] = { 0.95, 0.9, 0.0, 0.001 } ;   // omega, lambda, d, p
	 y[0] = 0.95;
	 y[1] = 0.9;
	 y[2] = 0.0;
	 y[3] = 0.001;
       }
       if (eqset == 77) {
	 //y[4] = { 0.95, 0.9, 0.12, 100 } ;   // omega, lambda, upsilon, x
	 y[0] = 0.95;
	 y[1] = 0.9;
	 y[2] = 0.12;
	 y[3] = 100.0;

       }
       int num_steps = 100;

       if (argc > 2) y[0] = atof(argv[2]);
       if (argc > 3) y[1] = atof(argv[3]);
       if (argc > 4) y[2] = atof(argv[4]);
       if (argc > 5) y[3] = atof(argv[5]);
       if (argc > 6) num_steps = atoi(argv[6]);

       Gnuplot gp1;
       Gnuplot gp2;
       Gnuplot gp3;
       Gnuplot gp4;

       std::ostringstream oss;
       oss << "set title \"";
       for (int j = 0; j < argc; j++)
	 { oss << argv[j] << ' '; }
       oss << "\\n\"" << std::endl ;
       std::cout << oss.str() << std::endl;

       std::ofstream funkout("minsky.funk.out");
       std::ofstream varout("minsky.var.out");
       formatted_output fout(funkout, 16, 6);
       formatted_output vout(varout, 12, 6);

       std::vector<std::pair<double, double> > xy_pts_A; // omega
       std::vector<std::pair<double, double> > xy_pts_B; // lambda
       std::vector<std::pair<double, double> > xy_pts_C; // d         // upsilon
       std::vector<std::pair<double, double> > xy_pts_D; // p         // x

       std::vector<std::pair<double, double> > xy_pts_E; // p (ponzi)
       std::vector<std::pair<double, double> > xy_pts_F; // d (debt)
       std::vector<std::pair<double, double> > xy_pts_G; // g (growth)
       std::vector<std::pair<double, double> > xy_pts_Y; // Y

       double alfa   = 0.025;
       double beta   = 0.02;
       double delta  = 0.01;
       double nu     = 3.0;
       double r      = 0.03;
       double k1     = -0.0065;
       double k2     = -5 ;
       double k3     =  20;
       double k4     = 0.04;
       double k5     = -0.25;
       double k6     = 0.25;
       double k7     = -0.36;
       double k8     = 12.0;
       double growth = 0.0;
       double Y      = 0.0;

       double zphi;
       double zkappa;
       double zpsi;

       if (eqset == 74) {
         growth = kappa(k1, k2, k3, 1 - y[0] - r * y[2]) / nu - delta;
       }
       else if (eqset == 77) {
	 growth = kappa(k1, k2, k3, 1 - y[0] - r/( y[2] * y[3] )) / nu - delta;
       }
       Y = 100.0 * ( 1.0 + growth);

       struct param_type my_params = {eqset, alfa, beta, delta, nu, r, k1, k2, k3, k4, k5, k6, k7, k8 };

       // gsl_odeiv2_system sys = {func, jac, 4, &my_params};
       // http://www.physics.buffalo.edu/phy411-506/tools/gsl/ode/index.html
       // rk8pd bruker ikke jac settes til NULL:
       // gsl_odeiv2_system sys = {func, jac, 4, &my_params};
       gsl_odeiv2_system sys = {func, NULL, 4, &my_params};
       gsl_odeiv2_driver * d =
       gsl_odeiv2_driver_alloc_y_new (&sys, gsl_odeiv2_step_rk8pd,
					1e-6, 1e-6, 0.0);
       int i = 0;

       double t = 0.0, t1 = 100.0;

       xy_pts_A.push_back(std::make_pair(t, y[0]));             //omega
       xy_pts_B.push_back(std::make_pair(t, y[1]));		//lambda
       xy_pts_C.push_back(std::make_pair(t, y[2]));             //d|upsilon
       xy_pts_D.push_back(std::make_pair(t, y[3]));		//p|x

       if (eqset == 77) {
	 xy_pts_E.push_back(std::make_pair(t, 1 / y[3]));         //ponzi
	 xy_pts_F.push_back(std::make_pair(t, 1 / y[3] / y[2]));  //debt
       }
       xy_pts_G.push_back(std::make_pair(t, growth));           //growth
       xy_pts_Y.push_back(std::make_pair(t, Y));                //Y

       zphi   = phi(k4, y[1]);
       if (eqset == 74) {
	 zkappa = kappa(k1, k2, k3, 1 - y[0] - r * y[2]);
	 zpsi   = psi(k5, k6, k7, k8, kappa(k1, k2, k3, 1 - y[0] -r * y[2]) / nu - delta);
       }
       if (eqset == 77) {
	 zkappa = kappa(k1, k2, k3, 1 - y[0] - r/( y[2] * y[3] ));
	 zpsi   = psi(k5, k6, k7, k8, kappa(k1, k2, k3, 1 - y[0] -r/(y[2] * y[3] )) / nu - delta);
       }
       fout << oss.str() << std::endl;
       fout << "t" << "phi" <<"kappa" << "psi" << std::endl;
       fout << t << zphi << zkappa << zpsi << std::endl;

       vout << oss.str() << std::endl;
       if (eqset == 74)  {
	 vout << "t" << "omega" <<"lambda" << "d" << "p" << "growth" << "Y"  << std::endl;
	 vout << t  << y[0] << y[1] << y[2] << y[3] << growth << Y <<std::endl;
       }
       if (eqset == 77) {
 	 vout << "t" << "omega" <<"lambda" << "upsilon" << "debt" << "ponzi" << "x" << "growth" << "Y"  << std::endl;
	 vout << t  << y[0] << y[1] << y[2] << y[3] <<  1/y[3]/y[2] <<1/y[3] << growth << Y <<std::endl;       }
       if (eqset == 74) {
	 printf ("%6s %10s %10s %10s %10s %10s %10s %10s %10s\n", "t","omega","lambda","d","p","","","growth","Y");
	 printf ("%6.0f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n", t, y[0], y[1], y[2], y[3], 0.0, 0.0, growth, Y);
       }
       if (eqset == 77) {
         printf ("%6s %10s %10s %10s %10s %10s %10s %10s %10s\n", "t","omega","lambda","upsilon","x","ponzi","debt","growth","Y");
         printf ("%6.0f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n", t, y[0], y[1], y[2], y[3], 1/y[3]/y[2], 1/y[3],growth, Y);
       }

       for (i = 1; i <= num_steps; i++)
         {
           double ti = i * t1 / 100.0;
           int status = gsl_odeiv2_driver_apply (d, &t, ti, y);
           if (status != GSL_SUCCESS)
	     {
	       printf ("error, return value=%d\n", status);
	       break;
	     }

           //Funksjonsverdiene:
	   zphi   = phi(k4, y[1]);
	   if (eqset == 74) {
	     zkappa = kappa(k1, k2, k3, 1 - y[0] - r * y[2]);
	     zpsi   = psi(k5, k6, k7, k8, kappa(k1, k2, k3, 1 - y[0] -r *y[2]) / nu - delta);
	     growth = kappa(k1, k2, k3, 1 - y[0] - r* y[2]) / nu - delta;
	   }
	   if (eqset == 77) {
	     zkappa = kappa(k1, k2, k3, 1 - y[0] - r/( y[2] * y[3] ));
	     zpsi   = psi(k5, k6, k7, k8, kappa(k1, k2, k3, 1 - y[0] -r/(y[2] * y[3] )) / nu - delta);
	     growth = kappa(k1, k2, k3, 1 - y[0] - r/( y[2] * y[3] )) / nu - delta;
	   }
	   Y = Y * ( 1 + growth);

	   fout << t << zphi << zkappa << zpsi << std::endl;
	   vout << t << y[0] << y[1] << y[2] << y[3] << growth << Y <<std::endl;
	   if (eqset == 74) {
	     printf ("%6.0f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n", t, y[0], y[1], y[2], y[3], 0.0, 0.0, growth, Y);
	   }
	   if (eqset == 77) {
	     printf ("%6.0f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n", t, y[0], y[1], y[2], y[3], 1/y[3]/y[2], 1/y[3], growth, Y);
	   }
 	   xy_pts_A.push_back(std::make_pair(t, y[0]));
	   xy_pts_B.push_back(std::make_pair(t, y[1]));
	   xy_pts_C.push_back(std::make_pair(t, y[2]));
	   xy_pts_D.push_back(std::make_pair(t, y[3]));
	   if (eqset == 77) {
	     xy_pts_E.push_back(std::make_pair(t, 1 / y[3]));         //ponzi
	     xy_pts_F.push_back(std::make_pair(t, 1 / y[3] / y[2]));  //debt
	   }
	   xy_pts_G.push_back(std::make_pair(t, growth)); //growth
           xy_pts_Y.push_back(std::make_pair(t, Y)); //Y
	 }

       gsl_odeiv2_driver_free (d);

       // GNPLOTS:
       if (eqset == 74){
	 // Omega & Lambda:
	 gp1 << "set terminal x11 1 persist size 640,450 position 50,50\n";
	 gp1 << "set grid\n";
	 gp1 << oss.str() ; // Title string
	 gp1 << "set ylabel 'omega lambda'\n";
	 gp1 << "plot '-' with lines title 'omega', '-' with lines title 'lambda'\n";
	 gp1.send1d(xy_pts_A);
	 gp1.send1d(xy_pts_B);
	 // p & d:
	 gp2 << "set terminal x11 1 persist size 640,450 position 50,536\n";
	 gp2 << "set grid\n";
	 gp2 << oss.str() ; // Title string
	 gp2 << "set ylabel 'p\n";
	 gp2 << "set y2label 'd'\n";
	 gp2 << "set y2tics\n";
	 //gp2 << "plot '-' with lines title 'd\n";
 	 gp2 << "plot '-' with lines title 'p', '-' with lines  title 'd' axes x1y2\n";
	 gp2.send1d(xy_pts_D);
	 gp2.send1d(xy_pts_C);

	 /*	 // p:
	 gp3 << "set terminal x11 1 persist size 640,450 position 690,536\n";
	 gp3 << "set grid\n";
	 gp3 << oss.str() ; // Title string
	 gp3 << "set ylabel 'p\n";
	 gp3 << "plot '-' with lines title 'p\n";
	 gp3.send1d(xy_pts_D);
	 */
       }

       if (eqset == 77) {
	 // Omega & Lambda & Upsilon & X:
	 gp1 << "set terminal x11 1 persist size 640,450 position 50,50\n";
	 gp1 << "set grid\n";
	 gp1 << oss.str() ; // Title string
	 gp1 << "set ylabel 'omega lambda upsilon'\n";
	 gp1 << "set y2label 'x'\n";
	 gp1 << "set y2tics\n";
	 gp1 << "plot '-' with lines title 'omega', '-' with lines title 'lambda', '-' with lines title 'upsilon','-' with lines  title 'x' axes x1y2\n";
	 gp1.send1d(xy_pts_A);
	 gp1.send1d(xy_pts_B);
	 gp1.send1d(xy_pts_C);
	 gp1.send1d(xy_pts_D);
	 // Ponzi & Debt:
	 gp2 << "set terminal x11 2 persist size 640,450 position 50,536\n";
	 gp2 << "set grid\n";
	 gp2 << oss.str() ; // Title string
	 gp2 << "set ylabel 'ponzi'\n";
	 gp2 << "set y2label 'debt'\n";
	 gp2 << "set y2tics\n";
	 gp2 << "plot '-' with lines title 'ponzi', '-' with lines title 'debt' axes x1y2\n";
	 gp2.send1d(xy_pts_E);
	 gp2.send1d(xy_pts_F);
       }
       //  p/ponzi & growth:
       gp3 << "set terminal x11 2 persist size 640,450 position 690,50\n";
       gp3 << "set grid\n";
       gp3 << oss.str() ; // Title string
       gp3 << "set ylabel 'p/ponzi'\n";
       gp3 << "set y2label 'growthi'\n";
       gp3 << "set y2tics\n";
       gp3 << "plot '-' with lines title 'p/ponzi', '-' with lines title 'growth' axes x1y2\n";
       if (eqset == 74) gp3.send1d(xy_pts_D);
       if (eqset == 77) gp3.send1d(xy_pts_E);
       gp3.send1d(xy_pts_G);

       //  Y & d/debt:
       gp4 << "set terminal x11 2 persist size 640,450 position 690,536\n";
       gp4 << "set grid\n";
       gp4 << oss.str() ; // Title string
       gp4 << "set ylabel 'Y'\n";
       gp4 << "set y2label 'd/debt'\n";
       gp4 << "set y2tics\n";
       gp4 << "plot '-' with lines title 'Y', '-' with lines title 'd/debt' axes x1y2\n";
       gp4.send1d(xy_pts_Y);
       if (eqset == 74) gp4.send1d(xy_pts_C);
       if (eqset == 77) gp4.send1d(xy_pts_F);

       return 0;
     }

// EOF


#include "processor.h"
#include "histogramutility.h"
#include <math.h>
#include <algorithm>
#include <iostream>
#include "boost/pending/disjoint_sets.hpp"
#include <boost/unordered/unordered_set.hpp>

using namespace cv;
using namespace std;

Processor::Processor()
{
}

Mat Processor::invert(Mat &img){
    Mat res(img.rows, img.cols, img.type());

    for (int i = 0; i < img.rows; i++){
        for (int j = 0; j < img.cols; j++){
            Vec3b& color = res.at<Vec3b>(Point(j, i));
            Vec3b& color_org = img.at<Vec3b>(Point(j, i));

            color[0] = 255 - color_org[0];
            color[1] = 255 - color_org[1];
            color[2] = 255 - color_org[2];
        }
    }
    return res;
}

Mat* Processor::kirsch(Mat& img){

    Mat* results = new Mat[5];

    Mat res_1(img.rows, img.cols, img.type());
    Mat res_2(img.rows, img.cols, img.type());
    Mat res_3(img.rows, img.cols, img.type());
    Mat res_4(img.rows, img.cols, img.type());

    Point anchor(1, 1);
    double delta = 0;
    int ddepth = -1;
    const int filter_width = 3;

    float elements_1[filter_width * filter_width] = {-1, 0, 1, -1, 0, 1, -1, 0, 1};
    float elements_2[filter_width * filter_width] = {1, 1, 1, 0, 0, 0, -1, -1, -1};
    float elements_3[filter_width * filter_width] = {0, 1, 1, -1, 0, 1, -1, -1, 0};
    float elements_4[filter_width * filter_width] = {1, 1, 0, 1, 0, -1, 0, -1, -1};

    Mat kernel_1(Size(filter_width, filter_width), CV_32F, elements_1);
    filter2D(img, res_1, ddepth, kernel_1, anchor, delta, BORDER_DEFAULT);

    Mat kernel_2(Size(filter_width, filter_width), CV_32F, elements_2);
    filter2D(img, res_2, ddepth, kernel_2, anchor, delta, BORDER_DEFAULT);

    Mat kernel_3(Size(filter_width, filter_width), CV_32F, elements_3);
    filter2D(img, res_3, ddepth, kernel_3, anchor, delta, BORDER_DEFAULT);

    Mat kernel_4(Size(filter_width, filter_width), CV_32F, elements_4);
    filter2D(img, res_4, ddepth, kernel_4, anchor, delta, BORDER_DEFAULT);

    Mat res(img.rows, img.cols, img.type());
    for(int i = 0; i < img.cols; i++){
        for(int j = 0; j < img.rows; j++){
            Vec3b& color1 = res_1.at<Vec3b>(Point(i, j));
            Vec3b& color2 = res_2.at<Vec3b>(Point(i, j));
            Vec3b& color3 = res_3.at<Vec3b>(Point(i, j));
            Vec3b& color4 = res_4.at<Vec3b>(Point(i, j));

            Vec3b& color_res = res.at<Vec3b>(Point(i, j));

            int max = color1[0] > color2[0] ? color1[0] : color2[0];
            max = max > color3[0] ? max : color3[0];
            max = max > color4[0] ? max : color4[0];

            color_res[0] = max;
            color_res[1] = max;
            color_res[2] = max;
        }
    }

    results[0] = res_1;
    results[1] = res_2;
    results[2] = res_3;
    results[3] = res_4;
    results[4] = res;

    return results;
}

Mat Processor::prewitt(Mat &img){
    Mat res_x(img.rows, img.cols, img.type());
    Mat res_y(img.rows, img.cols, img.type());

    Point anchor(1, 1);
    double delta = 0;
    int ddepth = -1;
    const int filter_width = 3;

    float elements_x[filter_width * filter_width] = {-1, 0, 1, -1, 0, 1, -1, 0, 1};
    float elements_y[filter_width * filter_width] = {-1, -1, -1, 0, 0, 0, 1, 1, 1};

    Mat kernel_x(Size(filter_width, filter_width), CV_32F, elements_x);
    filter2D(img, res_x, ddepth, kernel_x, anchor, delta, BORDER_DEFAULT);

    Mat kernel_y(Size(filter_width, filter_width), CV_32F, elements_y);
    filter2D(img, res_y, ddepth, kernel_y, anchor, delta, BORDER_DEFAULT);

    Mat res(img.rows, img.cols, img.type());
    for(int i = 0; i < img.cols; i++){
        for(int j = 0; j < img.rows; j++){
            Vec3b& color1 = res_x.at<Vec3b>(Point(i, j));
            Vec3b& color2 = res_y.at<Vec3b>(Point(i, j));

            Vec3b& color_res = res.at<Vec3b>(Point(i, j));

            int sum = sqrt(color1[0] * color1[0] + color2[0] * color2[0]);

            sum = sum > 255 ? 255:sum;
            sum = sum < 0 ? 0 : sum;

            color_res[0] = sum;
            color_res[1] = sum;
            color_res[2] = sum;
        }
    }

    return res;
}

Mat Processor::gaussian(Mat &img){
    Mat res(img.rows, img.cols, img.type());

    Point anchor(1, 1);
    double delta = 0;
    int ddepth = -1;
    const int filter_width = 7;

    float elements[filter_width * filter_width] = {0.00000067, 0.00002292, 0.00019117, 0.00038771, 0.00019117, 0.00002292, 0.00000067,
                                                  0.00002292, 0.00078634, 0.00655965, 0.01330373, 0.00655965, 0.00078633, 0.00002292,
                                                  0.00019117, 0.00655965, 0.05472157, 0.11098164, 0.05472157, 0.00655965, 0.00019117,
                                                  0.00038771, 0.01330373, 0.11098164, 0.22508352, 0.11098164, 0.01330373, 0.00038771,
                                                  0.00019117, 0.00655965, 0.05472157, 0.11098164, 0.05472157, 0.00655965, 0.00019117,
                                                  0.00002292, 0.00078634, 0.00655965, 0.01330373, 0.00655965, 0.00078633, 0.00002292,
                                                  0.00000067, 0.00002292, 0.00019117, 0.00038771, 0.00019117, 0.00002292, 0.00000067
                                                  };

    Mat kernel(Size(filter_width, filter_width), CV_32F, elements);
    filter2D(img, res, ddepth, kernel, anchor, delta, BORDER_DEFAULT);

    return res;
}

Mat Processor::laplacian(Mat &img){
    Mat res(img.rows, img.cols, img.type());

    Point anchor(1, 1);
    double delta = 0;
    int ddepth = -1;
    const int filter_width = 3;

    float elements[filter_width * filter_width] = {0, 1, 0, 1, -4, 1, 0, 1, 0};

    Mat kernel(Size(filter_width, filter_width), CV_32F, elements);
    filter2D(img, res, ddepth, kernel, anchor, delta, BORDER_DEFAULT);

    int filterWidth = 3;

//    double **filter;
//    filter = new double *[filterWidth];
//    for(int i = 0; i < filterWidth; i++)
//        filter[i] = new double[filterWidth];

//    filter[0][0] = 0;
//    filter[0][1] = -1;
//    filter[0][2] = 0;

//    filter[1][0] = -1;
//    filter[1][1] = 4;
//    filter[1][2] = -1;

//    filter[2][0] = 0;
//    filter[2][1] = -1;
//    filter[2][2] = 0;

//    res = apply_filter(img, filter, filterWidth);

//    int min = 10000;
//    int max = -10000;
//    for(int i = 0; i < res.cols; i++){
//        for(int j = 0; j < res.rows; j++){
//            Vec3b& color_res = res.at<Vec3b>(Point(i, j));

//            if (color_res[0] < min){
//                min = color_res[0];
//            }

//            if (color_res[0] > max){
//                max = color_res[0];
//            }
//        }
//    }

//    for(int i = 0; i < res.cols; i++){
//        for(int j = 0; j < res.rows; j++){
//            Vec3b& color_res = res.at<Vec3b>(Point(i, j));

//            int sum = ((double)(color_res[0] - min) / (double)(max - min)) * 255;

//            color_res[0] = sum;
//            color_res[1] = sum;
//            color_res[2] = sum;
//        }
//    }

//    cout << "Max: " << max << endl;
//    cout << "Min: " << min << endl;

    for(int i = 0; i < res.cols; i++){
        for(int j = 0; j < res.rows; j++){
            Vec3b& color_res = res.at<Vec3b>(Point(i, j));

            int sum = color_res[0] + 128;

            color_res[0] = sum;
            color_res[1] = sum;
            color_res[2] = sum;
        }
    }

    return res;
}

Mat Processor::sobel(Mat &img){
    Mat res_x(img.rows, img.cols, img.type());
    Mat res_y(img.rows, img.cols, img.type());

    Point anchor(1, 1);
    double delta = 0;
    int ddepth = -1;
    const int filter_width = 3;

    float elements_x[filter_width * filter_width] = {-1, 0, 1, -2, 0, 2, -1, 0, 1};
    float elements_y[filter_width * filter_width] = {-1, -2, -1, 0, 0, 0, 1, 2, 1};

    Mat kernel_x(Size(filter_width, filter_width), CV_32F, elements_x);
    filter2D(img, res_x, ddepth, kernel_x, anchor, delta, BORDER_DEFAULT);

    Mat kernel_y(Size(filter_width, filter_width), CV_32F, elements_y);
    filter2D(img, res_y, ddepth, kernel_y, anchor, delta, BORDER_DEFAULT);

    Mat res(img.rows, img.cols, img.type());
    for(int i = 0; i < img.cols; i++){
        for(int j = 0; j < img.rows; j++){
            Vec3b& color1 = res_x.at<Vec3b>(Point(i, j));
            Vec3b& color2 = res_y.at<Vec3b>(Point(i, j));

            Vec3b& color_res = res.at<Vec3b>(Point(i, j));

            int sum = sqrt(color1[0] * color1[0] + color2[0] * color2[0]);

            sum = sum > 255 ? 255:sum;
            sum = sum < 0 ? 0 : sum;

            color_res[0] = sum;
            color_res[1] = sum;
            color_res[2] = sum;
        }
    }

    return res;
}

Mat Processor::roberts(Mat &img){
    Mat res_x(img.rows, img.cols, img.type());
    Mat res_y(img.rows, img.cols, img.type());

    Point anchor(0, 1);
    double delta = 0;
    int ddepth = -1;
    const int filter_width = 2;

    float elements_x[filter_width * filter_width] = {1.0, 0.0, 0.0, -1.0};
    float elements_y[filter_width * filter_width] = {0.0, 1.0, -1.0, 0.0};

    Mat kernel_x(Size(filter_width, filter_width), CV_32F, elements_x);
    filter2D(img, res_x, ddepth, kernel_x, anchor, delta, BORDER_DEFAULT);

    Mat kernel_y(Size(filter_width, filter_width), CV_32F, elements_y);
    filter2D(img, res_y, ddepth, kernel_y, anchor, delta, BORDER_DEFAULT);

    Mat res(img.rows, img.cols, img.type());
    for(int i = 0; i < img.cols; i++){
        for(int j = 0; j < img.rows; j++){
            Vec3b& color1 = res_x.at<Vec3b>(Point(i, j));
            Vec3b& color2 = res_y.at<Vec3b>(Point(i, j));

            Vec3b& color_res = res.at<Vec3b>(Point(i, j));

            int sum = sqrt(color1[0] * color1[0] + color2[0] * color2[0]);

            sum = sum > 255 ? 255:sum;
            sum = sum < 0 ? 0 : sum;

            color_res[0] = sum;
            color_res[1] = sum;
            color_res[2] = sum;
        }
    }

    return res;
}

Mat Processor::component_labeling(Mat &img, int white, int black){

    img = iterative_thresholding(img);

    int current_label = 0;

    Mat res(img.rows, img.cols, img.type());

    for (int i = 0; i < img.rows; i++){
        for (int j = 0; j < img.cols; j++){
            Vec3b& color = res.at<Vec3b>(Point(j, i));

            color[0] = 0;
        }
    }

    vector<int> rank(2000);
    vector<int> parent(2000);
    boost::disjoint_sets<int*, int*> ds(&rank[0], &parent[0]);
    boost::unordered_set<int> labels;

    for (int i = 0; i < img.rows; i++){
        for (int j = 0; j < img.cols; j++){

            Vec3b& color = img.at<Vec3b>(Point(j, i));
            int pixel = color[0];

            if (pixel == white){

                int pixel_top = black;
                if (i - 1 >= 0){
                    pixel_top = img.at<Vec3b>(Point(j, i - 1))[0];
                }

                int pixel_left = black;
                if (j - 1 >= 0){
                    pixel_left = img.at<Vec3b>(Point(j - 1, i))[0];
                }

                Vec3b& res_color = res.at<Vec3b>(Point(j, i));
                int result_v = -1;

                if (pixel_left == black && pixel_top == black){
                    current_label++;

                    cout << current_label << endl;

                    ds.make_set(current_label);
                    labels.insert(current_label);

                    result_v = current_label;
                }else if (pixel_left == black && pixel_top == white){

                    result_v = res.at<Vec3b>(Point(j, i - 1))[0];

                }else if (pixel_left == white && pixel_top == black){

                    result_v = res.at<Vec3b>(Point(j - 1, i))[0];

                }else if (pixel_left == white && pixel_top == white){
                    int left_label = res.at<Vec3b>(Point(j - 1, i))[0];
                    int top_label = res.at<Vec3b>(Point(j, i - 1))[0];

                    if (left_label != top_label){
                        ds.union_set(left_label, top_label);
                    }

                    result_v = left_label < top_label ? left_label : top_label;

                }

                if (result_v == -1){
                    cout << "ERROR" << endl;
                    exit(-1);
                }

                res_color[0] = result_v;
                res_color[1] = result_v;
                res_color[2] = result_v;
            }

        }
    }

    ds.normalize_sets(labels.begin(), labels.end());
    cout << "Number of components: " << ds.count_sets(labels.begin(), labels.end()) << endl;

    int r[] = {0, 150, 175, 200, 225, 250, 25, 50, 75, 100, 125};
    int g[] = {125, 150, 175, 200, 225, 250, 0, 25, 50, 75, 100};
    int b[] = {0, 25, 50, 175, 200, 225, 150, 250, 75, 100, 125};

    map<int, int> final_eq;
    int index = 0;
    int area = 0;
    for (int i = 0; i < img.rows; i++){
        for (int j = 0; j < img.cols; j++){
            Vec3b& color = res.at<Vec3b>(Point(j, i));
            int temp = (int)color[0];

            temp = ds.find_set(temp);

            if (color[0] != 0){
                if (final_eq[temp] == 0){
                    index++;
                    final_eq[temp] = index;
                }

                int index = (final_eq[temp] * 13) % 11;

                color[0] = r[index];
                color[1] = g[index];
                color[2] = b[index];

                area++;
            }else{
                color[0] = black;
                color[1] = black;
                color[2] = black;
            }
        }
    }

    cout << "Number of components: " << index << endl;
    cout << "Average area of the components: " << (double)(area) / index << endl;

    return res;

}

void Processor::convert_to_grayscale(Mat &img){
    for(int i = 0; i < img.cols; i++)
    {
        for(int j = 0; j < img.rows; j++)
        {
            Vec3b& color = img.at<Vec3b>(Point(i, j));
            int r = color[0];
            int g = color[1];
            int b = color[2];
            int gray_scale = 0.21 * r + 0.72 * g + 0.07 * b;

            color[0] = gray_scale;
            color[1] = gray_scale;
            color[2] = gray_scale;
        }
    }
}

Mat Processor::median_filter(Mat &img, int filter_w){
    int redu = filter_w / 2;

    Mat res(img.rows - redu * 2, img.cols - redu * 2, img.type());

    for(int i = redu; i < res.cols - redu; i++){
        for(int j = redu; j < res.rows - redu; j++){

            vector<int> values;
            for (int ii = 0; ii < filter_w; ii++){
                for (int jj = 0; jj < filter_w; jj++){
                    Vec3b& color = img.at<Vec3b>(Point(i + ii - redu, j + jj - redu));
                    values.push_back(color[0]);
                }
            }
            sort(values.begin(), values.end());

            Vec3b& color = res.at<Vec3b>(Point(i, j));

            int new_val = values[values.size() / 2 + 1];

            color[0] = new_val;
            color[1] = new_val;
            color[2] = new_val;
        }
    }

    return res;
}

Mat Processor::apply_even_filter(Mat& img, double** filter, int filter_w){
    Mat res(img.rows, img.cols, img.type());

    for(int i = 0; i < res.cols; i++){
        for(int j = 0; j < res.rows; j++){

            double sum = 0;
            for (int ii = 0; ii < filter_w; ii++){
                for (int jj = 0; jj < filter_w; jj++){
                    Vec3b& color = img.at<Vec3b>(Point(i + ii, j + jj));
                    sum += (color[0] * filter[ii][jj]);
                }
            }

            Vec3b& color = res.at<Vec3b>(Point(i, j));

            color[0] = sum;
            color[1] = sum;
            color[2] = sum;
        }
    }

    return res;
}

Mat Processor::apply_filter(Mat& img, double** filter, int filter_w, int threshold){
    int redu = filter_w / 2;

    Mat res(img.rows - redu * 2, img.cols - redu * 2, img.type());

    for(int i = redu; i < res.cols - redu; i++){
        for(int j = redu; j < res.rows - redu; j++){

            double sum = 0;
            for (int ii = 0; ii < filter_w; ii++){
                for (int jj = 0; jj < filter_w; jj++){
                    Vec3b& color = img.at<Vec3b>(Point(i + ii - redu, j + jj - redu));
                    sum += (color[0] * filter[ii][jj]);
                }
            }

            Vec3b& color = res.at<Vec3b>(Point(i, j));

            if (abs(sum - img.at<Vec3b>(Point(i, j))[0]) < threshold) {
                color[0] = sum;
                color[1] = sum;
                color[2] = sum;
            }else{
                color[1] = color[0];
                color[2] = color[0];
            }
        }
    }

    return res;
}

Mat Processor::contrast_enhance(Mat& img, int lower , int higher){

    int CLT[256];

    for (int i = 0; i < 256; i++){
        if (i < lower){
            CLT[i] = 0;
        }else if(i > higher){
            CLT[i] = 255;
        }else{
            CLT[i] = (int)(((double)i - (double)lower) / ((double)higher - (double)lower) * 255);
        }
    }

    Mat res(img.size(), img.type());
    for(int i = 0; i < res.cols; i++){
        for(int j = 0; j < res.rows; j++){
            int r = img.at<Vec3b>(Point(i, j))[0];
            Vec3b& color = res.at<Vec3b>(Point(i, j));

            color[0] = CLT[r];
            color[1] = CLT[r];
            color[2] = CLT[r];
        }
    }

    return res;
}

Mat Processor::adaptive_thresholding(Mat& img, int num_tiles){
    Mat res(img.size(), img.type());

    for (int i = 0; i < num_tiles; i++){
        for (int j = 0; j < num_tiles; j++){

            Range rows_r(i * img.rows / num_tiles, (i + 1) * img.rows / num_tiles);
            Range cols_r(j * img.cols / num_tiles, (j + 1) * img.cols / num_tiles);

            Mat small = img(rows_r, cols_r);
            small = iterative_thresholding(small);
            Mat small_part = res(rows_r, cols_r);

            small.copyTo(small_part);
        }
    }

    return res;
}

Mat Processor::p_tile_thresholding(Mat& img, int p_tile){
    int* hist = new int[256];
    HistogramUtility histUtil;
    histUtil.getHistogram(img, hist);

    int pixel_count = (int)((100 - p_tile) / 100.0 * img.cols * img.rows);

    int count = 0;
    int index = 0;
    while(count < pixel_count){
        count += hist[index];
        index++;
    }

    Mat res = threshold_binary(img, index);
    return res;
}

Mat Processor::iterative_thresholding(Mat& img, int init_threshold, double diff_threshold, int max_iterations){

    double diff = 256;
    int counter = 0;
    int current_threshold = init_threshold;
    while (diff > diff_threshold && counter < max_iterations){
        init_threshold = current_threshold;

        double sum1 = 0;
        double sum2 = 0;
        int count1 = 0;
        int count2 = 0;

        for(int i = 0; i < img.cols; i++){
            for(int j = 0; j < img.rows; j++){
                int r = img.at<Vec3b>(Point(i, j))[0];

                if (r < init_threshold){
                    sum1 += r;
                    count1++;
                }else{
                    sum2 += r;
                    count2++;
                }
            }
        }

        if (count1 != 0 && count2 != 0){
            sum1 /= count1;
            sum2 /= count2;
        }else{
            if (count1 == 0){
                sum1 = 0;
                sum2 /= count2;
            }else{
                sum1 /= count1;
                sum2 = 0;
            }
        }

        current_threshold = (int) ((1.0 / 2.0) * (sum1 + sum2));
        diff = abs(current_threshold - init_threshold);
        counter++;
    }

    Mat res = threshold_binary(img, current_threshold);
    return res;
}

Mat Processor::threshold_binary(Mat &img, int threshold){
    Mat res(img.size(), img.type());

    for(int i = 0; i < res.cols; i++){
        for(int j = 0; j < res.rows; j++){
            int r = img.at<Vec3b>(Point(i, j))[0];

            Vec3b& color = res.at<Vec3b>(Point(i, j));
            if (r < threshold){
                color[0] = 0;
                color[1] = 0;
                color[2] = 0;
            }else{
                color[0] = 255;
                color[1] = 255;
                color[2] = 255;
            }
        }
    }

    return res;
}

Mat* Processor::slicing_threshold(Mat& img, int number_of_slices){
    HistogramUtility histUtil;
    int* hist = new int[256];
    histUtil.getHistogram(img, hist);

    Mat* results = new Mat[number_of_slices];
    for (int i = 0; i < number_of_slices; i++){
        int range_start = (int)(i * (256.0 / number_of_slices));
        int range_end = (int)((i + 1) * (256.0 / number_of_slices));

        Mat res(img.size(), img.type());

        for(int i = 0; i < res.cols; i++){
            for(int j = 0; j < res.rows; j++){
                int r = img.at<Vec3b>(Point(i, j))[0];

                Vec3b& color = res.at<Vec3b>(Point(i, j));
                if (r > range_start && r <= range_end){
                    color[0] = 255;
                    color[1] = 255;
                    color[2] = 255;
                }else{
                    color[0] = 0;
                    color[1] = 0;
                    color[2] = 0;
                }
            }
        }

        results[i] = res;

    }

    return results;
}

Mat Processor::add_noise(Mat &img, int percent){
    Mat res(img.size(), img.type());
    img.copyTo(res);

    srand (time(NULL));

    for(int i = 0; i < res.cols; i++){
        for(int j = 0; j < res.rows; j++){
            int r = rand() % 100;

            if (r < percent){
                Vec3b& color = res.at<Vec3b>(Point(i, j));

                if (color[0] == color[1] && color[1] == color[2]){
                    r = rand() % 256;
                    //color[0] += r;
                    //color[1] += r;
                    //color[2] += r;
                    color[0] = color[1] = color[2] = r;
                }else{
                    //color[0] += rand() % 256;
                    //color[1] += rand() % 256;
                    //color[2] += rand() % 256;
                    color[0] = rand() % 256;
                    color[1] = rand() % 256;
                    color[2] = rand() % 256;
                }
            }
        }
    }

    return res;
}


//  (C) Copyright Matt Borland 2021.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
//  Constexpr implementation of sqrt function

#ifndef BOOST_MATH_CCMATH_SQRT
#define BOOST_MATH_CCMATH_SQRT

#include <cmath>
#include <limits>
#include <type_traits>
#include <boost/math/ccmath/isnan.hpp>
#include <boost/math/ccmath/isinf.hpp>
#include <boost/math/tools/is_constant_evaluated.hpp>

namespace boost::math::ccmath {

namespace detail {

template <typename Real>
inline constexpr Real sqrt_impl_2(Real x, Real s, Real s2)
{
    return !(s < s2) ? s2 : sqrt_impl_2(x, (x / s + s) / 2, s);
}

template <typename Real>
inline constexpr Real sqrt_impl_1(Real x, Real s)
{
    return sqrt_impl_2(x, (x / s + s) / 2, s);
}

template <typename Real>
inline constexpr Real sqrt_impl(Real x)
{
    return sqrt_impl_1(x, x > 1 ? x : Real(1));
}

} // namespace detail

template <typename Real, std::enable_if_t<!std::is_integral_v<Real>, bool> = true>
inline constexpr Real sqrt(Real x)
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(x))
    {
        return boost::math::ccmath::isnan(x) ? std::numeric_limits<Real>::quiet_NaN() :
               boost::math::ccmath::isinf(x) ? std::numeric_limits<Real>::infinity() :
               detail::sqrt_impl<Real>(x);
    }
    else
    {
        using std::sqrt;
        return sqrt(x);
    }
}

template <typename Z, std::enable_if_t<std::is_integral_v<Z>, bool> = true>
inline constexpr double sqrt(Z x)
{
    return detail::sqrt_impl<double>(static_cast<double>(x));
}

} // Namespaces

#endif // BOOST_MATH_CCMATH_SQRT


#include <iostream>
#include <Eigen/Dense>
#include <vector>
#include "ukf.h"

using namespace std;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using std::vector;

int main() {

	//Create a UKF instance
	UKF ukf;

/*******************************************************************************
* Programming assignment calls
*******************************************************************************/

    MatrixXd Xsig = MatrixXd(11, 5);
    ukf.GenerateSigmaPoints(&Xsig);

    //print result
    std::cout << "Xsig = " << std::endl << Xsig << std::endl;

	return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

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

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[append
//` Shows usage of Boost.Geometry's append to append a point or a range to a polygon

#include <iostream>

#include <boost/assign.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

int main()
{
    using boost::assign::tuple_list_of;
    using boost::make_tuple;
    using boost::geometry::append;

    typedef boost::geometry::model::polygon<boost::tuple<int, int> > polygon;

    polygon poly;

    // Append a range
    append(poly, tuple_list_of(0, 0)(0, 10)(11, 11)(10, 0)); /*< tuple_list_of delivers a range and can therefore be used in boost::geometry::append >*/
    // Append a point (in this case the closing point)
    append(poly, make_tuple(0, 0));

    // Create an interior ring (append does not do this automatically)
    boost::geometry::interior_rings(poly).resize(1);

    // Append a range to the interior ring
    append(poly, tuple_list_of(2, 2)(2, 5)(6, 6)(5, 2), 0); /*< The last parameter ring_index 0 denotes the first interior ring >*/
    // Append a point to the first interior ring
    append(poly, make_tuple(2, 2), 0);

    std::cout << boost::geometry::dsv(poly) << std::endl;

    return 0;
}

//]

//[append_output
/*`
Output:
[pre
(((0, 0), (0, 10), (11, 11), (10, 0), (0, 0)), ((2, 2), (2, 5), (6, 6), (5, 2), (2, 2)))
]
*/
//]


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

#include <boost/hana/assert.hpp>
#include <boost/hana/bool.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/map.hpp>
#include <boost/hana/maybe.hpp>
#include <boost/hana/pair.hpp>
#include <boost/hana/set.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>

#include <type_traits>
using namespace boost::hana;

int main() {

{

//! [all_of]
using namespace literals;

BOOST_HANA_CONSTEXPR_LAMBDA auto odd = [](auto x) {
    return x % 2_c != 0_c;
};

BOOST_HANA_CONSTEXPR_CHECK(all_of(make<Tuple>(1, 3), odd));
BOOST_HANA_CONSTANT_CHECK(!all_of(make<Tuple>(3_c, 4_c), odd));

BOOST_HANA_CONSTANT_CHECK(
    !all_of(make<Tuple>(type<void>, type<char&>), trait<std::is_void>)
);
BOOST_HANA_CONSTANT_CHECK(
    all_of(make<Tuple>(type<int>, type<char>), trait<std::is_integral>)
);
//! [all_of]

}{

//! [all]
BOOST_HANA_CONSTEXPR_CHECK(all(make<Tuple>(true_, true, true_)));
BOOST_HANA_CONSTANT_CHECK(!all(make<Tuple>(true, false_, true_)));
//! [all]

}{

//! [any_of]
using namespace literals;

BOOST_HANA_CONSTEXPR_LAMBDA auto odd = [](auto x) {
    return x % 2_c != 0_c;
};

BOOST_HANA_CONSTEXPR_CHECK(any_of(make<Tuple>(1, 2), odd));
BOOST_HANA_CONSTANT_CHECK(!any_of(make<Tuple>(2_c, 4_c), odd));

BOOST_HANA_CONSTANT_CHECK(
    any_of(make<Tuple>(type<void>, type<char&>), trait<std::is_void>)
);
BOOST_HANA_CONSTANT_CHECK(
    !any_of(make<Tuple>(type<void>, type<char&>), trait<std::is_integral>)
);
//! [any_of]

}{

//! [any]
BOOST_HANA_CONSTANT_CHECK(any(make<Tuple>(false, false_, true_)));
BOOST_HANA_CONSTEXPR_CHECK(any(make<Tuple>(false, false_, true)));
BOOST_HANA_CONSTEXPR_CHECK(!any(make<Tuple>(false, false_, false_)));
//! [any]

}{

//! [elem]
BOOST_HANA_CONSTANT_CHECK(elem(make<Tuple>(2, int_<2>, int_<3>, 'x'), int_<3>));
BOOST_HANA_CONSTANT_CHECK(elem(set(1, '2', type<int>, "foobar"), type<int>));
//! [elem]

}{

//! [find_if]
BOOST_HANA_CONSTEXPR_CHECK(
    find_if(make<Tuple>(1.0, 2, '3'), trait_<std::is_integral>) == just(2)
);
BOOST_HANA_CONSTANT_CHECK(
    find_if(make<Tuple>(1.0, 2, '3'), trait_<std::is_class>) == nothing
);

constexpr auto types = tuple_t<char, int, unsigned, long, unsigned long>;
BOOST_HANA_CONSTANT_CHECK(
    find_if(types, _ == type<unsigned>) == just(type<unsigned>)
);
BOOST_HANA_CONSTANT_CHECK(
    find_if(types, _ == type<void>) == nothing
);
//! [find_if]

}{

//! [find]
BOOST_HANA_CONSTANT_CHECK(
    find(make<Tuple>(int_<1>, type<int>, '3'), type<int>) == just(type<int>)
);
BOOST_HANA_CONSTANT_CHECK(
    find(make<Tuple>(int_<1>, type<int>, '3'), type<void>) == nothing
);

BOOST_HANA_CONSTEXPR_LAMBDA auto m = make<Map>(
    make<Pair>(1, 'x'),
    make<Pair>(type<float>, 3.3),
    make<Pair>(type<char>, type<int>)
);
BOOST_HANA_CONSTEXPR_CHECK(find(m, type<float>) == just(3.3));
//! [find]

}{

//! [in]
BOOST_HANA_CONSTEXPR_LAMBDA auto xs = make<Tuple>(
    int_<1>, type<int>, int_<2>, type<float>, int_<3>, type<void>, type<char>
);
BOOST_HANA_CONSTANT_CHECK(
    filter(xs, in ^ make<Tuple>(int_<3>, type<int>, type<void>))
    ==
    make<Tuple>(type<int>, int_<3>, type<void>)
);
//! [in]

}{

//! [none_of]
using namespace literals;

BOOST_HANA_CONSTEXPR_LAMBDA auto odd = [](auto x) {
    return x % 2_c != 0_c;
};

BOOST_HANA_CONSTANT_CHECK(none_of(make<Tuple>(2_c, 4_c), odd));
BOOST_HANA_CONSTEXPR_CHECK(!none_of(make<Tuple>(1, 2), odd));

BOOST_HANA_CONSTANT_CHECK(
    !none_of(make<Tuple>(type<void>, type<char&>), trait<std::is_void>)
);
BOOST_HANA_CONSTANT_CHECK(
    none_of(make<Tuple>(type<void>, type<char&>), trait<std::is_integral>)
);
//! [none_of]

}{

//! [none]
BOOST_HANA_CONSTEXPR_CHECK(none(make<Tuple>(false, false_, false_)));
BOOST_HANA_CONSTEXPR_CHECK(!none(make<Tuple>(false, false_, true)));
BOOST_HANA_CONSTANT_CHECK(!none(make<Tuple>(false, false_, true_)));
//! [none]

}{

//! [subset]
BOOST_HANA_CONSTEXPR_CHECK(subset(make<Tuple>(1, '2', 3.3), make<Tuple>(3.3, 1, '2', nullptr)));
//! [subset]

}

}