#pragma once

#include <set>
#include <queue>
#include <vector>
#include <string>
#include <memory>
#include <random>
#include <algorithm>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <chrono>

#include "prx/utilities/defs.hpp"
#include "prx/utilities/data_structures/gnn.hpp"
#include "prx/utilities/data_structures/undirected_graph.hpp"
// #include "prx/utilities/data_structures/tree.hpp"
#include "prx/simulation/playback/trajectory.hpp"
#include "prx/planning/planner_functions/planner_functions.hpp"

// #define z_obstacle std::complex<double>(std::numeric_limits<double>::max(),std::numeric_limits<double>::max())
#define MA_DEBUG(i,j,iL,jL,str) if (i == iL && j == jL) std::cout << str << std::endl;
#define EXPAND_CMP(cmp) cmp.real(), cmp.imag()

namespace prx
{
	typedef std::function<void(int,int,space_point_t)> mapping_f;
	typedef std::complex<double> ma_pt;
	/**
	 * Compute the medial axis and vector field for a 2D map
	 */
	// static std::function<bool(std::complex<double>,std::complex<double>)> cmpx_comp = [] (std::complex<double> a, std::complex<double> b)
	// 	{
	// 		return std::abs(a) < std::abs(b);
	// 	};

	struct cmpx_comp {
    	bool operator()(const std::complex<int> a, const std::complex<int> b) const
    	{
        	return std::abs(a) < std::abs(b);
    	}
	};
	class ma_map_t;
	class medial_axis_t;

	class ma_cell
	{
	private:
		bool is_edge;
		bool is_node;
		edge_index_t edge_id;
		node_index_t node_id;
		double dist_to_ma;
		double dist_to_obs;
		std::complex<double> close;
		std::complex<double> far;
		std::complex<double> integrated;
		std::complex<double> to_ma;
		int close_count;
		// static int next_edge_id;
		// static int next_node_id;

	public:
		static constexpr std::complex<double> z_obstacle = std::complex<double>(std::numeric_limits<double>::max(),std::numeric_limits<double>::max());
		ma_cell()
		{
			is_node = false;
			is_edge = false;
			close = std::complex<double>(0,0);
			far = std::complex<double>(0,0);
			integrated = std::complex<double>(0,0);
			to_ma = std::complex<double>(0,0);
			dist_to_obs = std::numeric_limits<double>::infinity();
			dist_to_ma = std::numeric_limits<double>::infinity();
			edge_id = -1;
			node_id = -1;
			close_count = 0;
		};
		~ma_cell() = default;
		bool is_obstacle()
		{
			return close == z_obstacle || far == z_obstacle || integrated == z_obstacle;
		}
		friend class medial_axis_t;
	};

	class medial_axis_t
	{
	private:
		/**
		 * The computation of the vector field has the following flowchart:
		 *
		 * 						   Construction
		 * 								|
		 *         					   init
		 *         					    |
		 *          --------------------+---------------------------
		 *          |												|
		 * 		 set_map								set_close_vf_from_file
		 *   		|												|
		 *   compute_close_VF 										|
		 *          |												|
		 *   		+------------+----------------------------------+
		 *   					 |									|
		 * 		   			 set_goal						set_far_vf_from_file
		 *          			 |									|
		 *          	+-----------------+							|
		 *          	|				  |							|
		 * 			set_sknw 		compute_sknw					|
		 *    			|				  |							|
		 *    			---------+---------							|
		 *    					 |									|
		 * 			   	   prepare_graph							|
		 *           		     |									|
		 *           	      	 |									|
		 *                    	 |				   					|
		 * 	               compute_far_VF	 						|
		 *  		 	         |				  					|
		 * 			 			 |				  					|
		 *           			 +-----------+----------------------+
		 * 					            	 |
		 * 					     +-------(Queries)*
		 *           		     |			 |
		 *                 	     |			 |
		 * 					     |	  (Save to files)*
		 *        			     |			 |
		 *               	     +-----------+
		 *
		 * Queries: integrated_vector_at, close_vector_at, far_vector_at
		 *
		 * Save to files: integrated_vf_to_file, close_vf_to_file, far_vf_to_file, grap_vertices_to_file, gnn_to_file
		 *
		 */
		enum Stage { instatiated, init_done, map_set, close_ready, goal_set, sknw_set, graph_ready, far_ready};
		int next_edge_id = 0;
		int next_node_id = 0;
		space_t* state_space;
		// Eigen::Matrix <std::complex<double>, Eigen::Dynamic, Eigen::Dynamic> close;
		// Eigen::Matrix <std::complex<double>, Eigen::Dynamic, Eigen::Dynamic> map_close;
		// Eigen::Matrix <std::complex<double>, Eigen::Dynamic, Eigen::Dynamic> map_far;
		// ma_map_t* ma_map;
		undirected_graph_t graph;
		undirected_graph_t obstacles_graph;
		// undirected_graph_t graph_obstacles;
		graph_nearest_neighbors_t* metric_close;
		graph_nearest_neighbors_t* metric_obstacles;
		std::vector<double*> state_memory;
		trajectory_t* aux_traj;
		std::complex<double> v_zero;//(0,0);
		std::complex<double> v_not_computed;//(0,0);
		// std::complex<double> z_obstacle;
		node_index_t goal_index;
		space_point_t goal;

		Stage stage;

		// Auxiliary variables for different functions
		double h,w;
		space_point_t pt;
		space_point_t pt_cl1, pt_cl2;
		// space_point_t space_point;
		// Eigen::Vector2d v1, v2, v3;
		ma_pt v1, v2, v3;

		std::string map_file_name;

		// Line of Sight clearance
		double los_clearance;

		int height;
		int width;
		std::vector<std::vector<ma_cell>> ma_map;
		std::queue<std::pair<int,int>> ma_queue;
		std::unordered_map<int, std::pair<int, int>> nodes_edges;

		// key: node_index
		// value: vector of edges_index
		std::unordered_map<int, std::vector<int>> edges_list;

		// std::random_device rd;
    	// std::mt19937 rd_mt19937(std::random_device());
		// ------------
		// | 7 | 0 | 1 |
		// | 6 | p | 2 |
		// | 5 | 4 | 3 |
		// ------------
		std::vector<std::pair<ma_pt,double>> dirs = {
						std::pair<ma_pt,double>(std::complex<double>(+1,+0), std::abs(std::complex<double>(+1,+0))),
						std::pair<ma_pt,double>(std::complex<double>(+1,+1), std::abs(std::complex<double>(+1,+1))),
						std::pair<ma_pt,double>(std::complex<double>(+0,+1), std::abs(std::complex<double>(+0,+1))),
						std::pair<ma_pt,double>(std::complex<double>(-1,+1), std::abs(std::complex<double>(-1,+1))),
						std::pair<ma_pt,double>(std::complex<double>(-1,+0), std::abs(std::complex<double>(-1,+0))),
						std::pair<ma_pt,double>(std::complex<double>(-1,-1), std::abs(std::complex<double>(-1,-1))),
						std::pair<ma_pt,double>(std::complex<double>(+0,-1), std::abs(std::complex<double>(+0,-1))),
						std::pair<ma_pt,double>(std::complex<double>(+1,-1), std::abs(std::complex<double>(+1,-1)))
					};

		std::vector<std::pair<ma_pt,double>> dir4 = {
						std::pair<ma_pt,double>(std::complex<double>(+1,+0), std::abs(std::complex<double>(+1,+0))),
						std::pair<ma_pt,double>(std::complex<double>(+0,+1), std::abs(std::complex<double>(+0,+1))),
						std::pair<ma_pt,double>(std::complex<double>(-1,+0), std::abs(std::complex<double>(-1,+0))),
						std::pair<ma_pt,double>(std::complex<double>(+0,-1), std::abs(std::complex<double>(+0,-1))),
					};
		// std::set<std::complex<int>, cmpx_comp> edges_set;
		// ma_map related functions
		void init_ma_map()
		{
			for (int i = 0; i < height; ++i)
			{
				std::vector<ma_cell> row;
				for (int j = 0; j < width; ++j)
				{
					row.push_back(ma_cell());
				}
				ma_map.push_back(row);
			}
		}

		void set_obstacle(int i, int j, bool obst)
		{
			if (obst)
			{
				ma_map[i][j].far = ma_cell::z_obstacle;
				ma_map[i][j].close = ma_cell::z_obstacle;
				ma_map[i][j].integrated = ma_cell::z_obstacle;
			}
			else
			{
				ma_map[i][j].far = v_not_computed;
				ma_map[i][j].close = v_not_computed;
				ma_map[i][j].integrated = v_not_computed;
			}
		}

		std::complex<double>& map_far(int i, int j)
		{
			return ma_map[i][j].far;
		}
		const std::complex<double> map_far(int i, int j) const
		{
			return ma_map[i][j].far;
		}

		std::complex<double>& map_close(int i, int j)
		{
			return ma_map[i][j].close;
		}
		const std::complex<double> map_close(int i, int j) const
		{
			return ma_map[i][j].close;
		}
		std::complex<double>& to_ma(int i, int j)
		{
			// printf("(%d, %d)\n", i, j);
			i = std::max(0, std::min(i, height - 1));
			j = std::max(0, std::min(j, width  - 1));
			return ma_map[i][j].to_ma;
		}
		const std::complex<double> to_ma(int i, int j) const
		{
			i = std::max(0, std::min(i, height - 1));
			j = std::max(0, std::min(j, width  - 1));
			return ma_map[i][j].to_ma;
		}

		inline int rows(){return height;}
		inline int cols(){return width;}

		void set_as_edge(int i, int j)
		{
			// if (ma_map[i][j].close == ma_cell::z_obstacle) return;
			// if (ma_map[i][j].is_node) return;
			ma_map[i][j].is_edge = true;
			// ma_map[i][j].dist_to_ma = 0;
		}

		void set_as_edge(int i, int j, int edge_id)
		{
			if (ma_map[i][j].close == ma_cell::z_obstacle) return;
			if (ma_map[i][j].is_node) return;
			ma_map[i][j].is_edge = true;
			ma_map[i][j].dist_to_ma = 0;
			ma_map[i][j].edge_id = edge_id;
			next_edge_id = std::max(next_edge_id, edge_id+1);
			ma_queue.push(std::make_pair(i,j));

		}

		void unset_edge(int i, int j)
		{
			ma_map[i][j].is_edge = false;
			ma_map[i][j].edge_id = -1;

		}

		double get_dist_to_ma(int i, int j)
		{
			return ma_map[i][j].dist_to_ma;
		}

		inline void set_as_node(ma_pt v, int node_id)
		{
			set_as_node(v.real(), v.imag(), node_id);
		}
		inline void set_as_node(int i, int j, int node_id)
		{
			i = std::max(0, std::min(i, height - 1));
			j = std::max(0, std::min(j, width  - 1));
			ma_map[i][j].is_node = true;
			ma_map[i][j].dist_to_ma = 0;
			ma_map[i][j].node_id = node_id;
			next_node_id = std::max(next_node_id, node_id+1);
			ma_queue.push(std::make_pair(i,j));
		}
		inline void set_node_as(int i, int j, bool val)
		{
			ma_map[i][j].is_node = val;
		}
		// bool surrounded_by_obstacles(int i, int j);

