//============================================================================
// Name        : Application.cpp
// Author      : Rian van Gijlswijk
// Description : Main application file.
//============================================================================

#include "Application.h"
#include <string>
#include <regex>
#include <boost/log/core.hpp>
#include <boost/log/trivial.hpp>
#include <boost/log/expressions.hpp>
#include <boost/log/utility/setup/file.hpp>
#include "Timer.cpp"
#include "CommandLine.h"
#include "../math/Matrix3d.h"
#include "../exporter/JsonExporter.h"
#include "../exporter/MatlabExporter.h"
#include "../radio/AntennaFactory.h"
#include "../radio/IsotropicAntenna.h"

namespace raytracer {
namespace core {

	using namespace std;
	using namespace tracer;
	using namespace exporter;
	using namespace math;
	using namespace threading;
	using namespace radio;

	boost::mutex datasetMutex;
	boost::mutex tracingIncMutex;

	boost::threadpool::pool tp;

	void Application::init(int argc, char * argv[]) {

		BOOST_LOG_TRIVIAL(debug) << "Init application";

		AntennaFactory::printMappedTypes();

		parseCommandLineArgs(argc, argv);
		start();
		run();
	}

	void Application::parseCommandLineArgs(int argc, char * argv[]) {

		BOOST_LOG_TRIVIAL(debug) << "parseCommandLineArgs";

		for (int i = 0; i < argc; i++) {

			if (strcmp(argv[i], "-h") == 0) {
				std::cout 	<< "Ionospheric Ray Tracer\n\n"
							<< "Synopsis:\n"
							<< "\tirt [-opts] scenarioConfig\n\n"
							<< "Description: \n"
							<< "\tPerform ionospheric ray tracing on a celestial object described by the _celestialConfig json file. "
							<< "If no config file is supplied, use a default scenario.\n\n"
							<< "Options:\n"
							<< "\t-c | --config\t Application config file\n"
							<< "\t-i | --iterations\t The number of consecutive times every ray option should be run.\n"
							<< "\t-h | --help\t This help.\n"
							<< "\t-o | --output\t Path where output file should be stored.\n"
							<< "\t-p | --parallelism\t Multithreading indicator.\n"
							<< "\t-v | --verbose\t Verbose, display log output\n"
							<< "\t-vv \t\t Very verbose, display log and debug output\n";
				std::exit(0);

			} else if (strcmp(argv[i], "-s") == 0 || strcmp(argv[i], "--scenario") == 0) {
				_celestialConfigFile = argv[i+1];

			} else if (strcmp(argv[i], "-c") == 0 || strcmp(argv[i], "--config") == 0) {
				_applicationConfigFile = argv[i+1];

			} else if (strcmp(argv[i], "-p") == 0 || strcmp(argv[i], "--parallelism") == 0) {
				_parallelism = atoi(argv[i+1]);

			} else if (strcmp(argv[i], "-o") == 0 || strcmp(argv[i], "--output") == 0) {
				_outputFile = argv[i+1];

			} else if (strcmp(argv[i], "-i") == 0 || strcmp(argv[i], "--iterations") == 0) {
				_iterations = atoi(argv[i+1]);

			} else if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
				_verbosity = boost::log::trivial::info;

			} else if (strcmp(argv[i], "-vv") == 0) {
				_verbosity = boost::log::trivial::debug;

			} else if (strcmp(argv[i], "-fmin") == 0) {
				_fmin = atoi(argv[i+1]);

			} else if (strcmp(argv[i], "-fstep") == 0) {
				_fstep = atoi(argv[i+1]);

			} else if (strcmp(argv[i], "-fmax") == 0) {
				_fmax = atoi(argv[i+1]);

			}
		}

		// load scenario config file. Must be given.
		if (!std::regex_match (argv[argc-1], std::regex("[A-Za-z0-9_/]+\.json") )) {
			BOOST_LOG_TRIVIAL(fatal) << "No scenario file given! Exiting.";
			std::exit(0);
		}
		_celestialConfigFile = argv[argc-1];
	}

	void Application::start() {

		BOOST_LOG_TRIVIAL(debug) << "Start application";

		_isRunning = true;

		_applicationConfig = Config(_applicationConfigFile);
		_celestialConfig = Config(_celestialConfigFile);

		if (_parallelism < 1) {
			_parallelism = _applicationConfig.getInt("parallelism");
		}
		if (_iterations < 1) {
			_iterations = _applicationConfig.getInt("iterations");
		}
		if (_fmin < 1) {
			_fmin = _applicationConfig.getObject("frequencies")["min"].asInt();
		}
		if (_fstep < 1) {
			_fstep = _applicationConfig.getObject("frequencies")["step"].asInt();
		}
		if (_fmax < 1) {
			_fmax = _applicationConfig.getObject("frequencies")["max"].asInt();
		}

//		boost::log::add_file_log("log/sample.log");

		boost::log::core::get()->set_filter(
				boost::log::trivial::severity >= _verbosity);

		tp = boost::threadpool::pool(_parallelism);

		BOOST_LOG_TRIVIAL(debug) << "applicationConfig: " << _applicationConfigFile << endl
				<< _applicationConfig << endl
				<< "celestialConfig:" << _celestialConfigFile << endl
				<< _celestialConfig;

		_me = new MatlabExporter(_outputFile);
	}

	void Application::run() {

		BOOST_LOG_TRIVIAL(debug) << "Run application";

		Timer tmr;
		int radius = _celestialConfig.getInt("radius");

		BOOST_LOG_TRIVIAL(info) << "Parallelism is " << _applicationConfig.getInt("parallelism");
		if (_verbosity > boost::log::trivial::info) {
			std::ostringstream stringStream;
			stringStream << "Parallelism is " << _applicationConfig.getInt("parallelism");
			CommandLine::getInstance().addToHeader(stringStream.str().c_str());
		}
		BOOST_LOG_TRIVIAL(info) << _applicationConfig.getInt("iterations") << " iterations";

		// load config values
		double SZAmin = _applicationConfig.getObject("SZA")["min"].asDouble();
		double SZAstep = _applicationConfig.getObject("SZA")["step"].asDouble();
		double SZAmax = _applicationConfig.getObject("SZA")["max"].asDouble();
		double azimuthMin = _applicationConfig.getObject("azimuth")["min"].asDouble();
		double azimuthStep = _applicationConfig.getObject("azimuth")["step"].asDouble();
		double azimuthMax = _applicationConfig.getObject("azimuth")["max"].asDouble();
		const Json::Value beacons = _applicationConfig.getArray("beacons");

		// trace a ray
		int rayCounter = 0;
		for (int iteration = 0; iteration < _applicationConfig.getInt("iterations"); iteration++) {

			BOOST_LOG_TRIVIAL(info) << "Iteration " << (iteration+1) << " of " << _applicationConfig.getInt("iterations");

			createScene();

			BOOST_LOG_TRIVIAL(info) << "Simulating " << beacons.size() << " beacons";
			BOOST_LOG_TRIVIAL(info) << "Scanning frequencies " << _fmin << " Hz to " << _fmax << "Hz with steps of " << _fstep << "Hz";
			BOOST_LOG_TRIVIAL(info) << "Scanning SZA " << SZAmin << " deg to " << SZAmax << " deg with steps of " << SZAstep << " deg";
			BOOST_LOG_TRIVIAL(info) << "Scanning azimuth " << azimuthMin << " deg to " << azimuthMax << " deg with steps of " << azimuthStep << " deg";

			if (_verbosity > boost::log::trivial::info) {
				std::ostringstream stringStream;
				stringStream << "Simulating " << beacons.size() << " beacons\n" << "Scanning frequencies " << _fmin << " Hz to " << _fmax
						<< "Hz with steps of " << _fstep << "Hz\n" << "Scanning SZA " << SZAmin << " deg to "
						<< SZAmax << " deg with steps of " << SZAstep << " deg\n"
						<< "Scanning azimuth " << azimuthMin << " deg to " << azimuthMax << " deg with steps of " << azimuthStep << " deg";
						CommandLine::getInstance().addToHeader(stringStream.str().c_str());
			}

			for(int b = 0; b < beacons.size(); b++) {

				double latitudeOffset = beacons[b].get("latitudeOffset", "").asDouble() * Constants::PI / 180.0;
				double longitudeOffset = beacons[b].get("longitudeOffset", "").asDouble() * Constants::PI / 180.0;
				const Json::Value antenna = beacons[b].get("antenna", "");
				//IAntenna* ant = AntennaFactory::createInstance(antenna.get("type", "").asString());
				IAntenna* ant = new IsotropicAntenna();
				ant->setConfig(antenna);

				Matrix3d latitude = Matrix3d::createRotationMatrix(latitudeOffset, Matrix3d::ROTATION_X);
				Matrix3d longitude = Matrix3d::createRotationMatrix(longitudeOffset, Matrix3d::ROTATION_Z);
				Matrix3d rotationMatrix = latitude * longitude;

				Vector3d startPosition = rotationMatrix * Vector3d(0, (radius+2), 0);
				BOOST_LOG_TRIVIAL(debug) << startPosition;

				for(double azimuth = azimuthMin; azimuth <= azimuthMax; azimuth += azimuthStep) {

					Matrix3d azimuthRotation = Matrix3d::createRotationMatrix(azimuth * Constants::PI / 180, Matrix3d::ROTATION_Y);

					for (int freq = _fmin; freq <= _fmax; freq += _fstep) {
						for (double elevation = SZAmin; elevation <= SZAmax; elevation += SZAstep) {

							Ray r;
							r.rayNumber = ++rayCounter;
							r.frequency = (double)freq;
							r.signalPower = ant->getSignalPowerAt(azimuth, elevation);
							r.o = startPosition;
							r.originalAngle = elevation * Constants::PI / 180.0;
							r.originBeaconId = b+1;
							r.originalAzimuth = azimuth * Constants::PI / 180.0;
							Vector3d direction = Vector3d(cos(Constants::PI/2.0 - r.originalAngle),
									sin(Constants::PI/2.0 - r.originalAngle),
									0).norm();
							r.d = azimuthRotation * direction;

							Worker w;
							w.schedule(&tp, r);

							numWorkers++;
						}
					}
				}
			}

			BOOST_LOG_TRIVIAL(info) << numWorkers << " workers queued";
			if (_verbosity > boost::log::trivial::info) {
				std::ostringstream stringStream;
				stringStream << numWorkers << " workers queued";
				CommandLine::getInstance().addToHeader(stringStream.str().c_str());
			}

			tp.wait();

			flushScene();
		}

		stop();

		double t = tmr.elapsed();
		double tracingsPerSec = _numTracings / t;
		char buffer[80];
		CommandLine::getInstance().updateBody("\n");
	    sprintf(buffer, "Elapsed: %5.2f sec. %d tracings done. %5.2f tracings/sec",
	    		t, _numTracings, tracingsPerSec);
	    BOOST_LOG_TRIVIAL(warning) << buffer;

		//CsvExporter ce;
		//ce.dump("Debug/data.csv", dataSet);
		_me->dump(_outputFile, dataSet);

	    BOOST_LOG_TRIVIAL(warning) << "Results stored at: " << _outputFile;
	}

	void Application::stop() {

		_isRunning = false;
	}

	/**
	 * Add geometries to the scenemanager
	 */
	void Application::createScene() {

		_scm = SceneManager();
		_scm.loadStaticEnvironment();

		int numSceneObjectsCreated = 0;
		double R = _celestialConfig.getInt("radius");
		double angularStepSize = _applicationConfig.getDouble("angularStepSize");

		for (double latitude = 0 * Constants::PI; latitude <= 0.3 * Constants::PI; latitude += angularStepSize) {
			for (double longitude = 0 * Constants::PI; longitude <= 2 * Constants::PI; longitude += angularStepSize) {

				Matrix3d latitudeM = Matrix3d::createRotationMatrix(latitude, Matrix3d::ROTATION_X);
				Matrix3d longitudeM = Matrix3d::createRotationMatrix(longitude, Matrix3d::ROTATION_Z);
				Matrix3d rotationMatrix = latitudeM * longitudeM;

				Vector3d startPosition = rotationMatrix * Vector3d(0, R, 0);

				Plane3d mesh = Plane3d(startPosition.norm(), startPosition);
				mesh.size = angularStepSize * R;
				Terrain* tr = new Terrain(mesh);

				numSceneObjectsCreated++;
				_scm.addToScene(tr);
			}
		}

		if (numSceneObjectsCreated > 1e9)
			BOOST_LOG_TRIVIAL(info) << setprecision(3) << numSceneObjectsCreated/1.0e9 << "G scene objects created";
		else if (numSceneObjectsCreated > 1e6)
			BOOST_LOG_TRIVIAL(info) << setprecision(3) << numSceneObjectsCreated/1.0e6 << "M scene objects created";
		else if (numSceneObjectsCreated > 1e3)
			BOOST_LOG_TRIVIAL(info) << setprecision(3) << numSceneObjectsCreated/1.0e3 << "K scene objects created";
		else
			BOOST_LOG_TRIVIAL(info) << setprecision(3) << numSceneObjectsCreated << " scene objects created";
	}

