/**
 * Copyright (C) 2015 Dato, Inc.
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms
 * of the BSD license. See the LICENSE file for details.
 */
/**
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  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.
 *
 * For more about this software visit:
 *
 *      http://www.graphlab.ml.cmu.edu
 *
 */
#ifndef GRAPHLAB_GRAPH_HASH_UTIL
#define GRAPHLAB_GRAPH_HASH_UTIL

#include <boost/function.hpp>
#include <boost/functional/hash.hpp>
#include <graphlab/util/integer_mix.hpp>

namespace graphlab {
  namespace graph_hash {
    /** \brief Returns the hashed value of a vertex. */
    inline static size_t hash_vertex (const vertex_id_type vid) {
      return integer_mix(vid);
    }

    /** \brief Returns the hashed value of an edge. */
    inline static size_t hash_edge (const std::pair<vertex_id_type, vertex_id_type>& e, const uint32_t seed = 5) {
      // a bunch of random numbers
#if (__SIZEOF_PTRDIFF_T__ == 8)
      static const size_t a[8] = {0x6306AA9DFC13C8E7,
        0xA8CD7FBCA2A9FFD4,
        0x40D341EB597ECDDC,
        0x99CFA1168AF8DA7E,
        0x7C55BCC3AF531D42,
        0x1BC49DB0842A21DD,
        0x2181F03B1DEE299F,
        0xD524D92CBFEC63E9};
#else
      static const size_t a[8] = {0xFC13C8E7,
        0xA2A9FFD4,
        0x597ECDDC,
        0x8AF8DA7E,
        0xAF531D42,
        0x842A21DD,
        0x1DEE299F,
        0xBFEC63E9};
#endif
      vertex_id_type src = e.first;
      vertex_id_type dst = e.second;
      return (integer_mix(src^a[seed%8]))^(integer_mix(dst^a[(seed+1)%8]));
    }
  } // end of graph_hash namespace
} // end of graphlab namespace
#endif


#include <vector>
#include "Drone.h"
#include <math.h>
#include <boost/asio/io_service.hpp>
#include <Fullnavdata.h>
#include <gnuplot_iostream.h>
#include <deque>

/*
 * PRIVATE HEADER
 */

#define DRONE_IP                    "10.42.0.10"
#define CALIBRATION_FILE            "res/calib_bd2.xml"
#define HULLPROTECTIONON            true

/*
 * UTILITY FUNCTIONS
 */
/**
 * Returns:
 *      value if value is inside [min; max],
 *      min if value < min,
 *      max if value > max
 * @param value
 * @param min
 * @param max
 * @return
 */
int interval(int value, int min, int max){
    if( min < value) {
        if( value < max){
            return value;
        }else{
            return max;
        }
    }else{
        return min;
    }
}

/**
 * Returns value if value is greater than +epsilon or lower than -epsilon. Otherwise, 0 is returned
 * @param value
 * @param epsilon
 * @return
 */
float valueIfAboveEpsilon(float value, float epsilon){
    if(0 < value and value < epsilon){
        return 0;
    }
    if( -epsilon < value and value < 0){
        return 0;
    }
    return value;

}

/*
 * MAIN
 */
int main(){

    /// ***************************************************************************** CONNECT TO AND CONFIGURE THE DRONE
    Drone d("10.42.0.10");
    Drone d2("10.42.0.11");
    assert(d.connect());
    assert(d2.connect());

    // Waiting for the drone to be ready
    while(!d.isRunning()){ sleep(1); }
    while(!d2.isRunning()){ sleep(1); }

    std::cout << "INITIALISATION IS OK" << std::endl;

    // Flat trim at start. The drone MUST be on the ground at that time
    if(d.blockingFlatTrim()) {
        std::cout << "FLAT TRIM IS OK" << std::endl;
    }else{
        std::cerr << "FLAT TRIM NOT OK" << std::endl;
        return 1;
    }
    // Flat trim at start. The drone MUST be on the ground at that time
    if(d2.blockingFlatTrim()) {
        std::cout << "FLAT TRIM IS OK" << std::endl;
    }else{
        std::cerr << "FLAT TRIM NOT OK" << std::endl;
        return 1;
    }
    assert(d.setHullPresence(HULLPROTECTIONON));
    assert(d2.setHullPresence(HULLPROTECTIONON));

    assert(d.setVideoAutorecord(false));
    assert(d2.setVideoAutorecord(false));
    //assert(d.stopStreaming());

    assert(d.useFullNavdata());
    assert(d2.useFullNavdata());
    sleep(1);
    assert(d.isUsingFullNavdata());
    assert(d2.isUsingFullNavdata());

    Gnuplot gp;
    gp << "set title \"10 Gyroscope (filt)\"\n";
    gp << "set ylabel \"rad/s\"\n";
    std::deque<float> x1;
    std::deque<float> y1;
    std::deque<float> z1;
    Eigen::Vector3f vec;

    Gnuplot gp2;
    gp2 << "set title \"11 Gyroscope (filt)\"\n";
    gp2 << "set ylabel \"m/s²\"\n";
    std::deque<float> x2;
    std::deque<float> y2;
    std::deque<float> z2;
    Eigen::Vector3f vec2;

/*
    Gnuplot gp3;
    gp3 << "set title \"Magnetometer (raw)\"\n";
    gp3 << "set ylabel \"mG\"\n";
    std::deque<float> x3;
    std::deque<float> y3;
    std::deque<float> z3;
    Eigen::Vector3f vec3;
*/
    while(true)
    {
        if (x1.size() > 150) {
            x1.pop_front();
            y1.pop_front();
            z1.pop_front();
        }

        if (x2.size() > 150) {
            x2.pop_front();
            y2.pop_front();
            z2.pop_front();
        }
/*
        if (x3.size() > 150) {
            x3.pop_front();
            y3.pop_front();
            z3.pop_front();
        }
*/
        d._navdata->lock();
        vec = d._navdata->get_gyroscope_filt();
        vec2 = d2._navdata->get_gyroscope_filt();
        //vec3 = d._navdata->get_magnetometer_raw();
        d._navdata->release();

        x1.push_back(vec(0));
        y1.push_back(vec(1));
        z1.push_back(vec(2));

        x2.push_back(vec2(0));
        y2.push_back(vec2(1));
        z2.push_back(vec2(2));
/*
        x3.push_back(vec3(0));
        y3.push_back(vec3(1));
        z3.push_back(vec3(2));
*/
        gp << "plot '-' binary" << gp.binFmt1d(x1, "array") << "with lines title \"x\",";
        gp << "'-' binary" << gp.binFmt1d(y1, "array") << "with lines title \"y\",";
        gp << "'-' binary" << gp.binFmt1d(z1, "array") << "with lines title \"z\"\n";

        gp.sendBinary1d(x1);
        gp.sendBinary1d(y1);
        gp.sendBinary1d(z1);

        gp.flush();

        gp2 << "plot '-' binary" << gp2.binFmt1d(x2, "array") << "with lines title \"x\",";
        gp2 << "'-' binary" << gp2.binFmt1d(y2, "array") << "with lines title \"y\",";
        gp2 << "'-' binary" << gp2.binFmt1d(z2, "array") << "with lines title \"z\"\n";

        gp2.sendBinary1d(x2);
        gp2.sendBinary1d(y2);
        gp2.sendBinary1d(z2);

        gp2.flush();

/*
        gp3 << "plot '-' binary" << gp3.binFmt1d(x3, "array") << "with lines title \"x\",";
        gp3 << "'-' binary" << gp3.binFmt1d(y3, "array") << "with lines title \"y\",";
        gp3 << "'-' binary" << gp3.binFmt1d(z3, "array") << "with lines title \"z\"\n";

        gp3.sendBinary1d(x3);
        gp3.sendBinary1d(y3);
        gp3.sendBinary1d(z3);

        gp3.flush();
*/
        // 100Hz
        usleep(25 * 1000);
    }

    return 0;
}


/*
* Copyright (C) 2017 Incognito (Edited by ProMetheus)
*
* 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 <streamer/config.hpp>
#include "../natives.h"

#include "../core.h"
#include "../utility.h"

#include <streamer/pickups.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/intrusive_ptr.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/unordered_map.hpp>

#include <Eigen/Core>

STREAMER_BEGIN_NS

int CreateDynamicPickup(int id, int type, float x, float y, float z, int worldid, int interiorid, int playerid, float streamDistance, int areaid, int priority) {
	if (core->getData()->getGlobalMaxItems(STREAMER_TYPE_PICKUP) == core->getData()->pickups.size()) {
		return 0;
	}

	int pickupId = Item::Pickup::identifier.get();
	Item::SharedPickup pickup(new Item::Pickup);
	//pickup->amx = amx;
	pickup->pickupId = pickupId;
	pickup->inverseAreaChecking = false;
	pickup->originalComparableStreamDistance = -1.0f;
	pickup->positionOffset = Eigen::Vector3f::Zero();
	pickup->streamCallbacks = false;
	pickup->modelId = id;
	pickup->type = type;
	pickup->position = Eigen::Vector3f(x, y, z);

	Utility::addToContainer(pickup->worlds, worldid);
	Utility::addToContainer(pickup->interiors, interiorid);
	Utility::addToContainer(pickup->players, playerid);

	pickup->comparableStreamDistance = streamDistance < STREAMER_STATIC_DISTANCE_CUTOFF ? streamDistance : streamDistance * streamDistance;
	pickup->streamDistance = streamDistance;
	Utility::addToContainer(pickup->areas, areaid);
	pickup->priority = priority;
	core->getGrid()->addPickup(pickup);
	core->getData()->pickups.insert(std::make_pair(pickupId, pickup));
	return static_cast<cell>(pickupId);
}

int DestroyDynamicPickup(int id) {
	boost::unordered_map<int, Item::SharedPickup>::iterator p = core->getData()->pickups.find(id);
	if (p != core->getData()->pickups.end()) {
		Utility::destroyPickup(p);
		return 1;
	}
	return 0;
}

int IsValidDynamicPickup(int id) {
	boost::unordered_map<int, Item::SharedPickup>::iterator p = core->getData()->pickups.find(id);
	if (p != core->getData()->pickups.end()) {
		return 1;
	}
	return 0;
}

STREAMER_END_NS


// Copyright 2017-2018 Coen Tempelaars (Falcons)
// SPDX-License-Identifier: Apache-2.0
/*
 * ruleAvoidAreas.hpp
 *
 *  Created on: Apr 24, 2017
 *      Author: Coen Tempelaars
 */

#ifndef RULEAVOIDAREAS_HPP_
#define RULEAVOIDAREAS_HPP_

#include "polygon2D.hpp"
#include "position2d.hpp"
#include "int/utilities/timer.hpp"

#include <boost/shared_ptr.hpp>
#include <vector>

class ruleAvoidAreas
{
public:
    ruleAvoidAreas(boost::shared_ptr<teamplay::timer>);
    virtual ~ruleAvoidAreas();

    virtual bool isCurrentPositionValid() const;
    virtual std::vector<polygon2D> getForbiddenAreas() const;

private:
    virtual bool isCurrentPositionInOwnPenaltyAreaValid() const;
    virtual bool isCurrentPositionInOppPenaltyAreaValid() const;
    virtual bool isCurrentPositionInOwnGoalAreaValid() const;
    virtual bool isCurrentPositionInOppGoalAreaValid() const;

    boost::shared_ptr<teamplay::timer> _timer;
};

#endif /* RULEAVOIDAREAS_HPP_ */


// This file is part of DM-HEOM (https://github.com/noma/dm-heom)
//
// Copyright (c) 2015-2019 Matthias Noack, Zuse Institute Berlin
//
// Licensed under the 3-clause BSD License, see accompanying LICENSE,
// CONTRIBUTORS.md, and README.md for further information.

#include <iostream>
#include <sstream>

#include <boost/format.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <noma/num/meta_stepper.hpp>

#include "heom/command_line.hpp"
#include "heom/common.hpp"
#include "heom/constants.hpp"
#include "heom/dipole_matrix.hpp"
#include "heom/dipole_pseudo_pathway.hpp"
#include "heom/e_field.hpp"
#include "heom/handle_main_exception.hpp"
#include "heom/hierarchy_graph.hpp"
#include "heom/hierarchy_mask.hpp"
#include "heom/hierarchy_norm.hpp"
#include "heom/instance.hpp"
#include "heom/make_file_observer_list.hpp"
#include "heom/population_dynamics_solver.hpp"
#include "heom/ocl_config.hpp"
#include "heom/ode.hpp"
#include "heom/population_dynamics_solver.hpp"
#include "heom/sites_to_states.hpp"
#include "heom/transient_absorption_config.hpp"