		inline bool is_edge(std::complex<double> v)
		{
			return is_edge(v.real(), v.imag());
		}
		inline bool is_edge(int i, int j)
		{
			if (i < 0 || i >= rows()) return false;
			if (j < 0 || j >= cols()) return false;
			return ma_map[i][j].is_edge;
		}

		int get_edge_id(int i, int j)
		{
			return ma_map[i][j].edge_id;
		}

		bool is_node(ma_pt v)
		{
			if (v.real() < 0 || v.real() >= rows()) return false;
			if (v.imag() < 0 || v.imag() >= cols()) return false;

			return  ma_map[v.real()][v.imag()].is_node;
		}
		bool is_node(int i, int j)
		{
			if (i < 0 || i >= rows()) return false;
			if (j < 0 || j >= cols()) return false;
			return ma_map[i][j].is_node;
		}

		bool is_goal(double i, double j)
		{
			if (i < 0 || i >= rows()) return false;
			if (j < 0 || j >= cols()) return false;
			auto id1 = ma_map[std::floor(i)][std::floor(j)].node_id;
			auto id2 = ma_map[std::ceil(i)][std::ceil(j)].node_id;
			if (id1 == goal_index || id2 == goal_index) printf("GOAL!\n");
			return id1 == goal_index || id2 == goal_index;
		}

		inline node_index_t get_node_id(int i, int j)
		{
			return ma_map[i][j].node_id;
		}
		inline edge_index_t get_node_id(ma_pt v)
		{
			return get_node_id(v.real(), v.imag());
		}
		inline const double dist_to_obstacle(ma_pt v) const
		{
			return ma_map[v.real()][v.imag()].dist_to_obs;
		}
		inline double& dist_to_obstacle(ma_pt v)
		{
			return ma_map[v.real()][v.imag()].dist_to_obs;
		}
		inline bool is_obstacle(int i, int j)
		{
			// Lets say the bounds are obstacles...
			// if (i < 0 || i >= rows()) return true;
			// if (j < 0 || j >= cols()) return true;
			return 	(i < 0 || i >= rows()) ||
			 		(j < 0 || j >= cols()) ||
			 		ma_map[i][j].is_obstacle();
		}

		inline bool in_bounds(int i, int j)
		{
			return 0 <= i && 0 <= j && i <  cols() && j <  rows();
		}

		void compute_distances_to_ma();

		/**
		 * Check if there are no obstacles in a straigh line from init to end
		 * @param  init Point where to start the line segment
		 * @param  end  Point where the line segment ends
		 * @return      True if no obstacles are found, false otherwise
		 */
		// bool has_direct_line_of_sight(space_point_t init, space_point_t end);
		bool has_direct_line_of_sight(space_point_t init, space_point_t end, double clearance = 0.0);

		void add_goal_to_graph();

		// read a vector field from file.
		// If close_far == 0 ==> close
		// If close_far == 1 ==> far
		// Each line of the file has to be in the form:
		// i j u v
		// Where map(i,j) = (u,v).
		void vector_field_from_file(std::string file_name, int close_far);

		void add_obstacles_to_graph();

	public:
		// valid_trajectory_t valid_check;
		//distance function to the medial axis
		distance_function_t df_ma;
		// distance function: pixel to medial axis to goal
		medial_axis_t()
		{
			// valid_check = nullptr;
			df_ma = nullptr;
			goal = nullptr;
			v_zero = std::complex<double>(0,0);
			v_not_computed = std::complex<double>(std::numeric_limits<double>::min(),std::numeric_limits<double>::min());

			state_memory = {&h, &w};
			state_space = new space_t("EE",state_memory,"HW");
			state_space -> set_bounds(	{-std::numeric_limits<double>::max(), -std::numeric_limits<double>::max()},
										{ std::numeric_limits<double>::max(),  std::numeric_limits<double>::max()});

			aux_traj = new trajectory_t(state_space);
    		df_ma = [](const space_point_t& p1, const space_point_t& p2)
			{
				double cost = 0;
				for (int i = 0; i < p1 -> get_dim(); ++i)
				{
					cost += std::pow(p1 -> at(i) - p2 -> at(i), 2);
				}
				return std::sqrt(cost);
			};
			// pt =  state_space -> make_point();
			pt =  state_space -> make_point();
			pt_cl1 =  state_space -> make_point();
			pt_cl2 =  state_space -> make_point();

			los_clearance = 5;
			stage = instatiated;

		};

		void init()
		{
			prx_assert(df_ma != nullptr, "distance_function not set for medial_axis_t!");
			metric_close = new graph_nearest_neighbors_t(df_ma);
			metric_obstacles = new graph_nearest_neighbors_t(df_ma);
			graph.clear();
			graph.allocate_memory<undirected_node_t,undirected_edge_t>(1000);
			obstacles_graph.clear();
			obstacles_graph.allocate_memory<undirected_node_t,undirected_edge_t>(1000);
			metric_close -> clear();
			metric_obstacles -> clear();
			stage = init_done;
		}

		void set_goal(std::vector<double> _goal)
		{
			prx_assert(stage >= close_ready, "Close VF has not been set/computed.");

			goal = state_space -> make_point();
			state_space -> copy_point_from_vector(goal, _goal);
			// std::cout << state_space -> print_point(goal, 2) << std::endl;
			stage = goal_set;
		}

		void set_map(const std::string map_file);

		void set_sknw(const std::string nodes_file, const std::string sknw_file, const std::string edges_file); // Should get skeletonization within c++

		void cost_to_go_to_file(std::string file_name);

		void integrated_vf_to_file(std::string file_name);

		void close_vf_to_file(std::string file_name, bool normalized = true);

		void far_vf_to_file(std::string file_name, bool normalized = true);

		void grap_vertices_to_file(std::string file_name);

		void gnn_to_file(std::string file_name);

		void set_close_vf_from_file(std::string file_name);

		void set_far_vf_from_file(std::string file_name);

		void prepare_graph();

		/**
		 * Compute the close vector at (i, j)
		 * @param i Compute the element of the ith row
		 * @param j Compute the element of the jth column
		 */
		void compute_close_vector(int i, int j);

		/**
		 * Compute the far vector at (i, j)
		 * @param i Compute the element of the ith row
		 * @param j Compute the element of the jth column
		 */
		void compute_far_vector(int i, int j);

		/**
		 * Compute the full close vector field
		 */
		void compute_close_vector_field();

		/**
		 * Compute the full far vector field
		 */
		void compute_far_vector_field();

		/**
		 * Compute both vector fields at the same time.
		 * This method is more efficient than computing
		 * both vector fields by they own.
		 */
		void compute_vector_fields();

		void edges_to_file(std::string file_name);

		void nodes_to_file(std::string file_name);

		std::complex<double> compute_integrated_vector(int i, int j);

		std::vector<std::complex<double>> blossom(int i, int j, int n, double max_magnitud = std::numeric_limits<double>::max(), double min_magnitud = 0.0, bool avg_if_obstacle = false);

		/**
		 * Compute the integrated vector at (i,j)
		 * The magnitud m of the output vector is such that: m \in [min_magnitud, max_magnitud]
		 * The default values are [0, \inf]
		 * Notice that if 0 < min_magnitud the resulting vector could be in collision.
		 */
		std::complex<double> integrated_vector_at(int i, int j, double max_magnitud = std::numeric_limits<double>::max(), double min_magnitud = 0.0, bool avg_if_obstacle = false);

		std::complex<double> close_vector_at(int i, int j, bool normalized = true);

		std::complex<double> far_vector_at(int i, int j, bool normalized = true);

		void blossom_to_file(std::string file_name);

		void compute_sknw_and_save_to_files(std::string nodes_file, std::string edges_file, std::string sknw_file,
					std::string py_sknw = lib_path + "/scripts/medial_axis.py", std::string py_cmd = "python3");

		void gradient_vector_at(int i, int j);

		// @Aravind: I had to include this line to get this file to compile.
		typedef std::function<void (int,int,space_point_t)> mapping_f;

		void set_map(int x_grid, int y_grid, valid_state_t& valid_state, mapping_f f,
			space_t* state_space, std::string _map_name = lib_path + "/out/ma_custom.map");

		void find_medial_axis();

		double cost_to_go(int i, int j);

	};
}


/*
 *            Copyright 2009-2017 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      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.
 *
 */

// Overload of uBLAS prod function with MKL/GSL implementations
#include <votca/tools/linalg.h>

#include <votca/xtp/qmmachine.h>
#include <sys/stat.h>
#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>
#include <boost/filesystem.hpp>
#include <votca/xtp/elements.h>
#include <votca/tools/linalg.h>
#include <votca/xtp/espfit.h>

using boost::format;

namespace votca {
    namespace xtp {

        template<class QMPackage>
        QMMachine<QMPackage>::QMMachine(ctp::XJob *job, ctp::XInductor *xind, QMPackage *qmpack,
                Property *opt, string sfx, int nst, bool mav)
        : _job(job), _xind(xind), _qmpack(qmpack), _subthreads(nst),
        _isConverged(false) {

            string key = sfx + ".qmmmconvg";

            _crit_dR = opt->ifExistsReturnElseReturnDefault<double>(key + ".dR", 0.01); //nm
            _crit_dQ = opt->ifExistsReturnElseReturnDefault<double>(key + ".dQ", 0.01); //e
            _crit_dE_QM = opt->ifExistsReturnElseReturnDefault<double>(key + ".dQdE_QM", 0.001); //eV
            _crit_dE_MM = opt->ifExistsReturnElseReturnDefault<double>(key + ".dE_MM", _crit_dE_QM); //eV
            _maxIter = opt->ifExistsReturnElseReturnDefault<int>(key + ".max_iter", 32);

            key = sfx;
            bool split_dpl = opt->ifExistsReturnElseReturnDefault<bool>(key + ".split_dpl", true);
            double dpl_spacing = opt->ifExistsReturnElseReturnDefault<double>(key + ".dpl_spacing", 1e-3);
            qminterface.setMultipoleSplitting(split_dpl, dpl_spacing);

            // check for archiving
            std::string _archiving_string = opt->ifExistsReturnElseReturnDefault<std::string>(key + ".archiving", "");
            if (_archiving_string.find("iterations") != std::string::npos) {
                _do_archive = true;
            } else {
                _do_archive = false;
            }

            // check for static or polarized qmmm
            key = sfx + ".tholemodel";
            _static_qmmm = true;
            if (opt->exists(key + ".induce")) {
                int induce = opt->get(key + ".induce").as< int >();
                _static_qmmm = (induce == 1) ? false : true;
            }

            // GDMA options
            key = sfx + ".gdma";
            if (opt->exists(key)) {
                _do_gdma = true;
                string _gdma_xml = opt->get(key).as<string> ();
                //cout << endl << "... ... Parsing " << _package_xml << endl ;
                load_property_from_xml(_gdma_options, _gdma_xml.c_str());
            } else {
                _do_gdma = false;
            }

            // GWBSE options
            key = sfx + ".gwbse";
            if (opt->exists(key)) {
                _do_gwbse = true;

            // PUT IN some useful type definitions for excited state
            // - singlet -> singlet exciton
            // - triplet -> triplet exciton
            // - quasiparticle -> GW quasiparticle (diagqp)
            // - kohn-sham -> DFT MO

                string _gwbse_xml = opt->get(key + ".gwbse_options").as<string> ();
                //cout << endl << "... ... Parsing " << _package_xml << endl ;
                load_property_from_xml(_gwbse_options, _gwbse_xml.c_str());
                _state = opt->get(key + ".state").as< int >();
                _type = opt->get(key + ".type").as< string >();
                if (_type != "singlet" && _type != "triplet" && _type != "quasiparticle") {
                    throw runtime_error(" Invalid excited state type! " + _type);
                }

                key = sfx + ".gwbse.filter";
                if (opt->exists(key + ".oscillator_strength") && _type != "triplet") {
                    _has_osc_filter = true;
                    _osc_threshold = opt->get(key + ".oscillator_strength").as<double> ();
                }
                if (opt->exists(key + ".overlap")) {
                    _has_overlap_filter = true;
                    // _osc_threshold = opt->get(key + ".oscillator_strength").as<double> ();
                }
                if (opt->exists(key + ".localisation")) {
                    _has_loc_filter = true;

                    string temp = opt->get(key + ".localisation").as<string> ();
                    Tokenizer tok_cleanup(temp, ", \n\t");
                    std::vector <std::string> strings_vec;
                    tok_cleanup.ToVector(strings_vec);
                    if (strings_vec.size()!=2){
                        throw runtime_error("qmmmachine: Fragment and localisation threshold are not separated");
                    }
                    if(strings_vec[0]=="a" || strings_vec[0]=="A"){
                        _localiseonA=true;
                    }else if(strings_vec[0]=="b" || strings_vec[0]=="B"){
                         _localiseonA=false;
                    }else{
                        throw runtime_error("qmmmachine: Fragment label not known, either A or B");
                    }
                    _loc_threshold=boost::lexical_cast<double>(strings_vec[1]);
                }

                if (opt->exists(key + ".charge_transfer")) {
                    _has_dQ_filter = true;
                    _dQ_threshold = opt->get(key + ".charge_transfer").as<double> ();
                }
                if(_has_dQ_filter && _has_loc_filter){
                    throw runtime_error("Cannot use localisation and charge_transfer filter at the same time.");
                }
            } else {
                _do_gwbse = false;
            }

            return;
        }