	/**
	 * Flush the scene by clearing the list of scene objects
	 */
	void Application::flushScene() {

		_scm.removeAllFromScene();
	}

	void Application::addToDataset(Data dat) {

		datasetMutex.lock();
		dataSet.push_back(dat);
		if (dataSet.size() > Data::MAX_DATASET_SIZE) {
			_me->dump(_outputFile, dataSet);
			dataSet.clear();
		}
		datasetMutex.unlock();
	}

	void Application::incrementTracing() {

		tracingIncMutex.lock();
		_numTracings++;
		tracingIncMutex.unlock();
	}

	SceneManager Application::getSceneManager() {

		return _scm;
	}

	Config Application::getApplicationConfig() {

		return _applicationConfig;
	}

	Config Application::getCelestialConfig() {

		return _celestialConfig;
	}

	void Application::setCelestialConfig(Config conf) {

		_celestialConfig = conf;
	}

	void Application::setApplicationConfig(Config conf) {

		_applicationConfig = conf;
	}

	int Application::getVerbosity() {

		return _verbosity;
	}

} /* namespace core */
} /* namespace raytracer */


#include "../include/Multigraph.hpp"
#include <limits>
#include <queue>
#include <iostream>
#include <iomanip>
#include <stack>
#include <algorithm>
#include <fstream>
#include <string>
#include <numeric>

using namespace std;

const uint16_t Multigraph::INFINITY = numeric_limits<uint16_t>::max() / 4;

Multigraph::Multigraph(/* args */)
{
}

Multigraph::Multigraph(uint16_t n)
{
    adjacencyMatrix.resize(n);
    for (uint16_t i = 0; i < n; i++)
    {
        Vertex_t v;
        v.id = i;
        vertices.push_back(v);
        adjacencyMatrix[i].resize(n, Multigraph::INFINITY);
        adjacencyMatrix[i][i] = 0.0;
    }

    neighbors.resize(n);
}

Multigraph::~Multigraph()
{
}

void Multigraph::addVertex(Vertex_t &vertex)
{
    //add vertex to vertices list
    vertices.push_back(vertex);

    //resize adjacencyMatrix due to new vertex
    uint16_t last = adjacencyMatrix.size();
    uint16_t newSize = adjacencyMatrix.size() + 1;
    adjacencyMatrix.resize(newSize);

    for (uint16_t i = 0; i < last; i++)
        adjacencyMatrix[i].push_back(Multigraph::INFINITY);

    //resizing and putting infinity value in the last line
    adjacencyMatrix[last].resize(newSize, Multigraph::INFINITY);
    adjacencyMatrix[last][last] = 0;

    //resize neighbors matrix due to new vertex
    neighbors.resize(newSize);
}

void Multigraph::addEdge(Edge_t &edge)
{
    //updates weight in edges
    adjacencyMatrix[edge.from][edge.to] = edge.weight;
    adjacencyMatrix[edge.to][edge.from] = edge.weight;
    //add neighbors
    neighbors[edge.from].insert(edge.to);
    neighbors[edge.to].insert(edge.from);
}

void Multigraph::addEdge(uint16_t from, uint16_t to)
{
    //just add a neighbor. used to duplicate edges
    neighbors[from].insert(to);
    neighbors[to].insert(from);
}

void Multigraph::addEdge(uint16_t from, uint16_t to, uint16_t weight)
{
    //updates weight in edges
    adjacencyMatrix[from][to] = weight;
    adjacencyMatrix[to][from] = weight;
    //add neighbors
    neighbors[from].insert(to);
    neighbors[to].insert(from);
}

void Multigraph::addVertices(vector<Vertex_t> &newVertices)
{
    //add vertex to vertices list
    for (auto &v : newVertices)
        vertices.push_back(v);

    //resize adjacencyMatrix due to new vertex
    uint16_t last = adjacencyMatrix.size();
    uint16_t newSize = adjacencyMatrix.size() + newVertices.size();
    adjacencyMatrix.resize(newSize);

    //set infinity in new coluns
    for (uint16_t i = 0; i < last; i++)
        adjacencyMatrix[i].resize(newSize, Multigraph::INFINITY);

    //set infinity in new lines
    for (uint16_t i = last; i < newSize; i++)
    {
        adjacencyMatrix[i].resize(newSize, Multigraph::INFINITY);
        adjacencyMatrix[i][i] = 0.0;
    }

    //resize neighbors matrix due to new vertex
    neighbors.resize(newSize);
}

void Multigraph::addEdges(vector<Edge_t> &edges)
{
    for (auto &edge : edges)
    {
        //update weight in edges
        adjacencyMatrix[edge.from][edge.to] = edge.weight;
        adjacencyMatrix[edge.to][edge.from] = edge.weight;
        //add neighbors
        neighbors[edge.from].insert(edge.to);
        neighbors[edge.to].insert(edge.from);
    }
}

vector<vector<uint16_t>> *Multigraph::getAdjacencyMatrix()
{
    return &adjacencyMatrix;
}

vector<unordered_multiset<uint16_t>> *Multigraph::getNeighbors()
{
    return &neighbors;
}

bool Multigraph::isEulerian(vector<uint16_t> &oddVertices)
{
    bool tst = true;
    for (uint16_t i = 0; i < neighbors.size(); i++)
    {
        //if the vertex has neighbors and the number of neighbors is odd
        //add to the list of odd vertices
        if ((neighbors[i].size() > 0) && !(neighbors[i].size() % 2 == 0))
        {
            tst = false;
            oddVertices.push_back(i);
        }
    }
    return tst;
}

//first is cost, second is id vertex
typedef pair<uint16_t, uint16_t> pq_pair;
bool Multigraph::dijkstra(uint16_t start, uint16_t end, list<uint16_t> &outputPath, uint16_t &totalDistance)
{
    priority_queue<pq_pair, vector<pq_pair>, greater<pq_pair>> pq;

    vector<uint16_t> distance(vertices.size(), Multigraph::INFINITY);
    vector<int32_t> ancester(vertices.size(), -1);
    vector<bool> visited(vertices.size(), false);

    distance[start] = 0;

    pq.push(make_pair(0, start));

    while (!pq.empty())
    {
        pq_pair best;
        best = pq.top();
        pq.pop();
        visited[best.second] = true;

        //when usgin stl, if the best the the priority queue) has value diferent of the vector of distances it is out of date
        //therefore just pop it
        if (best.first == distance[best.second])
        {
            //if found the end can stop
            if (best.second == end)
            {
                uint16_t curr = end;
                totalDistance = 0;
                //geting path e distance in the path
                while (curr != start)
                {
                    outputPath.push_front(curr);
                    totalDistance += adjacencyMatrix[curr][ancester[curr]];
                    curr = ancester[curr];
                }
                outputPath.push_front(start);
                return true;
            }

            for (auto n : neighbors[best.second])
            {
                if (!visited[n])
                {
                    uint16_t tempDist = distance[best.second] + adjacencyMatrix[best.second][n];

                    if (tempDist < distance[n])
                    {
                        distance[n] = tempDist;
                        ancester[n] = best.second;
                        pq.push(make_pair(tempDist, n));
                    }
                }
            }
        }
    }
    return false;
}

//structures to use fibonacci_heap
//the heap are ordered by distance
#include <boost/heap/fibonacci_heap.hpp>
struct node_heap
{
    uint16_t distance;
    uint16_t vertex;
    node_heap(uint16_t distance, uint16_t vertex) : distance(distance), vertex(vertex) {}
    node_heap() {}
};

struct compare_node
{
    bool operator()(const node_heap &n1, const node_heap &n2) const
    {
        return n1.distance > n2.distance;
    }
};
typedef boost::heap::fibonacci_heap<node_heap, boost::heap::compare<compare_node>> fibonacci_heap_t;
typedef fibonacci_heap_t::handle_type handle_t;
bool Multigraph::dijkstra_boost(uint16_t start, uint16_t end, list<uint16_t> &outputPath, uint16_t &totalDistance)
{
    fibonacci_heap_t fh;
    vector<uint16_t> distance(vertices.size(), Multigraph::INFINITY);
    vector<int32_t> ancester(vertices.size(), -1);
    vector<bool> visited(vertices.size(), false);
    //variable used to update vertices in the priority queue
    vector<handle_t> nodeHandle;

    for (uint16_t i = 0; i < vertices.size(); i++)
    {
        node_heap nh(Multigraph::INFINITY, vertices[i].id);
        //if (vertices[i].id == start)
        //    nh.distance = 0;
        nodeHandle.push_back(fh.push(nh));
    }

    fh.update(nodeHandle[start], node_heap(0, start));

    distance[start] = 0;

    while (!fh.empty())
    {
        node_heap best = fh.top();
        fh.pop();
        visited[best.vertex] = true;

        //if reach the end vertex the algorithm can stop
        if (best.vertex == end)
        {
            uint16_t curr = end;
            totalDistance = 0;
            //geting path e distance in the path
            while (curr != start)
            {
                outputPath.push_front(curr);
                totalDistance += adjacencyMatrix[curr][ancester[curr]];
                curr = ancester[curr];
            }
            outputPath.push_front(start);
            return true;
        }

        for (auto n : neighbors[best.vertex])
        {
            if (!visited[n])
            {
                uint16_t tempDist = distance[best.vertex] + adjacencyMatrix[best.vertex][n];
                if (tempDist < distance[n])
                {
                    distance[n] = tempDist;
                    ancester[n] = best.vertex;
                    nodeHandle[n].node_->value.distance = tempDist;
                    fh.update(nodeHandle[n]);
                    //fh.update(nodeHandle[n], node_heap(tempDist, n));
                }
            }
        }
    }
    return false;
}

bool Multigraph::hierholzer(uint16_t start, list<uint16_t> &outputPath, uint16_t &totalDistance)
{
    vector<unordered_multiset<uint16_t>> n(neighbors);

    stack<uint16_t> tempEulerPath;

    tempEulerPath.push(start);
    totalDistance = 0;
    while (!tempEulerPath.empty())
    {
        uint16_t u = tempEulerPath.top();

        if (n[u].size() == 0)
        {
            tempEulerPath.pop();
            outputPath.push_front(u);
        }
        else
        {
            uint16_t v = *n[u].begin();
            totalDistance += adjacencyMatrix[u][v];
            tempEulerPath.push(v);
            n[u].erase(n[u].begin());
            n[v].erase(n[v].find(u));
        }
    }
    return true;
}

void Multigraph::print()
{
    cout << "========================================\n";
    cout << "\nVertices: \n";
    for (auto &v : vertices)
    {
        cout << v.id << "\t";
    }
    cout << "\n";

    cout << "\nNeighbors: \n";
    for (uint16_t i = 0; i < neighbors.size(); i++)
    {
        cout << "[" << i << "]: ";
        for (auto &e : neighbors[i])
        {
            cout << e << " ";
        }
        cout << "\n";
    }

    cout << "\nAdjacency Matrix: \n";
    for (auto &line : adjacencyMatrix)
    {
        for (auto &e : line)
        {
            if (e != Multigraph::INFINITY)
                cout << std::setw(10) << e << " ";
            else
                cout << std::setw(10) << "inf"
                     << " ";
        }
        cout << "\n";
    }

    cout << "\n========================================" << endl;
}

void gotoLine(std::ifstream &ifs, uint16_t num)
{
    ifs.seekg(std::ios::beg);
    for (uint16_t i = 0; i < num - 1; i++)
        ifs.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
}

void Multigraph::readGraphFromFile(string file)
{
    ifstream ifs;
    ifs.open(file, ifstream::in);

    //Read number of vertices
    string line;
    getline(ifs, line);
    stringstream linestream(line);
    getline(linestream, line, ' ');
    getline(linestream, line, ' ');

    uint16_t num_ver = stol(line);
    vertices.resize(num_ver);
    adjacencyMatrix.resize(num_ver);
    for (uint16_t i = 0; i < num_ver; i++)
    {
        adjacencyMatrix[i].resize(num_ver, Multigraph::INFINITY);
        adjacencyMatrix[i][i] = 0.0;
    }

    neighbors.resize(num_ver);

    //Go to line, start of Edges
    gotoLine(ifs, num_ver + 3);

    while (ifs.good())
    {
        string line;
        getline(ifs, line);
        stringstream linestream(line);

        Edge_t e;
        e.weight = 1;

        getline(linestream, line, ' ');
        e.from = stoi(line) - 1;
        getline(linestream, line, ' ');
        e.to = stoi(line) - 1;

        addEdge(e);
    }
}