namespace bmt = ::heom::bmt;
namespace num = ::heom::num;
namespace ocl = ::heom::ocl;
using heom::int_t;
using heom::real_t;
using heom::real_format;
using heom::complex_t;
using heom::default_delimiter;

int main(int argc, char* argv[])
{
	// start runtime measurement
	bmt::timer app_timer;

	// output compile time configuration
	std::cout << "-------------------- Compile-Time Configuration ------------" << std::endl;
	heom::write_compile_config(std::cout);
	std::cout << "------------------------------------------------------------" << std::endl;

	// command line parsing
	// usage: app_* <ocl_config> <heom_config> [partition_file]
	std::string ocl_config_file = "ocl_config.cfg";
	std::string heom_config_file = "transient_absorption.cfg";

	if (argc >= 2)
		ocl_config_file = argv[1];
	if (argc >= 3)
		heom_config_file = argv[2];

	std::exception_ptr eptr;
	try {
		// command line parsing
		heom::command_line command_line { argc, argv };

		std::cout << "Parsing OpenCL configuration from file: " << command_line.ocl_config_filename() << std::endl;
		heom::ocl_config ocl_config(command_line.ocl_config_filename());
		std::cout << "Parsing HEOM configuration from file: " << command_line.heom_config_filename() << std::endl;
		heom::transient_absorption_config heom_config(command_line.heom_config_filename());

		heom::hierarchy_graph complete_graph(heom_config.baths_number(), heom_config.baths_matsubaras(), heom_config.system_ado_depth());

		using stepper_t = num::meta_stepper; // uses solver_stepper_type from config
		using solver_t = heom::population_dynamics_solver<heom::ode, stepper_t, heom::hierarchy_norm>;

		// prepare output file (so we run into possible file errors before starting the computation)
		auto& observation_filename = heom_config.observations().get().front().get().second; // filename of first specified observation, guaranteed to exist by config checks
		std::ofstream observation_file(observation_filename);
		if (observation_file.fail())
			throw std::runtime_error("Error: could not open output file: " + observation_filename);

		// transient absorption computation over all combinations of dipole matrices and phases
		const size_t num_dipole_matrices = heom_config.dipole_tensor_prefactors().size();
		const size_t num_pulse_phases = heom_config.probe_pulse_phases().size();
		const size_t num_experiments = num_dipole_matrices * num_pulse_phases;

		// solver iterations and steps
		const int_t iterations = heom_config.solver_steps() / heom_config.program_observe_steps();
		const int_t steps_per_iteration = heom_config.program_observe_steps();
		const int_t total_steps = iterations * heom_config.program_observe_steps();

		// output data structure for averaged traces
		const int_t num_observations = iterations + 1;
		std::vector<heom::matrix_trace_observation_view> observations(num_observations);
		// e_field is needed once, and is the same across runs
		std::vector<complex_t> e_field_observations(num_observations);

		// transient absorption pseudo pathway
		const auto& pathway_spec = heom_config.spectra_include_esa() ? heom::dipole_pseudo_pathway_esa_spec : heom::dipole_pseudo_pathway_wgs_spec;
		const auto sts_mode = pathway_spec.sites_to_states_mode();

		// for progress estimate
		const size_t solver_runs = num_dipole_matrices * num_pulse_phases;
		size_t solver_run = 0;

		// iterate over all dipoles and compute population dynamics
		for (size_t tensor_index = 0; tensor_index < num_dipole_matrices; ++tensor_index) {
			// loop over all pulse phases
			for (size_t phase_index = 0; phase_index < num_pulse_phases; ++phase_index)
			{
				const bool first_solver_run = tensor_index == 0 && phase_index == 0; // only observe first solver run

				heom::instance heom_instance(heom_config, sts_mode, complete_graph);
				heom_instance.set_hierarchy_top(); // NOTE: hierarchy top is initialised to zero with first element being complex_t(1.0,0.0) here

				// OpenCL range
				ocl::nd_range range {
					{}, // offset
					{1, static_cast<std::uint64_t>(heom_instance.matrices())}, // global size
					{1, 1} // local size
				};

				// create a solver from configuration and instance
				solver_t solver(ocl_config, range, heom_config, heom_instance);
				std::cout << "-------------------- OpenCL Runtime Configuration ----------" << std::endl;
				solver.write_ocl_runtime_config(std::cout);
				std::cout << "------------------------------------------------------------" << std::endl;
				++solver_run; // for progress estimate

				// compute dipole matrices
				auto get_dipole_matrix = [&](size_t spec_index) {
					return heom::get_dipole_matrix_for_pathway(pathway_spec, spec_index, heom_config.system_sites(), heom_config, tensor_index);
				};
				auto dipole_matrix_minus = get_dipole_matrix(0);
				auto dipole_matrix_plus = get_dipole_matrix(1);
				// TODO: check scaling
				dipole_matrix_minus.scale(heom::get_dipole_tensor_prefactor(pathway_spec, heom_config, tensor_index));

				// TODO: remove this scaling, i.e. fix unit of dipole matrices across all applications, maybe use factor during initialisation of config object or change input file format
				//       scaling dipole strengths in dipole_config_entries does work for this app, but messes with the result of others (tested with linear_absorption)
				dipole_matrix_minus.scale(heom::constants::debye_to_coulomb_meter);
				dipole_matrix_plus.scale(heom::constants::debye_to_coulomb_meter);
//std::cout << dipole_matrix_minus << std::endl << std::endl;
//std::cout << dipole_matrix_plus << std::endl << std::endl;

				// TODO: check if this is really plus and not prescaled... prefactor might be one in config (?) vs. other apps
				// setup observer with pre-scaled dipole matrix
				heom::matrix_trace_observer<solver_t> trace_observer(complete_graph, solver, dipole_matrix_plus.data()); // D+ from left via observer

				// extract hamiltonian as complex_matrix_t for the e_field
				heom::complex_matrix_t hamiltonian(dipole_matrix_plus.rows(), dipole_matrix_plus.cols());
				for(size_t i = 0; i < hamiltonian.rows(); ++i) {
					for(size_t j = 0; j < hamiltonian.cols(); ++j) {
						const size_t real_index = 2 * (i * hamiltonian.rows() + j);
						const size_t imag_index = real_index + 1;
						hamiltonian.at(i, j) = heom::complex_t(heom_instance.hamiltonian()[real_index], heom_instance.hamiltonian()[imag_index]);
					}
				}

				// add e-field pre-evaluation action to the ODE for transient spectra computation
				auto e_field = heom::e_field<decltype(heom_config), heom::ode>(heom_config, phase_index, dipole_matrix_plus, dipole_matrix_minus, hamiltonian);
				solver.ode().clear_pre_evaluate_actions();
				solver.ode().add_pre_evaluate_action(e_field);

				// with create_hierarchy_mask we create mask corresponding to configured filtering strategy, then update solver
				// NOTE: .data() returns pointer to mask_t array
				heom::hierarchy_mask hierarchy_mask = heom::create_hierarchy_mask(heom_config.filtering_strategy(), complete_graph, heom_config.baths_matsubaras(), heom_config.filtering_first_layer());
				solver.update_hierarchy_mask(hierarchy_mask.data());

				std::cout << "-------------------- Hierarchy Mask Counter ----------------" << std::endl;
				heom::write_hierarchy_mask_stats(hierarchy_mask, std::cout);
				std::cout << "------------------------------------------------------------" << std::endl;

				// generate first observation with initial value
				observations[0] += trace_observer.observe_trace(0.0);
				if (first_solver_run)
					e_field_observations[0] = e_field.compute_field(0.0);

				// setup output per propagation output
				heom::config::obervation_type_list per_propagation_observations;

				// build single-propagation observation list from config list
				for (auto& pair : heom_config.observations().get()) {
					auto& obs_type = pair.get().first;
					auto& filename = pair.get().second;

					if (heom::is_single_propagation_observation(obs_type)) {
						std::remove_const<std::remove_reference<decltype(pair)>::type>::type new_pair;
						new_pair.get().first = pair.get().first; // copy observation type

						std::stringstream new_filename;
						size_t dot_pos = filename.find_first_of('.');
						if (dot_pos != std::string::npos) {
							new_filename << filename.substr(0, dot_pos); // copy everything prior to first dot
						} else {
							new_filename << filename; // copy whole filename, in case not dot was found
						}
						new_filename << "_d" << tensor_index << "_p" << phase_index; // add create suffix
						if (dot_pos != std::string::npos) {
							new_filename << filename.substr(dot_pos); // copy file ending
						}

						new_pair.get().second = new_filename.str(); // set new filename
						per_propagation_observations.get().push_back(new_pair);
					}
				}
				heom::observer_list observer_list = heom::make_file_observer_list(per_propagation_observations, complete_graph, solver);
				observer_list.observe(0.0);

				// write file header once
				if (first_solver_run) {
					trace_observer.write_header(observation_file, false);
					observation_file << default_delimiter;
					observation_file << "e-field_real";
					observation_file << default_delimiter;
					observation_file << "e-field_imag";
					observation_file << '\n';
				}

				// propagate system
				for (int_t k = 0; k < iterations; ++k)
				{
					// propagate
					solver.step_forward(steps_per_iteration);

					// after propagation:
					const auto current_step = (k + 1) * steps_per_iteration;
					const real_t current_time = current_step * heom_config.solver_step_size();

					// observe trace of dipole_matrix_minus - rho
					observations[k + 1] += trace_observer.observe_trace(current_time);
					if (first_solver_run)
						e_field_observations[k + 1] = e_field.compute_field(current_time);

					// observe single-propagation observation types
					observer_list.observe(current_time);

					// update status
					std::stringstream status_prefix;
					status_prefix << "Calculation transient absorption ("
					              << "dipole: " << tensor_index + 1 << '/' << num_dipole_matrices
					              << ", "
					              << "phase: " << phase_index + 1 << '/' << num_pulse_phases
					              << "): ";
					heom::write_progress(current_step - 1, total_steps, solver_run, solver_runs, status_prefix.str(), std::cout);
				}

				// write_complex_matrix(result_buffer_top, heom_instance.states(), std::cout);
				std::cout << "-------------------- Solver Runtime Summary ----------------" << std::endl;
				solver.write_runtime_summary(std::cout);
				std::cout << "------------------------------------------------------------" << std::endl;
			}
		}

		// write averaged traces, with added columns for e-field
		auto format = real_format;
		//for (auto& obs : observations) {
		for (int_t i = 0; i < num_observations; ++i) {
			observations[i].avg(num_experiments);
			observation_file << observations[i];
			observation_file << default_delimiter;
			observation_file << format % e_field_observations[i].real();
			observation_file << default_delimiter;
			observation_file << format % e_field_observations[i].imag();
			observation_file << '\n';
		}
		observation_file << std::flush; // tell the OS to write to disk

	} catch (...) {
		eptr = std::current_exception();
	}
	int ret = heom::handle_main_exception(eptr);

	// print application runtime
	std::cout << "main(): application runtime: " << boost::format("%11.2f") % std::chrono::duration_cast<bmt::seconds>(bmt::duration(app_timer.elapsed())).count() << " s" << std::endl;

	return ret;
}


// Copyright 2016. 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.
//
// 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.
//
// Author: ryan.latture@gmail.com (Ryan Latture)

#include "explicit_system.h"
#include <boost/format.hpp>
#include <fstream>

namespace explicit_fea {

    ExplicitSystem::ExplicitSystem(Mesh _mesh,
                                   ForceList _external_forces,
                                   const ColumnVector& _initial_displacements,
                                   const ColumnVector& _initial_velocities,
                                   double _t_0,
                                   const Options& _options) :
        mesh(std::move(_mesh)),
        external_forces(std::move(_external_forces)),
        displacements_0(_initial_displacements),
        velocities_0(_initial_velocities),
        velocities_1(ColumnVector::Zero(_initial_displacements.size())),
        accelerations_0(ColumnVector::Zero(_initial_displacements.size())),
        accelerations_1(ColumnVector::Zero(_initial_displacements.size())),
        forces(ColumnVector::Zero(_initial_displacements.size())),
        RHS(ColumnVector::Zero(_initial_displacements.size())),
        t_0(_t_0),
        dt_0(0.0),
        options(_options) {
        if (displacements_0.size() != velocities_0.size())
            throw std::runtime_error("Size of initial velocities and initial displacements are not equal.");
        if (displacements_0.size() != mesh.get_global_stiffness_matrix().cols())
            throw std::runtime_error("Size of displacements and velocities does not match the number of columns in the global matrices.");

        LHS.resize(mesh.get_global_stiffness_matrix().rows(), mesh.get_global_stiffness_matrix().cols());
        assemble_damping_matrix();
        apply_bcs(t_0);
    }