        template<class QMPackage>
        QMMachine<QMPackage>::~QMMachine() {

            std::vector<QMMIter*> ::iterator qit;
            for (qit = _iters.begin(); qit < _iters.end(); ++qit) {
                delete *qit;
            }
            _iters.clear();
        }

        template<class QMPackage>
        int QMMachine<QMPackage>::Evaluate(ctp::XJob *job) {

            CTP_LOG(ctp::logINFO, *_log)
                    << format("... dR %1$1.4f dQ %2$1.4f QM %3$1.4f MM %4$1.4f IT %5$d")
                    % _crit_dR % _crit_dQ % _crit_dE_QM % _crit_dE_MM % _maxIter << flush;

            // FIGURE OUT CHARGE + MULTIPLICITY
            double dQ = 0.0;
            for (unsigned int i = 0; i < _job->getPolarTop()->QM0().size(); ++i) {
                dQ += _job->getPolarTop()->QM0()[i]->CalcTotQ();
            }
            int chrg = round(dQ);
            int spin = ((chrg < 0) ? -chrg : chrg) % 2 + 1;
            CTP_LOG(ctp::logINFO, *_log) << "... Q = " << chrg << ", 2S+1 = " << spin << flush;

            // PREPARE JOB DIRECTORY
            string jobFolder = "xjob_" + boost::lexical_cast<string>(_job->getId())
                    + "_" + _job->getTag();
            bool created = boost::filesystem::create_directory(jobFolder);
            if (created) {
                CTP_LOG(ctp::logINFO, *_log) << "Created directory " << jobFolder << flush;
            }

            // SET ITERATION-TIME CONSTANTS
            // TO ADJUST

            _qmpack->setCharge(chrg);
            _qmpack->setSpin(spin);

            int iterCnt = 0;
            int iterMax = _maxIter;
            for (; iterCnt < iterMax; ++iterCnt) {

                // check for polarized QM/MM convergence
            int info= Iterate(jobFolder, iterCnt);
            if(info!=0){
                 CTP_LOG(ctp::logERROR, *_log)
                        << format("Iterating job failed!")<<flush;
                 return 1;
            }
                if (_static_qmmm) {
                    _isConverged = true;
                    break;
                } else if (hasConverged()) {
                    break;
                }

            }

            if (iterCnt == iterMax - 1 && !_isConverged) {
                CTP_LOG(ctp::logWARNING, *_log)
                        << format("Not converged within %1$d iterations.") % iterMax;
                return 2;
            }

            return 0;
        }