void Multigraph::generateGraph(uint16_t n, uint16_t p)
{
    //4 even vertices + number of odd vertices
    uint16_t nv = 4 + 4 + 6 * n + 2 * p;

    adjacencyMatrix.resize(nv);
    for (uint16_t i = 0; i < nv; i++)
    {
        Vertex_t v;
        v.id = i;
        vertices.push_back(v);
        adjacencyMatrix[i].resize(nv, Multigraph::INFINITY);
        adjacencyMatrix[i][i] = 0.0;
    }

    neighbors.resize(nv);

    //first part
    addEdge(0, 5, 1);
    addEdge(0, 1, 1);
    addEdge(1, 3, 1);
    addEdge(1, 2, 1);
    addEdge(3, 4, 1);
    addEdge(2, 4, 1);
    addEdge(2, 6, 1);
    addEdge(4, 7, 1);
    addEdge(6, 5, 1);
    addEdge(6, 7, 1);

    for (uint16_t i = 8; i < nv - 2 * p; i = i + 6)
    {
        addEdge(i, i - 5, 1);
        addEdge(i + 4, i - 1, 1);
        addEdge(i, i + 1, 1);
        addEdge(i, i + 2, 1);
        addEdge(i + 1, i + 3, 1);
        addEdge(i + 2, i + 3, 1);
        addEdge(i + 2, i + 4, 1);
        addEdge(i + 3, i + 5, 1);
        addEdge(i + 4, i + 5, 1);
    }

    if (p > 0)
    {
        uint16_t first_v = nv - 2 * p;
        addEdge(0, first_v, 1);
        addEdge(5, first_v + 1, 1);
        addEdge(first_v, first_v + 1, 1);

        for (uint16_t i = first_v; i < nv - 2; i += 2)
        {
            addEdge(i, i + 2, 1);
            addEdge(i + 1, i + 3, 1);
            addEdge(i + 2, i + 3, 1);
        }
    }
}


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#include "mig_rewriting.hpp"

#include <functional>

#include <boost/assign/std/vector.hpp>
#include <boost/dynamic_bitset.hpp>
#include <boost/format.hpp>
#include <boost/range/iterator_range.hpp>

#include <core/graph/depth.hpp>
#include <core/utils/timer.hpp>
#include <classical/mig/mig_utils.hpp>

using namespace boost::assign;

namespace cirkit
{

/******************************************************************************
 * Types                                                                      *
 ******************************************************************************/

using mig_function_vec_t = std::vector<mig_function>;

struct children_pair_t
{
  unsigned left_a;
  unsigned left_b;
  unsigned right_a;
  unsigned right_b;
};

inline std::pair<unsigned, unsigned> three_without( unsigned x )
{
  return std::make_pair( x == 0u ? 1u : 0u, x == 2u ? 1u : 2u );
}

class mig_rewriting_manager
{
public:
  mig_rewriting_manager( const mig_graph& mig, bool verbose );

  void swap_current( const std::string& method );
  inline unsigned depth() const { return max_depth; }

  void run_distributivity_rtl();
  void run_associativity();
  void run_compl_associativity();
  void run_push_up();
  void run_relevance();
  void run_memristor_optimization();
  void run_memristor_inverter();

  mig_function distributivity_rtl( const mig_node& f );
  mig_function distributivity_ltr( const mig_node& f );
  mig_function associativity( const mig_node& f );
  mig_function compl_associativity( const mig_node& f );
  mig_function relevance( const mig_node& f, std::map<mig_function, mig_function> replacements );

  mig_function push_up( const mig_node& f );
  mig_function memristor_optimization( const mig_node& f );
  mig_function memristor_inverter( const mig_node& f );

private:
  inline mig_function distributivity_rtl_apply( const children_pair_t& pair,
                                                const mig_function_vec_t& children_a,
                                                const mig_function_vec_t& children_b,
                                                const mig_function& other )
  {
    assert( children_a[pair.left_a] == children_b[pair.right_a] );
    assert( children_a[pair.left_b] == children_b[pair.right_b] );

    const auto x = pair.left_a;
    const auto y = pair.left_b;
    const auto u = 3u - pair.left_a - pair.left_b;
    const auto v = 3u - pair.right_a - pair.right_b;

    if ( verbose )
    {
      std::cout << boost::format( "[i] x: %d, y: %d, u: %d, v: %d") % x % y % u % v << std::endl;
    }

    return mig_create_maj( mig_current,
                           make_function( distributivity_rtl( children_a[x].node ), children_a[x].complemented ),
                           make_function( distributivity_rtl( children_a[y].node ), children_a[y].complemented ),
                           mig_create_maj(
                                          mig_current,
                                          make_function( distributivity_rtl( children_a[u].node ), children_a[u].complemented ),
                                          make_function( distributivity_rtl( children_b[v].node ), children_b[v].complemented ),
                                          make_function( distributivity_rtl( other.node ), other.complemented ) ) );
  }

  inline mig_function associativity_apply( const mig_function_vec_t& grand_children,
                                           const mig_function& common,
                                           const mig_function& extra )
  {
    const auto common_f = make_function( associativity( common.node ), common.complemented );
    return mig_create_maj( mig_current,
                           make_function( associativity( grand_children[0u].node ), grand_children[0u].complemented ),
                           common_f,
                           mig_create_maj( mig_current,
                                           make_function( associativity( grand_children[1u].node ), grand_children[1u].complemented ),
                                           common_f,
                                           make_function( associativity( extra.node ), extra.complemented ) ) );
  }

  inline mig_function compl_associativity_apply( const mig_function_vec_t& grand_children,
                                                 const mig_function& common,
                                                 const mig_function& extra )
  {
    const auto extra_f = make_function( compl_associativity( extra.node ), extra.complemented );
    return mig_create_maj( mig_current,
                           extra_f,
                           make_function( compl_associativity( common.node ), common.complemented ),
                           mig_create_maj( mig_current,
                                           make_function( compl_associativity( grand_children[0u].node ), grand_children[0u].complemented ),
                                           extra_f,
                                           make_function( compl_associativity( grand_children[1u].node ), grand_children[1u].complemented ) ) );
  }

  /**
   * @return (a,b) where z = node[a][b]
   */
  boost::optional<std::pair<unsigned, unsigned>> find_depth_distributivity_candidate( const mig_node& node ) const;

  /**
   * @return (a,b,c) where x = node[a] and z = node[b][c]
   */
  boost::optional<std::tuple<unsigned, unsigned, unsigned>> find_depth_associativity_candidate( const mig_node& node ) const;

  /**
   * @return (a,b) where x = node[a] and u = node[b]
   */
  boost::optional<std::pair<unsigned, unsigned>> find_depth_compl_associativity_candidate( const mig_node& node ) const;

  /**
   * @return (a,b,c) where x = node[a], y = node[b], and z = node[c]
   */
  boost::optional<std::tuple<mig_function, mig_function, mig_function>> find_depth_relevance_candidate( const mig_node& node ) const;

public:
  mig_graph                        mig_old;
  mig_graph                        mig_current;
  std::map<mig_node, mig_function> old_to_new;
  bool                             verbose;
  std::vector<unsigned>            indegree;
  std::vector<unsigned>            depths;
  unsigned                         max_depth;

  bool                             use_distributivity       = true;
  bool                             use_associativity        = true;
  bool                             use_compl_associativity  = true;