    void ExplicitSystem::Options::load(const rapidjson::Document &config_doc) {
        if (config_doc.HasMember("options")) {
            if (config_doc["options"].HasMember("beta")) {
                if (!config_doc["options"]["beta"].IsNumber()) {
                    throw std::runtime_error("beta provided in options configuration is not a number.");
                }
                beta = config_doc["options"]["beta"].GetDouble();
            }
            if (config_doc["options"].HasMember("gamma")) {
                if (!config_doc["options"]["gamma"].IsNumber()) {
                    throw std::runtime_error("gamma provided in options configuration is not a number.");
                }
                gamma = config_doc["options"]["gamma"].GetDouble();
            }
            if (config_doc["options"].HasMember("damping_beta")) {
                if (!config_doc["options"]["damping_beta"].IsNumber()) {
                    throw std::runtime_error("damping_beta provided in options configuration is not a number.");
                }
                damping_beta = config_doc["options"]["damping_beta"].GetDouble();
            }
            if (config_doc["options"].HasMember("damping_alpha")) {
                if (!config_doc["options"]["damping_alpha"].IsNumber()) {
                    throw std::runtime_error("damping_alpha provided in options configuration is not a number.");
                }
                damping_alpha = config_doc["options"]["damping_alpha"].GetDouble();
            }
        }
    }

    void ExplicitSystem::update(double dt_1) {
        double t_1 = t_0 + dt_1;
        if (!compare.equal(dt_1, dt_0)) {
            assemble_LHS(dt_1);
        }

        apply_external_forces(t_1);
        RHS = forces;
        RHS.noalias() -= damping_matrix * (velocities_0 + (1.0 - options.gamma) * dt_1 * accelerations_0);
        RHS.noalias() -= mesh.get_global_stiffness_matrix() * (displacements_0 + dt_1 * velocities_0 + (0.5 - options.beta) * dt_1 * dt_1 * accelerations_0);
        apply_bcs(t_1);

        accelerations_1 = solver.solve(RHS);
        velocities_1.noalias() += (1.0 - options.gamma) * dt_1 * accelerations_0 + options.gamma * dt_1 * accelerations_1;

        // advance all current time values to next iteration
        displacements_0.noalias() += dt_1 * velocities_0 + dt_1 * dt_1 * (0.5 - options.beta) * accelerations_0 + dt_1 * dt_1 * options.beta * accelerations_1;
        velocities_0 = velocities_1;
        accelerations_0 = accelerations_1;
        t_0 = t_1;
        dt_0 = dt_1;
    }

	Mesh const& ExplicitSystem::getMesh() const {
		return mesh;
	}

    ColumnVector const& ExplicitSystem::getDisplacements() const {
        return displacements_0;
    }

    ColumnVector ExplicitSystem::getForces() const {
        ColumnVector out(displacements_0.size());
        out = mesh.get_global_stiffness_matrix() * displacements_0;
        out += mesh.get_mass_matrix() * accelerations_0;
        return out;
    }

    ColumnVector const& ExplicitSystem::getVelocities() const {
        return velocities_0;
    }

    double ExplicitSystem::getTime() const {
        return t_0;
    }

    void ExplicitSystem::assemble_damping_matrix() {
        damping_matrix.resize(mesh.get_global_stiffness_matrix().rows(), mesh.get_global_stiffness_matrix().cols());
        damping_matrix = options.damping_alpha * mesh.get_mass_matrix() + options.damping_beta * mesh.get_global_stiffness_matrix();
        damping_matrix.prune(1.e-14);
        damping_matrix.makeCompressed();
    }

    void ExplicitSystem::assemble_LHS(double dt){
        LHS = mesh.get_mass_matrix() + options.gamma * dt * damping_matrix + options.beta * dt * dt * mesh.get_global_stiffness_matrix();
        LHS.prune(1.e-14);
        LHS.makeCompressed();
        solver.compute(LHS);
    }

    void ExplicitSystem::apply_bcs(double time) {
        for (BCList::const_iterator it = mesh.get_bcs().begin(); it != mesh.get_bcs().end(); ++it) {
            RHS[(*it)->global_index] = 0.0;
            if ((*it)->type == BC::Type::DISPLACEMENT) {
                displacements_0[(*it)->global_index] = (*it)->get_value(time);
                velocities_0[(*it)->global_index] = 0.0;
            } else if ((*it)->type == BC::Type::VELOCITY) {
                velocities_0[(*it)->global_index] = (*it)->get_value(time);
            }
        }
    }

    void ExplicitSystem::apply_external_forces(double time) {
        for (ForceList::const_iterator it = external_forces.begin(); it != external_forces.end(); ++it) {
            forces[(*it)->global_index] = (*it)->get_value(time);
        }
    }

} // namespace explicit_fea


#ifndef STAN_MCMC_HMC_INTEGRATORS_IMPL_LEAPFROG_HPP
#define STAN_MCMC_HMC_INTEGRATORS_IMPL_LEAPFROG_HPP

#include <Eigen/Dense>
#include <stan/mcmc/hmc/integrators/base_leapfrog.hpp>

namespace stan {
  namespace mcmc {

    template <typename Hamiltonian>
    class impl_leapfrog: public base_leapfrog<Hamiltonian> {
    public:
      impl_leapfrog(): base_leapfrog<Hamiltonian>(),
                       max_num_fixed_point_(10),
                       fixed_point_threshold_(1e-8) {}

      void begin_update_p(typename Hamiltonian::PointType& z,
                          Hamiltonian& hamiltonian,
                          double epsilon,
                          callbacks::logger& logger) {
        hat_phi(z, hamiltonian, epsilon, logger);
        hat_tau(z, hamiltonian, epsilon, this->max_num_fixed_point_,
                logger);
      }

      void update_q(typename Hamiltonian::PointType& z,
                    Hamiltonian& hamiltonian,
                    double epsilon,
                    callbacks::logger& logger) {
        // hat{T} = dT/dp * d/dq
        Eigen::VectorXd q_init = z.q + 0.5 * epsilon * hamiltonian.dtau_dp(z);
        Eigen::VectorXd delta_q(z.q.size());

        for (int n = 0; n < this->max_num_fixed_point_; ++n) {
          delta_q = z.q;
          z.q.noalias() = q_init + 0.5 * epsilon * hamiltonian.dtau_dp(z);
          hamiltonian.update_metric(z, logger);

          delta_q -= z.q;
          if (delta_q.cwiseAbs().maxCoeff() < this->fixed_point_threshold_)
            break;
        }
        hamiltonian.update_gradients(z, logger);
      }

      void end_update_p(typename Hamiltonian::PointType& z,
                        Hamiltonian& hamiltonian,
                        double epsilon,
                        callbacks::logger& logger) {
        hat_tau(z, hamiltonian, epsilon, 1, logger);
        hat_phi(z, hamiltonian, epsilon, logger);
      }

      // hat{phi} = dphi/dq * d/dp
      void hat_phi(typename Hamiltonian::PointType& z,
                   Hamiltonian& hamiltonian,
                   double epsilon,
                   callbacks::logger& logger) {
        z.p -= epsilon * hamiltonian.dphi_dq(z, logger);
      }

      // hat{tau} = dtau/dq * d/dp
      void hat_tau(typename Hamiltonian::PointType& z,
                   Hamiltonian& hamiltonian,
                   double epsilon,
                   int num_fixed_point,
                   callbacks::logger& logger) {
        Eigen::VectorXd p_init = z.p;
        Eigen::VectorXd delta_p(z.p.size());

        for (int n = 0; n < num_fixed_point; ++n) {
          delta_p = z.p;
          z.p.noalias() = p_init - epsilon * hamiltonian.dtau_dq(z, logger);
          delta_p -= z.p;
          if (delta_p.cwiseAbs().maxCoeff() < this->fixed_point_threshold_)
            break;
        }
      }

      int max_num_fixed_point() {
        return this->max_num_fixed_point_;
      }

      void set_max_num_fixed_point(int n) {
        if (n > 0) this->max_num_fixed_point_ = n;
      }

      double fixed_point_threshold() {
        return this->fixed_point_threshold_;
      }

      void set_fixed_point_threshold(double t) {
        if (t > 0) this->fixed_point_threshold_ = t;
      }

    private:
      int max_num_fixed_point_;
      double fixed_point_threshold_;
    };

  }  // mcmc
}  // stan

#endif


#pragma once

#include "Common.hpp"

#include <boost/serialization/utility.hpp>
#include <boost/multiprecision/miller_rabin.hpp>

#include "LatticeEncryption.hpp"
#include "Math.hpp"


/*=========================================================================
 *
 * Copyright Universitat Pompeu Fabra, Department of Information and
 * Comunication Technologies.
 *
 *  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.txt
 *
 *  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.
 *
 *=========================================================================*/

#ifndef itkCudaRWSegmentationFilter_hxx
#define itkCudaRWSegmentationFilter_hxx

#include <itkImageRegionIterator.h>
#include <itkImageRegionConstIterator.h>

#include <vector>
#include <Eigen/Dense>
#include <Eigen/Sparse>

#define CLEANUP(s)     \
  do                   \
  {                    \
    printf("%s\n", s); \
    fflush(stdout);    \
  } while (0)