        template<class QMPackage>
        bool QMMachine<QMPackage>::Iterate(string jobFolder, int iterCnt) {

            // CREATE ITERATION OBJECT & SETUP RUN DIRECTORY
            QMMIter *thisIter = this->CreateNewIter();
            int iter = iterCnt;
            string runFolder = jobFolder + "/iter_" + boost::lexical_cast<string>(iter);

            bool created = boost::filesystem::create_directory(runFolder);
            if (created)
                CTP_LOG(ctp::logDEBUG, *_log) << "Created directory " << runFolder << flush;
            else
                CTP_LOG(ctp::logWARNING, *_log) << "Could not create directory " << runFolder << flush;

            // RUN CLASSICAL INDUCTION & SAVE
            _job->getPolarTop()->PrintPDB(runFolder + "/QM0_MM1_MM2.pdb");
            _xind->Evaluate(_job);

            assert(_xind->hasConverged());
            thisIter->setE_FM(_job->getEF00(), _job->getEF01(), _job->getEF02(),
                    _job->getEF11(), _job->getEF12(), _job->getEM0(),
                    _job->getEM1(), _job->getEM2(), _job->getETOT());

            std::vector<ctp::Segment*> empty;
            /* Translate atoms in QM0() to QMAtoms in orbitals object
             * to be used in writing the QM input files for the
             * external QMPackages or directly in internal DFT engine.
             * DRAWBACK: QMAtom positions are fixed for all iterations
             *           unless the UPDATE function at the end of the
             *           iteration updates orb_iter_input (TO BE TESTED)
             */
            if (iterCnt == 0) qminterface.GenerateQMAtomsFromPolarSegs(_job->getPolarTop(), orb_iter_input);

            /* Generate list of polar segments in the MM1() and MM2()
             * region to be used in writing the background multipoles
             * in the external QMPackages or in the direct evaluation
             * in the internal DFT engine.
             */
            std::vector<ctp::PolarSeg*> MultipolesBackground = qminterface.GenerateMultipoleList( _job->getPolarTop() );

            // if XTP DFT is used, pass this list of polar segments
            if ( _qmpack->getPackageName() == "xtp" )  _qmpack->setMultipoleBackground( MultipolesBackground );

            // setting RUNDIR for the external QMPackages, dummy for internal
            _qmpack->setRunDir(runFolder);

            /* Call to WriteInputFile writes the appropriate input files
             * for the respective external QMPackages. For the internal
             * DFT engine, this function sets logger and runs DFTENGINE's
             * Prepare() function. ONLY the first iteration, this will
             * initialize the atoms, basissets, ecps, etc. In all
             * subsequent iterations it will recalculate all the "static"
             * AOmatrices (overlap, kinetic energy, nuc/ecp) which is
             * strictly unnecessary but at the same time also those
             * for external point charges, dipoles, and quadrupoles.
             * Since in polarized calculations, the dipoles can change
             * this recalculation is required. Should be split off the
             * Prepare() function for efficiency.
             */
            CTP_LOG(ctp::logDEBUG, *_log) << "Writing input file " << runFolder << flush;
            _qmpack->WriteInputFile(empty, &orb_iter_input, MultipolesBackground);

            FILE *out;
            out = fopen((runFolder + "/system.pdb").c_str(), "w");
            orb_iter_input.WritePDB(out);
            fclose(out);

            /* Runs the external QMPackage or the self-consistent part of
             * DFTENGINE
             */
            _qmpack->Run( &orb_iter_input );

            /* Parse information from the LOGFILE into orbitals, if
             * external QMPackage is run. Dummy for internal DFTENGINE.
             * The QMPackage's MOcoefficients are not automatically
             * parsed in DFT-only calculations and ESP fits for
             * polarized QMMM are expected in the LOGFILE.
             * IF the internal ESPFITs should be used, the MOcoefficients
             * need to be parsed too.
             */
            bool success=_qmpack->ParseLogFile(&orb_iter_input);
            if(!success){
                return 1;
            }

            ub::matrix<double> DMAT_tot;
            // GW-BSE starts here
            double energy___ex = 0.0;
            std::vector<int> _state_index;

            if (_do_gwbse) {

                /* Parses the MOcoefficients from the external QMPackages
                 * for GW-BSE. Internal DFTENGINE has stored coefficients
                 * into orb_iter_input already, so this is a dummy for that.
                 */
                _qmpack->ParseOrbitalsFile(&orb_iter_input);
                orb_iter_input.setDFTbasis(_qmpack->getBasisSetName());

                // Get a GWBSE object
                GWBSE _gwbse = GWBSE(&orb_iter_input);
                // define own logger for GW-BSE that is written into a runFolder logfile
                ctp::Logger gwbse_logger(ctp::logDEBUG);
                gwbse_logger.setMultithreading(false);
                _gwbse.setLogger(&gwbse_logger);
                gwbse_logger.setPreface(ctp::logINFO, (format("\nGWBSE INF ...")).str());
                gwbse_logger.setPreface(ctp::logERROR, (format("\nGWBSE ERR ...")).str());
                gwbse_logger.setPreface(ctp::logWARNING, (format("\nGWBSE WAR ...")).str());
                gwbse_logger.setPreface(ctp::logDEBUG, (format("\nGWBSE DBG ...")).str());

                // Initialize with options
                _gwbse.Initialize(&_gwbse_options);

                /* Only execute GWBSE if excited state is requested. This is a bit
                 * weird construction to have ground state calculation treated in
                 * exactly the same way for polarized QMMM.
                 */
                if (_state > 0) {
                    CTP_LOG(ctp::logDEBUG, *_log) << "Excited state via GWBSE: " << flush;
                    CTP_LOG(ctp::logDEBUG, *_log) << "  --- type:              " << _type << flush;
                    CTP_LOG(ctp::logDEBUG, *_log) << "  --- state:             " << _state << flush;
                    if (_has_overlap_filter) {
                        CTP_LOG(ctp::logDEBUG, *_log) << "  --- filter: overlap  " <<  flush;
                    }
                    if (_has_osc_filter) {
                        CTP_LOG(ctp::logDEBUG, *_log) << "  --- filter: osc.str. > " << _osc_threshold << flush;
                    }
                    if (_has_dQ_filter) {
                        CTP_LOG(ctp::logDEBUG, *_log) << "  --- filter: crg.trs. > " << _dQ_threshold << flush;
                    }
                    if (_has_loc_filter){
                        if (_localiseonA){
                         CTP_LOG(ctp::logDEBUG, *_log) << "  --- filter: localisation on A > " << _loc_threshold << flush;
                        }else{
                            CTP_LOG(ctp::logDEBUG, *_log) << "  --- filter: localisation on B > " << _loc_threshold << flush;
                        }
                    }

                    if (_has_osc_filter && _has_dQ_filter) {
                        CTP_LOG(ctp::logDEBUG, *_log) << "  --- WARNING: filtering for optically active CT transition - might not make sense... " << flush;
                    }

                    // actual GW-BSE run
                    _gwbse.Evaluate();

                    // write logger to log file
                    ofstream ofs;
                    string gwbse_logfile = runFolder + "/gwbse.log";
                    ofs.open(gwbse_logfile.c_str(), ofstream::out);
                    if (!ofs.is_open()) {
                        throw runtime_error("Bad file handle: " + gwbse_logfile);
                    }
                    ofs << gwbse_logger << endl;
                    ofs.close();

                    // PROCESSING the GW-BSE result
                    // - find the excited state of interest
                    // oscillator strength filter

                    // quasiparticle filter
                    if (_has_overlap_filter) {
                        if (iter == 0) {

                            // One - to - One LIST in 0th iteration
                            for (unsigned _i = 0; _i < orb_iter_input.QPdiagEnergies().size(); _i++) {
                                _state_index.push_back(_i);
                            }

                        } else {
                            // get AO overlap matrix
                            AOOverlap _dftoverlap;
                            // load dft  basis set (element-wise information) from xml file
                            BasisSet dftbs;
                            dftbs.LoadBasisSet(orb_iter_input.getDFTbasis());
                            CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Loaded DFT Basis Set " << orb_iter_input.getDFTbasis() << flush;

                            // fill auxiliary GW AO basis by going through all atoms
                            AOBasis dftbasis;
                            dftbasis.AOBasisFill(&dftbs, orb_iter_input.QMAtoms());
                            CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Filled DFT Basis of size " << dftbasis.AOBasisSize() << flush;

                            // Fill overlap
                            _dftoverlap.Fill(dftbasis);
                            CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Filled DFT Overlap matrix of dimension: " << _dftoverlap.Matrix().size1() << flush;

                            // 'LAMBDA' matrix of the present iteration
                            ub::matrix<real_gwbse> lambda_N = orb_iter_input.LambdaMatrixQuasiParticle();

                            // 'LAMBDA' matrix of the previous iteration
                            string runFolder_N_1 = jobFolder + "/iter_" + boost::lexical_cast<string>(iter - 1);
                            string orbfile_N_1 = runFolder_N_1 + "/system.orb";
                            Orbitals _orbitals_N_1;
                            // load the QM data from serialized orbitals object
                            std::ifstream ifs((orbfile_N_1).c_str());
                            CTP_LOG(ctp::logDEBUG, *_log) << " Loading QM data from " << orbfile_N_1 << flush;
                            boost::archive::binary_iarchive ia(ifs);
                            ia >> _orbitals_N_1;
                            ifs.close();

                            ub::matrix<real_gwbse> lambda_N_1 = _orbitals_N_1.LambdaMatrixQuasiParticle();
                            // calculate QP overlaps
                            ub::matrix<real_gwbse> qptemp = ub::prod(_dftoverlap.Matrix(), ub::trans(lambda_N_1));
                            ub::matrix<real_gwbse> qpoverlaps = ub::prod(lambda_N, qptemp);

                            // test output
                            if (tools::globals::verbose) {
                                for (unsigned i = 0; i < qpoverlaps.size1(); i++) {
                                    for (unsigned j = 0; j < qpoverlaps.size2(); j++) {
                                        CTP_LOG(ctp::logDEBUG, *_log) << " [" << i << " , " << j << "]: " << qpoverlaps(i, j) << flush;
                                    }
                                }
                            }

                            // filter for max absolute value (hopefully close to 1)
                            for (unsigned _j = 0; _j < qpoverlaps.size2(); _j++) {
                                int maxi = 0;
                                for (unsigned _i = 0; _i < qpoverlaps.size1(); _i++) {
                                    if (std::abs(qpoverlaps(_i, _j)) > std::abs(qpoverlaps(maxi, _j))) {
                                        maxi = _i;
                                    }
                                }
                                _state_index.push_back(maxi);
                                CTP_LOG(ctp::logDEBUG, *_log) << " [" << maxi << " , " << _j << "]: " << qpoverlaps(maxi, _j) << flush;
                            }

                        }

                    }

                    if (_has_osc_filter) {

                        // go through list of singlets
                        const std::vector<double>oscs = orb_iter_input.Oscillatorstrengths();
                        for (unsigned _i = 0; _i < oscs.size(); _i++) {

                            double osc = oscs[_i];
                            if (osc > _osc_threshold) _state_index.push_back(_i);
                        }

                    } else {
                        const ub::vector<real_gwbse>& energies = (_type=="singlet")
                        ? orb_iter_input.BSESingletEnergies() : orb_iter_input.BSETripletEnergies();

                        for (unsigned _i = 0; _i < energies.size(); _i++) {
                            _state_index.push_back(_i);
                        }
                    }

                    // filter according to charge transfer, go through list of excitations in _state_index
                    if (_has_dQ_filter) {
                        std::vector<int> _state_index_copy;
                        const std::vector< ub::vector<double> >& dQ_frag= (_type=="singlet")
                        ? orb_iter_input.getFragmentChargesSingEXC():orb_iter_input.getFragmentChargesTripEXC();
                        for (unsigned _i = 0; _i < _state_index.size(); _i++) {
                            if (std::abs(dQ_frag[_state_index[_i]](0)) > _dQ_threshold) {
                                _state_index_copy.push_back(_state_index[_i]);
                            }
                        }
                        _state_index = _state_index_copy;
                    }
                    else if (_has_loc_filter) {
                        std::vector<int> _state_index_copy;
                        const std::vector< ub::vector<double> >& popE= (_type=="singlet")
                        ? orb_iter_input.getFragment_E_localisation_singlet():orb_iter_input.getFragment_E_localisation_triplet();
                        const std::vector< ub::vector<double> >& popH= (_type=="singlet")
                        ? orb_iter_input.getFragment_H_localisation_singlet():orb_iter_input.getFragment_H_localisation_triplet();
                        if(_localiseonA){
                            for (unsigned _i = 0; _i < _state_index.size(); _i++) {
                                if (popE[_state_index[_i]](0) > _loc_threshold && popH[_state_index[_i]](0) > _loc_threshold ) {
                                    _state_index_copy.push_back(_state_index[_i]);
                                }
                            }
                        }else{
                            for (unsigned _i = 0; _i < _state_index.size(); _i++) {
                                if (popE[_state_index[_i]](1) > _loc_threshold && popH[_state_index[_i]](1) > _loc_threshold ) {
                                    _state_index_copy.push_back(_state_index[_i]);
                                }
                            }
                        }
                        _state_index = _state_index_copy;
                    }

                    if (_state_index.size() < 1) {
                        CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " WARNING: FILTER yielded no state. Taking lowest excitation" << flush;
                        _state_index.push_back(0);
                    }else{
                        if ( _type == "quasiparticle" ){
                            CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Filter yielded QP index: "<<_state_index[_state - 1 - orb_iter_input.getGWAmin()]<< flush;
                        }else {
                            CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Filter yielded state"<<_type<<":"<<_state_index[_state - 1]+1<< flush;
                        }
                    }
                    // - output its energy
                    if (_type == "singlet") {
                        energy___ex = orb_iter_input.BSESingletEnergies()[_state_index[_state - 1]] * tools::conv::hrt2ev; // to eV
                    } else if (_type == "triplet") {
                        energy___ex = orb_iter_input.BSETripletEnergies()[_state_index[_state - 1]] * tools::conv::hrt2ev; // to eV
                    } else if (_type == "quasiparticle") {
                        if ( _state > orb_iter_input.getNumberOfElectrons()  ) {
                            // unoccupied QPs: E_a = E_0 + eps_l
                            energy___ex = orb_iter_input.QPdiagEnergies()[_state_index[_state - 1 - orb_iter_input.getGWAmin()]] * tools::conv::hrt2ev; // to eV
                        } else {
                            // occupied QPs: E_c = E_0 - eps_h
                            energy___ex = -1.0*orb_iter_input.QPdiagEnergies()[_state_index[_state - 1 - orb_iter_input.getGWAmin()]] * tools::conv::hrt2ev; // to eV
                        }
                    }

                } // only if state >0

                if (!_static_qmmm) {
                    Density2Charges(&_gwbse,_state_index);
                } // for polarized QMMM

            } //_do_gwbse

            /* new ESP fit only required for
             * - polarizable QMMM
             * AND
             * - GWBSE or DFT with internal DFTENGINE
             */
            if (!_static_qmmm && _qmpack->getPackageName() == "xtp" && !_do_gwbse) {
                Density2Charges();
            } // for polarized QMMM

            // Test: go via GDMA instead of point charges, only for DFT with Gaussian!
            GDMA _gdma;
            if (_do_gdma) {
                if (_qmpack->getPackageName() != "gaussian" || _qmpack->getExecutable() != "g03") {

                    throw runtime_error(" Invalid QMPackage! " + _type + " Gaussian 03 only!");

                } else {
                    // get a GDMA object
                    _gdma.Initialize(&_gdma_options);
                    _gdma.setLog(_log);
                    _gdma.SetRunDir(runFolder);

                    CTP_LOG(ctp::logINFO, *_log) << "Running GDMA " << flush;
                    // prepare a GDMA input file
                    _gdma.WriteInputFile();

                    // run GDMA external
                    _gdma.RunExternal();

                    // parse output of gdma and update multipoles_full
                    _gdma.ParseOutputFile();

                } // use gdma
            } // _do_gdma

            out = fopen((runFolder + "/InputConfig.pdb").c_str(), "w");
            orb_iter_input.WritePDB(out);
            fclose(out);

            assert(orb_iter_input.hasSelfEnergy());
            assert(orb_iter_input.hasQMEnergy());

            // EXTRACT & SAVE QM ENERGIES
            double energy___sf = orb_iter_input.getSelfEnergy();
            double energy_qmsf = orb_iter_input.getQMEnergy();
            double energy_qm__ = energy_qmsf - energy___sf;
            thisIter->setQMSF(energy_qm__, energy___sf, energy___ex);
            _job->setEnergy_QMMM(thisIter->getQMEnergy(), thisIter->getGWBSEEnergy(), thisIter->getSFEnergy(),
                    thisIter->getQMMMEnergy());

            // EXTRACT & SAVE QMATOM DATA
            std::vector< ctp::QMAtom* > &atoms = orb_iter_input.QMAtoms();

            thisIter->UpdatePosChrgFromQMAtoms(atoms, _job->getPolarTop()->QM0());

            if (_do_gdma) {

                // update PolarTop
                thisIter->UpdateMPSFromGDMA(_gdma.GetMultipoles(), _job->getPolarTop()->QM0());

            }

            // Update state variable
            if (_type == "quasiparticle" || _has_overlap_filter ){

                _state = _state_index[ _state -1 - orb_iter_input.getGWAmin() ] + 1 + orb_iter_input.getGWAmin();

            }

            unsigned qmsize = 0;
            std::vector< ctp::QMAtom* > ::iterator ait;
            for (ait = atoms.begin(); ait < atoms.end(); ++ait) {

                if ( !(*ait)->from_environment ) qmsize++;
                //CTP_LOG(ctp::logINFO, *_log) << (*ait)->type << " " << (*ait)->x << " "  << (*ait)->y << " " << (*ait)->z << flush;

            }
            // serialize this iteration
            if (_do_archive) {
                // save orbitals
                std::string ORB_FILE = runFolder + "/system.orb";
                CTP_LOG(ctp::logDEBUG, *_log) << "Archiving data to " << ORB_FILE << flush;
                orb_iter_input.Save(ORB_FILE);
            }

            CTP_LOG(ctp::logINFO, *_log)
                    << format("Summary - iteration %1$d:") % (iterCnt + 1) << flush;
            CTP_LOG(ctp::logINFO, *_log)
                    << format("... QM Size  = %1$d atoms") % int(qmsize) << flush;
            CTP_LOG(ctp::logINFO, *_log)
                    << format("... E(QM)    = %1$+4.9e") % thisIter->getQMEnergy() << flush;
            CTP_LOG(ctp::logINFO, *_log)
                    << format("... E(GWBSE) = %1$+4.9e") % thisIter->getGWBSEEnergy() << flush;
            CTP_LOG(ctp::logINFO, *_log)
                    << format("... E(SF)    = %1$+4.9e") % thisIter->getSFEnergy() << flush;
            CTP_LOG(ctp::logINFO, *_log)
                    << format("... E(FM)    = %1$+4.9e") % thisIter->getFMEnergy() << flush;
            CTP_LOG(ctp::logINFO, *_log)
                    << format("... E(MM)    = %1$+4.9e") % thisIter->getMMEnergy() << flush;
            CTP_LOG(ctp::logINFO, *_log)
                    << format("... E(QMMM)  = %1$+4.9e") % thisIter->getQMMMEnergy() << flush;
            if (!_static_qmmm) {
                CTP_LOG(ctp::logINFO, *_log)
                        << format("... RMS(dR)  = %1$+4.9e") % thisIter->getRMSdR() << flush;
                CTP_LOG(ctp::logINFO, *_log)
                        << format("... RMS(dQ)  = %1$+4.9e") % thisIter->getRMSdQ() << flush;
                CTP_LOG(ctp::logINFO, *_log)
                        << format("... SUM(dQ)  = %1$+4.9e") % thisIter->getSUMdQ() << flush;
            }
            // CLEAN DIRECTORY
            _qmpack->CleanUp();

            return 0;
        }