  /* statistics */
  unsigned                         distributivity_count       = 0u;
  unsigned                         associativity_count        = 0u;
  unsigned                         compl_associativity_count  = 0u;
};

/******************************************************************************
 * Private functions                                                          *
 ******************************************************************************/

inline bool is_terminal( const mig_graph& mig, const mig_function& f )
{
  return out_degree( f.node, mig ) == 0u;
}

inline bool is_regular_nonterminal( const mig_graph& mig, const mig_function& f )
{
  return !f.complemented && out_degree( f.node, mig );
}

boost::dynamic_bitset<> get_pair_pattern( const mig_function_vec_t& c1,
                                          const mig_function_vec_t& c2 )
{
  boost::dynamic_bitset<> equals( 9u );

  equals.set( 0u, c1[0u] == c2[0u] );
  equals.set( 1u, c1[0u] == c2[1u] );
  equals.set( 2u, c1[0u] == c2[2u] );
  equals.set( 3u, c1[1u] == c2[0u] );
  equals.set( 4u, c1[1u] == c2[1u] );
  equals.set( 5u, c1[1u] == c2[2u] );
  equals.set( 6u, c1[2u] == c2[0u] );
  equals.set( 7u, c1[2u] == c2[1u] );
  equals.set( 8u, c1[2u] == c2[2u] );

  return equals;
}

std::vector<children_pair_t> get_children_pairs( const mig_function_vec_t& c1,
                                                 const mig_function_vec_t& c2 )
{
  std::vector<children_pair_t> pairs;

  const auto pattern = get_pair_pattern( c1, c2 );

  auto pos_a = pattern.find_first();

  while ( pos_a != boost::dynamic_bitset<>::npos )
  {
    auto pos_b = pattern.find_next( pos_a );
    while ( pos_b != boost::dynamic_bitset<>::npos )
    {
      pairs.push_back({(unsigned)pos_a / 3u,
                       (unsigned)pos_b / 3u,
                       (unsigned)pos_a % 3u,
                       (unsigned)pos_b % 3u} );
      pos_b = pattern.find_next( pos_b );
    }
    pos_a = pattern.find_next( pos_a );
  }

  return pairs;
}

mig_rewriting_manager::mig_rewriting_manager( const mig_graph& mig, bool verbose )
  : mig_current( mig ),
    verbose( verbose )
{
}

void mig_rewriting_manager::swap_current( const std::string& method )
{
  mig_old = mig_current;
  mig_current = mig_graph();

  const auto& info_old = mig_info( mig_old );
  mig_initialize( mig_current, info_old.model_name );

  auto& info_current = mig_info( mig_current );
  info_current.constant_used = info_old.constant_used;

  /* node to node mapping from old to new mig */
  old_to_new.clear();
  old_to_new.insert( {info_old.constant, {info_current.constant, false}} );

  for ( const auto& input : info_old.inputs )
  {
    old_to_new.insert( {input, mig_create_pi( mig_current, info_old.node_names.at( input ) )} );
  }

  /* indegrees */
  indegree = precompute_in_degrees( mig_old );

  /* depth */
  std::vector<mig_node> outputs;
  for ( const auto& output : info_old.outputs )
  {
    outputs += output.first.node;
  }

  max_depth = compute_depth( mig_old, outputs, depths );

  if ( verbose )
  {
    std::cout << "[i] current depth: " << max_depth << ", run " << method << std::endl;
  }
}

void mig_rewriting_manager::run_distributivity_rtl()
{
  swap_current( "D_RTL" );

  for ( const auto& output : mig_info( mig_old ).outputs )
  {
    mig_create_po( mig_current, make_function( distributivity_rtl( output.first.node ), output.first.complemented ), output.second );
  }
}

void mig_rewriting_manager::run_associativity()
{
  swap_current( "A" );

  for ( const auto& output : mig_info( mig_old ).outputs )
  {
    mig_create_po( mig_current, make_function( associativity( output.first.node ), output.first.complemented ), output.second );
  }
}

void mig_rewriting_manager::run_compl_associativity()
{
  swap_current( "C" );

  for ( const auto& output : mig_info( mig_old ).outputs )
  {
    mig_create_po( mig_current, make_function( compl_associativity( output.first.node ), output.first.complemented ), output.second );
  }
}

void mig_rewriting_manager::run_push_up()
{
  swap_current( "PU" );

  for ( const auto& output : mig_info( mig_old ).outputs )
  {
    mig_create_po( mig_current, make_function( push_up( output.first.node ), output.first.complemented ), output.second );
  }
}

void mig_rewriting_manager::run_relevance()
{
  swap_current( "R" );

  for ( const auto& output : mig_info( mig_old ).outputs )
  {
    std::map<mig_function, mig_function> replacements;
    mig_create_po( mig_current, make_function( relevance( output.first.node, replacements ), output.first.complemented ), output.second );
  }
}

void mig_rewriting_manager::run_memristor_optimization()
{
  /* mig_old is removed
     mig_old = mig_current
     mig_current is empty */
  swap_current( "MO" );

  for ( const auto& output : mig_info( mig_old ).outputs )
  {
    mig_create_po( mig_current, make_function( memristor_optimization( output.first.node ), output.first.complemented ), output.second );
  }
}

void mig_rewriting_manager::run_memristor_inverter()
{
  /* mig_old is removed
     mig_old = mig_current
     mig_current is empty */
  swap_current( "MO_INV" );

  for ( const auto& output : mig_info( mig_old ).outputs )
  {
    mig_create_po( mig_current, make_function( memristor_optimization( output.first.node ), output.first.complemented ), output.second );
  }
}

/**
 * 〈〈xyu〉〈xyv〉z〉↦〈xy〈uvz〉〉
 */
mig_function mig_rewriting_manager::distributivity_rtl( const mig_node& node )
{
  /* node is terminal */
  const auto it = old_to_new.find( node );
  if ( it != old_to_new.end() ) { return it->second; }

  const auto children = get_children( mig_old, node );
  mig_function_vec_t children_a, children_b, children_c;
  mig_function res;

  if ( is_regular_nonterminal( mig_old, children[0u] ) && indegree[children[0u].node] == 1u )
  {
    /* first child is not terminal */
    children_a = get_children( mig_old, children[0u].node );

    /* check (0,1) */
    if ( is_regular_nonterminal( mig_old, children[1u] ) && indegree[children[1u].node] == 1u )
    {
      children_b = get_children( mig_old, children[1u].node );

      const auto pairs = get_children_pairs( children_a, children_b );

      if ( !pairs.empty() )
      {
        res = distributivity_rtl_apply( pairs.front(), children_a, children_b, children[2u] );
        goto cache_and_return;
      }
    }

    /* check (0,2) */
    if ( is_regular_nonterminal( mig_old, children[2u] ) && indegree[children[2u].node] == 1u )
    {
      children_c = get_children( mig_old, children[2u].node );

      const auto pairs = get_children_pairs( children_a, children_c );

      if ( !pairs.empty() )
      {
        res = distributivity_rtl_apply( pairs.front(), children_a, children_c, children[1u] );
        goto cache_and_return;
      }
    }
  }

  /* check (1,2) */
  if ( is_regular_nonterminal( mig_old, children[1u] ) && is_regular_nonterminal( mig_old, children[2u] ) &&
       indegree[children[1u].node] == 1u && indegree[children[2u].node] == 1u )
  {
    if ( children_b.empty() ) { children_b = get_children( mig_old, children[1u].node ); }
    if ( children_c.empty() ) { children_c = get_children( mig_old, children[2u].node ); }

    const auto pairs = get_children_pairs( children_b, children_c );

    if ( !pairs.empty() )
    {
      res = distributivity_rtl_apply( pairs.front(), children_b, children_c, children[0u] );
      goto cache_and_return;
    }
  }

  /* recur */
  res = mig_create_maj( mig_current,
                        make_function( distributivity_rtl( children[0u].node ), children[0u].complemented ),
                        make_function( distributivity_rtl( children[1u].node ), children[1u].complemented ),
                        make_function( distributivity_rtl( children[2u].node ), children[2u].complemented ) );

cache_and_return:
  old_to_new.insert( {node, res} );
  return res;
}

/**
 * 〈xy〈uvz〉〉↦〈〈xyu〉〈xyv〉z〉
 */
mig_function mig_rewriting_manager::distributivity_ltr( const mig_node& node )
{
  /* node is terminal */
  const auto it = old_to_new.find( node );
  if ( it != old_to_new.end() ) { return it->second; }

  assert( false );

  return mig_function();
}

boost::optional<std::pair<unsigned, unsigned>> mig_rewriting_manager::find_depth_distributivity_candidate( const mig_node& node ) const
{
  const auto children = get_children( mig_old, node );

  for ( auto i = 0u; i < 3u; ++i )
  {
    if ( is_regular_nonterminal( mig_old, children[i] ) )
    {
      auto grand_children = get_children( mig_old, children[i].node );

      for ( auto j = 0u; j < 3u; ++j )
      {
        auto valid = true;

        for ( auto k = 0u; k < 3u; ++k )
        {
          if ( i == k ) { continue; }

          int diff = (int)depths[grand_children[j].node] - (int)depths[children[k].node];

          if ( diff < 2 ) { valid = false; break; }
        }

        if ( !valid ) { continue; }

        /* WE HAVE A CANDIDATE: z = ( i -> j ) */
        return std::make_pair( i, j );
      }
    }
  }

  return boost::none;
}

boost::optional<std::tuple<unsigned, unsigned, unsigned>> mig_rewriting_manager::find_depth_associativity_candidate( const mig_node& node ) const
{
  const auto children = get_children( mig_old, node );

  /* i iterates to find grand children (which contain z) */
  for ( auto i = 0u; i < 3u; ++i )
  {
    if ( !is_regular_nonterminal( mig_old, children[i] ) ) { continue; }

    const auto grand_children = get_children( mig_old, children[i].node );

    /* j iterates to find u */
    for ( auto j = 0u; j < 3u; ++j )
    {
      if ( i == j ) { continue; }

      /* if u ( = children[j] ) can be found in the grand_children */
      if ( boost::find( grand_children, children[j] ) != grand_children.end() )
      {
        for ( auto k = 0u; k < 3u; ++k )
        {
          if ( grand_children[k] == children[j] ) { continue; }

          int diff = (int)depths[grand_children[k].node] - (int)depths[children[3u - j - i].node];

          if ( diff >= 2 )
          {
            return std::make_tuple( 3u - j - i, i, k );
          }
        }
      }
    }
  }

  return boost::none;
}

boost::optional<std::pair<unsigned, unsigned>> mig_rewriting_manager::find_depth_compl_associativity_candidate( const mig_node& node ) const
{
  const auto children = get_children( mig_old, node );

  /* i iterates to find grand children (which contain z) */
  for ( auto i = 0u; i < 3u; ++i )
  {
    if ( !is_regular_nonterminal( mig_old, children[i] ) ) { continue; }

    const auto grand_children = get_children( mig_old, children[i].node );

    /* j iterates to find u */
    for ( auto j = 0u; j < 3u; ++j )
    {
      if ( i == j ) { continue; }

      /* if u ( = children[j] ) can be found in the grand_children */
      if ( boost::find( grand_children, !children[j] ) != grand_children.end() )
      {
        int diff = (int)depths[children[j].node] - (int)depths[children[3u - j - i].node];

        if ( diff >= 2 )
        {
          return std::make_pair( 3u - j - i, j );
        }
      }
    }
  }

  return boost::none;
}

boost::optional<std::tuple<mig_function, mig_function, mig_function>> mig_rewriting_manager::find_depth_relevance_candidate( const mig_node& node ) const
{
  auto children = get_children( mig_old, node );

  boost::sort( children, [this]( const mig_function& a, const mig_function& b ) { return depths.at( a.node ) > depths.at( b.node ); } );

  if ( indegree[children[1u].node] > 1u )
  {
    return std::make_tuple( children[0u], children[2u], children[1u] );
  }
  else if ( indegree[children[2u].node] > 1u )
  {
    return std::make_tuple( children[0u], children[1u], children[2u] );
  }
  else if ( indegree[children[0u].node] > 1u )
  {
    return std::make_tuple( children[1u], children[2u], children[0u] );
  }

  return boost::none;
}

/**
 * 〈xu〈yuz〉〉↦〈zu〈yux〉〉
 */
mig_function mig_rewriting_manager::associativity( const mig_node& node )
{
  /* node is terminal */
  const auto it = old_to_new.find( node );
  if ( it != old_to_new.end() ) { return it->second; }

  const auto children = get_children( mig_old, node );
  mig_function res;

  for ( auto i = 0u; i < 3u; ++i )
  {
    if ( is_regular_nonterminal( mig_old, children[i] ) && indegree[children[i].node] == 1u )
    {
      for ( auto j = 0u; j < 3u; ++j )
      {
        if ( i == j ) { continue; }

        auto grand_children = get_children( mig_old, children[i].node );

        const auto it = boost::find( grand_children, children[j] );
        if ( it != grand_children.end() )
        {
          grand_children.erase( it );
          res = associativity_apply( grand_children, children[j], children[3u - j - i] );
          goto cache_and_return;
        }
      }
    }
  }

  /* recur */
  res = mig_create_maj( mig_current,
                        make_function( associativity( children[0u].node ), children[0u].complemented ),
                        make_function( associativity( children[1u].node ), children[1u].complemented ),
                        make_function( associativity( children[2u].node ), children[2u].complemented ) );

cache_and_return:
  old_to_new.insert( {node, res} );
  return res;
}

/**
 * 〈xu〈yu'z〉〉↦〈xu〈yxz〉〉
 */
mig_function mig_rewriting_manager::compl_associativity( const mig_node& node )
{
  /* node is terminal */
  const auto it = old_to_new.find( node );
  if ( it != old_to_new.end() ) { return it->second; }

  const auto children = get_children( mig_old, node );
  mig_function res;

  for ( auto i = 0u; i < 3u; ++i )
  {
    if ( is_regular_nonterminal( mig_old, children[i] ) && indegree[children[i].node] == 1u )
    {
      for ( auto j = 0u; j < 3u; ++j )
      {
        if ( i == j ) { continue; }

        auto grand_children = get_children( mig_old, children[i].node );

        const auto it = boost::find( grand_children, !children[j] );
        if ( it != grand_children.end() )
        {
          grand_children.erase( it );
          res = compl_associativity_apply( grand_children, children[j], children[3u - j - i] );
          goto cache_and_return;
        }
      }
    }
  }

  /* recur */
  res = mig_create_maj( mig_current,
                        make_function( compl_associativity( children[0u].node ), children[0u].complemented ),
                        make_function( compl_associativity( children[1u].node ), children[1u].complemented ),
                        make_function( compl_associativity( children[2u].node ), children[2u].complemented ) );

cache_and_return:
  old_to_new.insert( {node, res} );
  return res;
}

/**
 * 〈xyz〉↦〈xyz_{x/y'}〉
 */
mig_function mig_rewriting_manager::relevance( const mig_node& f, std::map<mig_function, mig_function> replacements )
{
  /* node should be replaced? */
  auto it_replace = replacements.find( {f, false} );
  if ( it_replace != replacements.end() ) { return it_replace->second; }

  it_replace = replacements.find( {f, true} );
  if ( it_replace != replacements.end() ) { return !it_replace->second; }

  /* node is terminal */
  const auto it = old_to_new.find( f );
  if ( it != old_to_new.end() ) { return it->second; }

  mig_function res;

  const auto cand = find_depth_relevance_candidate( f );

  if ( cand != boost::none )
  {
    const auto zcand = *cand;

    const auto x = std::get<0>( zcand );
    const auto y = std::get<1>( zcand );
    const auto z = std::get<2>( zcand );

    assert( !( x == y ) );
    assert( !( x == z ) );
    assert( !( y == z ) );

    const auto xf = make_function( relevance( x.node, replacements ), x.complemented );
    const auto yf = make_function( relevance( y.node, replacements ), y.complemented );

    if ( xf.node != 0u && yf.node != 0u )
    {
      replacements.insert( {xf, !yf} );
    }
    const auto zf = make_function( relevance( z.node, replacements ), z.complemented );

    res = mig_create_maj( mig_current, xf, yf, zf );
  }
  else
  {
    const auto children = get_children( mig_old, f );

    /* recur */
    res = mig_create_maj( mig_current,
                          make_function( relevance( children[0u].node, replacements ), children[0u].complemented ),
                          make_function( relevance( children[1u].node, replacements ), children[1u].complemented ),
                          make_function( relevance( children[2u].node, replacements ), children[2u].complemented ) );
  }

  old_to_new.insert( {f, res} );
  return res;
}

mig_function mig_rewriting_manager::push_up( const mig_node& f )
{
  /* node is terminal */
  const auto it = old_to_new.find( f );
  if ( it != old_to_new.end() ) { return it->second; }

  mig_function res;

  const auto children = get_children( mig_old, f );

  /* distributivity */
    if ( use_distributivity )
  {
    const auto cand_dist = find_depth_distributivity_candidate( f );

    if ( cand_dist != boost::none )
    {
      const auto zcand = *cand_dist;

      const auto grand_children = get_children( mig_old, children[zcand.first].node );

      const auto xy = three_without( zcand.first );
      const auto uv = three_without( zcand.second );

      const auto x = children[xy.first];
      const auto y = children[xy.second];
      const auto u = grand_children[uv.first];
      const auto v = grand_children[uv.second];
      const auto z = grand_children[zcand.second];

      const auto xf = make_function( push_up( x.node ), x.complemented );
      const auto yf = make_function( push_up( y.node ), y.complemented );
      const auto uf = make_function( push_up( u.node ), u.complemented ^ children[zcand.first].complemented );
      const auto vf = make_function( push_up( v.node ), v.complemented ^ children[zcand.first].complemented );
      const auto zf = make_function( push_up( z.node ), z.complemented ^ children[zcand.first].complemented );

      res = mig_create_maj( mig_current,
                            mig_create_maj( mig_current, xf, yf, uf ),
                            mig_create_maj( mig_current, xf, yf, vf ),
                            zf );

      ++distributivity_count;
      goto cache_and_return;
    }
  }

  /* associativity */
  if ( use_associativity )
  {
    const auto cand_assoc = find_depth_associativity_candidate( f );

    if ( cand_assoc != boost::none )
    {
      const auto zcand = *cand_assoc;

      const auto grand_children = get_children( mig_old, children[std::get<1>( zcand )].node );

      assert( !children[std::get<1>( zcand )].complemented );

      const auto yu = three_without( std::get<2>( zcand ) );

      const auto x = children[std::get<0>( zcand )];
      const auto u = children[3u - std::get<0>( zcand ) - std::get<1>( zcand )];
      const auto z = grand_children[std::get<2>( zcand )];
      const auto y = ( u == grand_children[yu.first] ) ? grand_children[yu.second] : grand_children[yu.first];

      assert( grand_children[yu.first] == u || grand_children[yu.second] == u );

      const auto xf = make_function( push_up( x.node ), x.complemented );
      const auto uf = make_function( push_up( u.node ), u.complemented );
      const auto zf = make_function( push_up( z.node ), z.complemented );
      const auto yf = make_function( push_up( y.node ), y.complemented );

      res = mig_create_maj( mig_current,
                            zf,
                            uf,
                            mig_create_maj( mig_current, yf, uf, xf ) );

      ++associativity_count;
      goto cache_and_return;
    }
  }

  /* complementary associativity */
  if ( use_compl_associativity )
  {
    const auto cand = find_depth_compl_associativity_candidate( f );

    if ( cand != boost::none )
    {
      const auto zcand = *cand;

      const auto grand_children = get_children( mig_old, children[3u - zcand.first - zcand.second].node );

      const auto x = children[zcand.first];
      const auto u = children[zcand.second];

      const auto ui = std::distance( grand_children.begin(), boost::find( grand_children, !u ) );
      const auto yz = three_without( ui );

      const auto y = grand_children[yz.first];
      const auto z = grand_children[yz.second];

      const auto xf = make_function( push_up( x.node ), x.complemented );
      const auto uf = make_function( push_up( u.node ), u.complemented );
      const auto zf = make_function( push_up( z.node ), z.complemented );
      const auto yf = make_function( push_up( y.node ), y.complemented );

      res = mig_create_maj( mig_current,
                            xf,
                            uf,
                            mig_create_maj( mig_current, yf, xf, zf ) );

      ++compl_associativity_count;
      goto cache_and_return;
    }
  }

  /* recur */
  res = mig_create_maj( mig_current,
                        make_function( push_up( children[0u].node ), children[0u].complemented ),
                        make_function( push_up( children[1u].node ), children[1u].complemented ),
                        make_function( push_up( children[2u].node ), children[2u].complemented ) );

cache_and_return:
  old_to_new.insert( {f, res} );
  return res;
}

mig_function mig_rewriting_manager::memristor_optimization( const mig_node& f )
{
  /* node is terminal */
  const auto it = old_to_new.find( f );
  if ( it != old_to_new.end() ) { return it->second; }

  mig_function res;

  const auto children = get_children( mig_old, f );

  /* if at least two children are complemented */
  if ( ( static_cast<int>( children[0u].complemented ) + static_cast<int>( children[1u].complemented ) + static_cast<int>( children[2u].complemented ) >= 2 ) ) //&& indegree[f] == 1u )
  {
    res = !mig_create_maj( mig_current,
                           make_function( memristor_optimization( children[0u].node ), !children[0u].complemented ),
                           make_function( memristor_optimization( children[1u].node ), !children[1u].complemented ),
                           make_function( memristor_optimization( children[2u].node ), !children[2u].complemented ) );
  }
  else
  {
    res = mig_create_maj( mig_current,
                          make_function( memristor_optimization( children[0u].node ), children[0u].complemented ),
                          make_function( memristor_optimization( children[1u].node ), children[1u].complemented ),
                          make_function( memristor_optimization( children[2u].node ), children[2u].complemented ) );
  }

  old_to_new.insert( {f, res} );
  return res;
}

mig_function mig_rewriting_manager::memristor_inverter( const mig_node& f )
{
  /* node is terminal */
  const auto it = old_to_new.find( f );
  if ( it != old_to_new.end() ) { return it->second; }

  mig_function res;

  const auto children = get_children( mig_old, f );

  /* if at least two children are complemented */
  if ( ( static_cast<int>( children[0u].complemented ) + static_cast<int>( children[1u].complemented ) + static_cast<int>( children[2u].complemented ) == 3u ) && indegree[f] == 1u )
  {
    res = !mig_create_maj( mig_current,
                           make_function( memristor_optimization( children[0u].node ), !children[0u].complemented ),
                           make_function( memristor_optimization( children[1u].node ), !children[1u].complemented ),
                           make_function( memristor_optimization( children[2u].node ), !children[2u].complemented ) );
  }
  else
  {
    res = mig_create_maj( mig_current,
                          make_function( memristor_optimization( children[0u].node ), children[0u].complemented ),
                          make_function( memristor_optimization( children[1u].node ), children[1u].complemented ),
                          make_function( memristor_optimization( children[2u].node ), children[2u].complemented ) );
  }

  old_to_new.insert( {f, res} );
  return res;
}

/******************************************************************************
 * Public functions                                                           *
 ******************************************************************************/

mig_graph mig_area_rewriting( const mig_graph& mig,
                              const properties::ptr& settings,
                              const properties::ptr& statistics )
{
  /* settings */
  const auto effort  = get( settings, "effort",  1u );
  const auto verbose = get( settings, "verbose", false );

  /* timer */
  properties_timer t( statistics );

  mig_rewriting_manager mgr( mig, verbose );

  for ( auto k = 0u; k < effort; ++k )
  {
    mgr.run_distributivity_rtl();
    mgr.run_associativity();
    mgr.run_compl_associativity();
    mgr.run_distributivity_rtl();
  }

  set( statistics, "distributivity_count",       mgr.distributivity_count );
  set( statistics, "associativity_count",        mgr.associativity_count );
  set( statistics, "compl_associativity_count",  mgr.compl_associativity_count );

  return mgr.mig_current;
}

mig_graph mig_depth_rewriting( const mig_graph& mig,
                               const properties::ptr& settings,
                               const properties::ptr& statistics )
{
  /* settings */
  const auto effort                   = get( settings, "effort",  1u );
  const auto use_distributivity       = get( settings, "use_distributivity", true );
  const auto use_associativity        = get( settings, "use_associativity", true );
  const auto use_compl_associativity  = get( settings, "use_compl_associativity", true );
  const auto verbose                  = get( settings, "verbose", false );

  /* timer */
  properties_timer t( statistics );

  mig_rewriting_manager mgr( mig, verbose );
  mgr.use_distributivity = use_distributivity;
  mgr.use_associativity  = use_associativity;
  mgr.use_compl_associativity = use_compl_associativity;

  for ( auto k = 0u; k < effort; ++k )
  {
    mgr.run_push_up();
    mgr.run_relevance();
    mgr.run_push_up();
  }

  set( statistics, "distributivity_count",       mgr.distributivity_count );
  set( statistics, "associativity_count",        mgr.associativity_count );
  set( statistics, "compl_associativity_count",  mgr.compl_associativity_count );

  return mgr.mig_current;
}

mig_graph mig_memristor_rewriting( const mig_graph& mig,
                                   const properties::ptr& settings,
                                   const properties::ptr& statistics )
{
  /* settings */
  const auto effort   = get( settings, "effort",  1u );
  const auto verbose  = get( settings, "verbose", false );
  const auto strategy = get( settings, "strategy", 0u ); /* 0u: multi-objective, 1u: RRAM step, 2u: PLiM */

  /* timer */
  properties_timer t( statistics );

  mig_rewriting_manager mgr( mig, verbose );
  mgr.use_distributivity = true;
  mgr.use_associativity  = true;
  mgr.use_compl_associativity = true;

  for ( auto k = 0u; k < effort; ++k )
  {
    switch ( strategy )
    {
    case 0u:
      mgr.run_push_up();
      mgr.run_memristor_optimization();
      mgr.run_push_up();
      mgr.run_associativity();
      mgr.run_distributivity_rtl();
      break;
    case 1u:
      mgr.run_push_up();
      mgr.run_memristor_inverter();
      mgr.run_memristor_optimization();
      mgr.run_push_up();
      break;
    case 2u:
      mgr.run_distributivity_rtl();
      mgr.run_associativity();
      mgr.run_compl_associativity();
      mgr.run_distributivity_rtl();
      mgr.run_memristor_optimization();
      mgr.run_memristor_inverter();
      break;
    case 3u:
      mgr.run_memristor_optimization();
      mgr.run_memristor_inverter();
      break;
    }
  }

  set( statistics, "distributivity_count",       mgr.distributivity_count );
  set( statistics, "associativity_count",        mgr.associativity_count );
  set( statistics, "compl_associativity_count",  mgr.compl_associativity_count );

  return mgr.mig_current;
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2021 INRIA.
 *
 * 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 "BlockVector.hpp"

#include <boost/numeric/ublas/vector_proxy.hpp>  // for project
#include <boost/numeric/ublas/vector_sparse.hpp>
#include <vector>

#include "SiconosVector.hpp"

#include "SiconosAlgebra.hpp"
#include "SiconosAlgebraTools.hpp" // for isComparableTo
#include "Tools.hpp"
//#define DEBUG_STDOUT
//#define DEBUG_MESSAGES
#include "siconos_debug.h"
#include "SiconosException.hpp"

using Siconos::Algebra::isComparableTo;

// =================================================
//                CONSTRUCTORS
// =================================================
BlockVector::BlockVector()
{
  _tabIndex.reset(new Index());
}

BlockVector::BlockVector(const BlockVector &v)
{
  Index::size_type nbBlocks = v.numberOfBlocks();
  _tabIndex.reset(new Index());
  _tabIndex->reserve(nbBlocks);
  _vect.reserve(nbBlocks);
  VectorOfVectors::const_iterator it;
  for(it = v.begin(); it != v.end(); ++it)
  {
    _vect.push_back(std::shared_ptr<SiconosVector>(new SiconosVector(**it))) ;
    _sizeV += (*it)->size();
    _tabIndex->push_back(_sizeV);
  }
}

BlockVector::BlockVector(SP::SiconosVector v1, SP::SiconosVector v2)
{
  // Insert the two vectors in the container
  // NO COPY !!
  if(! v1  && ! v2)
    THROW_EXCEPTION("both vectors are nullptr.");

  _tabIndex.reset(new Index());

  _tabIndex->reserve(2);
  _vect.reserve(2);

  if(v1)
  {
    _vect.push_back(v1);
    _sizeV = v1->size();
    _tabIndex->push_back(_sizeV);

  }
  else
    // If first parameter is a nullptr pointer, then set this(1) to a SiconosVector of the same size as v2, and equal to 0.
  {
    // This case is usefull to set xDot in LagrangianDS.
    _sizeV = v2->size();

    _vect.push_back(std::shared_ptr<SiconosVector>(new SiconosVector(_sizeV)));
    _tabIndex->push_back(_sizeV);

  }
  if(v2)
  {
    _vect.push_back(v2);
    _sizeV += v2->size();
    _tabIndex->push_back(_sizeV);

  }
  else // If second parameter is a nullptr pointer, then set this(2) to a SiconosVector of the same size as v1, and equal to 0.
  {
    // This case is usefull to set xDot in LagrangianDS.

    _vect.push_back(std::shared_ptr<SiconosVector>(new SiconosVector(v1->size())));
    _sizeV += v1->size();
    _tabIndex->push_back(_sizeV);
  }
}

BlockVector::BlockVector(unsigned int numberOfBlocks, unsigned int dim)
{
  _tabIndex.reset(new Index());
  _tabIndex->reserve(numberOfBlocks);
  _vect.reserve(numberOfBlocks);
  for(unsigned int i = 0; i < numberOfBlocks; ++i)
  {
    _vect.push_back(std::shared_ptr<SiconosVector>(new SiconosVector(dim)));
    _tabIndex->push_back(dim * (i + 1));
  }
  _sizeV = dim * numberOfBlocks;
}

BlockVector::BlockVector(unsigned int numberOfBlocks)
{
  _tabIndex.reset(new Index());
  _tabIndex->resize(numberOfBlocks);
  _vect.resize(numberOfBlocks);
}

// ===========================
//      private method
// ===========================

void BlockVector::_update()
{
  _sizeV=0;
  _tabIndex.reset(new Index());

  VectorOfVectors::iterator it;
  for(it = _vect.begin(); it != _vect.end(); ++it)
  {
    if(*it)
    {
      _sizeV += (*it)->size();
    }
    _tabIndex->push_back(_sizeV);
  }
}

// ===========================
//       fill vector
// ===========================

bool BlockVector::isDense() const
{
  return std::find_if(_vect.begin(), _vect.end(), TestDense()) != _vect.end();
}

void BlockVector::zero()
{
  VectorOfVectors::iterator it;
  for(it = _vect.begin(); it != _vect.end(); ++it)
    (*it)->zero();
}

void BlockVector::fill(double value)
{
  VectorOfVectors::iterator it;
  for(it = _vect.begin(); it != _vect.end(); ++it)
    if((*it))(*it)->fill(value);
}

//=====================
// screen display
//=====================

void BlockVector::display() const
{
  VectorOfVectors::const_iterator it;
  std::cout << "=======> Block Vector Display (" << _tabIndex->size() << " block(s)): " << std::endl;
  for(it = _vect.begin(); it != _vect.end(); ++it)
  {
    DEBUG_EXPR(std::cout <<"(*it)" << (*it) << std::endl;);
    if(*it)
      (*it)->display();
    else
      std::cout << "(*it)-> nullptr" <<std::endl;
  }
}

//=====================
// convert to a string
//=====================

std::string BlockVector::toString() const
{
  return ::toString(*this);
}

//=====================
// convert to an ostream
//=====================

std::ostream& operator<<(std::ostream& os, const BlockVector& bv)
{
  VectorOfVectors::const_iterator it;
  os << "[" << bv._vect.size() << "](";
  for(it = bv._vect.begin(); it != bv._vect.end(); ++it)
  {
    if(it != bv._vect.begin()) os << ",";
    if(*it) os << **it;
    else os << "(nil)";
  }
  os << ")";
  return os;
}

//=============================
// Elements access (get or set)
//=============================

double BlockVector::getValue(unsigned int pos) const
{
  unsigned int blockNum = 0;

  while(pos >= (*_tabIndex)[blockNum] && blockNum < _tabIndex->size())
    blockNum ++;

  unsigned int relativePos = pos;

  if(blockNum != 0)
    relativePos -= (*_tabIndex)[blockNum - 1];

  return (*_vect[blockNum])(relativePos);
}

void BlockVector::setValue(unsigned int pos, double value)
{
  unsigned int blockNum = 0;

  while(pos >= (*_tabIndex)[blockNum] && blockNum < _tabIndex->size())
    blockNum ++;

  unsigned int relativePos = pos;

  if(blockNum != 0)
    relativePos -= (*_tabIndex)[blockNum - 1];

  (*_vect[blockNum])(relativePos) = value;
}

double& BlockVector::operator()(unsigned int pos)
{
  unsigned int blockNum = 0;

  while(pos >= (*_tabIndex)[blockNum] && blockNum < _tabIndex->size())
    blockNum ++;

  unsigned int relativePos = pos;

  if(blockNum != 0)
    relativePos -= (*_tabIndex)[blockNum - 1];

  return (*_vect[blockNum])(relativePos);
}

double BlockVector::operator()(unsigned int pos) const
{
  return getValue(pos);
}

//============================================
// Access (get or set) to blocks of elements
//============================================

void BlockVector::setVector(unsigned int pos, const SiconosVector& v)
{
  assert(pos < _vect.size() && "insertion out of vector size");
  if(! _vect[pos])
    THROW_EXCEPTION("this[pos] == nullptr pointer.");
  *_vect[pos] = v ;
}

void BlockVector::setVectorPtr(unsigned int pos, SP::SiconosVector v)
{
  assert(pos < _vect.size() && "insertion out of vector size");
  _vect[pos] = v;
  _update();
}

void BlockVector::setAllVect(VectorOfVectors& v)
{
  _vect = v;
  _update();
}

// SP::SiconosVector BlockVector::operator [](unsigned int pos)
// {
//   return  _vect[pos];
// }

// SPC::SiconosVector BlockVector::operator [](unsigned int pos) const
// {
//   return  _vect[pos];
// }

unsigned int BlockVector::getNumVectorAtPos(unsigned int pos) const
{
  unsigned int blockNum = 0;

  while(pos >= (*_tabIndex)[blockNum] && blockNum < _tabIndex->size() - 1)
    blockNum ++;
  return blockNum;
}

BlockVector& BlockVector::operator = (const BlockVector& vIn)
{
  if(&vIn == this) return *this;
  else
  {
    if(isComparableTo(*this, vIn))  // if vIn and this are "block-consistent"
    {
      VectorOfVectors::iterator it1;
      VectorOfVectors::const_iterator it2 = vIn.begin();

      for(it1 = _vect.begin(); it1 != _vect.end(); ++it1)
      {
        (**it1) = (**it2);
        it2++;
      }
    }
    else
    {
      for(unsigned int i = 0; i < _sizeV; ++i)
        (*this)(i) = vIn(i);
    }
    return *this;
  }
}

BlockVector& BlockVector::operator = (const double* data)
{
  VectorOfVectors::iterator it1;
  unsigned indxPos = 0;

  for(it1 = _vect.begin(); it1 != _vect.end(); ++it1)
  {
    SiconosVector& v = **it1;
    v = &data[indxPos];
    indxPos += v.size();
  }
  return *this;
}

BlockVector& BlockVector::operator -= (const BlockVector& vIn)
{
  if(isComparableTo(*this, vIn))  // if vIn and this are "block-consistent"
  {
    unsigned int i = 0;
    VectorOfVectors::iterator it1;

    for(it1 = _vect.begin(); it1 != _vect.end(); ++it1)
      **it1 -= *(vIn.vector(i++));
  }
  else // use of a temporary SimpleVector... bad way, to be improved. But this case happens rarely ...
  {
    for(unsigned int i = 0; i < _sizeV; ++i)
      (*this)(i) -= vIn(i);
  }
  return *this;
}

BlockVector& BlockVector::operator -= (const SiconosVector& vIn)
{
  unsigned int dim = vIn.size(); // size of the block to be added.
  if(dim > _sizeV) THROW_EXCEPTION("invalid ranges");

  VectorOfVectors::const_iterator it;
  Siconos::UBLAS_TYPE numVIn = vIn.num();
  unsigned int currentSize;
  Siconos::UBLAS_TYPE currentNum;
  unsigned int index = 0;
  for(it = _vect.begin(); it != _vect.end(); ++it)
  {
    currentSize = (*it)->size();
    currentNum = (*it)->num();
    if(numVIn != currentNum)
      THROW_EXCEPTION("inconsistent types.");
    if(numVIn == Siconos::DENSE)
      noalias(*(*it)->dense()) -=  ublas::subrange(*vIn.dense(), index, index + currentSize) ;
    else
      noalias(*(*it)->sparse()) -=  ublas::subrange(*vIn.sparse(), index, index + currentSize) ;
    index += currentSize;
  }
  return *this;
}

BlockVector& BlockVector::operator += (const BlockVector& vIn)
{
  if(isComparableTo(*this, vIn))  // if vIn and this are "block-consistent"
  {
    unsigned int i = 0;
    VectorOfVectors::iterator it1;

    for(it1 = _vect.begin(); it1 != _vect.end(); ++it1)
      **it1 += *(vIn.vector(i++));
  }
  else // use of a temporary SimpleVector... bad way, to be improved. But this case happens rarely ...
  {
    for(unsigned int i = 0; i < _sizeV; ++i)
      (*this)(i) += vIn(i);
  }
  return *this;
}

BlockVector& BlockVector::operator += (const SiconosVector& vIn)
{
  // Add a part of vIn (starting from index) to the current vector.
  // vIn must be a SimpleVector.

  // At the end of the present function, index is equal to index + the dim. of the added sub-vector.

  unsigned int dim = vIn.size(); // size of the block to be added.
  if(dim > _sizeV) THROW_EXCEPTION("invalid ranges");

  VectorOfVectors::const_iterator it;
  Siconos::UBLAS_TYPE numVIn = vIn.num(), currentNum;
  unsigned int currentSize;
  unsigned int index = 0;

  for(it = _vect.begin(); it != _vect.end(); ++it)
  {
    currentSize = (*it)->size();
    currentNum = (*it)->num();
    if(numVIn != currentNum) THROW_EXCEPTION("inconsistent types.");
    if(numVIn == Siconos::DENSE)
      noalias(*(*it)->dense()) += ublas::subrange(*vIn.dense(), index, index + currentSize) ;
    else
      noalias(*(*it)->sparse()) += ublas::subrange(*vIn.sparse(), index, index + currentSize) ;
    index += currentSize;
  }
  return *this;
}

// void BlockVector::insert(const  SiconosVector& v)
// {
//   _sizeV += v.size();

//   _vect.push_back(std::shared_ptr<SiconosVector>(new SiconosVector(v))); // Copy

//   _tabIndex->push_back(_sizeV);
// }

void BlockVector::insertPtr(SP::SiconosVector v)
{
  if(!v)
    THROW_EXCEPTION("v is a nullptr vector.");

  _sizeV += v->size();
  _vect.push_back(v);
  _tabIndex->push_back(_sizeV);
}

void BlockVector::setBlock(const SiconosVector& vIn, unsigned int sizeB, unsigned int startIn, unsigned int startOut)
{
  // Check dim ...
  unsigned int endOut = startOut + sizeB;

  assert(startIn < vIn.size());
  assert(startOut < size());
  assert((startIn + sizeB) <= vIn.size());
  assert(endOut <= size());

  // We look for the block of vOut that include index startOut
  unsigned int blockOutStart = 0;
  while(startOut >= (*_tabIndex)[blockOutStart] && blockOutStart < _tabIndex->size())
    blockOutStart++;
  // Relative position in the block blockOutStart.
  unsigned int posOut = startOut;
  if(blockOutStart != 0)
    posOut -= (*_tabIndex)[blockOutStart - 1];

  // We look for the block of vOut that include index endOut
  unsigned int blockOutEnd = blockOutStart;
  while(endOut > (*_tabIndex)[blockOutEnd] && blockOutEnd < _tabIndex->size())
    blockOutEnd ++;

  // => the block to be set runs from block number blockOutStart to block number blockOutEnd.

  if(blockOutEnd == blockOutStart)  //
  {
    vIn.toBlock(*_vect[blockOutStart], sizeB, startIn, posOut);
  }
  else // More that one block of vOut are concerned
  {

    // The current considered block ...
    SP::SiconosVector currentBlock = _vect[blockOutStart];

    // Size of the subBlock of vOut to be set.
    size_t subSizeB = currentBlock->size() - posOut;
    unsigned int posIn = startIn;

    // Set first sub-block (currentBlock) values, between index posOut and posOut+subSizeB,
    // with vIn values from posIn to posIn+subSizeB.
    vIn.toBlock(*currentBlock, subSizeB, posIn, posOut);

    // Other blocks, except number blockOutEnd.
    unsigned int currentBlockNum = blockOutStart + 1;
    while(currentBlockNum != blockOutEnd)
    {
      posIn += subSizeB;
      currentBlock = _vect[currentBlockNum];
      subSizeB = currentBlock->size();
      vIn.toBlock(*currentBlock, subSizeB, posIn, 0);
      currentBlockNum++;
    }
    // set last subBlock ...
    currentBlock = _vect[blockOutEnd];

    posIn += subSizeB;

    // Size of the considered sub-block
    subSizeB = endOut - (*_tabIndex)[blockOutEnd - 1];

    vIn.toBlock(*currentBlock, subSizeB, posIn, 0);
  }

}

double BlockVector::norm2() const
{
  double d = 0;
  VectorOfVectors::const_iterator it;
  for(it = _vect.begin(); it != _vect.end(); ++it)
  {
    assert(*it);
    d += pow((*it)->norm2(), 2);
  }
  return sqrt(d);
}

double BlockVector::normInf() const
{
  double d = 0;
  VectorOfVectors::const_iterator it;
  for(it = _vect.begin(); it != _vect.end(); ++it)
  {
    assert(*it);
    d = fmax((*it)->normInf(), d);
  }
  return d;
}

SP::SiconosVector BlockVector::prepareVectorForPlugin() const
{
  {
    if(_tabIndex->size()> 1)
    {
      SP::SiconosVector copy(new SiconosVector(*this));
      return copy;
    }
    else
    {
      // No copy, just a ref.
      return _vect[0];
    }
  }
}

BlockVector& BlockVector::operator =(const SiconosVector& vIn)
{
  setBlock(vIn, _sizeV, 0, 0);
  return *this;
}

BlockVector& BlockVector::operator *= (double s)
{
  VectorOfVectors::iterator it;
  for(it = begin(); it != end(); ++it)
    (**it) *= s;
  return *this;
}

BlockVector& BlockVector::operator /= (double s)
{
  VectorOfVectors::iterator it;
  for(it = begin(); it != end(); ++it)
    (**it) /= s;
  return *this;
}


// Copyright (c) 2009, Hikaru Inoue, Akihiro Yamasaki,
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//    * Redistributions of source code must retain the above copyright
//      notice, this list of conditions and the following disclaimer.
//    * Redistributions in binary form must reproduce the above
//      copyright notice, this list of conditions and the following
//      disclaimer in the documentation and/or other materials provided
//      with the distribution.
//    * The names of the contributors may not be used to endorse or promote
//      products derived from this software without specific prior written
//      permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <string>
#include <vector>
#include <boost/bind.hpp>
#include <boost/spirit.hpp>
#include "jitasm.h"

class RenderExpr : public jitasm::function<void, RenderExpr, void *, size_t, void *, size_t, void *, size_t, int, int>
{
private:
	struct Calculator : public boost::spirit::grammar<Calculator>
	{
		RenderExpr& renderExpr_;
		Calculator(RenderExpr& renderExpr) : renderExpr_(renderExpr) {}

		template <typename ScannerT>
		struct definition
		{
			definition(Calculator const& self)
			{
				using namespace boost;
				using namespace boost::spirit;
				expression
					=	term
						>> *(   (L'+' >> term)[boost::bind(&do_add, ref(self.renderExpr_), _1, _2)]
							|	(L'-' >> term)[boost::bind(&do_sub, ref(self.renderExpr_), _1, _2)]
							);

				term
					=   factor
						>> *(   (L'*' >> factor)[boost::bind(&do_mul, ref(self.renderExpr_), _1, _2)]
							|   (L'/' >> factor)[boost::bind(&do_div, ref(self.renderExpr_), _1, _2)]
							);

				factor
					=   real_p[boost::bind(&do_real, ref(self.renderExpr_), _1)]
					|	chseq_p(L"src1")[boost::bind(&do_src1, ref(self.renderExpr_), _1, _2)]
					|	chseq_p(L"src2")[boost::bind(&do_src2, ref(self.renderExpr_), _1, _2)]
					|   L'(' >> expression >> L')'
					|   (L'-' >> factor)[boost::bind(&do_neg, ref(self.renderExpr_), _1, _2)]
					|   (L'+' >> factor);
			}

			boost::spirit::rule<ScannerT> expression, term, factor;

			boost::spirit::rule<ScannerT> const& start() const { return expression; }
		};
	};

	void do_add(const wchar_t *, const wchar_t *)
	{
		XmmReg reg1 = variableStack_.back();
		variableStack_.pop_back();
		XmmReg reg2 = variableStack_.back();
		addps(reg2, reg1);
	}

	void do_sub(const wchar_t *, const wchar_t *)
	{
		XmmReg reg1 = variableStack_.back();
		variableStack_.pop_back();
		XmmReg reg2 = variableStack_.back();
		subps(reg2, reg1);
	}

	void do_mul(const wchar_t *, const wchar_t *)
	{
		XmmReg reg1 = variableStack_.back();
		variableStack_.pop_back();
		XmmReg reg2 = variableStack_.back();
		mulps(reg2, reg1);
	}

	void do_div(const wchar_t *, const wchar_t *)
	{
		XmmReg reg1 = variableStack_.back();
		variableStack_.pop_back();
		XmmReg reg2 = variableStack_.back();
		divps(reg2, reg1);
	}

	void do_real(double val)
	{
		float fval = static_cast<float>(val);
		mov(eax, *(unsigned int*)&fval);
		XmmReg var;
		movd(var, eax);
		shufps(var, var, 0);
		variableStack_.push_back(var);
	}

	void do_src(int i)
	{
		__declspec(align(16)) const static float factor8bpp[4] = {1.0f/255.0f, 1.0f/255.0f, 1.0f/255.0f, 1.0f/255.0f};
		XmmReg src;
		movd(src, dword_ptr[i == 0 ? zsi : zbx]);
		punpcklbw(src, zero_);
		punpcklwd(src, zero_);
		cvtdq2ps(src, src);
		mov(zax, (uintptr_t)factor8bpp);
		mulps(src, xmmword_ptr[zax]);
		variableStack_.push_back(src);
	}

	void do_src1(const wchar_t *, const wchar_t *)
	{
		do_src(0);
	}

	void do_src2(const wchar_t *, const wchar_t *)
	{
		do_src(1);
	}

	void do_neg(const wchar_t *, const wchar_t *)
	{
		XmmReg var = variableStack_.back();
		XmmReg tmp;
		xorps(tmp, tmp);
		subps(tmp, var);
		movaps(var, tmp);
	}

public:
	RenderExpr(wchar_t *expr) : expr_(expr) {}

	void main(Addr dst, Addr dstSkip, Addr src1, Addr src1Skip, Addr src2, Addr src2Skip, Addr width, Addr height)
	{
		mov(zsi, ptr[src1]);
		mov(zbx, ptr[src2]);
		mov(zdi, ptr[dst]);
		xorps(zero_, zero_);

		L("LoopY");
		{
			mov(ecx, dword_ptr[width]);

			L("LoopX");
			{
				Calculator calc(*this);
				boost::spirit::parse_info<const wchar_t *> info = boost::spirit::parse(expr_, calc, boost::spirit::space_p);
				if (!info.full)
					throw info;

				__declspec(align(16)) const static float factor8bpp[4] = {255.0f, 255.0f, 255.0f, 255.0f};
				mov(zax, (uintptr_t)factor8bpp);
				XmmReg dstReg = variableStack_.back();
				mulps(dstReg, xmmword_ptr[zax]);
				cvtps2dq(dstReg, dstReg);
				packssdw(dstReg, dstReg);
				packuswb(dstReg, dstReg);
				movd(eax, dstReg);
				movnti(dword_ptr[zdi], eax);

				add(zsi, 4);
				add(zbx, 4);
				add(zdi, 4);
				dec(ecx);
				jnz("LoopX");
			}

			add(zsi, ptr[src1Skip]);
			add(zbx, ptr[src2Skip]);
			add(zdi, ptr[dstSkip]);
			dec(dword_ptr[height]);
			jnz("LoopY");
		}
	}

private:
	wchar_t *expr_;
	std::vector<XmmReg> variableStack_;
	XmmReg zero_;
};

bool RenderJIT(wchar_t *expr, void *dst, size_t dstStride, void *src1, size_t src1Stride, void *src2, size_t src2Stride, int width, int height)
{
	try {
		RenderExpr renderExpr(expr);
		renderExpr(dst, dstStride - width * 4, src1, src1Stride - width * 4, src2, src2Stride - width * 4, width, height);

		//FILE *file = fopen("render.dmp", "wb");
		//fwrite(renderExpr.GetCode(), 1, renderExpr.GetCodeSize(), file);
		//fclose(file);

		return true;
	}
	catch (...) {
		return false;
	}
}


/*
 * This file is part of the Visual Computing Library (VCL) release under the
 * MIT license.
 *
 * Copyright (c) 2015 Basil Fierz
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
#include <vcl/graphics/frustum.h>

// Eigen library
#include <Eigen/Eigenvalues>

// VCL
#include <vcl/core/contract.h>
#include <vcl/graphics/matrixfactory.h>
#include <vcl/graphics/camera.h>
#include <vcl/math/math.h>

namespace Vcl { namespace Graphics
{
	template<typename Scalar>
	PerspectiveViewFrustum<Scalar>::PerspectiveViewFrustum()
	: PerspectiveViewFrustum(0, 0, (float) (M_PI / 4.0), 0.01, 100, {0, 0, 0}, {0, 0, 1}, {0, 1, 0}, {1, 0, 0})
	{
	}

	template<typename Scalar>
	PerspectiveViewFrustum<Scalar>::PerspectiveViewFrustum
	(
		real_t width, real_t height, real_t fov, real_t near_plane, real_t far_plane,
		vector3_t pos, vector3_t dir, vector3_t up, vector3_t right
	)
	:	_x(width), _y(height), _fov(fov), _near(near_plane), _far(far_plane),
		_position(pos), _direction(dir), _up(up), _right(right)
	{
		computePlanes();
	}

	template<typename Scalar>
	PerspectiveViewFrustum<Scalar>::PerspectiveViewFrustum(const Vcl::Graphics::Camera* cam)
	: PerspectiveViewFrustum
	  (
		cam->viewportWidth(), cam->viewportHeight(), cam->fieldOfView(), cam->nearPlane(), cam->farPlane(),
		cam->position().cast<Scalar>(),
		cam->direction().cast<Scalar>(),
		cam->direction().cross(cam->up().cross(cam->direction()).normalized()).normalized().cast<Scalar>(),
		cam->up().cross(cam->direction()).normalized().cast<Scalar>()
	  )
	{
	}

	template<typename Scalar>
	PerspectiveViewFrustum<Scalar>::PerspectiveViewFrustum(const PerspectiveViewFrustum<real_t>& rhs)
	{
		_x = rhs._x;
		_y = rhs._y;
		_fov = rhs._fov;
		_near = rhs._near;
		_far = rhs._far;
		_position = rhs._position;
		_direction = rhs._direction;
		_up = rhs._up;
		_right = rhs._right;

		_corners = rhs._corners;
		_planes = rhs._planes;
	}

	template<typename Scalar>
	const typename PerspectiveViewFrustum<Scalar>::vector3_t& PerspectiveViewFrustum<Scalar>::position() const
	{
		return _position;
	}

	template<typename Scalar>
	const typename PerspectiveViewFrustum<Scalar>::vector3_t& PerspectiveViewFrustum<Scalar>::direction() const
	{
		return _direction;
	}

	template<typename Scalar>
	const typename PerspectiveViewFrustum<Scalar>::vector3_t& PerspectiveViewFrustum<Scalar>::up() const
	{
		return _up;
	}

	template<typename Scalar>
	const typename PerspectiveViewFrustum<Scalar>::vector3_t& PerspectiveViewFrustum<Scalar>::right() const
	{
		return _right;
	}

	template<typename Scalar>
	typename PerspectiveViewFrustum<Scalar>::real_t PerspectiveViewFrustum<Scalar>::width() const
	{
		return _x;
	}

	template<typename Scalar>
	typename PerspectiveViewFrustum<Scalar>::real_t PerspectiveViewFrustum<Scalar>::height() const
	{
		return _y;
	}

	template<typename Scalar>
	typename PerspectiveViewFrustum<Scalar>::real_t PerspectiveViewFrustum<Scalar>::fieldOfView() const
	{
		return _fov;
	}

	template<typename Scalar>
	typename PerspectiveViewFrustum<Scalar>::real_t PerspectiveViewFrustum<Scalar>::nearPlane() const
	{
		return _near;
	}

	template<typename Scalar>
	typename PerspectiveViewFrustum<Scalar>::real_t PerspectiveViewFrustum<Scalar>::farPlane() const
	{
		return _far;
	}

	template<typename Scalar>
	bool PerspectiveViewFrustum<Scalar>::isInside(vector3_t p)
	{
		for (int i = 0; i < 6; i++)
			if (_planes[i].signedDistance(p) < 0)
				return false;

		return true;
	}

	template<typename Scalar>
	const typename PerspectiveViewFrustum<Scalar>::vector3_t& PerspectiveViewFrustum<Scalar>::corner(unsigned int i) const
	{
		Require(i < 8, "Index is valid.");

		return _corners[i];
	}

	template<typename Scalar>
	void PerspectiveViewFrustum<Scalar>::computePlanes()
	{
		using Vcl::Mathematics::equal;

		Require(equal(_direction.norm(), 1, (Scalar) 1e-6), "Direction is unit length.", "Length: %f", _direction.norm());
		Require(equal(_up.norm(), 1, (Scalar) 1e-6), "Up is unit length.", "Length: %f", _up.norm());
		Require(equal(_right.norm(), 1, (Scalar) 1e-6), "Right is unit length.", "Length: %f", _right.norm());

		real_t ratio = _x / _y;

		real_t near_height = real_t(2) * std::tan(real_t(0.5) * _fov) * _near;
		real_t near_width = near_height * ratio;

		real_t far_height = real_t(2) * std::tan(real_t(0.5) * _fov) * _far;
		real_t far_width = far_height * ratio;

		// Far plane center
		vector3_t fc = _position + _direction * _far;

		// Far plane (top, left/top, right/bottom, left/top, left)
		vector3_t ftl = fc + (_up * far_height*real_t(0.5)) - (_right * far_width*real_t(0.5));
		vector3_t ftr = fc + (_up * far_height*real_t(0.5)) + (_right * far_width*real_t(0.5));
		vector3_t fbl = fc - (_up * far_height*real_t(0.5)) - (_right * far_width*real_t(0.5));
		vector3_t fbr = fc - (_up * far_height*real_t(0.5)) + (_right * far_width*real_t(0.5));

		// Near plane center
		vector3_t nc = _position + _direction * _near;

		// Near plane (top, left/top, right/bottom, left/top, left)
		vector3_t ntl = nc + (_up * near_height*real_t(0.5)) - (_right * near_width*real_t(0.5));
		vector3_t ntr = nc + (_up * near_height*real_t(0.5)) + (_right * near_width*real_t(0.5));
		vector3_t nbl = nc - (_up * near_height*real_t(0.5)) - (_right * near_width*real_t(0.5));
		vector3_t nbr = nc - (_up * near_height*real_t(0.5)) + (_right * near_width*real_t(0.5));

		// Store the corners
		_corners[0] = nbl;
		_corners[1] = nbr;
		_corners[2] = ntr;
		_corners[3] = ntl;

		_corners[4] = fbl;
		_corners[5] = fbr;
		_corners[6] = ftr;
		_corners[7] = ftl;

		// Compute the bounding planes
		//vector3_t aux, normal;
		//
		//aux = (nc + _y*near_height) - _position;
		//aux.normalize();
		//normal = aux * _x;
		//_planes[Top] = Eigen::Hyperplane<real_t, 3>(normal, nc+_up*near_height);
		//
		//aux = (nc - _y*near_height) - _position;
		//aux.normalize();
		//normal = _x * aux;
		//_planes[Bottom] = Eigen::Hyperplane<real_t, 3>(normal, nc-_y*near_height);
		//
		//aux = (nc - _x*far_width) - _position;
		//aux.normalize();
		//normal = aux * _y;
		//_planes[Left] = Eigen::Hyperplane<real_t, 3>(normal, nc-_right*near_width);
		//
		//aux = (nc + _x*far_width) - _position;
		//aux.normalize();
		//normal = _y * aux;
		//_planes[Right] = Eigen::Hyperplane<real_t, 3>(normal, nc+_right*near_width);
		//
		//_planes[Near] = Eigen::Hyperplane<real_t, 3>(-_direction, nc);
		//_planes[Far] = Eigen::Hyperplane<real_t, 3>(_direction, fc);
	}

	template<typename Scalar>
	OrthographicViewFrustum<Scalar>::OrthographicViewFrustum()
	: _x(0)
	, _y(0)
	, _near(0.01)
	, _far(100)
	, _position(0, 0, 0)
	, _direction(0, 0, 1)
	, _up(0, 1, 0)
	, _right(1, 0, 0)
	{
		computePlanes();
	}

	template<typename Scalar>
	OrthographicViewFrustum<Scalar>::OrthographicViewFrustum
	(
		real_t width, real_t height, real_t near_plane, real_t far_plane, vector3_t pos, vector3_t dir, vector3_t up, vector3_t right
	)
	: _x(width)
	, _y(height)
	, _near(near_plane)
	, _far(far_plane)
	, _position(pos)
	, _direction(dir)
	, _up(up)
	, _right(right)
	{
		computePlanes();
	}

	template<typename Scalar>
	OrthographicViewFrustum<Scalar>::OrthographicViewFrustum
	(
		const OrthographicViewFrustum<real_t>& rhs
	)
	{
		_x = rhs._x;
		_y = rhs._y;
		_near = rhs._near;
		_far = rhs._far;
		_position = rhs._position;
		_direction = rhs._direction;
		_up = rhs._up;
		_right = rhs._right;

		_corners = rhs._corners;
		_planes = rhs._planes;
	}

	template<typename Scalar>
	const typename OrthographicViewFrustum<Scalar>::vector3_t& OrthographicViewFrustum<Scalar>::position() const
	{
		return _position;
	}

	template<typename Scalar>
	const typename OrthographicViewFrustum<Scalar>::vector3_t& OrthographicViewFrustum<Scalar>::direction() const
	{
		return _direction;
	}

	template<typename Scalar>
	const typename OrthographicViewFrustum<Scalar>::vector3_t& OrthographicViewFrustum<Scalar>::up() const
	{
		return _up;
	}

	template<typename Scalar>
	const typename OrthographicViewFrustum<Scalar>::vector3_t& OrthographicViewFrustum<Scalar>::right() const
	{
		return _right;
	}

	template<typename Scalar>
	typename OrthographicViewFrustum<Scalar>::real_t OrthographicViewFrustum<Scalar>::nearPlane() const
	{
		return _near;
	}

	template<typename Scalar>
	typename OrthographicViewFrustum<Scalar>::real_t OrthographicViewFrustum<Scalar>::farPlane() const
	{
		return _far;
	}

	template<typename Scalar>
	typename OrthographicViewFrustum<Scalar>::real_t OrthographicViewFrustum<Scalar>::width() const
	{
		return _x;
	}

	template<typename Scalar>
	typename OrthographicViewFrustum<Scalar>::real_t OrthographicViewFrustum<Scalar>::height() const
	{
		return _y;
	}

	template<typename Scalar>
	bool OrthographicViewFrustum<Scalar>::isInside(vector3_t p)
	{
		for (int i = 0; i < 6; i++)
			if (_planes[i].signedDistance(p) < 0)
				return false;

		return true;
	}

	template<typename Scalar>
	const typename OrthographicViewFrustum<Scalar>::vector3_t& OrthographicViewFrustum<Scalar>::corner(unsigned int i) const
	{
		Require(i < 8, "Index is valid.");

		return _corners[i];
	}

	template<typename Scalar>
	Eigen::Matrix<Scalar, 4, 4> OrthographicViewFrustum<Scalar>::computeViewMatrix(const MatrixFactory& factory) const
	{
		using Vcl::Mathematics::equal;

		Require(equal(_direction.cross(_up).dot(_right), 1, (Scalar) 1e-4), "Frame is orthogonal.", "Angle: %f", _direction.cross(_up).dot(_right));

		return factory.createLookAt(_position.template cast<float>(), _direction.template cast<float>(), _up.template cast<float>(), Handedness::RightHanded).template cast<Scalar>();
	}

	template<typename Scalar>
	Eigen::Matrix<Scalar, 4, 4> OrthographicViewFrustum<Scalar>::computeProjectionMatrix(const MatrixFactory& factory) const
	{
		Require(_x > 0, "Width is valid");
		Require(_y > 0, "Height is valid");
		Require(_near > 0, "Near plane is valid");
		Require(_far > 0, "Far plane is valid");

		return factory.createOrtho((float) _x, (float) _y, (float) nearPlane(), (float) farPlane(), Handedness::RightHanded).cast<Scalar>();
	}

	template<typename Scalar>
	OrthographicViewFrustum<Scalar> OrthographicViewFrustum<Scalar>::enclose(const PerspectiveViewFrustum<real_t>& frustum, const vector3_t& orthographic_direction)
	{
		using Vcl::Mathematics::equal;

		vector3_t dir = -orthographic_direction.normalized();

		// OBB positions & normals
		std::array<vector3_t, 6> p; p.fill(frustum.corner(0));
		std::array<vector3_t, 6> n;
		std::array<int, 6> idx;     idx.fill(0);

		// Near/Far plane
		n[0] =  dir;
		n[1] = -dir;
		for (int i = 1; i < 8; i++)
		{
			real_t d0 = n[0].dot(frustum.corner(i) - p[0]);
			real_t d1 = n[1].dot(frustum.corner(i) - p[1]);
			if (d0 > 0) { p[0] = frustum.corner(i); idx[0] = i; }
			if (d1 > 0) { p[1] = frustum.corner(i); idx[1] = i; }
		}

		// Project points on near plane
		std::array<vector3_t, 8> proj_points;
		for (int i = 0; i < 8; i++)
		{
			real_t d = dir.dot(frustum.corner(i) - p[0]);
			proj_points[i] = frustum.corner(i) - d * dir;
			Check(equal(dir.dot(proj_points[i] - p[0]), 0, (Scalar) 1e-3), "Projected point is on plane", "d = %f", dir.dot(proj_points[i] - p[0]));
		}

		// Compute center of projected points
		vector3_t m = vector3_t::Zero();
		for (int i = 0; i < 8; i++)
			m += proj_points[i];
		m /= 8;

		// Compute projected center point
		//vector3_t c =
		//	real_t(0.5)*(frustum.position() + frustum.nearPlane()*frustum.direction()) +
		//	real_t(0.5)*(frustum.position() +  frustum.farPlane()*frustum.direction())  ;
		//real_t dc = dir.dot(c - p[0]);
		//c -= dc * dir;

		// Compute PCA of projected points
		Eigen::Matrix<real_t, 3, 8> Y;
		for (int i = 0; i < 8; i++)
			Y.col(i) = proj_points[i] - m;

		Eigen::Matrix<real_t, 3, 3> S = Y*Y.transpose();
		Eigen::SelfAdjointEigenSolver<Eigen::Matrix<real_t, 3, 3>> eig(S);

		// Compute planes orthogonal to the main directions
		n[2] =  eig.eigenvectors().col(2).normalized();
		n[3] = -eig.eigenvectors().col(2).normalized();
		n[4] =  eig.eigenvectors().col(1).normalized();
		n[5] = -eig.eigenvectors().col(1).normalized();

		// Check if frame is orthogonal, else change it
		if (-dir.cross(n[5]).dot(n[3]) < 0)
			std::swap(n[4], n[5]);

		for (int i = 1; i < 8; i++)
		{
			real_t d2 = n[2].dot(frustum.corner(i) - p[2]);
			real_t d3 = n[3].dot(frustum.corner(i) - p[3]);
			real_t d4 = n[4].dot(frustum.corner(i) - p[4]);
			real_t d5 = n[5].dot(frustum.corner(i) - p[5]);
			if (d2 > 0) { p[2] = frustum.corner(i); idx[2] = i; }
			if (d3 > 0) { p[3] = frustum.corner(i); idx[3] = i; }
			if (d4 > 0) { p[4] = frustum.corner(i); idx[4] = i; }
			if (d5 > 0) { p[5] = frustum.corner(i); idx[5] = i; }
		}

		// Compute frustum parameters
		real_t near_to_far   = n[1].dot(p[1] - p[0]);
		real_t left_to_right = n[3].dot(p[3] - p[2]);
		real_t bottom_to_top = n[5].dot(p[5] - p[4]);

		// Compute frustum center on near plane
		// - Project m on to left plane, displace by left_to_right/2 -> t
		// - Project t on to bottom plane, displace by bottom_to_top/2 -> nc
		real_t dt = -n[3].dot(m - p[3]) - left_to_right/2;
		vector3_t t = m + dt * n[3];
		real_t dnc = -n[5].dot(t - p[5]) - bottom_to_top/2;
		vector3_t nc = t + dnc * n[5];

		AssertBlock
		{
			vector3_t pos = nc + dir*near_to_far;
			real_t d0 = n[0].dot(pos - p[0]);
			real_t d1 = n[1].dot(pos - p[1]);

			real_t dm0 = n[0].dot(nc - p[0]);
			real_t dm1 = n[1].dot(nc - p[1]);
			real_t dm2 = n[2].dot(nc - p[2]);
			real_t dm3 = n[3].dot(nc - p[3]);
			real_t dm4 = n[4].dot(nc - p[4]);
			real_t dm5 = n[5].dot(nc - p[5]);

			Check(equal(abs(d0),   near_to_far, (Scalar) 1e-3), "Frustum position is correct.");
			Check(equal(abs(d1), 2*near_to_far, (Scalar) 1e-3), "Frustum position is correct.");

			Check(equal(abs(dm0),           0, (Scalar) 1e-3), "Frustum depth is correct.");
			Check(equal(abs(dm1), near_to_far, (Scalar) 1e-3), "Frustum depth is correct.");
			Check(equal(abs(dm2), left_to_right/2, (Scalar) 1e-3), "Frustum width is correct.");
			Check(equal(abs(dm3), left_to_right/2, (Scalar) 1e-3), "Frustum width is correct.");
			Check(equal(abs(dm4), bottom_to_top/2, (Scalar) 1e-3), "Frustum height is correct.");
			Check(equal(abs(dm5), bottom_to_top/2, (Scalar) 1e-3), "Frustum height is correct.");

			Check(equal(-dir.cross(n[5]).dot(n[3]), 1, (Scalar) 1e-4), "Frame is orthogonal.", "Angle: %f", -dir.cross(n[5]).dot(n[3]));
		}

		OrthographicViewFrustum<real_t> ortho
		(
			left_to_right, bottom_to_top, near_to_far, 2*near_to_far, nc + dir*near_to_far, -dir, n[5], n[3]
		);
		return ortho;
	}

	template<typename Scalar>
	void OrthographicViewFrustum<Scalar>::computePlanes()
	{
		using Vcl::Mathematics::equal;

		Require(equal(_direction.squaredNorm(), 1, (Scalar) 1e-6), "Direction is unit length.");
		Require(equal(_up.squaredNorm(), 1, (Scalar) 1e-6), "Up is unit length.");
		Require(equal(_right.squaredNorm(), 1, (Scalar) 1e-6), "Right is unit length.");

		real_t ratio = _x / _y;

		real_t height = _y;
		real_t width  = _x;

		// Far plane center
		vector3_t fc = _position + _direction * _far;

		// Far plane (top, left/top, right/bottom, left/top, left)
		vector3_t ftl = fc + (_up * height*real_t(0.5)) - (_right * width*real_t(0.5));
		vector3_t ftr = fc + (_up * height*real_t(0.5)) + (_right * width*real_t(0.5));
		vector3_t fbl = fc - (_up * height*real_t(0.5)) - (_right * width*real_t(0.5));
		vector3_t fbr = fc - (_up * height*real_t(0.5)) + (_right * width*real_t(0.5));

		// Near plane center
		vector3_t nc = _position + _direction * _near;

		// Near plane (top, left/top, right/bottom, left/top, left)
		vector3_t ntl = nc + (_up * height*real_t(0.5)) - (_right * width*real_t(0.5));
		vector3_t ntr = nc + (_up * height*real_t(0.5)) + (_right * width*real_t(0.5));
		vector3_t nbl = nc - (_up * height*real_t(0.5)) - (_right * width*real_t(0.5));
		vector3_t nbr = nc - (_up * height*real_t(0.5)) + (_right * width*real_t(0.5));

		// Store the corners
		_corners[0] = nbl;
		_corners[1] = nbr;
		_corners[2] = ntr;
		_corners[3] = ntl;

		_corners[4] = fbl;
		_corners[5] = fbr;
		_corners[6] = ftr;
		_corners[7] = ftl;

		// Compute the bounding planes
		//vector3_t aux, normal;
		//
		//aux = (nc + _y*near_height) - _position;
		//aux.normalize();
		//normal = aux * _x;
		//_planes[Top] = Eigen::Hyperplane<real_t, 3>(normal, nc+_up*near_height);
		//
		//aux = (nc - _y*near_height) - _position;
		//aux.normalize();
		//normal = _x * aux;
		//_planes[Bottom] = Eigen::Hyperplane<real_t, 3>(normal, nc-_y*near_height);
		//
		//aux = (nc - _x*far_width) - _position;
		//aux.normalize();
		//normal = aux * _y;
		//_planes[Left] = Eigen::Hyperplane<real_t, 3>(normal, nc-_right*near_width);
		//
		//aux = (nc + _x*far_width) - _position;
		//aux.normalize();
		//normal = _y * aux;
		//_planes[Right] = Eigen::Hyperplane<real_t, 3>(normal, nc+_right*near_width);
		//
		//_planes[Near] = Eigen::Hyperplane<real_t, 3>(-_direction, nc);
		//_planes[Far] = Eigen::Hyperplane<real_t, 3>(_direction, fc);
	}

	template class PerspectiveViewFrustum<float>;
	template class PerspectiveViewFrustum<double>;

	template class OrthographicViewFrustum<float>;
	template class OrthographicViewFrustum<double>;
}}