namespace itk
{
template <typename TInputImage, typename TOutputImage>
void CudaRWSegmentationFilter<TInputImage, TOutputImage>::GenerateData()
{
  if (m_LabelImage == nullptr) // Exit if SetLabelImage has not been called
  {
    std::cout << "Label image has not been set" << std::endl;
    return;
  }
  if (OutputImageType::ImageDimension != 2 && OutputImageType::ImageDimension != 3)
  {
    std::cout << "Exit segmentation. Image dimension must be 2 or 3 but is " << OutputImageType::ImageDimension << std::endl;
    return;
  }

  // Get regions to iterate for original and label images
  typename OutputImageType::RegionType regionLabel = m_LabelImage->GetLargestPossibleRegion();

  // Set label image iterator to get the bounding box size and the quantity of marked nodes
  typedef itk::ImageRegionIterator<OutputImageType> IteratorLabelType;
  IteratorLabelType itLabel(m_LabelImage, regionLabel);

  // Crop image to bounds containin labels
  typename OutputImageType::RegionType regionLabelCrop;
  typename InputImageType::RegionType regionCrop;

  for (int i = 0; i != OutputImageType::ImageDimension; ++i)
  {
    regionLabelCrop.SetSize(i, regionLabel.GetIndex()[i]);
    regionLabelCrop.SetIndex(i, regionLabel.GetSize()[i]);
  }

  int markedLength = 0;

  itLabel.GoToBegin();
  while (!itLabel.IsAtEnd())
  {
    // Get image boundaries to crop image
    if (itLabel.Get() != 0)
    {
      ++markedLength;
      // Get bounding region
      for (int i = 0; i != OutputImageType::ImageDimension; ++i)
      {
        if (itLabel.GetIndex()[i] < regionLabelCrop.GetIndex()[i])
        {
          regionCrop.SetIndex(i, itLabel.GetIndex()[i]);
          regionLabelCrop.SetIndex(i, itLabel.GetIndex()[i]);
        }
        else if (itLabel.GetIndex()[i] > regionLabelCrop.GetSize()[i])
        {
          regionCrop.SetSize(i, itLabel.GetIndex()[i]);
          regionLabelCrop.SetSize(i, itLabel.GetIndex()[i]);
        }
      }
    }
    ++itLabel;
  }

  int totalNodes = 1;
  for (int i = 0; i != OutputImageType::ImageDimension; ++i)
  {
    regionCrop.SetIndex(i, regionCrop.GetIndex()[i]);
    regionLabelCrop.SetIndex(i, regionLabelCrop.GetIndex()[i]);
    regionCrop.SetSize(i, regionCrop.GetSize()[i] - regionCrop.GetIndex()[i] + 1);
    regionLabelCrop.SetSize(i, regionLabelCrop.GetSize()[i] - regionLabelCrop.GetIndex()[i] + 1);

    totalNodes *= regionCrop.GetSize()[i];
  }

  /////////////////////// Build Graph /////////////////////////////
  /*
    // Create graph. Each node will correspond to a pixel connected to its neighbors by an edge.
    // Neighbors are those nodes which distance is 1 ( d = sqrt((x-xi)² + (y-yi)² + (z-zi)²) = 1 ),
    // where x, y, and z are the indices of a pixel and xi, yi, and zi are the indeces of a neighboring pixel
    // Iterate through all pixels of input and label images
    */

  int unmarkedLength = totalNodes - markedLength; /* Quantity of unmarked nodes */

  // Define vectors to store graph data
  std::vector<float> *nodes = new std::vector<float>(totalNodes);  /* Pixel intensity for all nodes/pixels */
  std::vector<float> *labels = new std::vector<float>(totalNodes); /* Label of each node/pixel */
  /*  For node 'i'. If 'i' is a marked node, previousFound->at(i) is how many unmarked nodes there are before node 'i'.
        If 'i' is an unmarked node, previousFound->at(i) is how many marked nodes there are before node 'i'.
        This values are needed to build ordered Lu and BT matrices.
    */
  std::vector<int> *previousFound = new std::vector<int>(totalNodes);
  std::vector<int> *unmarked = new std::vector<int>(unmarkedLength); /* Indices of unmarked nodes ordered */
  std::vector<int> *marked = new std::vector<int>(markedLength);     /* Store labels of marked nodes */
  std::vector<int> *markedIdx = new std::vector<int>(markedLength);  /* Indices of marked nodes ordered */
  std::vector<int> *nameLabels = new std::vector<int>();             /* Values of the different labels of the prior */

  // Set bounding box image iterators
  typedef itk::ImageRegionConstIterator<InputImageType> ConstIteratorImageType;
  ConstIteratorImageType itImageCrop(this->GetInput(), regionCrop);
  IteratorLabelType itLabelCrop(m_LabelImage, regionLabelCrop);

  int foundMarked = 0;
  int foundUnmarked = 0;
  int NodeIdx = 0;

  itImageCrop.GoToBegin();
  itLabelCrop.GoToBegin();
  while (!itImageCrop.IsAtEnd())
  {
    // Store intensity in a std::vector
    nodes->at(NodeIdx) = itImageCrop.Get();
    // Store labels in a std::vector. Each index of 'nodes' and 'labels' correspond to the same pixel
    labels->at(NodeIdx) = itLabelCrop.Get();

    if (itLabelCrop.Get() == 0)
    {
      // Get the index of each node that is unmarked
      unmarked->at(foundUnmarked) = NodeIdx;
      // Store how many marked points have been found before this unmarked node
      previousFound->at(NodeIdx) = foundMarked;
      ++foundUnmarked;
    }
    else
    {
      // Set to label value if label != 0
      marked->at(foundMarked) = itLabelCrop.Get();
      // Get the index of each node that is marked
      markedIdx->at(foundMarked) = NodeIdx;
      // Store how many unmarked points have been found before this marked node
      previousFound->at(NodeIdx) = foundUnmarked;
      ++foundMarked;
      bool found;

      // Store the different labels in a std::vector
      found = std::find(nameLabels->begin(), nameLabels->end(), itLabelCrop.Get()) != nameLabels->end();
      if (!found)
      {
        nameLabels->push_back(itLabelCrop.Get());
      }
    }
    ++itImageCrop;
    ++itLabelCrop;
    ++NodeIdx;
  }

  // Sort labels
  std::sort(nameLabels->begin(), nameLabels->end());

  int totalLabels = nameLabels->size();
  if (m_SolveForAllLabels)
  {
    totalLabels += 1;
  }
  // Linear system: Lu * X = -BT * M
  // Convert marked (M) into a Eigen::Sparse matrix markedRHS. Needed for the computation of -BT * M (Eigen::SparseMatrix * Eigen::SparseMatrix)
  Eigen::SparseMatrix<float, Eigen::ColMajor> *markedRHS = new Eigen::SparseMatrix<float, Eigen::ColMajor>(markedLength, totalLabels - 1);
  for (int i = 0; i != totalLabels - 1; ++i)
  {
    int pos = 0;
    for (auto itMarked = marked->begin(); itMarked != marked->end(); ++itMarked)
    {
      if (*itMarked == nameLabels->at(i))
        markedRHS->insert(pos, i) = 1;
      ++pos;
    }
  }

  marked->clear();
  delete marked;

  /////////////////////// Build Laplacian matrix /////////////////////////////
  // Normalize intensity gradient over image spacing
  std::vector<float> spacing;
  typename InputImageType::SpacingType space = this->GetInput()->GetSpacing();
  if (OutputImageType::ImageDimension == 2)
    spacing = {space[1], space[0], space[0], space[1]};
  else if (OutputImageType::ImageDimension == 3)
    spacing = {space[2], space[1], space[0], space[0], space[1], space[2]};

  std::vector<int> neighbors;
  int x = regionCrop.GetSize()[0];
  int y = regionCrop.GetSize()[1];
  //  Right hand of the equation: -BT * M
  Eigen::SparseMatrix<float, Eigen::ColMajor> *BTxM = new Eigen::SparseMatrix<float, Eigen::ColMajor>(markedLength, totalLabels - 1);

  // Build BT
  // Compare whether NumCols < NumRows for less iterations during BT building
  if (markedIdx->size() < unmarked->size())
  {
    Eigen::SparseMatrix<float, Eigen::ColMajor> *BT = new Eigen::SparseMatrix<float, Eigen::ColMajor>(unmarkedLength, markedLength);
    BT->reserve(Eigen::VectorXi::Constant(markedLength, 6));

    int node;
    float valNode, valNeighbor, w;

    // Iterate through marked nodes to build BT. Rows correspond to unmarked nodes, columns to marked nodes
    for (auto itMarked = markedIdx->begin(); itMarked != markedIdx->end(); ++itMarked)
    {
      valNode = nodes->at(*itMarked); // Intensity of node
      // Obtain neighbors indexes. right and left, top and bottom, front and back.
      node = *itMarked;
      if (OutputImageType::ImageDimension == 2)
        neighbors = {node - x, node - 1, node + 1, node + x};
      else if (OutputImageType::ImageDimension == 3)
        neighbors = {node - x * y, node - x, node - 1, node + 1, node + x, node + x * y};
      for (int i = 0; i != neighbors.size(); ++i)
      {
        // Make sure all the neighbors computed fall within the bounding box dimension
        if (neighbors.at(i) >= 0 && neighbors.at(i) < totalNodes && labels->at(neighbors.at(i)) == 0)
        {
          valNeighbor = nodes->at(neighbors.at(i));                                    // Intensity of neighbor pixel
          w = (exp(-m_Beta * pow((valNode - valNeighbor) / spacing.at(i), 2)) + 1e-6); // Intensity gradient following a Gaussian function
          //  Columns of BT correspond to marked nodes, rows to unmarked
          BT->insert(neighbors.at(i) - previousFound->at(neighbors.at(i)), node - previousFound->at(node)) = -w;
        }
      }
    }
    markedIdx->clear();
    //  Right hand of the equation: -BT * M
    *BTxM = -*BT * *markedRHS;
    BT->resize(0, 0);
    BT->data().squeeze();
    markedRHS->resize(0, 0);
    markedRHS->data().squeeze();
    delete markedIdx, markedRHS, BT;
  }
  else
  {
    markedIdx->clear();
    delete markedIdx;

    Eigen::SparseMatrix<float, Eigen::RowMajor> *BT = new Eigen::SparseMatrix<float, Eigen::RowMajor>(unmarkedLength, markedLength);
    BT->reserve(Eigen::VectorXi::Constant(markedLength, 6));

    int node;
    float valNode, valNeighbor, w;

    // Iterate through unmarked nodes to build BT. Rows correspond to unmarked nodes, columns to marked nodes
    for (auto itUnmarked = unmarked->begin(); itUnmarked != unmarked->end(); ++itUnmarked)
    {
      valNode = nodes->at(*itUnmarked); // Intensity of node
      // Obtain neighbors indexes. right and left, top and bottom, front and back.
      node = *itUnmarked;
      if (OutputImageType::ImageDimension == 2)
        neighbors = {node - x, node - 1, node + 1, node + x};
      else if (OutputImageType::ImageDimension == 3)
        neighbors = {node - x * y, node - x, node - 1, node + 1, node + x, node + x * y};
      for (int i = 0; i != neighbors.size(); ++i)
      {
        // Make sure all the neighbors computed fall within the bounding box dimension
        if (neighbors.at(i) >= 0 && neighbors.at(i) < totalNodes && labels->at(neighbors.at(i)) != 0)
        {
          valNeighbor = nodes->at(neighbors.at(i));                                    // Intensity of neighbor pixel
          w = (exp(-m_Beta * pow((valNode - valNeighbor) / spacing.at(i), 2)) + 1e-6); // Intensity gradient following a Gaussian function
          //  Columns of BT correspond to marked nodes, rows to unmarked
          BT->insert(node - previousFound->at(node), neighbors.at(i) - previousFound->at(neighbors.at(i))) = -w;
        }
      }
    }
    //  Right hand of the equation: -BT * M
    *BTxM = -*BT * *markedRHS;
    BT->resize(0, 0);
    BT->data().squeeze();
    markedRHS->resize(0, 0);
    markedRHS->data().squeeze();
    delete markedRHS, BT;
  }

  int nnz = 0;

  // Build Lu. LHS of the equation
  Eigen::SparseMatrix<float, Eigen::RowMajor> *Lu = new Eigen::SparseMatrix<float, Eigen::RowMajor>(unmarkedLength, unmarkedLength);
  Lu->reserve(Eigen::VectorXi::Constant(unmarkedLength, 7));

  // Iterate through unmarked nodes to build Lu. Rows correspond to unmarked nodes, columns to unmarked nodes
  for (auto itUnmarked = unmarked->begin(); itUnmarked != unmarked->end(); ++itUnmarked)
  {
    int node;
    float valNode, valNeighbor, w, degree;

    valNode = nodes->at(*itUnmarked); // Intensity of node
    // Obtain neighbors indexes. right and left, top and bottom, front and back.
    node = *itUnmarked;
    if (OutputImageType::ImageDimension == 2)
      neighbors = {node - x, node - 1, node + 1, node + x};
    else if (OutputImageType::ImageDimension == 3)
      neighbors = {node - x * y, node - x, node - 1, node + 1, node + x, node + x * y};
    degree = 0.0;
    for (int i = 0; i != neighbors.size(); ++i)
    {
      // Make sure all the neighbors computed fall within the bounding box dimension
      if (neighbors.at(i) >= 0 && neighbors.at(i) < totalNodes)
      {
        valNeighbor = nodes->at(neighbors.at(i));                                    // Intensity of neighbor pixel
        w = (exp(-m_Beta * pow((valNode - valNeighbor) / spacing.at(i), 2)) + 1e-6); // Intensity gradient following a Gaussian function
        degree += w;                                                                 // Sum of the weights
        //  Columns of Lu correspond to unmarked nodes
        if (labels->at(neighbors.at(i)) == 0) // If neighbor is an unmarked node, build Lu
        {
          Lu->insert(node - previousFound->at(node), neighbors.at(i) - previousFound->at(neighbors.at(i))) = -w;
          ++nnz;
        }
      }
    }
    // Add node degree to diagonal of Lu
    Lu->insert(node - previousFound->at(node), node - previousFound->at(node)) = degree;
    ++nnz;
  }
  Lu->makeCompressed();

  Eigen::SparseMatrix<float, Eigen::RowMajor> *Lu_PreconditionerGpu = new Eigen::SparseMatrix<float, Eigen::RowMajor>(unmarkedLength, unmarkedLength);
  Lu_PreconditionerGpu->reserve(Eigen::VectorXi::Constant(unmarkedLength, 1));
  for (int i = 0; i != unmarkedLength; ++i)
    Lu_PreconditionerGpu->insert(i, i) = 1 / Lu->coeffRef(i, i);
  Lu_PreconditionerGpu->makeCompressed();

  nodes->clear();
  unmarked->clear();
  previousFound->clear();
  neighbors.clear();
  delete nodes, unmarked, previousFound;

  // /////////////////////// Solve linear system /////////////////////////////
  // Convert BTxM into a float array for bicgstab cuda solver
  float *bhost_all_labels = new float[unmarkedLength * (totalLabels - 1)];

  for (int i = 0; i != unmarkedLength * (totalLabels - 1); ++i)
    bhost_all_labels[i] = 0;

  for (int k = 0; k < BTxM->outerSize(); ++k)
  {
    for (Eigen::SparseMatrix<float, Eigen::ColMajor>::InnerIterator itMat(*BTxM, k); itMat; ++itMat)
    {
      bhost_all_labels[itMat.row() + k * unmarkedLength] = itMat.value();
    }
  }

  // Get pointers for CSR matrices Lu and Lu_PreconditionerGpu
  cooValAhost = Lu->valuePtr();
  csrColPtrAhost = Lu->outerIndexPtr();
  cooRowPtrAhost = Lu->innerIndexPtr();

  cooValAhostM = Lu_PreconditionerGpu->valuePtr();
  csrColPtrAhostM = Lu_PreconditionerGpu->outerIndexPtr();
  cooRowPtrAhostM = Lu_PreconditionerGpu->innerIndexPtr();

  int bicgstab_exit_status, cuda_mem_cpy_exit_status, cuda_mem_free_exit_status;

  // Result vector
  probabilities = new float[unmarkedLength * (totalLabels - 1)];
  probabilitiesInner = new float[unmarkedLength];

  // Allocate GPU memory
  cuda_mem_cpy_exit_status = this->AllocGPUMemory(unmarkedLength, nnz);
  if (cuda_mem_cpy_exit_status != 0)
  {
    std::cout << "GPU memory allocation failed with error " << cuda_mem_cpy_exit_status << std::endl;
    return;
  }

  // Solve S-1 linear systems
  for (int linearSystem = 0; linearSystem != totalLabels - 1; ++linearSystem)
  {
    // Get pinter for each linear system RHS
    bhost = &bhost_all_labels[linearSystem * unmarkedLength];
    // Call CUDA BiCGStab solver
    bicgstab_exit_status = this->BiCGStab(unmarkedLength, nnz);
    if (bicgstab_exit_status != 0)
    {
      std::cout << "BiCStab failed with error" << bicgstab_exit_status << std::endl;
      return;
    }

    for (int i = 0; i != unmarkedLength; ++i)
      probabilities[i + linearSystem * unmarkedLength] = probabilitiesInner[i];
  }

  // Free GPU memeory when all linear systems have been solved
  cuda_mem_free_exit_status = this->FreeGPUMemory();
  if (cuda_mem_free_exit_status != 0)
  {
    std::cout << "GPU memory free failed with error" << cuda_mem_free_exit_status << std::endl;
  }
  Lu->resize(0, 0);
  Lu->data().squeeze();
  Lu_PreconditionerGpu->resize(0, 0);
  Lu_PreconditionerGpu->data().squeeze();
  BTxM->resize(0, 0);
  BTxM->data().squeeze();
  delete Lu, Lu_PreconditionerGpu, BTxM;
  delete[] probabilitiesInner, bhost, bhost_all_labels, cooRowPtrAhost, csrColPtrAhost, cooValAhost, cooRowPtrAhostM, csrColPtrAhostM, cooValAhostM;

  std::vector<int> *RWLabels = new std::vector<int>(unmarkedLength);

  /*  Assign a label to each unmarked node according to the result of the solver.
      The label that is assigned is that one corresponding to the highest probability.
      Since we solver for S-1 systems, last label probability is computed by subtraction */
  for (int i = 0; i != unmarkedLength; ++i)
  {
    float maxProbability = probabilities[i];
    int maxLabelPos = 0;
    float accumulatedProbability = maxProbability;
    for (int j = 1; j != totalLabels - 1; ++j)
    {
      accumulatedProbability += probabilities[i + j * unmarkedLength];
      if (probabilities[i + j * unmarkedLength] > maxProbability)
      {
        maxProbability = probabilities[i + j * unmarkedLength];
        maxLabelPos = j;
      }
    }
    RWLabels->at(i) = 0.95 - accumulatedProbability > maxProbability ? nameLabels->back() : nameLabels->at(maxLabelPos);
  }

  delete[] probabilities;

  int valBackground;
  if (!m_WriteBackground)
    valBackground = 0;
  else
    valBackground = nameLabels->back();

  typename OutputImageType::Pointer outputLabels = this->GetOutput();

  outputLabels->Graft(m_LabelImage);
  outputLabels->FillBuffer(valBackground);

  // Iterate through original label image to create segmentation image
  // Assign labels known from label image to their original value. Assign unmarked labels
  // according to the result from the solver

  IteratorLabelType itOut1(outputLabels, regionLabelCrop);

  // Set computed labels to segmentation image
  int unmarkedIdx = 0;
  int idxOutput = 0;

  itOut1.GoToBegin();
  while (!itOut1.IsAtEnd())
  {
    if (labels->at(idxOutput) == 0)
    {
      if (RWLabels->at(unmarkedIdx) != nameLabels->back())
        itOut1.Set(RWLabels->at(unmarkedIdx));
      else
        itOut1.Set(valBackground);
      ++unmarkedIdx;
    }
    else if (labels->at(idxOutput) != nameLabels->back())
      itOut1.Set(labels->at(idxOutput));

    ++idxOutput;
    ++itOut1;
  }

  labels->clear();
  RWLabels->clear();
  nameLabels->clear();
  delete labels, RWLabels, nameLabels;

  return;
}

template <typename TInputImage, typename TOutputImage>
int CudaRWSegmentationFilter<TInputImage, TOutputImage>::BiCGStab(int M, int nnz)
{

  int iter, flag;

  cusparseStatus_t status;
  cusparseHandle_t handle = 0;
  cusparseMatDescr_t descra = 0;

  int nnzM = M;
  int N = M;
  int N2 = N;

  float timesOne[1] = {1.0};
  float timesZero[1] = {0.0};
  float timesMinusOne[1] = {-1.0};

  cudaStat4 = cudaMemcpy(r, bhost,
                         (size_t)(N * sizeof(r[0])),
                         cudaMemcpyHostToDevice);

  cudaStat5 = cudaMemcpy(xdev, xhost,
                         (size_t)(N * sizeof(xdev[0])),
                         cudaMemcpyHostToDevice);

  // Copy right hand side to GPU memory
  if ((cudaStat4 != cudaSuccess) ||
      (cudaStat5 != cudaSuccess))
  {
    CLEANUP("Memcpy from Host to Device failed");
    return EXIT_FAILURE;
  }

  /* initialize cusparse library */
  status = cusparseCreate(&handle);
  if (status != CUSPARSE_STATUS_SUCCESS)
  {
    CLEANUP("CUSPARSE Library initialization failed");
    return EXIT_FAILURE;
  }
  /* create and setup matrix descriptor */
  status = cusparseCreateMatDescr(&descra);
  if (status != CUSPARSE_STATUS_SUCCESS)
  {
    CLEANUP("Matrix descriptor initialization failed");
    return EXIT_FAILURE;
  }
  cusparseSetMatType(descra, CUSPARSE_MATRIX_TYPE_GENERAL);
  cusparseSetMatIndexBase(descra, CUSPARSE_INDEX_BASE_ZERO);

  bnrm2 = cublasSnrm2(N, r, 1);

  float tol = m_Tolerance * bnrm2;

  status = cusparseScsrmv(handle, CUSPARSE_OPERATION_NON_TRANSPOSE, N, N, nnz, timesMinusOne, descra, cooValAdev, csrColPtrAdev, cooRowPtrAdev, xdev, timesOne, r); /* r = r - A*x; r is now residual */

  if (status != CUSPARSE_STATUS_SUCCESS)
  {
    CLEANUP("Matrix‐vector multiplication failed");
    return EXIT_FAILURE;
  }

  error = cublasSnrm2(N, r, 1) / bnrm2; /* norm_r = norm(b) */
  if (error < tol)
  {
    CLEANUP("Error smaller than tolerance failed");
    return EXIT_FAILURE; /* x is close enough already */
  }

  omega = 1.0;

  cudaStat1 = cudaMemcpy(r_tld, r, (size_t)(N * sizeof(r[0])),
                         cudaMemcpyDeviceToDevice); /* r_tld = r */

  if ((cudaStat1 != cudaSuccess))
  {
    CLEANUP("Memcpy from r to r_tld failed");
    return EXIT_FAILURE;
  }

  // Loop of the BiCGStab solver
  for (iter = 0; iter < m_MaximumNumberOfIterations; ++iter)
  {
    rho = cublasSdot(N, r_tld, 1, r, 1); /* rho = r_tld'*r */

    if (rho == 0.0)
      break;

    if (iter > 0)
    {
      beta = (rho / rho_1) * (alpha / omega);
      cublasSaxpy(N, -omega, v, 1, p, 1);
      cublasSaxpy(N, 1.0 / beta, r, 1, p, 1);
      cublasSscal(N, beta, p, 1); /* p = r + beta*( p - omega*v ) */
    }
    else
    {
      cudaStat1 = cudaMemcpy(p, r,
                             (size_t)(N * sizeof(r[0])),
                             cudaMemcpyDeviceToDevice); /* p = r */
      if ((cudaStat1 != cudaSuccess))
      {
        CLEANUP("Memcpy from r to p failed");
        return EXIT_FAILURE;
      }
    }

    status = cusparseScsrmv(handle, CUSPARSE_OPERATION_NON_TRANSPOSE, N2, N2, nnzM, timesOne, descra, cooValAdevM, csrColPtrAdevM, cooRowPtrAdevM, p, timesZero, p_hat);

    status = cusparseScsrmv(handle, CUSPARSE_OPERATION_NON_TRANSPOSE, N, N, nnz, timesOne, descra, cooValAdev, csrColPtrAdev, cooRowPtrAdev, p_hat, timesZero, v); /* v = A*p_hat */

    alpha = rho / cublasSdot(N, r_tld, 1, v, 1); /* alph = rho / ( r_tld'*v ) */

    cudaStat1 = cudaMemcpy(s, r, (size_t)(N * sizeof(r[0])),
                           cudaMemcpyDeviceToDevice); /* s = r */
    if ((cudaStat1 != cudaSuccess))
    {
      CLEANUP("Memcpy from r to s failed");
      return EXIT_FAILURE;
    }
    cublasSaxpy(N, -alpha, v, 1, s, 1);
    snrm2 = cublasSnrm2(N, s, 1);

    cublasSaxpy(N, alpha, p_hat, 1, xdev, 1); /*  h = x + alph*p_hat */

    if (snrm2 < tol)
    {
      // cublasSaxpy(N, alpha, p_hat, 1, s, 1);
      resid = snrm2 / bnrm2;

      cudaStat5 = cudaMemcpy(probabilitiesInner, xdev,
                             (size_t)(N * sizeof(probabilitiesInner[0])),
                             cudaMemcpyDeviceToHost); /* x = h */
      if ((cudaStat5 != cudaSuccess))
      {
        CLEANUP("Memcpy from x to xhost failed");
        return EXIT_FAILURE;
      }

      break;
    }

    status = cusparseScsrmv(handle, CUSPARSE_OPERATION_NON_TRANSPOSE, N2, N2, nnzM, timesOne, descra, cooValAdevM, csrColPtrAdevM, cooRowPtrAdevM, s, timesZero, s_hat);

    status = cusparseScsrmv(handle, CUSPARSE_OPERATION_NON_TRANSPOSE, N, N, nnz, timesOne, descra, cooValAdev, csrColPtrAdev, cooRowPtrAdev, s_hat, timesZero, t); /* t = A*s_hat */

    omega = cublasSdot(N, t, 1, s, 1) / cublasSdot(N, t, 1, t, 1); /* omega = ( t'*s) / ( t'*t ) */

    cublasSaxpy(N, omega, s_hat, 1, xdev, 1); /*  x = h + omega*s_hat */

    cudaStat1 = cudaMemcpy(r, s, (size_t)(N * sizeof(r[0])),
                           cudaMemcpyDeviceToDevice); /* r = s */
    if ((cudaStat1 != cudaSuccess))
    {
      CLEANUP("Memcpy from s to r failed");
      return EXIT_FAILURE;
    }

    cublasSaxpy(N, -omega, t, 1, r, 1); /* r = s - omega*t */
    snrm2 = cublasSnrm2(N, r, 1);

    if (snrm2 <= tol)
    {
      resid = snrm2 / bnrm2;

      cudaStat5 = cudaMemcpy(probabilitiesInner, xdev,
                             (size_t)(N * sizeof(probabilitiesInner[0])),
                             cudaMemcpyDeviceToHost); /* x = x */
      if ((cudaStat5 != cudaSuccess))
      {
        CLEANUP("Memcpy from x to xhost failed");
        return EXIT_FAILURE;
      }
      break;
    }
    if (omega == 0.0)
      break;
    rho_1 = rho;
  }
  ++iter;

  error = snrm2 / bnrm2;

  if (error <= tol)
  {
    flag = 0;
  }
  else if (omega == 0.0)
  {
    flag = -2;
  }
  else if (rho == 0.0)
  {
    flag = -1;
  }
  else
    flag = 1;

  m_SolverIterations += iter;
  m_SolverError = std::min(m_SolverError, error);

  return flag;
}

// Allocate GPU memory and copy data to device
template <typename TInputImage, typename TOutputImage>
int CudaRWSegmentationFilter<TInputImage, TOutputImage>::AllocGPUMemory(int M, int nnz)
{

  int nnzM = M; // Lu has size MxM, Lu_PreconditionerGpu has size MxM
  int N = M;

  // Define initial guess
  xhost = new float[N];
  for (int i = 0; i < N; i++)
  {
    xhost[i] = 0;
  }

  cudaStat1 = cudaMalloc((void **)&cooRowPtrAdev, nnz * sizeof(cooRowPtrAdev[0]));
  cudaStat3 = cudaMalloc((void **)&cooValAdev, nnz * sizeof(cooValAdev[0]));
  cudaStat4 = cudaMalloc((void **)&r, N * sizeof(r[0]));
  cudaStat5 = cudaMalloc((void **)&s, N * sizeof(s[0]));
  cudaStat6 = cudaMalloc((void **)&t, N * sizeof(t[0]));
  cudaStat7 = cudaMalloc((void **)&r_tld, N * sizeof(r_tld[0]));
  cudaStat8 = cudaMalloc((void **)&p, N * sizeof(p[0]));
  cudaStat9 = cudaMalloc((void **)&p_hat, N * sizeof(p_hat[0]));
  cudaStat10 = cudaMalloc((void **)&s_hat, N * sizeof(s_hat[0]));
  cudaStat11 = cudaMalloc((void **)&v, N * sizeof(v[0]));
  cudaStat12 = cudaMalloc((void **)&xdev, N * sizeof(xdev[0]));

  cudaStat13 = cudaMalloc((void **)&csrColPtrAdev, (N + 1) * sizeof(csrColPtrAdev[0]));

  cudaStat14 = cudaMalloc((void **)&cooRowPtrAdevM, nnzM * sizeof(cooRowPtrAdevM[0]));
  cudaStat16 = cudaMalloc((void **)&cooValAdevM, nnzM * sizeof(cooValAdevM[0]));
  cudaStat17 = cudaMalloc((void **)&csrColPtrAdevM, (N + 1) * sizeof(csrColPtrAdevM[0]));

  if ((cudaStat1 != cudaSuccess) ||
      (cudaStat3 != cudaSuccess) ||
      (cudaStat4 != cudaSuccess) ||
      (cudaStat5 != cudaSuccess) ||
      (cudaStat6 != cudaSuccess) ||
      (cudaStat7 != cudaSuccess) ||
      (cudaStat8 != cudaSuccess) ||
      (cudaStat9 != cudaSuccess) ||
      (cudaStat10 != cudaSuccess) ||
      (cudaStat11 != cudaSuccess) ||
      (cudaStat12 != cudaSuccess) ||

      (cudaStat14 != cudaSuccess) ||
      (cudaStat16 != cudaSuccess) ||
      (cudaStat17 != cudaSuccess))
  {
    CLEANUP("Device malloc failed");
    return EXIT_FAILURE;
  }

  cudaStat1 = cudaMemcpy(cooRowPtrAdev, cooRowPtrAhost,
                         (size_t)(nnz * sizeof(cooRowPtrAdev[0])),
                         cudaMemcpyHostToDevice);
  cudaStat13 = cudaMemcpy(csrColPtrAdev, csrColPtrAhost,
                          (size_t)((N + 1) * sizeof(csrColPtrAdev[0])),
                          cudaMemcpyHostToDevice);
  cudaStat3 = cudaMemcpy(cooValAdev, cooValAhost,
                         (size_t)(nnz * sizeof(cooValAdev[0])),
                         cudaMemcpyHostToDevice);

  cudaStat14 = cudaMemcpy(cooRowPtrAdevM, cooRowPtrAhostM,
                          (size_t)(nnzM * sizeof(cooRowPtrAdevM[0])),
                          cudaMemcpyHostToDevice);
  cudaStat17 = cudaMemcpy(csrColPtrAdevM, csrColPtrAhostM,
                          (size_t)((N + 1) * sizeof(cooColPtrAdevM[0])),
                          cudaMemcpyHostToDevice);
  cudaStat16 = cudaMemcpy(cooValAdevM, cooValAhostM,
                          (size_t)(nnzM * sizeof(cooValAdevM[0])),
                          cudaMemcpyHostToDevice);

  if ((cudaStat1 != cudaSuccess) ||
      (cudaStat13 != cudaSuccess) ||
      (cudaStat3 != cudaSuccess) ||
      (cudaStat14 != cudaSuccess) ||
      (cudaStat17 != cudaSuccess) ||
      (cudaStat16 != cudaSuccess))
  {
    CLEANUP("Memcpy from Host to Device failed");
    return EXIT_FAILURE;
  }

  return 0;
}

// Free GPU memeory when all linear systems have been solved
template <typename TInputImage, typename TOutputImage>
int CudaRWSegmentationFilter<TInputImage, TOutputImage>::FreeGPUMemory()
{
  /* shutdown CUBLAS */
  cublas_status = cublasShutdown();
  if (cublas_status != CUBLAS_STATUS_SUCCESS)
  {
    fprintf(stderr, "!!!! shutdown error (A)\n");
    return EXIT_FAILURE;
  }

  cudaStat1 = cudaFree(cooRowPtrAdev);
  cudaStat3 = cudaFree(cooValAdev);
  cudaStat4 = cudaFree(r);
  cudaStat5 = cudaFree(s);
  cudaStat6 = cudaFree(t);
  cudaStat7 = cudaFree(r_tld);
  cudaStat8 = cudaFree(p);
  cudaStat9 = cudaFree(p_hat);
  cudaStat10 = cudaFree(s_hat);
  cudaStat11 = cudaFree(v);
  cudaStat12 = cudaFree(xdev);

  cudaStat13 = cudaFree(csrColPtrAdev);

  cudaStat14 = cudaFree(cooRowPtrAdevM);
  cudaStat16 = cudaFree(cooValAdevM);
  cudaStat17 = cudaFree(csrColPtrAdevM);

  if ((cudaStat1 != cudaSuccess) ||
      (cudaStat3 != cudaSuccess) ||
      (cudaStat4 != cudaSuccess) ||
      (cudaStat5 != cudaSuccess) ||
      (cudaStat6 != cudaSuccess) ||
      (cudaStat7 != cudaSuccess) ||
      (cudaStat8 != cudaSuccess) ||
      (cudaStat9 != cudaSuccess) ||
      (cudaStat10 != cudaSuccess) ||
      (cudaStat11 != cudaSuccess) ||
      (cudaStat12 != cudaSuccess) ||

      (cudaStat14 != cudaSuccess) ||
      (cudaStat16 != cudaSuccess) ||
      (cudaStat17 != cudaSuccess))
  {
    CLEANUP("Device memory free failed");
    return EXIT_FAILURE;
  }

  return 0;
}

template <typename TInputImage, typename TOutputImage>
void CudaRWSegmentationFilter<TInputImage, TOutputImage>::PrintSelf(std::ostream &os, Indent indent) const
{
  Superclass::PrintSelf(os, indent);
  os << indent << "Beta: " << m_Beta << std::endl;
  os << indent << "Tolerance: " << m_Tolerance << std::endl;
  os << indent << "MaximumNumberOfIterations: " << m_MaximumNumberOfIterations << std::endl;
  os << indent << "WriteBackground: " << m_WriteBackground << std::endl;
}

} // namespace itk
#endif