        template<class QMPackage>
        void QMMachine<QMPackage>::Density2Charges( GWBSE* _gwbse, std::vector<int> _state_index ){

                    // load DFT basis set (element-wise information) from xml file
                    BasisSet dftbs;
                    if (orb_iter_input.getDFTbasis() != "") {
                        dftbs.LoadBasisSet(orb_iter_input.getDFTbasis());
                        CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Loaded DFT Basis Set " << orb_iter_input.getDFTbasis() << flush;
                    } else {
                        //dftbs.LoadBasisSet(_gwbse->get_dftbasis_name());
                        //CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Loaded DFT Basis Set " << _gwbse.get_dftbasis_name() << flush;
                    }

                    // fill DFT AO basis by going through all atoms
                    AOBasis dftbasis;
                    dftbasis.AOBasisFill(&dftbs, orb_iter_input.QMAtoms());

                    ub::matrix<double> DMATGS = orb_iter_input.DensityMatrixGroundState();

                    ub::matrix<double> DMAT_tot = DMATGS; // Ground state + hole_contribution + electron contribution

                    if (_state > 0 ) {

                        if ( _type == "singlet" && _type == "triplet"){

                            std::vector<ub::matrix<double> > DMAT = orb_iter_input.DensityMatrixExcitedState(_type, _state_index[_state - 1]);
                            DMAT_tot = DMAT_tot - DMAT[0] + DMAT[1]; // Ground state + hole_contribution + electron contribution
                        } else if ( _type == "quasiparticle"){

                            ub::matrix<double> DMATQP = orb_iter_input.DensityMatrixQuasiParticle(  _state_index[_state - 1 - orb_iter_input.getGWAmin()]);

                            if ( _state > orb_iter_input.getNumberOfElectrons() ) {
                                DMAT_tot = DMAT_tot + DMATQP;
                            } else {
                                DMAT_tot = DMAT_tot - DMATQP;
                            }
                        }
                    }

                    // fill DFT AO basis by going through all atoms
                    std::vector< ctp::QMAtom* >& Atomlist = orb_iter_input.QMAtoms();

                    Espfit esp = Espfit(_log);
                    if (_qmpack->ECPRequested()) {
                        esp.setUseECPs(true);
                    }
                    esp.Fit2Density(Atomlist, DMAT_tot, dftbasis, dftbs, "medium");

            return;
        }

        template<class QMPackage>
        QMMIter *QMMachine<QMPackage>::CreateNewIter() {

            QMMIter *newIter = new QMMIter(_iters.size());
            this->_iters.push_back(newIter);
            return newIter;
        }

        template<class QMPackage>
        bool QMMachine<QMPackage>::hasConverged() {

            _convg_dR = false;
            _convg_dQ = false;
            _convg_dE_QM = false;
            _convg_dE_MM = false;

            if (_iters.size() > 1) {

                QMMIter *iter_0 = _iters[_iters.size() - 2];
                QMMIter *iter_1 = _iters[_iters.size() - 1];

                double dR = iter_1->getRMSdR();
                double dQ = iter_1->getRMSdQ();
                double dE_QM = iter_1->getQMEnergy() - iter_0->getQMEnergy();
                double dE_MM = iter_1->getMMEnergy() - iter_0->getMMEnergy();

                CTP_LOG(ctp::logINFO, *_log)
                        << format("... dE_QM  = %1$+4.9e") % dE_QM << flush;
                CTP_LOG(ctp::logINFO, *_log)
                        << format("... dE_MM  = %1$+4.9e") % dE_MM << flush;

                if (dR <= _crit_dR) _convg_dR = true;
                if (dQ <= _crit_dQ) _convg_dQ = true;
                if (dE_QM * dE_QM <= _crit_dE_QM * _crit_dE_QM) _convg_dE_QM = true;
                if (dE_MM * dE_MM <= _crit_dE_MM * _crit_dE_MM) _convg_dE_MM = true;
            }

            _isConverged = ((_convg_dR && _convg_dQ) && (_convg_dE_QM && _convg_dE_MM));

            CTP_LOG(ctp::logINFO, *_log)
                    << format("... Convg dR = %s") % (_convg_dR ? "true" : "false") << flush;
            CTP_LOG(ctp::logINFO, *_log)
                    << format("... Convg dQ = %s") % (_convg_dQ ? "true" : "false") << flush;
            CTP_LOG(ctp::logINFO, *_log)
                    << format("... Convg QM = %s") % (_convg_dE_QM ? "true" : "false") << flush;
            CTP_LOG(ctp::logINFO, *_log)
                    << format("... Convg MM = %s") % (_convg_dE_MM ? "true" : "false") << flush;

            return _isConverged;
        }

        // REGISTER QM PACKAGES
        template class QMMachine<QMPackage>;

    }
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2016 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MP_MIN_MAX_HPP
#define BOOST_MP_MIN_MAX_HPP

#include <boost/multiprecision/traits/is_backend.hpp>

namespace boost { namespace multiprecision {

//
// Expression template overloads for (min) and (max):
//
// Introduced in response to https://svn.boost.org/trac/boost/ticket/11149
// note that these can not legally be injected into namespace std, and that doing so
// may break future enhancements to the standard.  None the less adding
// namespace std{ using boost::multiprecision::(min); using boost::multiprecision::(max); }
// to your code may get some generic code working that wouldn't work otherwise.
//
// The use of enable_if on the return type is to avoid poisoning std::min/max,
// otherwise attempting to make an explicit call to min<long>(a, b) when these and std
// versions are in scope, will cause the compiler to try to instantiate the signatures
// for our versions as well as the std ones, which in turn instantiates number<long>
// which fails to compile as "long" is not a valid backend type.
//
template <class Backend>
inline typename boost::enable_if_c<boost::multiprecision::detail::is_backend<Backend>::value, const number<Backend, et_on>&>::type(min)(const number<Backend, et_on>& a, const number<Backend, et_on>& b)
{
   return a < b ? a : b;
}
template <class Backend, class tag, class A1, class A2, class A3, class A4>
inline typename boost::enable_if_c<boost::multiprecision::detail::is_backend<Backend>::value, const number<Backend, et_on> >::type(min)(const number<Backend, et_on>& a, const detail::expression<tag, A1, A2, A3, A4>& b)
{
   number<Backend, et_on> t(b);
   if (a < t)
      return a;
   return BOOST_MP_MOVE(t);
}
template <class tag, class A1, class A2, class A3, class A4, class Backend>
inline typename boost::enable_if_c<boost::multiprecision::detail::is_backend<Backend>::value, const number<Backend, et_on> >::type(min)(const detail::expression<tag, A1, A2, A3, A4>& a, const number<Backend, et_on>& b)
{
   number<Backend, et_on> t(a);
   if (t < b)
      return BOOST_MP_MOVE(t);
   return b;
}
template <class tag, class A1, class A2, class A3, class A4, class tagb, class A1b, class A2b, class A3b, class A4b>
inline typename detail::expression<tag, A1, A2, A3, A4>::result_type(min)(const detail::expression<tag, A1, A2, A3, A4>& a, const detail::expression<tagb, A1b, A2b, A3b, A4b>& b)
{
   typename detail::expression<tag, A1, A2, A3, A4>::result_type t1(a), t2(b);
   if (t1 < t2)
      return BOOST_MP_MOVE(t1);
   return BOOST_MP_MOVE(t2);
}
template <class tag, class A1, class A2, class A3, class A4>
inline typename detail::expression<tag, A1, A2, A3, A4>::result_type(min)(const detail::expression<tag, A1, A2, A3, A4>& a, const detail::expression<tag, A1, A2, A3, A4>& b)
{
   typename detail::expression<tag, A1, A2, A3, A4>::result_type t1(a), t2(b);
   if (t1 < t2)
      return BOOST_MP_MOVE(t1);
   return BOOST_MP_MOVE(t2);
}

template <class Backend>
inline typename boost::enable_if_c<boost::multiprecision::detail::is_backend<Backend>::value, const number<Backend, et_on>&>::type(max)(const number<Backend, et_on>& a, const number<Backend, et_on>& b)
{
   return a > b ? a : b;
}
template <class Backend, class tag, class A1, class A2, class A3, class A4>
inline typename boost::enable_if_c<boost::multiprecision::detail::is_backend<Backend>::value, const number<Backend, et_on> >::type(max)(const number<Backend, et_on>& a, const detail::expression<tag, A1, A2, A3, A4>& b)
{
   number<Backend, et_on> t(b);
   if (a > t)
      return a;
   return BOOST_MP_MOVE(t);
}
template <class tag, class A1, class A2, class A3, class A4, class Backend>
inline typename boost::enable_if_c<boost::multiprecision::detail::is_backend<Backend>::value, const number<Backend, et_on> >::type(max)(const detail::expression<tag, A1, A2, A3, A4>& a, const number<Backend, et_on>& b)
{
   number<Backend, et_on> t(a);
   if (t > b)
      return BOOST_MP_MOVE(t);
   return b;
}
template <class tag, class A1, class A2, class A3, class A4, class tagb, class A1b, class A2b, class A3b, class A4b>
inline typename detail::expression<tag, A1, A2, A3, A4>::result_type(max)(const detail::expression<tag, A1, A2, A3, A4>& a, const detail::expression<tagb, A1b, A2b, A3b, A4b>& b)
{
   typename detail::expression<tag, A1, A2, A3, A4>::result_type t1(a), t2(b);
   if (t1 > t2)
      return BOOST_MP_MOVE(t1);
   return BOOST_MP_MOVE(t2);
}
template <class tag, class A1, class A2, class A3, class A4>
inline typename detail::expression<tag, A1, A2, A3, A4>::result_type(max)(const detail::expression<tag, A1, A2, A3, A4>& a, const detail::expression<tag, A1, A2, A3, A4>& b)
{
   typename detail::expression<tag, A1, A2, A3, A4>::result_type t1(a), t2(b);
   if (t1 > t2)
      return BOOST_MP_MOVE(t1);
   return BOOST_MP_MOVE(t2);
}

}} // namespace boost::multiprecision

#endif


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2017.
//
// This software is released under a three-clause BSD license:
//  * 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.
//  * Neither the name of any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// 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 ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS 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.
//
// --------------------------------------------------------------------------
// $Maintainer: Timo Sachsenberg $
// $Authors: $
// --------------------------------------------------------------------------
//

#include <OpenMS/TRANSFORMATIONS/RAW2PEAK/OptimizePeakDeconvolution.h>
#include <boost/math/special_functions/acosh.hpp>

#ifdef DEBUG_DECONV
#include <iostream>
#include <fstream>
#endif

namespace OpenMS
{