/* 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 "esop_synthesis.hpp"

#include <numeric>

#include <boost/assign/std/set.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/range/irange.hpp>
#include <boost/range/iterator_range.hpp>

#include <core/functor.hpp>
#include <core/utils/range_utils.hpp>
#include <core/utils/timer.hpp>

#include <reversible/truth_table.hpp>
#include <reversible/functions/add_gates.hpp>
#include <reversible/functions/clear_circuit.hpp>
#include <reversible/io/read_pla.hpp>

#include <misc/vec/vecInt.h>

using namespace boost::assign;

namespace cirkit
{

typedef std::vector<std::pair<binary_truth_table::cube_type, binary_truth_table::cube_type> > cubes_type;

void no_reordering( std::vector<std::pair<binary_truth_table::cube_type, binary_truth_table::cube_type> >& cubes )
{
}

struct cube_cost_sum
{
  explicit cube_cost_sum( unsigned var, bool _abs ) : var( var ), _abs( _abs ) {}

  unsigned operator()( unsigned result, const cubes_type::value_type& cube ) const
  {
    return result + ( cube.first.at( var ) ? ( ( _abs || *cube.first.at( var ) ) ? 1 : -1 ) : 0 );
  }

private:
  unsigned var;
  bool _abs;
};

struct cube_is_negative
{
  explicit cube_is_negative( unsigned var ) : var( var ) {}

  bool operator()( const cubes_type::value_type& cube ) const
  {
    return !( cube.first.at( var ) && *cube.first.at( var ) );
  }

private:
  unsigned var;
};

struct cube_sort
{
  explicit cube_sort( unsigned var ) : var( var ) {}

  bool operator()( const cubes_type::value_type& cube1, const cubes_type::value_type& cube2 ) const
  {
    return !cube_is_negative( var )( cube1 ) && cube_is_negative( var)( cube2 );
  }

private:
  unsigned var;
};

weighted_reordering::weighted_reordering()
  : alpha( 0.5 ), beta( 0.5 ) {}

weighted_reordering::weighted_reordering( float alpha, float beta )
  : alpha( alpha ), beta( beta ) {}

void weighted_reordering::reorder( cubes_type::iterator begin, cubes_type::iterator end, const std::vector<unsigned>& vars ) const
{
  if ( begin == end || !vars.size() )
  {
    return;
  }

  // find best var
  std::vector<float> costs_by_var;
  for ( unsigned var : vars )
  {
    unsigned sum1 = std::accumulate( begin, end, 0u, cube_cost_sum( var, true ) );
    unsigned sum2 = std::accumulate( begin, end, 0u, cube_cost_sum( var, false ) );

    if ( sum1 == 0u )
    {
      costs_by_var += 0;
    }
    else
    {
      costs_by_var += alpha * ( 1 / sum1 ) + beta * sum2;
    }
  }

  // maximum
  unsigned max_var_index = std::max_element( costs_by_var.begin(), costs_by_var.end() ) - costs_by_var.begin();
  std::sort( begin, end, cube_sort( vars.at( max_var_index ) ) );
  cubes_type::iterator it = std::find_if( begin, end, cube_is_negative( vars.at( max_var_index ) ) );

  std::vector<unsigned> new_vars = vars;
  new_vars.erase( new_vars.begin() + max_var_index );

  reorder( begin, it, new_vars );
  reorder( it, end, new_vars );
}

void weighted_reordering::operator()( cubes_type& cubes ) const
{
  if ( !cubes.size() )
  {
    return;
  }

  std::vector<unsigned> vars( cubes.at( 0 ).first.size() );
  boost::copy( boost::irange( 0u, (unsigned)vars.size() ), vars.begin() );

  reorder( cubes.begin(), cubes.end(), vars );
}

bool esop_synthesis( circuit& circ, const std::string& filename, properties::ptr settings, properties::ptr statistics )
{

  // Settings parsing
  const auto separate_polarities    = get( settings, "separate_polarities", false );
  const auto negative_control_lines = get( settings, "negative_control_lines", true );
  const auto share_cube_on_target   = get( settings, "share_cube_on_target", true );
  const auto reordering             = get( settings, "reordering", cube_reordering_func( weighted_reordering() ) );
  const auto garbage_name           = get( settings, "garbage_name", std::string( "--" ) );

  if ( separate_polarities && negative_control_lines )
  {
    set_error_message( statistics, "Cannot separate polarities with negative control lines enabled." );
    return false;
  }

  // Run-time measuring
  properties_timer t( statistics );

  // parse ESOP file
  binary_truth_table spec;
  read_pla_settings rp_settings;
  rp_settings.extend = false;
  std::string error_msg;
  bool r = read_pla( spec, filename, rp_settings, &error_msg );

  if (!r)
  {
    set_error_message ( statistics, error_msg );
    return false;
  }

  // empty I/O
  if ( spec.inputs().empty() )
  {
    spec.set_inputs( create_name_list( "x%d", spec.num_inputs(), 1u ) );
  }
  if ( spec.outputs().empty() )
  {
    spec.set_outputs( create_name_list( "y%d", spec.num_outputs(), 1u ) );
  }

  // check for inputs and outputs
  if ( spec.inputs().size() != spec.num_inputs() || spec.outputs().size() != spec.num_outputs() )
  {
    set_error_message( statistics, "Input and Output names are not correctly specified in ESOP file." );
    return false;
  }

  // number of variables
  unsigned n = spec.num_inputs();
  clear_circuit( circ );

  if ( separate_polarities )
  {
    circ.set_lines( n * 2 + spec.num_outputs() );

    // metadata
    std::vector<std::string> inputs( circ.lines() );
    std::copy( spec.inputs().begin(), spec.inputs().end(), inputs.begin() );
    std::fill( inputs.begin() + n, inputs.begin() + 2 * n, "1" );
    std::fill( inputs.begin() + 2 * n, inputs.end(), "0" );

    std::vector<std::string> outputs( circ.lines(), garbage_name );
    std::vector<std::string> spec_outputs = spec.outputs();
    std::copy( spec_outputs.begin(), spec_outputs.end(), outputs.begin() + 2 * n );

    std::vector<constant> constants( circ.lines() );
    std::fill( constants.begin(), constants.begin() + n, constant() );
    std::fill( constants.begin() + n, constants.begin() + 2 * n, constant( true ) );
    std::fill( constants.begin() + 2 * n, constants.end(), constant( false ) );

    std::vector<bool> garbage( circ.lines(), false );
    std::fill( garbage.begin(), garbage.begin() + 2 * n, true );

    circ.set_inputs( inputs );
    circ.set_outputs( outputs );
    circ.set_constants( constants );
    circ.set_garbage( garbage );

    // apply inverter gates
    for ( unsigned i = 0u; i < n; ++i )
    {
      append_cnot( circ, make_var( i ), n + i );
    }

    // apply gates
    for ( binary_truth_table::const_iterator it = spec.begin(); it != spec.end(); ++it )
    {
      gate::control_container controls;

      // iterate through input cube (bit by bit)
      unsigned index = 0u;
      for ( const auto& in_bit : boost::make_iterator_range( it->first ) )
      {
        if ( in_bit )
        {
          controls += make_var( ( 1u - *in_bit ) * n + index ); // considers polarity to choose line
        }
        ++index;
      }

      // iterate through output cube (bit by bit)
      index = 0u;
      for ( const auto& out_bit : boost::make_iterator_range( it->second ) )
      {
        if ( out_bit && *out_bit )
        {
          append_toffoli( circ, controls, 2 * n + index );
        }
        ++index;
      }
    }
  }
  else
  {
    // smarter approach with reusing lines
    std::vector<std::pair<binary_truth_table::cube_type, binary_truth_table::cube_type> > cubes;
    for ( binary_truth_table::const_iterator it = spec.begin(); it != spec.end(); ++it )
    {
      cubes += std::make_pair( binary_truth_table::cube_type( it->first.first, it->first.second ),
                               binary_truth_table::cube_type( it->second.first, it->second.second ) );
    }

    // only reorder with positive control lines
    if ( !negative_control_lines )
    {
      reordering( cubes );
    }

    circ.set_lines( n + spec.num_outputs() );

    // metadata
    std::vector<std::string> inputs( circ.lines() );
    std::copy( spec.inputs().begin(), spec.inputs().end(), inputs.begin() );
    std::fill( inputs.begin() + n, inputs.end(), "0" );

    std::vector<std::string> outputs( circ.lines(), garbage_name );
    std::vector<std::string> spec_outputs = spec.outputs();
    std::copy( spec_outputs.begin(), spec_outputs.end(), outputs.begin() + n );

    std::vector<constant> constants( circ.lines() );
    std::fill( constants.begin(), constants.begin() + n, constant() );
    std::fill( constants.begin() + n, constants.end(), constant( false ) );

    std::vector<bool> garbage( circ.lines(), false );
    std::fill( garbage.begin(), garbage.begin() + n, true );

    circ.set_inputs( inputs );
    circ.set_outputs( outputs );
    circ.set_constants( constants );
    circ.set_garbage( garbage );

    // apply gates
    std::vector<bool> polarity( n, true );

    for ( std::vector<std::pair<binary_truth_table::cube_type, binary_truth_table::cube_type> >::const_iterator it = cubes.begin(); it != cubes.end(); ++it )
    {
      gate::control_container controls;

      // iterate through input cube (bit by bit)
      unsigned index = 0u;
      for ( const auto& in_bit : it->first )
      {
        if ( in_bit )
        {
          if ( negative_control_lines )
          {
            controls += make_var( index, *in_bit );
          }
          else
          {
            if ( polarity.at( index ) != *in_bit )
            {
              append_not( circ, index );
              polarity.at( index ) = *in_bit;
            }
            controls += make_var( index );
          }
        }
        ++index;
      }

      // iterate through output cube (bit by bit)
      if ( !share_cube_on_target )
      {
        index = 0u;
        for ( const auto& out_bit : it->second )
        {
          if ( out_bit && *out_bit )
          {
            append_toffoli( circ, controls, n + index );
          }
          ++index;
        }
      }
      else
      {
        index = 0u;
        int first = -1;
        auto pos = circ.num_gates();

        for ( const auto& out_bit : it->second )
        {
          if ( out_bit && *out_bit )
          {
            if ( first == -1 )
            {
              append_toffoli( circ, controls, n + index );
              first = n + index;
            }
            else
            {
              insert_cnot( circ, pos, make_var( first ), n + index );
              append_cnot( circ, make_var( first ), n + index );
            }
          }
          ++index;
        }
      }
    }
  }

  return true;
}

bool esop_synthesis( circuit& circ, const gia_graph::esop_ptr& esop_cover, unsigned ninputs, unsigned noutputs, const properties::ptr& settings, const properties::ptr& statistics )
{
  const auto line_map     = get( settings, "line_map",     std::vector<unsigned>() );
  const auto no_constants = get( settings, "no_constants", false );                   /* if true, also output lines have PIs */

  properties_timer t( statistics );

  /* meta data */
  if ( line_map.empty() )
  {
    clear_circuit( circ );
    circ.set_lines( ninputs + noutputs );

    if ( !no_constants )
    {
      auto constants = circ.constants();
      std::vector<bool> garbage( circ.lines(), true );
      for ( auto i = ninputs; i < circ.lines(); ++i )
      {
        constants[i] = false;
        garbage[i] = false;
      }
      circ.set_constants( constants );
      circ.set_garbage( garbage );
    }
  }

  /* cubes */
  int i;
  abc::Vec_Int_t *vec;
  Vec_WecForEachLevel( esop_cover.get(), vec, i )
  {
    /* controls */
    gate::control_container controls;
    std::vector<unsigned> targets;
    for ( auto j = 0; j < abc::Vec_IntSize( vec ); ++j )
    {
      const auto lit = abc::Vec_IntEntry( vec, j );

      if ( lit < 0 )
      {
        const auto tgt = static_cast<int>( ninputs ) - lit - 1;
        targets.push_back( line_map.empty() ? tgt : line_map[tgt] );
      }
      else
      {
        const auto ctr = abc::Abc_Lit2Var( lit );
        controls.push_back( make_var( line_map.empty() ? ctr : line_map[ctr], !abc::Abc_LitIsCompl( lit ) ) );
      }
    }

    auto pos = circ.num_gates();

    for ( auto j = 0u; j < targets.size() - 1; ++j )
    {
      insert_cnot( circ, pos, targets.back(), targets[j] );
      insert_cnot( circ, pos++, targets.back(), targets[j] );
    }
    insert_toffoli( circ, pos, controls, targets.back() );
  }

  return true;
}