  const double OptimizePeakDeconvolution::dist_ = 1.003;

  //TODO: the operator() and the df function need heavy refactoring!!!
  struct OPDFunctor
  {
    int inputs() const { return m_inputs; }
    int values() const { return m_values; }

    OPDFunctor(unsigned dimensions, unsigned numDataPoints, const OptimizePeakDeconvolution::Data* data) :
      m_inputs(dimensions), m_values(numDataPoints), m_data(data){}

    int operator()(const Eigen::VectorXd& x, Eigen::VectorXd& fvec)
    {
      //TODO: holding the parameters to be optimized and additional values in the same vector is
      //      most likely not the best idea. should be split in two vectors.
      //
      // x contains the parameters to be optimized.
      // The first two entries are the left and right width, respectively.They are equal
      // for all peaks. Then the height and position of all peaks are stored.
      //
      // m_data might contain any additional parameters. We handle these using class members
      // instead.
      // The vector f is supposed to contain the result when we return from this function.
      const std::vector<double>& signal = m_data->signal;
      const std::vector<double>& positions = m_data->positions;
      const std::vector<PeakShape>& peaks = m_data->peaks;
      const OptimizationFunctions::PenaltyFactorsIntensity& penalties = m_data->penalties;
      Int charge = m_data->charge;
      double leftwidth = x(0);
      double rightwidth = x(1);
      //double posP1 = x(2);

      // iterate over all points of the signal
      for (Size current_point = 0; current_point < positions.size(); current_point++)
      {
        double computed_signal = 0.;
        double current_position = positions[current_point];
        double experimental_signal = signal[current_point];

        //iterate over all peaks
        for (Size current_peak = 0; current_peak < peaks.size(); current_peak++)
        {
          //Store the current parameters for this peak
          double p_height = x(2 + 2 * current_peak);
          double p_position = x(2 + 2 * current_peak + 1);
          double p_width = (current_position <= p_position) ? leftwidth : rightwidth;

          //is it a Lorentz or a Sech - Peak?
          if (peaks[current_peak].type == PeakShape::LORENTZ_PEAK)
          {
            computed_signal += p_height / (1. + pow(p_width * (current_position - p_position), 2));
          }
          else // It's a Sech - Peak
          {
            computed_signal += p_height / pow(cosh(p_width * (current_position - p_position)), 2);
          }
        }
        fvec(current_point) = computed_signal - experimental_signal;
      }

      // penalties : especially negative heights have to be penalised
      double penalty = 0.;

      double penalty_pos = penalties.pos;
      double penalty_lwidth = penalties.lWidth;
      double penalty_rwidth = penalties.rWidth;
      double penalty_intensity = penalties.height;

      //iterate over all peaks again to compute the penalties
      for (Size current_peak = 0; current_peak < peaks.size(); current_peak++)
      {
        double p_position = x(2 + 2 * current_peak + 1);
        if (current_peak < peaks.size() - 1)
        {

          double next_p_position  = x(2 + 2 * current_peak + 3);
          // if distance between peaks does not match the peptide mass rule
          if (fabs(fabs(p_position - next_p_position) - 1.003 / charge) > 0.05)
          {
            // penalize it
            penalty +=  penalty_pos * 10000
                       * pow(fabs(fabs(p_position - next_p_position) - 1.003 / charge), 2);
          }
        }
        double old_position = peaks[current_peak].mz_position;
        double old_width_l  = peaks[current_peak].left_width;
        double old_width_r = peaks[current_peak].right_width;
        double old_height = peaks[current_peak].height;

        double p_width_l = x(0);
        double p_width_r = x(1);
        double p_height = x(2 + 2 * current_peak);

        if (p_height <  1)
        {
          penalty += 100000* penalty_intensity* pow(fabs(p_height - old_height), 2);

        }
        if (p_width_l < 0)
        {
          penalty += penalty_lwidth * peaks.size() * 10000 * pow(fabs(p_width_l - old_width_l), 2);
        }
        else if (p_width_l < 1.5)
          penalty += 10000 * pow(fabs(p_width_l - old_width_l), 2);
        if (p_width_r < 0)
        {
          penalty += penalty_rwidth * peaks.size() * 10000 * pow(fabs(p_width_r - old_width_r), 2);
        }
        else if (p_width_r < 1.5)
          penalty += 10000 * pow(fabs(p_width_r - old_width_r), 2);
        if (fabs(old_position - p_position) > 0.1)
        {
          penalty += 10000* penalty_pos* pow(fabs(old_position - p_position), 2);
        }
      }
      fvec(fvec.size() - 1) = penalty;

      return 0;
    }

    // compute Jacobian matrix for the different parameters
    int df(const Eigen::VectorXd& x, Eigen::MatrixXd& J)
    {
      // For the conventions on x and params c.f. the commentary in residual()
      //
      // The matrix J is supposed to contain the result when we return from this function.
      // Note: Jacobian is expected as follows:
      //                    - each row corresponds to one data point
      //                    - each column corresponds to one parameter
      const std::vector<double>& positions = m_data->positions;
      const std::vector<PeakShape>& peaks = m_data->peaks;
      const OptimizationFunctions::PenaltyFactorsIntensity& penalties = m_data->penalties;
      Int charge = m_data->charge;

      double leftwidth = x(0);
      double rightwidth = x(1);

      //TODO: is the reset needed?
      J.setZero();

      // iterate over all points of the signal
      for (Size current_point = 0; current_point < positions.size(); current_point++)
      {
        double current_position = positions[current_point];

        // iterate over all peaks
        for (Size current_peak = 0; current_peak < peaks.size(); current_peak++)
        {
          //Store the current parameters for this peak
          double p_height = x(2 + 2 * current_peak);
          double p_position = x(2 + 2 * current_peak + 1);
          double p_width = (current_position <= p_position) ? leftwidth : rightwidth;

          //is it a Lorentz or a Sech - Peak?
          if (peaks[current_peak].type == PeakShape::LORENTZ_PEAK)
          {
            double diff = current_position - p_position;
            double denom_inv = 1. / (1. + pow(p_width * diff, 2));

            double ddl_left
              = (current_position <= p_position) ? -2* p_height* pow(diff, 2) * p_width * pow(denom_inv, 2) : 0;

            double ddl_right
              = (current_position  > p_position) ? -2* p_height* pow(diff, 2) * p_width * pow(denom_inv, 2) : 0;

            // left and right width are the same for all peaks,
            // the sums of the derivations over all peaks are stored in the first two columns
            J(current_point, 0) = J(current_point, 0) + ddl_left;
            J(current_point, 1) = J(current_point, 1) + ddl_right;

            double ddx0    = 2* p_height* pow(p_width, 2) * diff * pow(denom_inv, 2);

            // partial derivation with respect to intensity
            J(current_point, 2 + 2 * current_peak) = denom_inv;

            // partial derivation with respect to the mz-position
            J(current_point, 2 + 2 * current_peak + 1) = ddx0;
          }
          else // It's a Sech - Peak
          {
            double diff      = current_position - p_position;
            double denom_inv = 1. / cosh(p_width * diff);

            // The remaining computations are not stable if denom_inv == 0. In that case, we are far away from the peak
            // and can assume that all derivatives vanish
            double sinh_term = (fabs(denom_inv) < 1e-6) ? 0.0 : sinh(p_width * diff);

            double ddl_left  = (current_position <= p_position)
                               ? -2* p_height* sinh_term* diff* pow(denom_inv, 3) :
                               0;
            double ddl_right = (current_position  > p_position)
                               ? -2* p_height* sinh_term* diff* pow(denom_inv, 3) :
                               0;

            J(current_point, 0) = J(current_point, 0) + ddl_left;
            J(current_point, 1) = J(current_point, 1) + ddl_right;

            double ddx0      = 2* p_height* p_width* sinh_term* pow(denom_inv, 3);

            J(current_point, 2 + 2 * current_peak) = pow(denom_inv, 2);
            J(current_point, 2 + 2 * current_peak + 1) = ddx0;
          }
        }
      }

      /** Now iterate over all peaks again to compute the
       *  penalties.
       */
      for (Size current_peak = 0; current_peak < peaks.size(); current_peak++)
      {

        double penalty_p = 0;
        double p_position = x(2 + 2 * current_peak + 1);
        if (current_peak < peaks.size() - 1)
        {

          double next_p_position  = x(2 + 2 * current_peak + 3);
          // if distance between peaks does not match the peptide mass rule
          if (fabs(fabs(p_position - next_p_position) - 1.003 / charge) > 0.05)
          {
            // penalize it
            penalty_p += penalties.pos * 20000
                         * fabs(fabs(p_position - next_p_position) - 1.003 / charge);

          }
        }
        std::cout << "Eigen penalty_p " << penalty_p << std::endl;
        double p_width_left = x(0);
        double p_width_right = x(1);
        double p_height = x(2 + 2 * current_peak);

        double old_position = peaks[current_peak].mz_position;
        double old_width_left  = peaks[current_peak].left_width;
        double old_width_right = peaks[current_peak].right_width;
        double old_height = peaks[current_peak].height;

        double penalty_h = 0., penalty_l = 0., penalty_r = 0.;
        if (p_height < 1)
        {
          penalty_h += 100000 * 2 * penalties.height * (fabs(p_height) - fabs(old_height));
        }

        if (p_width_left < 0)
        {
          penalty_l += peaks.size() * 2 * penalties.lWidth * 10000 * (fabs(p_width_left - old_width_left));
        }
        else if (p_width_left < 1.5)
          penalty_l += 2 * penalties.lWidth * 10000 * pow(fabs(p_width_left - old_width_left), 2);
        if (p_width_right < 0)
        {
          penalty_r += peaks.size() * 2 * penalties.rWidth * 10000 * (fabs(p_width_right - old_width_right));
        }
        else if (p_width_right < 1.5)
          penalty_r += 2 * penalties.rWidth * 10000 * pow(fabs(p_width_right - old_width_right), 2);
        if (fabs(old_position - p_position) > 0.1)
        {
          penalty_p += 10000 * penalties.pos * 2 * fabs(old_position - p_position);
        }

        J(positions.size(), 2 + 2 * current_peak) = 100 * penalty_h;
        J(positions.size(), 0) = 100 * penalty_l;
        J(positions.size(), 1) = 100 * penalty_r;
        J(positions.size(), 2 + 2 * current_peak + 1) = 100 * penalty_p;
      }
      for (int i = 0; i < J.rows(); ++i)
      {
        for (int j = 0; j < J.cols(); ++j)
          std::cout << J(i, j) << " ";
        std::cout << std::endl;
      }
      std::cout << std::endl;
      return 0;
    }