bool esop_synthesis( circuit& circ, const gia_graph& gia, const properties::ptr& settings, const properties::ptr& statistics )
{
  const auto esop = gia.compute_esop_cover();
  return esop_synthesis( circ, esop, gia.num_inputs(), gia.num_outputs(), settings, statistics );
}

bool esop_synthesis( circuit& circ, const std::vector<cube2>& cubes, unsigned ninputs, const properties::ptr& settings, const properties::ptr& statistics )
{
  const auto line_map = get( settings, "line_map", std::vector<unsigned>() );

  properties_timer t( statistics );

  /* meta data */
  const auto noutputs = 1u;
  if ( line_map.empty() )
  {
    clear_circuit( circ );
    circ.set_lines( ninputs + noutputs );

    auto constants = circ.constants();
    std::vector<bool> garbage( circ.lines(), true );
    for ( auto i = ninputs; i < circ.lines(); ++i )
    {
      constants[i] = false;
      garbage[i] = false;
    }
    circ.set_constants( constants );
    circ.set_garbage( garbage );
  }

  for ( const auto& cube : cubes )
  {
    gate::control_container controls;
    for ( auto i = 0u; i < ninputs; ++i )
    {
      if ( ( cube.mask >> i ) & 1 )
      {
        controls.push_back( make_var( line_map.empty() ? i : line_map[i], ( cube.bits >> i ) & 1 ) );
      }
    }
    append_toffoli( circ, controls, ninputs );
  }

  return true;
}

pla_blif_synthesis_func esop_synthesis_func( properties::ptr settings, properties::ptr statistics )
{
  pla_blif_synthesis_func f = [&settings, &statistics]( circuit& circ, const std::string& filename ) {
    return esop_synthesis( circ, filename, settings, statistics );
  };
  f.init( settings, statistics );
  return f;
}

}

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


/* Copyright (C) 2017 IBM Corp.
 * 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.
 */
/*******************************************************************
test_DGSamp: a program for testing the DGSamp module
********************************************************************/

// ===> This test program is obsolete, need to be updated <===

// test_DGSamp.cpp - Test trapdoor sampling, including statistical
// tests to ensure that we get the right probability distribution.

#include <cassert>
#include <cmath>
#include <cstdarg>
#include <cstdio>
#include <iomanip>
#include <iostream>
#include <memory>
#include <stack>
#include <string>
#include <vector>

// #include <mpfr.h>

#include <NTL/mat_ZZ_p.h>
#include <NTL/mat_lzz_p.h>
#include <NTL/matrix.h>
#include <NTL/vec_vec_ZZ.h>
#include <NTL/mat_ZZ.h>
#include "../mat_l.h"
NTL_CLIENT

#include "../vec_l.h"
#include "../DGaussSampler.h"
#include "../utils/timing.h"
#include "../TDMatrix.h"
#include "../utils/argmap.h"

#include <stdio.h>
#include <string.h>

bool MovetoNextxTest(vec_l &xTest, int VecLen, int noVars);

int TestProbabilities(TDMatrixParams& TDMP, long nVars);

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

    long k = 2;
    amap.arg("k", k, "number of factors (~ log q)");

    long n = 2;
    amap.arg("n", n, "the small dimension"); // no default info

    long e=2;
    amap.arg("e", e, "the power of each factor");

    long m=0;
    amap.arg("m", m, "the dimension m", "0: set as n*(2+k*e)");

    long nVars=2;
    amap.arg("nVars", nVars, "# of values to check on each side of the mean (in addition to the mean itself), Total vals checked = 2*nVars+1");

    amap.parse(argc, argv); // parses and overrides initail values
    zz_pPush push; // backup NTL's current modulus
    TDMatrixParams TDMP(n,k,e,m);

    TestProbabilities(TDMP, nVars);
    cout << "maxSigma = "<< Gaussian1Dsampler::maxSigma << endl;
    cout << "maxSample = "<< Gaussian1Dsampler::maxSample << endl << endl;

    printAllTimers();

#ifdef CodeBlocks
    cin.get();
#endif

    return 0;
}

/*
Test that the probabilities of getting each sample are correct
*/