    const int m_inputs, m_values;
    const OptimizePeakDeconvolution::Data* m_data;
  };

  OptimizePeakDeconvolution::OptimizePeakDeconvolution() :
    DefaultParamHandler("OptimizePeakDeconvolution"), charge_(1)
  {

    defaults_.setValue("max_iteration", 10, "maximal number of iterations for the fitting step");
    defaults_.setValue("eps_abs", 1e-04, "if the absolute error gets smaller than this value the fitting is stopped", ListUtils::create<String>("advanced"));
    defaults_.setValue("eps_rel", 1e-04, "if the relative error gets smaller than this value the fitting is stopped", ListUtils::create<String>("advanced"));

    defaults_.setValue("penalties:left_width", 0.0, "penalty term for the fitting of the left width:" \
                                                    "If the left width gets too broad or negative during the fitting it can be penalized.");
    defaults_.setValue("penalties:right_width", 0.0, "penalty term for the fitting of the right width:" \
                                                     "If the right width gets too broad or negative during the fitting it can be penalized.");
    defaults_.setValue("penalties:height", 0.0, "penalty term for the fitting of the intensity:" \
                                                "If it gets negative during the fitting it can be penalized.");
    defaults_.setValue("penalties:position", 0.0, "penalty term for the fitting of the peak position:" \
                                                  "If the position changes more than 0.5Da during the fitting it can be penalized as well as " \
                                                  "discrepancies of the peptide mass rule.");

    defaults_.setValue("fwhm_threshold", 1.0, "If a peaks is broader than fwhm_threshold, it is assumed that it contains another peaks and an additional peak is added.");

    defaultsToParam_();
  }

  void OptimizePeakDeconvolution::updateMembers_()
  {
    penalties_.rWidth = (float)param_.getValue("penalties:right_width");
    penalties_.lWidth = (float)param_.getValue("penalties:left_width");
    penalties_.height = (float)param_.getValue("penalties:height");
    penalties_.pos    = (float)param_.getValue("penalties:position");

  }

  bool OptimizePeakDeconvolution::optimize(std::vector<PeakShape>& peaks, Data& data)
  {

    if (peaks.empty())
      return true;

#ifdef DEBUG_DECONV
    std::cout << "peaksanzahl:" << peaks.size();
    std::cout << "\tpeaks[0].mz_position:" << peaks[0].mz_position << std::endl;

    for (Size j = 0; j < peaks.size(); ++j)
    {
      std::cout << "\tpeaks[j].mz_position:" << peaks[j].mz_position;
      std::cout << "\tpeaks[j].height:" << peaks[j].height << std::endl;
      std::cout << "\tpeaks[j].left_width:" << peaks[j].left_width;
      std::cout << "\tpeaks[j].right_width:" << peaks[j].right_width << std::endl << std::endl;
    }

    for (Size j = 0; j < data.positions.size(); ++j)
    {
      std::cout << "positions[" << j << "]=" << data.positions[j] << std::endl;
    }

#endif

    // the input peaks are stored in a temporary vector
    std::vector<PeakShape> temp_shapes = peaks;

    Size global_peak_number = 0;

    double min(std::numeric_limits<double>::max());
    Int bestCharge = 0;
    Size bestNumPeaks = 0;
    Eigen::VectorXd bestResult(2 + 2 * data.peaks.size());
    bestResult.setZero();

    // try three different charge states : charge-1, charge, charge +1
    // take the best solution
    Int chargeState = (charge_ - 1 > 1) ? charge_ - 1 : charge_;
    Int firstChargeState = chargeState;
#ifdef DEBUG_DECONV
    std::cout << "charge " << chargeState << " max_charge" << charge_ + 1
              << "\tpeaks.size() " << peaks.size() << std::endl;
#endif
    bestCharge = chargeState;
    bestNumPeaks = peaks.size();
    for (; chargeState < charge_ + 2; ++chargeState)
    {

      setNumberOfPeaks_(data, temp_shapes, chargeState);
      Eigen::VectorXd x_init(2 + 2 * data.peaks.size());
      for (Size i = 0; i < data.peaks.size(); i++)
      {
        x_init(2 + 2 * i) = data.peaks[i].height;
        x_init(3 + 2 * i) = data.peaks[i].mz_position;
      }
      // Initialize the parameters for the optimization
      // all peaks shall have the same width
      double wl = data.peaks[0].left_width;
      double wr = data.peaks[0].right_width;
      if (boost::math::isnan(wl))
      {
        for (Size i = 0; i < data.peaks.size(); ++i)
        {
          data.peaks[i].left_width = 1;
        }
        wl = 1.;
      }
      if (boost::math::isnan(wr))
      {
        for (Size i = 0; i < data.peaks.size(); ++i)
        {
          data.peaks[i].right_width = 1;
        }
        wr = 1.;
      }
      x_init(0) = wl;
      x_init(1) = wr;
      data.penalties = penalties_;
      data.charge = chargeState;
      unsigned numDataPoints = std::max(data.positions.size() + 1, 2 + 2 * data.peaks.size());
      OPDFunctor functor(2, numDataPoints, &data);
      Eigen::LevenbergMarquardt<OPDFunctor> lmSolver(functor);
      Eigen::LevenbergMarquardt<OPDFunctor>::Parameters config;
      config.maxfev = (Int)param_.getValue("max_iteration");
      lmSolver.parameters = config;
      Eigen::LevenbergMarquardtSpace::Status status = lmSolver.minimize(x_init);

      //the states are poorly documented. after checking the source, we believe that
      //all states except NotStarted, Running and ImproperInputParameters are good
      //termination states.
      if (status <= Eigen::LevenbergMarquardtSpace::ImproperInputParameters)
      {
        throw Exception::UnableToFit(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION, "UnableToFit-OptimizePeakDeconvolution", "Could not fit the curve to the data: Error " + String(status));
      }
      double chi = lmSolver.fnorm;
      if ((chargeState == firstChargeState) || (chi < min))
      {

        bestResult = x_init;
        min = chi;
        bestCharge = chargeState;
        bestNumPeaks = data.peaks.size();
      }
    }
    global_peak_number += bestNumPeaks;
    // iterate over all peaks and store the optimized values in peaks
    if (bestNumPeaks > 0)
    {
      peaks.resize(bestNumPeaks);
      for (Size current_peak = 0; current_peak < bestNumPeaks; current_peak++)
      {

        // Store the current parameters for this peak

        peaks[current_peak].left_width  = bestResult(0);
        peaks[current_peak].right_width = bestResult(1);

        peaks[current_peak].height = bestResult(2 + 2 * current_peak);
        peaks[current_peak].mz_position = bestResult(2 + 2 * current_peak + 1);

        // compute the area
        // is it a Lorentz or a Sech - Peak?
        if (peaks[current_peak].type == PeakShape::LORENTZ_PEAK)
        {
          PeakShape p = peaks[current_peak];
          double x_left_endpoint = p.mz_position + 1 / p.left_width * sqrt(p.height / 1 - 1);
          double x_right_endpoint = p.mz_position + 1 / p.right_width * sqrt(p.height / 1 - 1);
#ifdef DEBUG_DECONV
          std::cout << "x_left_endpoint " << x_left_endpoint << " x_right_endpoint " << x_right_endpoint << std::endl;
          std::cout << "p.height" << p.height << std::endl;
#endif
          double area_left = -p.height / p.left_width * atan(p.left_width * (x_left_endpoint - p.mz_position));
          double area_right = -p.height / p.right_width * atan(p.right_width * (p.mz_position - x_right_endpoint));
          peaks[current_peak].area = area_left + area_right;

        }
        else                   //It's a Sech - Peak
        {
          PeakShape p = peaks[current_peak];
          double x_left_endpoint = p.mz_position + 1 / p.left_width * boost::math::acosh(sqrt(p.height / 0.001));
          double x_right_endpoint = p.mz_position + 1 / p.right_width * boost::math::acosh(sqrt(p.height / 0.001));
#ifdef DEBUG_DECONV
          std::cout << "x_left_endpoint " << x_left_endpoint << " x_right_endpoint " << x_right_endpoint << std::endl;
          std::cout << "p.height" << p.height << std::endl;
#endif
          double area_left = -p.height / p.left_width * (sinh(p.left_width * (p.mz_position - x_left_endpoint))
                                                         / cosh(p.left_width * (p.mz_position - x_left_endpoint)));
          double area_right = -p.height / p.right_width * (sinh(p.right_width * (p.mz_position - x_right_endpoint))
                                                           / cosh(p.right_width * (p.mz_position - x_right_endpoint)));
          peaks[current_peak].area = area_left + area_right;

        }

      }
    }
    charge_ = bestCharge;

    return true;
  }

  Size OptimizePeakDeconvolution::getNumberOfPeaks_(Int charge, std::vector<PeakShape>& temp_shapes, Data& data)
  {
    double dist = dist_ / charge;

    data.peaks.clear();

    Size shape = 0;
#ifdef DEBUG_DECONV
    std::cout << "temp_shapes[0].mz_position " << temp_shapes[0].mz_position
              << "\t dist " << dist << "\tp_index " << shape << std::endl;
#endif
    // while the peak's position is smaller than the last considered position
    // take the peak for optimization
    while ((temp_shapes[0].mz_position + shape * dist <
            data.positions[data.positions.size() - 1]) &&
           (shape < temp_shapes.size()))
    {
      data.peaks.push_back(temp_shapes[shape]);
#ifdef DEBUG_DECONV
      std::cout << "temp_shapes[0].mz_position + p_index*dist = " << temp_shapes[0].mz_position + shape * dist << std::endl;
#endif
      ++shape;
    }

    return shape;

  }