int TestProbabilities(TDMatrixParams& TDMP, long nVars)
{
#ifdef DEBUG
    NTL::SetSeed((unsigned char*)"TestProb",8);
#endif
    TDMP.m = 1; //1 dimensional
    //Test the probability of getting each sample
    DiscreteGaussianSampler DGS;
    //test the DGaussSampler

    long VectorLength = TDMP.m;

    Vec<double> zeroVector(INIT_SIZE, TDMP.m);
    clear(zeroVector);

    vec_l Counter;
    int noVars = 2*nVars+1; //5
    long counterSize = pow(noVars,VectorLength);
    int HalfVars = (noVars-1)/2;
    int HalfCounterSize = (counterSize - 1)/2;
    Counter.SetLength(counterSize);//NoVars);

    for (long i = 0; i < counterSize; i++)
    {
        Counter.put(i,0);
    }
#ifdef DEBUG
    cout << "Counter=" << Counter << "\n";
#endif

    mat_l SMS;
    SMS.SetDims(TDMP.m,TDMP.m); //SMS larger or equal to RI *(2+sigma G)*RTI

    mat_l A;
    A.SetDims(TDMP.m,TDMP.m);
    ident(A,TDMP.m);

    //A.put(1,0,1);

    mat_l ATrans;
    square(SMS,A);
    //mul(SMS,A,ATrans);
    mat_l SMSinv;

    if (TDMP.m==1)
    {
        SMS*=16;
        ident(SMSinv,1);
    }
    else
    {
        ZZ zz_det;
        mat_ZZ zz_SMS;
        zz_SMS = conv<mat_ZZ>(SMS);
        mat_ZZ zz_SMSINV;
        inv(zz_SMSINV,zz_SMS);
        determinant(zz_det,zz_SMS);

#ifdef DEBUG
        cout << "SMS= " << zz_SMS << "\n";
        cout << "zz_SMSINV= " << zz_SMSINV << "\n";
#endif

        SMSinv=conv<mat_l>(zz_SMSINV);
    }

#ifdef DEBUG
    {
        cout << "SMS = \n";
        cout << SMS << "\n";
        cout << SMSinv << det << "\n";
    }
#endif

    vec_l xOut;

    zeroVector.SetLength(TDMP.m);
    for (long i = 0; i< TDMP.m; i++)
        zeroVector.put(i,0);

    DGS.InitSampler(SMS);

    long NoTrials = 0;
    long minMidVal = 1000000;//10000*(nVars^2);
    //long var = zeroVector.get(0);

    while (Counter[HalfCounterSize] < minMidVal)
    {
        xOut.SetLength(0);

        DGS.SampleDiscreteGaussian(xOut,zeroVector);
        NoTrials++;

        long iIndex = 0;
        for (long j=0; j< TDMP.m; j++)
            iIndex += xOut[j] * (pow(noVars,j));

        //long iIndex = xOut[0];
        if (abs(iIndex) > HalfCounterSize) //index out of range
            continue;

        Counter[iIndex + HalfCounterSize]+=1;

#ifdef DEBUG
        long var2 = Counter.get(0);//iIndex+HalfVars);d
        cout << var2;
        var2 = Counter.get(1);
        cout << ", " << var2;
        var2 = Counter.get(2);
        cout << ", " << var2 << "\n";
#endif // DEBUG

        // if (Counter[HalfCounterSize] > minMidVal)
        //     bReachedLimit = true;

    }

#ifdef DEBUG
    cout << "\n";

    cout << "Counter=" << "\n";

    for (long ii = ((counterSize-1)/2 -HalfVars); ii <= ((counterSize-1)/2 +HalfVars); ii++)
        cout << "Counter["<<ii<<"]=" << Counter[ii] << "\n";

    cout << "NoTrials = " << NoTrials << "\n";
#endif // DEBUG

    //calculate the error value
    int counterIndex;
    double probVal;
    Vec<double> Prob_vec;
    vec_l val_vec;
    Vec<double> error_vec;

    //calculate avg error values for all results
    //prob = a * exp(-0.5 * X * SigInv * Xt);
    vec_l xTest;
    vec_l xTestNorm;
    xTest.SetLength(TDMP.m);
    xTestNorm.SetLength(TDMP.m);
    Prob_vec.SetLength(counterSize);
    val_vec.SetLength(counterSize);

    for (long ii = 0; ii < TDMP.m; ii++)
        xTest[ii] = 0;

    error_vec.SetLength(counterSize);
    //test for each combination

    while (1)
    {
        //generate counterindex
        counterIndex = 0;
        for (long i = 0; i < VectorLength; i++)
        {
            counterIndex += xTest[i];
            if (i < VectorLength-1)
                counterIndex*=noVars;
        }

        //p = a*exp(-0.5* X * SigInf * XT);

        xTestNorm = xTest - HalfVars;

        double sigma = sqrt(SMS.get(0,0));

        //calculate the probability value of each variable

        double lowerHalf = (double) xTestNorm[0] - 0.5;
        double upperHalf = (double) xTestNorm[0] + 0.5;

#ifdef DEBUG
        cout << "lowerHalf = " << lowerHalf << ", upperHalf = " << upperHalf  << "\n";
#endif

        //tested mean = 0
        lowerHalf/=sigma;
        lowerHalf/=sqrt(2);
        upperHalf/=sigma;
        upperHalf/=sqrt(2);
        double erfUpper = erf(upperHalf);
        double erfLower = erf(lowerHalf);
        probVal = erfUpper - erfLower;
        probVal*=0.5;
#ifdef DEBUG
        cout << "lowerHalf = " << lowerHalf << ", upperHalf = " << upperHalf << ", probVal = " << probVal << "\n";
#endif

        Prob_vec[counterIndex] = probVal;
        probVal*= ((double)NoTrials/Counter.get(counterIndex));
        error_vec.put(counterIndex,probVal);

        if (!MovetoNextxTest(xTest, TDMP.m, noVars))
            break;
    }

#ifdef DEBUG

    cout << "Prob_mat = \n";
    for (counterIndex = 0; counterIndex < counterSize; counterIndex++)
    {
        double dVal = Prob_vec[counterIndex];

        if (dVal != -1)
        {
            cout << "Index = " << counterIndex << "\n";
            cout << dVal << " ";
            dVal = error_vec[counterIndex];
            cout << " error_vec =" << dVal << "\n";
        }

    }
    cout << "\n";
#endif

    for (counterIndex = 0; counterIndex < counterSize; counterIndex++)
    {
        double dVal = Prob_vec[counterIndex];

        if (dVal != -1)
        {
            dVal = error_vec[counterIndex];

            if ((dVal > 0.98) && (dVal < 1.02))
            {
                cout << "PASSED" << counterIndex << ", difference =  " << (abs((dVal -1) * 100)) << "%" << endl;
            }
            else
            {
                cout << "FAILED " << counterIndex << ", difference = " << (abs((dVal -1) * 100)) << "%" << endl;

            }
        }
    }

    return 0;
}

//increase xTest vector in lexicographical order
//returns true as long as moves to next vector, false when it is done

bool MovetoNextxTest(vec_l &xTest, int VecLen, int noVars)
{
    if (xTest[VecLen-1]==(noVars-1))
        return false; //done

    for (long i = 0; i < VecLen; i++)
    {
        xTest[i] += 1;
        if (xTest[i] >= noVars)
            xTest[i] = 0;
        else
            break;
    }
    return true;

}


#include <mbgl/map/environment.hpp>
#include <mbgl/geometry/line_atlas.hpp>
#include <mbgl/platform/gl.hpp>
#include <mbgl/platform/log.hpp>
#include <mbgl/platform/platform.hpp>

#include <boost/functional/hash.hpp>

#include <sstream>
#include <cmath>

using namespace mbgl;

LineAtlas::LineAtlas(uint16_t w, uint16_t h)
    : width(w),
      height(h),
      data(std::make_unique<uint8_t[]>(w * h)),
      dirty(true) {
}

LineAtlas::~LineAtlas() {
    std::lock_guard<std::recursive_mutex> lock(mtx);

    Environment::Get().abandonTexture(texture);
    texture = 0;
}

LinePatternPos LineAtlas::getDashPosition(const std::vector<float> &dasharray, bool round) {
    size_t key = round ? std::numeric_limits<size_t>::min() : std::numeric_limits<size_t>::max();
    for (const float part : dasharray) {
        boost::hash_combine<float>(key, part);
    }

    // Note: We're not handling hash collisions here.

    std::lock_guard<std::recursive_mutex> lock(mtx);
    const auto it = positions.find(key);
    if (it == positions.end()) {
        auto inserted = positions.emplace(key, addDash(dasharray, round));
        assert(inserted.second);
        return inserted.first->second;
    } else {
        return it->second;
    }
}

LinePatternPos LineAtlas::addDash(const std::vector<float> &dasharray, bool round) {

    int n = round ? 7 : 0;
    int dashheight = 2 * n + 1;
    const uint8_t offset = 128;

    if (nextRow + dashheight > height) {
        Log::Warning(Event::OpenGL, "line atlas bitmap overflow");
        return LinePatternPos();
    }

    float length = 0;
    for (const float part : dasharray) {
        length += part;
    }

    float stretch = width / length;
    float halfWidth = stretch * 0.5;
    // If dasharray has an odd length, both the first and last parts
    // are dashes and should be joined seamlessly.
    bool oddLength = dasharray.size() % 2 == 1;

    for (int y = -n; y <= n; y++) {
        int row = nextRow + n + y;
        int index = width * row;

        float left = 0;
        float right = dasharray[0];
        unsigned int partIndex = 1;

        if (oddLength) {
            left -= dasharray.back();
        }

        for (int x = 0; x < width; x++) {

            while (right < x / stretch) {
                left = right;
                right = right + dasharray[partIndex];

                if (oddLength && partIndex == dasharray.size() - 1) {
                    right += dasharray.front();
                }

                partIndex++;
            }

            float distLeft = fabs(x - left * stretch);
            float distRight = fabs(x - right * stretch);
            float dist = fmin(distLeft, distRight);
            bool inside = (partIndex % 2) == 1;
            int signedDistance;

            if (round) {
                float distMiddle = n ? (float)y / n * (halfWidth + 1) : 0;
                if (inside) {
                    float distEdge = halfWidth - fabs(distMiddle);
                    signedDistance = sqrt(dist * dist + distEdge * distEdge);
                } else {
                    signedDistance = halfWidth - sqrt(dist * dist + distMiddle * distMiddle);
                }

            } else {
                signedDistance = int((inside ? 1 : -1) * dist);
            }

            data[index + x] = fmax(0, fmin(255, signedDistance + offset));
        }
    }

    LinePatternPos position;
    position.y = (0.5 + nextRow + n) / height;
    position.height = (2.0 * n) / height;
    position.width = length;

    nextRow += dashheight;

    dirty = true;
    bind();

    return position;
};

void LineAtlas::upload() {
    if (dirty) {
        bind();
    }
}

void LineAtlas::bind() {
    std::lock_guard<std::recursive_mutex> lock(mtx);

    bool first = false;
    if (!texture) {
        MBGL_CHECK_ERROR(glGenTextures(1, &texture));
        MBGL_CHECK_ERROR(glBindTexture(GL_TEXTURE_2D, texture));
        MBGL_CHECK_ERROR(glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR));
        MBGL_CHECK_ERROR(glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR));
        MBGL_CHECK_ERROR(glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT));
        MBGL_CHECK_ERROR(glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE));
        first = true;
    } else {
        MBGL_CHECK_ERROR(glBindTexture(GL_TEXTURE_2D, texture));
    }

    if (dirty) {
        if (first) {
            MBGL_CHECK_ERROR(glTexImage2D(
                GL_TEXTURE_2D, // GLenum target
                0, // GLint level
                GL_ALPHA, // GLint internalformat
                width, // GLsizei width
                height, // GLsizei height
                0, // GLint border
                GL_ALPHA, // GLenum format
                GL_UNSIGNED_BYTE, // GLenum type
                data.get() // const GLvoid * data
            ));
        } else {
            MBGL_CHECK_ERROR(glTexSubImage2D(
                GL_TEXTURE_2D, // GLenum target
                0, // GLint level
                0, // GLint xoffset
                0, // GLint yoffset
                width, // GLsizei width
                height, // GLsizei height
                GL_ALPHA, // GLenum format
                GL_UNSIGNED_BYTE, // GLenum type
                data.get() // const GLvoid *pixels
            ));
        }

        dirty = false;
    }
};