  void OptimizePeakDeconvolution::setNumberOfPeaks_(Data& data, const std::vector<PeakShape>& temp_shapes, Int charge)
  {
    double dist = dist_ / charge;

    data.peaks.clear();
#ifdef DEBUG_DECONV
    std::cout << "temp_shapes[0].mz_position " << temp_shapes[0].mz_position
              << "\t dist " << dist << "\tp_index " << shape << std::endl;
#endif
    // while the peak's position is smaller than the last considered position
    // take the peak for optimization
    Size shape = 0;
    while ((temp_shapes[0].mz_position + shape * dist <
            data.positions[data.positions.size() - 1]) &&
           (shape < temp_shapes.size()))
    {
      data.peaks.push_back(temp_shapes[shape]);
#ifdef DEBUG_DECONV
      std::cout << "temp_shapes[0].mz_position + p_index*dist = " << temp_shapes[0].mz_position + shape * dist << std::endl;
#endif
      shape++;
    }
  }

}


/*
 * Copyright (c) 2016, Vanderbilt University
 *
 * 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.
 *
 * Author: Addisu Z. Taddese
 */

#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>

#include <ros/callback_queue.h>
#include <ros/advertise_options.h>
#include <ros/ros.h>

#include <iostream>
#include <vector>
#include <cstdint>

#include "storm_gazebo_ros_magnet/dipole_magnet.h"

namespace gazebo {

DipoleMagnet::DipoleMagnet(): ModelPlugin() {
  this->connect_count = 0;
}

DipoleMagnet::~DipoleMagnet() {
  event::Events::DisconnectWorldUpdateBegin(this->update_connection);
  if (this->should_publish) {
    this->queue.clear();
    this->queue.disable();
    this->rosnode->shutdown();
    this->callback_queue_thread.join();
    delete this->rosnode;
  }
  if (this->mag){
    DipoleMagnetContainer::Get().Remove(this->mag);
  }
}

void DipoleMagnet::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
  // Store the pointer to the model
  this->model = _parent;
  this->world = _parent->GetWorld();
  gzdbg << "Loading DipoleMagnet plugin" << std::endl;

  this->mag = std::make_shared<DipoleMagnetContainer::Magnet>();

  // load parameters
  this->robot_namespace = "";
  if (_sdf->HasElement("robotNamespace"))
    this->robot_namespace = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";

  if(!_sdf->HasElement("bodyName")) {
    gzerr << "DipoleMagnet plugin missing <bodyName>, cannot proceed" << std::endl;
    return;
  }else {
    this->link_name = _sdf->GetElement("bodyName")->Get<std::string>();
  }

  this->link = this->model->GetLink(this->link_name);
  if(!this->link){
    gzerr << "Error: link named " << this->link_name << " does not exist" << std::endl;
    return;
  }

  this->should_publish = false;
  if (_sdf->HasElement("shouldPublish"))
  {
    this->should_publish = _sdf->GetElement("shouldPublish")->Get<bool>();
  }

  if (!_sdf->HasElement("updateRate"))
  {
    gzmsg << "DipoleMagnet plugin missing <updateRate>, defaults to 0.0"
        " (as fast as possible)" << std::endl;
    this->update_rate = 0;
  }
  else
    this->update_rate = _sdf->GetElement("updateRate")->Get<double>();

  if (_sdf->HasElement("calculate")){
    this->mag->calculate = _sdf->Get<bool>("calculate");
  } else
    this->mag->calculate = true;

  if (_sdf->HasElement("dipole_moment")){
    this->mag->moment = _sdf->Get<math::Vector3>("dipole_moment");
  }

  if (_sdf->HasElement("xyzOffset")){
    this->mag->offset.pos = _sdf->Get<math::Vector3>("xyzOffset");
  }

  if (_sdf->HasElement("rpyOffset")){
    math::Vector3 rpy_offset = _sdf->Get<math::Vector3>("rpyOffset");
    this->mag->offset.rot = math::Quaternion(rpy_offset);
  }

  if (this->should_publish) {
    if (!_sdf->HasElement("topicNs"))
    {
      gzmsg << "DipoleMagnet plugin missing <topicNs>,"
          "will publish on namespace " << this->link_name << std::endl;
    }
    else {
      this->topic_ns = _sdf->GetElement("topicNs")->Get<std::string>();
    }

    if (!ros::isInitialized())
    {
      gzerr << "A ROS node for Gazebo has not been initialized, unable to load "
        "plugin. Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in "
        "the gazebo_ros package. If you want to use this plugin without ROS, "
        "set <shouldPublish> to false" << std::endl;
      return;
    }

    this->rosnode = new ros::NodeHandle(this->robot_namespace);
    this->rosnode->setCallbackQueue(&this->queue);

    this->wrench_pub = this->rosnode->advertise<geometry_msgs::WrenchStamped>(
        this->topic_ns + "/wrench", 1,
        boost::bind( &DipoleMagnet::Connect,this),
        boost::bind( &DipoleMagnet::Disconnect,this), ros::VoidPtr(), &this->queue);
    this->mfs_pub = this->rosnode->advertise<sensor_msgs::MagneticField>(
        this->topic_ns + "/mfs", 1,
        boost::bind( &DipoleMagnet::Connect,this),
        boost::bind( &DipoleMagnet::Disconnect,this), ros::VoidPtr(), &this->queue);

    // Custom Callback Queue
    this->callback_queue_thread = boost::thread( boost::bind( &DipoleMagnet::QueueThread,this ) );
  }

  this->mag->model_id = this->model->GetId();

  gzmsg << "Loaded Gazebo dipole magnet plugin on " << this->model->GetName() << std::endl;

  DipoleMagnetContainer::Get().Add(this->mag);

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->update_connection = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&DipoleMagnet::OnUpdate, this, _1));
}

void DipoleMagnet::Connect() {
  this->connect_count++;
}

void DipoleMagnet::Disconnect() {
  this->connect_count--;
}

void DipoleMagnet::QueueThread() {
  static const double timeout = 0.01;

  while (this->rosnode->ok())
  {
    this->queue.callAvailable(ros::WallDuration(timeout));
  }
}

// Called by the world update start event
void DipoleMagnet::OnUpdate(const common::UpdateInfo & /*_info*/) {

  // Calculate the force from all other magnets
  math::Pose p_self = this->link->GetWorldPose();
  p_self.pos += -p_self.rot.RotateVector(this->mag->offset.pos);
  p_self.rot *= this->mag->offset.rot.GetInverse();

  this->mag->pose = p_self;

  if (!this->mag->calculate)
    return;

  DipoleMagnetContainer& dp = DipoleMagnetContainer::Get();

  math::Vector3 moment_world = p_self.rot.RotateVector(this->mag->moment);

  math::Vector3 force(0, 0, 0);
  math::Vector3 torque(0, 0, 0);
  math::Vector3 mfs(0, 0, 0);
  for(DipoleMagnetContainer::MagnetPtrV::iterator it = dp.magnets.begin(); it < dp.magnets.end(); it++){
    std::shared_ptr<DipoleMagnetContainer::Magnet> mag_other = *it;
    if (mag_other->model_id != this->mag->model_id) {
      math::Pose p_other = mag_other->pose;
      math::Vector3 m_other = p_other.rot.RotateVector(mag_other->moment);

      math::Vector3 force_tmp;
      math::Vector3 torque_tmp;
      GetForceTorque(p_self, moment_world, p_other, m_other, force_tmp, torque_tmp);

      force += force_tmp;
      torque += torque_tmp;

      math::Vector3 mfs_tmp;
      GetMFS(p_self, p_other, m_other, mfs_tmp);

      mfs += mfs_tmp;

      this->link->AddForce(force_tmp);
      this->link->AddTorque(torque_tmp);
    }
  }

  this->PublishData(force, torque, mfs);
}

void DipoleMagnet::PublishData(
    const math::Vector3& force,
    const math::Vector3& torque,
    const math::Vector3& mfs){
  if(this->should_publish && this->connect_count > 0) {
    // Rate control
    common::Time cur_time = this->world->GetSimTime();
    if (this->update_rate > 0 &&
        (cur_time-this->last_time).Double() < (1.0/this->update_rate))
      return;

    this->lock.lock();
    // copy data into wrench message
    this->wrench_msg.header.frame_id = "world";
    this->wrench_msg.header.stamp.sec = cur_time.sec;
    this->wrench_msg.header.stamp.nsec = cur_time.nsec;

    this->wrench_msg.wrench.force.x    = force.x;
    this->wrench_msg.wrench.force.y    = force.y;
    this->wrench_msg.wrench.force.z    = force.z;
    this->wrench_msg.wrench.torque.x   = torque.x;
    this->wrench_msg.wrench.torque.y   = torque.y;
    this->wrench_msg.wrench.torque.z   = torque.z;

    // now mfs
    this->mfs_msg.header.frame_id = this->link_name;
    this->mfs_msg.header.stamp.sec = cur_time.sec;
    this->mfs_msg.header.stamp.nsec = cur_time.nsec;

    this->mfs_msg.magnetic_field.x = mfs.x;
    this->mfs_msg.magnetic_field.y = mfs.y;
    this->mfs_msg.magnetic_field.z = mfs.z;

    this->wrench_pub.publish(this->wrench_msg);
    this->mfs_pub.publish(this->mfs_msg);

    this->lock.unlock();
  }
}

void DipoleMagnet::GetForceTorque(const math::Pose& p_self,
    const math::Vector3& m_self,
    const math::Pose& p_other,
    const math::Vector3& m_other,
    math::Vector3& force,
    math::Vector3& torque) {

  bool debug = false;
  math::Vector3 p = p_self.pos - p_other.pos;
  math::Vector3 p_unit = p/p.GetLength();

  math::Vector3 m1 = m_other;
  math::Vector3 m2 = m_self;
  if (debug)
    std::cout << "p: " << p << " m1: " << m1 << " m2: " << m2 << std::endl;

  double K = 3.0*1e-7/pow(p.GetLength(), 4);
  force = K * (m2 * (m1.Dot(p_unit)) +  m1 * (m2.Dot(p_unit)) +
      p_unit*(m1.Dot(m2)) - 5*p_unit*(m1.Dot(p_unit))*(m2.Dot(p_unit)));

  double Ktorque = 1e-7/pow(p.GetLength(), 3);
  math::Vector3 B1 = Ktorque*(3*(m1.Dot(p_unit))*p_unit - m1);
  torque = m2.Cross(B1);
  if (debug)
    std::cout << "B: " << B1 << " K: " << Ktorque << " t: " << torque << std::endl;
}

void DipoleMagnet::GetMFS(const math::Pose& p_self,
    const math::Pose& p_other,
    const math::Vector3& m_other,
    math::Vector3& mfs) {

  // sensor location
  math::Vector3 p = p_self.pos - p_other.pos;
  math::Vector3 p_unit = p/p.GetLength();

  // Get the field at the sensor location
  double K = 1e-7/pow(p.GetLength(), 3);
  math::Vector3 B = K*(3*(m_other.Dot(p_unit))*p_unit - m_other);

  // Rotate the B vector into the capsule/body frame
  math::Vector3 B_body = p_self.rot.RotateVectorReverse(B);

  // Assign vector
  mfs.x = B_body[0];
  mfs.y = B_body[1];
  mfs.z = B_body[2];
}

// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(DipoleMagnet)

}