/* \author Bastian Steder */

#include <iostream>

#include <boost/thread/thread.hpp>
#include <pcl/range_image/range_image.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/features/range_image_border_extractor.h>
#include <pcl/keypoints/narf_keypoint.h>
#include <pcl/features/narf_descriptor.h>
#include <pcl/console/parse.h>

typedef pcl::PointXYZ PointType;

// --------------------
// -----Parameters-----
// --------------------
float angular_resolution = 0.5f;
float support_size = 0.2f;
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
bool setUnseenToMaxRange = false;
bool rotation_invariant = true;

// --------------
// -----Help-----
// --------------
void
printUsage (const char* progName)
{
  std::cout << "\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
            << "Options:\n"
            << "-------------------------------------------\n"
            << "-r <float>   angular resolution in degrees (default "<<angular_resolution<<")\n"
            << "-c <int>     coordinate frame (default "<< (int)coordinate_frame<<")\n"
            << "-m           Treat all unseen points to max range\n"
            << "-s <float>   support size for the interest points (diameter of the used sphere - "
                                                                  "default "<<support_size<<")\n"
            << "-o <0/1>     switch rotational invariant version of the feature on/off"
            <<               " (default "<< (int)rotation_invariant<<")\n"
            << "-h           this help\n"
            << "\n\n";
}

void
setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
{
  Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f (0, 0, 0);
  Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f (0, 0, 1) + pos_vector;
  Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f (0, -1, 0);
  viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2],
                            look_at_vector[0], look_at_vector[1], look_at_vector[2],
                            up_vector[0], up_vector[1], up_vector[2]);
}

// --------------
// -----Main-----
// --------------
int
main (int argc, char** argv)
{
  // --------------------------------------
  // -----Parse Command Line Arguments-----
  // --------------------------------------
  if (pcl::console::find_argument (argc, argv, "-h") >= 0)
  {
    printUsage (argv[0]);
    return 0;
  }
  if (pcl::console::find_argument (argc, argv, "-m") >= 0)
  {
    setUnseenToMaxRange = true;
    cout << "Setting unseen values in range image to maximum range readings.\n";
  }
  if (pcl::console::parse (argc, argv, "-o", rotation_invariant) >= 0)
    cout << "Switching rotation invariant feature version "<< (rotation_invariant ? "on" : "off")<<".\n";
  int tmp_coordinate_frame;
  if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
  {
    coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
    cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
  }
  if (pcl::console::parse (argc, argv, "-s", support_size) >= 0)
    cout << "Setting support size to "<<support_size<<".\n";
  if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
    cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
  angular_resolution = pcl::deg2rad (angular_resolution);

  // ------------------------------------------------------------------
  // -----Read pcd file or create example point cloud if not given-----
  // ------------------------------------------------------------------
  pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
  pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
  pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
  Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
  std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");
  if (!pcd_filename_indices.empty ())
  {
    std::string filename = argv[pcd_filename_indices[0]];
    if (pcl::io::loadPCDFile (filename, point_cloud) == -1)
    {
      cerr << "Was not able to open file \""<<filename<<"\".\n";
      printUsage (argv[0]);
      return 0;
    }
    scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
                                                               point_cloud.sensor_origin_[1],
                                                               point_cloud.sensor_origin_[2])) *
                        Eigen::Affine3f (point_cloud.sensor_orientation_);
    std::string far_ranges_filename = pcl::getFilenameWithoutExtension (filename)+"_far_ranges.pcd";
    if (pcl::io::loadPCDFile (far_ranges_filename.c_str (), far_ranges) == -1)
      std::cout << "Far ranges file \""<<far_ranges_filename<<"\" does not exists.\n";
  }
  else
  {
    setUnseenToMaxRange = true;
    cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n";
    for (float x=-0.5f; x<=0.5f; x+=0.01f)
    {
      for (float y=-0.5f; y<=0.5f; y+=0.01f)
      {
        PointType point;  point.x = x;  point.y = y;  point.z = 2.0f - y;
        point_cloud.points.push_back (point);
      }
    }
    point_cloud.width = (int) point_cloud.points.size ();  point_cloud.height = 1;
  }

  // -----------------------------------------------
  // -----Create RangeImage from the PointCloud-----
  // -----------------------------------------------
  float noise_level = 0.0;
  float min_range = 0.0f;
  int border_size = 1;
  boost::shared_ptr<pcl::RangeImage> range_image_ptr (new pcl::RangeImage);
  pcl::RangeImage& range_image = *range_image_ptr;
  range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
                                   scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
  range_image.integrateFarRanges (far_ranges);
  if (setUnseenToMaxRange)
    range_image.setUnseenToMaxRange ();

  // --------------------------------------------
  // -----Open 3D viewer and add point cloud-----
  // --------------------------------------------
  pcl::visualization::PCLVisualizer viewer ("3D Viewer");
  viewer.setBackgroundColor (1, 1, 1);
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);
  viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
  //viewer.addCoordinateSystem (1.0f, "global");
  //PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
  //viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
  viewer.initCameraParameters ();
  setViewerPose (viewer, range_image.getTransformationToWorldSystem ());

  // --------------------------
  // -----Show range image-----
  // --------------------------
  pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
  range_image_widget.showRangeImage (range_image);

  // --------------------------------
  // -----Extract NARF keypoints-----
  // --------------------------------
  pcl::RangeImageBorderExtractor range_image_border_extractor;
  pcl::NarfKeypoint narf_keypoint_detector;
  narf_keypoint_detector.setRangeImageBorderExtractor (&range_image_border_extractor);
  narf_keypoint_detector.setRangeImage (&range_image);
  narf_keypoint_detector.getParameters ().support_size = support_size;

  pcl::PointCloud<int> keypoint_indices;
  narf_keypoint_detector.compute (keypoint_indices);
  std::cout << "Found "<<keypoint_indices.points.size ()<<" key points.\n";

  // ----------------------------------------------
  // -----Show keypoints in range image widget-----
  // ----------------------------------------------
  //for (size_t i=0; i<keypoint_indices.points.size (); ++i)
    //range_image_widget.markPoint (keypoint_indices.points[i]%range_image.width,
                                  //keypoint_indices.points[i]/range_image.width);

  // -------------------------------------
  // -----Show keypoints in 3D viewer-----
  // -------------------------------------
  pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_ptr (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>& keypoints = *keypoints_ptr;
  keypoints.points.resize (keypoint_indices.points.size ());
  for (size_t i=0; i<keypoint_indices.points.size (); ++i)
    keypoints.points[i].getVector3fMap () = range_image.points[keypoint_indices.points[i]].getVector3fMap ();
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler (keypoints_ptr, 0, 255, 0);
  viewer.addPointCloud<pcl::PointXYZ> (keypoints_ptr, keypoints_color_handler, "keypoints");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");

  // ------------------------------------------------------
  // -----Extract NARF descriptors for interest points-----
  // ------------------------------------------------------
  std::vector<int> keypoint_indices2;
  keypoint_indices2.resize (keypoint_indices.points.size ());
  for (unsigned int i=0; i<keypoint_indices.size (); ++i) // This step is necessary to get the right vector type
    keypoint_indices2[i]=keypoint_indices.points[i];
  pcl::NarfDescriptor narf_descriptor (&range_image, &keypoint_indices2);
  narf_descriptor.getParameters ().support_size = support_size;
  narf_descriptor.getParameters ().rotation_invariant = rotation_invariant;
  pcl::PointCloud<pcl::Narf36> narf_descriptors;
  narf_descriptor.compute (narf_descriptors);
  cout << "Extracted "<<narf_descriptors.size ()<<" descriptors for "
                      <<keypoint_indices.points.size ()<< " keypoints.\n";

  //--------------------
  // -----Main loop-----
  //--------------------
  while (!viewer.wasStopped ())
  {
    range_image_widget.spinOnce ();  // process GUI events
    viewer.spinOnce ();
    pcl_sleep(0.01);
  }
}


#include <boost/graph/adjacency_list.hpp>
#include <fstream>
#include <limits>
#include <vector>
#include <deque>
#include <map>

using namespace std;

struct vertex_info {
	string name;
	bool stop_vertex; //if==1 it's a stop vertex
	string lat, lon; //string to avoid rounding problem
};

struct edge_info {
	string name;
	double weight;  //time, cost, frequency or incomfort
};

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::bidirectionalS,
		vertex_info, edge_info> hypergraph;

typedef hypergraph::vertex_descriptor vertex_descriptor;
typedef hypergraph::edge_descriptor edge_descriptor;
typedef hypergraph::vertex_iterator vertex_iterator;
typedef hypergraph::edge_iterator edge_iterator;
typedef hypergraph::out_edge_iterator out_edge_iterator;
typedef hypergraph::in_edge_iterator in_edge_iterator;

class Hypergraph {

  private:

	hypergraph h;
	static const unsigned int theta = 30;

	vector<edge_descriptor> get_bstar(vertex_descriptor v) {
		vector<edge_descriptor> bstar;
		in_edge_iterator ie_i, ie_end;
		for(boost::tie(ie_i,ie_end)=in_edges(v,h); ie_i!=ie_end; ++ie_i) {
			bstar.push_back(*ie_i);
		}
		return bstar;
	}

  	/*--------------------------------------------- INPUT ---------------------------------------------*/
	bool load_hypergraph(const char* filename) {
		ifstream ifs(filename);
		if(!ifs) {
			cout<<"Failed to open file "<<filename<<endl;
			return false;
		}
		string garbage;
		unsigned int n;
		unsigned int id;
		bool stop_vertex;
		string lat, lon;
		string vertex_name;
		ifs >> n;
		getline(ifs, garbage);
		h = hypergraph(n);
		for(int line=0;line<n;line++) {
			ifs >> id >> stop_vertex >> lat >> lon;
			ifs.ignore(1,' '); //elimina lo spazio prima del nome
			getline(ifs, vertex_name);
			h[id].stop_vertex = stop_vertex;
			h[id].lat = lat;
			h[id].lon = lon;
			h[id].name = vertex_name;
		}
		getline(ifs, garbage);
		unsigned int m, i, j;
		double weight;
		string edge_name;
		ifs >> m;
		getline(ifs, garbage);
		edge_descriptor e;
		for(int line=0;line<m;line++) {
			ifs >> i >> j >> weight;
			ifs.ignore(1,' '); //elimina lo spazio prima del nome
			getline(ifs, edge_name);
			e = boost::add_edge(i,j,h).first;
			h[e].weight = weight;
			h[e].name = edge_name;
		}
		return true;
	}
	/*-------------------------------------------------------------------------------------------------*/

	/*--------------------------------------------- OUTPUT --------------------------------------------*/
	void export_shp(const char* filename, const char* alg, vertex_descriptor s, vertex_descriptor t, double expected_cost[],
			map<vertex_descriptor, double> comb_freq, vector<vertex_descriptor> successors[]) {
		char* filename_shp = (char*)malloc(strlen(filename)+5+strlen(alg)+1+h[s].name.size()+1+h[t].name.size()+1);
		char* s_name = (char*)malloc(h[s].name.size());
		char* t_name = (char*)malloc(h[t].name.size());
		strcpy(filename_shp, filename);
		strcpy(s_name, h[s].name.c_str());
		strcpy(t_name, h[t].name.c_str());
		strcat(filename_shp, "_shp_");
		strcat(filename_shp, alg);
		strcat(filename_shp, "_");
		strcat(filename_shp, s_name);
		strcat(filename_shp, "_");
		strcat(filename_shp, t_name);
		ofstream ofs(filename_shp);
		//nodi
		unsigned int n = num_vertices(h);
		bool in_shp[n];
		for(int i=0; i<n; i++)
			in_shp[i] = 0;
		ofs<<"n                              "<<endl; //segnaposto
		int shp_v_counter = 0;
		print_successors(ofs, shp_v_counter, s, expected_cost, comb_freq, successors, in_shp);
		ofs.seekp(0, ios_base::beg);
		ofs<<shp_v_counter;
		ofs.seekp(0, ios_base::end);
		ofs<<endl;
		//archi
		int edges_start_pos = ofs.tellp();
		ofs<<"m                              "<<endl; //segnaposto
		int shp_e_counter = 0;
		edge_iterator e_i, e_end;
		for(boost::tie(e_i,e_end)=edges(h); e_i!=e_end; ++e_i) {
			vertex_descriptor i, j;
			i = source(*e_i,h);
			j = target(*e_i,h);
			if(in_shp[i] && in_shp[j]) {
				string edge_name = h[*e_i].name;
				double edge_cost;
				double edge_prob;
				if(h[i].stop_vertex) {
					edge_cost = theta / comb_freq[i];
					edge_prob = h[*e_i].weight / comb_freq[i];
				}
				else {
					edge_cost = h[*e_i].weight;
					edge_prob = 1;
				}
				ofs<<i<<" "<<j<<" "<<edge_cost<<" "<<edge_prob<<" "<<edge_name<<endl;
				shp_e_counter++;
			}
		}
		ofs.seekp(edges_start_pos);
		ofs<<shp_e_counter;
		ofs.seekp(0, ios_base::end);
		ofs<<endl;
		ofs.close();
	}

	void print_successors(ofstream& ofs, int& shp_v_counter, vertex_descriptor v, double expected_cost[],
				map<vertex_descriptor, double> comb_freq, vector<vertex_descriptor> successors[], bool in_shp[]) {
		if(!in_shp[v]) {
			in_shp[v] = 1;
			shp_v_counter++;
			ofs<<v<<" "<<h[v].stop_vertex<<" "<<h[v].lat<<" "<<h[v].lon<<" "<<expected_cost[v]<<" "<<comb_freq[v]<<" "<<h[v].name<<endl;
			for(int i=0; i<successors[v].size(); i++) {
				vertex_descriptor suc = successors[v][i];
				print_successors(ofs, shp_v_counter, suc, expected_cost, comb_freq, successors, in_shp);
			}
		}
	}
	/*-------------------------------------------------------------------------------------------------*/

  public:

	/*------------------------------------------ PRIORITY SHT -----------------------------------------*/
	void priority_sht(vertex_descriptor s, vertex_descriptor t, const char* filename) {
		if(!load_hypergraph(filename)) return;
		cout<<"Algoritmo SHT - priority su : "<<filename<<endl; //
		unsigned int iteration_counter = 0; //
		unsigned int n = num_vertices(h);
		//expected_cost, successors and comb_freq as exterior property map
		double expected_cost[n];
		for(int k=0; k<n; ++k)
			expected_cost[k] = std::numeric_limits<double>::infinity();
		map<vertex_descriptor, double> comb_freq;
		for(int k=0; k<n; ++k) {
			if(h[k].stop_vertex)
				comb_freq[k] = 0;
		}
		vector<vertex_descriptor> successors[n];
		deque<vertex_descriptor> Q;
		expected_cost[t] = 0;
		Q.push_back(t);
		while(!Q.empty()) {
			vertex_descriptor v = Q.front();
			Q.pop_front();
			iteration_counter++; //
			cout << iteration_counter << ".  Q --> " << h[v].name << " , Q <-- "; //
			vector<edge_descriptor> bstar = get_bstar(v);
			for(int k=0; k<bstar.size(); ++k) {
				vertex_descriptor u = source(bstar[k], h);
				if(h[u].stop_vertex && expected_cost[u] > expected_cost[v]) {
					if(comb_freq[u]==0) {
						comb_freq[u] = h[bstar[k]].weight;
						expected_cost[u] = (theta/comb_freq[u])+expected_cost[v];
					}
					else {
						comb_freq[u] += h[bstar[k]].weight;
						expected_cost[u] -= (expected_cost[u]-expected_cost[v])*h[bstar[k]].weight/comb_freq[u];
					}
					successors[u].push_back(v);
					if(u!=s) {
						priority_insertion(u,Q,expected_cost);
						cout << h[u].name << "(" << expected_cost[u] << ")  "; //
					}
				}
				else if(!h[u].stop_vertex && expected_cost[u] > expected_cost[v] + h[bstar[k]].weight) {
					expected_cost[u] = expected_cost[v] + h[bstar[k]].weight;
					if(successors[u].empty())
						successors[u].push_back(v);
					else
						successors[u][0] = v;
					if(u!=s) {
						priority_insertion(u,Q,expected_cost);
						cout << h[u].name << "(" << expected_cost[u] << ")  "; //
					}
				}
			}
			cout<<endl; //
		}
		cout<<"Expected cost"<<endl;
		for(int k=0;k<n;++k) {
			cout<<h[k].name<<" - "<<expected_cost[k]<<" - successore/i: ";
			for(int suc=0; suc<successors[k].size(); ++suc)
				cout<<h[successors[k][suc]].name<<" ";
			cout<<endl;
		}
		cout<<"Numero estrazioni dalla coda: "<<iteration_counter<<endl;
		export_shp(filename, "priority", s, t, expected_cost, comb_freq, successors);
	}

	/*inserisce l'elemento in posizione corretta (se è già presente ha sicuramente un costo maggiore), quindi continuo a esplorare il resto del vettore e lo elimino se lo trovo. */
	void priority_insertion(vertex_descriptor u, deque<vertex_descriptor>& Q, double expected_cost[]) {
		int pos = 0;
		while(pos < Q.size() && expected_cost[u] > expected_cost[Q[pos]])
			pos++;
		Q.insert(Q.begin()+pos, u);
		pos++;
		while(pos<Q.size()) {
			if(u==Q[pos]) Q.erase(Q.begin()+pos);
			pos++;
		}
	}
	/*-------------------------------------------------------------------------------------------------*/

	/*---------------------------------------------- SHT ----------------------------------------------*/
	void sht(vertex_descriptor s, vertex_descriptor t, const char* filename) {
		if(!load_hypergraph(filename)) return;
		cout<<"Algoritmo SHT su : "<<filename<<endl; //
		unsigned int iteration_counter = 0; //
		unsigned int n = num_vertices(h);
		//expected_cost, successors and comb_freq as exterior property map
		double expected_cost[n];
		for(int k=0; k<n; ++k)
			expected_cost[k] = std::numeric_limits<double>::infinity();
		map<vertex_descriptor, double> comb_freq;
		for(int k=0; k<n; ++k) {
			if(h[k].stop_vertex)
				comb_freq[k] = 0;
		}
		vector<vertex_descriptor> successors[n];
		deque<vertex_descriptor> Q;
		expected_cost[t] = 0;
		Q.push_back(t);
		while(!Q.empty()) {
			vertex_descriptor v = Q.front();
			Q.pop_front();
			iteration_counter++; //
			cout << iteration_counter << ".  Q --> " << h[v].name << " , Q <-- "; //
			vector<edge_descriptor> bstar = get_bstar(v);
			for(int k=0; k<bstar.size(); ++k) {
				vertex_descriptor u = source(bstar[k], h);
				if(h[u].stop_vertex && expected_cost[u]>expected_cost[v]) {
					expected_cost[u] = attractive_set(u, expected_cost, comb_freq[u], successors[u]);
					if(not_in_queue(u, Q) && u!=s) {
						Q.push_back(u);
						cout << h[u].name << "(" << expected_cost[u] << ")  "; //
					}
				}
				else if(!h[u].stop_vertex && expected_cost[u]>expected_cost[v]+h[bstar[k]].weight) {
					expected_cost[u] = expected_cost[v] + h[bstar[k]].weight;
					if(successors[u].empty())
						successors[u].push_back(v);
					else
						successors[u][0] = v;
					if(not_in_queue(u, Q) && u!=s) {
						Q.push_back(u);
						cout << h[u].name << "(" << expected_cost[u] << ")  ";
					}
				}
			}
			cout<<endl; //
		}
		cout<<"Expected cost"<<endl;
		for(int k=0;k<n;++k) {
			cout<<h[k].name<<" - "<<expected_cost[k]<<" - successore/i: ";
			for(int suc=0; suc<successors[k].size(); ++suc)
				cout<<h[successors[k][suc]].name<<" ";
			cout<<endl;
		}
		cout<<"Numero estrazioni dalla coda: "<<iteration_counter<<endl;
		export_shp(filename, "no_priority", s, t, expected_cost, comb_freq, successors);
	}

	bool not_in_queue(vertex_descriptor u, deque<vertex_descriptor> Q) {
		bool not_in_queue = true;
		int pos = 0;
		while(pos<Q.size() && not_in_queue) {
			if(Q[pos]==u)
				not_in_queue = false;
			pos++;
		}
		return not_in_queue;
	}
	/*-------------------------------------------------------------------------------------------------*/

	/*----------------------------------------- ATTRACTIVE SET ----------------------------------------*/
	double attractive_set(vertex_descriptor u, double expected_cost[], double& comb_freq, vector<vertex_descriptor>& successors) {
		vector<edge_descriptor> sorted_fstar = get_sorted_fstar(u, expected_cost);
		successors.clear();
		edge_descriptor first_edge = sorted_fstar[0];
		successors.push_back(target(first_edge,h));
		comb_freq = h[first_edge].weight;
		double expected_cost_u = (theta/comb_freq) + expected_cost[target(first_edge,h)];
		int index = 1;
		bool stop_criterion = false;
		while(index<sorted_fstar.size() && stop_criterion==false) {
			edge_descriptor next_edge = sorted_fstar[index];
			if(expected_cost[target(next_edge,h)] < expected_cost_u) {
				successors.push_back(target(next_edge,h));
				comb_freq += h[next_edge].weight;
				expected_cost_u -= ((expected_cost_u - expected_cost[target(next_edge,h)]) * h[next_edge].weight)/comb_freq;
				index++;
			}
			else stop_criterion = true;
		}
		return expected_cost_u;
	}

	vector<edge_descriptor> get_sorted_fstar(vertex_descriptor i, double expected_cost[]) {
		vector<edge_descriptor> fstar;
		out_edge_iterator oe_i, oe_end;
		for(boost::tie(oe_i,oe_end)=out_edges(i,h); oe_i!=oe_end; ++oe_i) {
			int pos = 0;
			while(pos<fstar.size() && expected_cost[target(*oe_i,h)] > expected_cost[target(fstar[pos],h)])
				pos++;
			fstar.insert(fstar.begin()+pos, *oe_i);
		}
		return fstar;
	}
	/*-------------------------------------------------------------------------------------------------*/

};


// Copyright 2018 The Simons Foundation, Inc. - 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.

#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include "Lookup/lookup.hpp"
#include "Utils/all_utils.hpp"
#include "abstract_machine.hpp"
#include "rbm_spin.hpp"

#ifndef NETKET_RBM_SPIN_SYMM_HPP
#define NETKET_RBM_SPIN_SYMM_HPP

namespace netket {

// Rbm with permutation symmetries
template <typename T>
class RbmSpinSymm : public AbstractMachine<T> {
  using VectorType = typename AbstractMachine<T>::VectorType;
  using MatrixType = typename AbstractMachine<T>::MatrixType;

  std::vector<std::vector<int>> permtable_;
  int permsize_;

  // number of visible units
  int nv_;

  // ratio of hidden/visible
  int alpha_;

  // number of hidden units
  int nh_;

  // number of parameters
  int npar_;

  // number of parameters without symmetries
  int nbarepar_;

  // weights
  MatrixType W_;

  // weights with symmetries
  MatrixType Wsymm_;

  // visible units bias
  VectorType a_;

  T asymm_;

  // hidden units bias
  VectorType b_;

  VectorType bsymm_;

  VectorType thetas_;
  VectorType lnthetas_;
  VectorType thetasnew_;
  VectorType lnthetasnew_;

  Eigen::MatrixXd DerMatSymm_;

  bool usea_;
  bool useb_;

  const Hilbert &hilbert_;

  const Graph &graph_;

 public:
  using StateType = typename AbstractMachine<T>::StateType;
  using LookupType = typename AbstractMachine<T>::LookupType;

  // Json constructor
  explicit RbmSpinSymm(const Graph &graph, const Hilbert &hilbert,
                       const json &pars)
      : nv_(hilbert.Size()), hilbert_(hilbert), graph_(graph) {
    from_json(pars);
  }

  void Init(const Graph &graph) {
    permtable_ = graph.SymmetryTable();
    permsize_ = permtable_.size();
    nh_ = (alpha_ * permsize_);

    for (int i = 0; i < permsize_; i++) {
      assert(int(permtable_[i].size()) == nv_);
    }

    W_.resize(nv_, nh_);
    a_.resize(nv_);
    b_.resize(nh_);

    thetas_.resize(nh_);
    lnthetas_.resize(nh_);
    thetasnew_.resize(nh_);
    lnthetasnew_.resize(nh_);

    Wsymm_.resize(nv_, alpha_);
    bsymm_.resize(alpha_);

    npar_ = nv_ * alpha_;
    nbarepar_ = nv_ * nh_;

    if (usea_) {
      npar_ += 1;
      nbarepar_ += nv_;
    } else {
      asymm_ = 0;
      a_.setZero();
    }

    if (useb_) {
      npar_ += alpha_;
      nbarepar_ += nh_;
    } else {
      bsymm_.setZero();
      b_.setZero();
    }

    // Constructing the matrix that maps the bare derivatives to the symmetric
    // ones
    DerMatSymm_ = Eigen::MatrixXd::Zero(npar_, nbarepar_);

    int k = 0;
    int kbare = 0;

    if (usea_) {
      // derivative with respect to a
      for (int p = 0; p < nv_; p++) {
        DerMatSymm_(k, p) = 1;
        kbare++;
      }
      k++;
    }

    if (useb_) {
      // derivatives with respect to b
      for (int p = 0; p < nh_; p++) {
        int ksymm = std::floor(double(p) / double(permsize_));
        DerMatSymm_(ksymm + k, kbare) = 1;
        kbare++;
      }
      k += alpha_;
    }

    // derivatives with respect to W
    for (int i = 0; i < nv_; i++) {
      for (int j = 0; j < nh_; j++) {
        int isymm = permtable_.at(j % permsize_).at(i);
        int jsymm = std::floor(double(j) / double(permsize_));
        int ksymm = jsymm + alpha_ * isymm;

        DerMatSymm_(ksymm + k, kbare) = 1;

        kbare++;
      }
    }

    InfoMessage() << "RBM Initizialized with nvisible = " << nv_
                  << " and nhidden = " << nh_ << std::endl;
    InfoMessage() << "Symmetries are being used : " << npar_
                  << " parameters left, instead of " << nbarepar_ << std::endl;
  }

  int Nvisible() const override { return nv_; }

  int Nhidden() const { return nh_; }

  int Npar() const override { return npar_; }

  void InitRandomPars(int seed, double sigma) override {
    VectorType par(npar_);

    netket::RandomGaussian(par, seed, sigma);

    SetParameters(par);
  }

  void InitLookup(const Eigen::VectorXd &v, LookupType &lt) override {
    if (lt.VectorSize() == 0) {
      lt.AddVector(b_.size());
    }
    if (lt.V(0).size() != b_.size()) {
      lt.V(0).resize(b_.size());
    }
    lt.V(0) = (W_.transpose() * v + b_);
  }

  void UpdateLookup(const Eigen::VectorXd &v, const std::vector<int> &tochange,
                    const std::vector<double> &newconf,
                    LookupType &lt) override {
    if (tochange.size() != 0) {
      for (std::size_t s = 0; s < tochange.size(); s++) {
        const int sf = tochange[s];
        lt.V(0) += W_.row(sf) * (newconf[s] - v(sf));
      }
    }
  }

  VectorType BareDerLog(const Eigen::VectorXd &v) {
    VectorType der(nbarepar_);

    int k = 0;

    if (usea_) {
      for (; k < nv_; k++) {
        der(k) = v(k);
      }
    }

    RbmSpin<T>::tanh(W_.transpose() * v + b_, lnthetas_);

    if (useb_) {
      for (int p = 0; p < nh_; p++) {
        der(k) = lnthetas_(p);
        k++;
      }
    }

    for (int i = 0; i < nv_; i++) {
      for (int j = 0; j < nh_; j++) {
        der(k) = lnthetas_(j) * v(i);
        k++;
      }
    }
    return der;
  }

  VectorType DerLog(const Eigen::VectorXd &v) override {
    return DerMatSymm_ * BareDerLog(v);
  }

  VectorType GetParameters() override {
    VectorType pars(npar_);

    int k = 0;

    if (usea_) {
      pars(k) = asymm_;
      k++;
    }

    if (useb_) {
      for (int p = 0; p < (alpha_); p++) {
        pars(k) = bsymm_(p);
        k++;
      }
    }

    for (int i = 0; i < nv_; i++) {
      for (int j = 0; j < alpha_; j++) {
        pars(k) = Wsymm_(i, j);
        k++;
      }
    }

    return pars;
  }

  void SetParameters(const VectorType &pars) override {
    int k = 0;

    if (usea_) {
      asymm_ = pars(k);
      k++;
    } else {
      asymm_ = 0;
    }

    if (useb_) {
      for (int p = 0; p < alpha_; p++) {
        bsymm_(p) = pars(k);
        k++;
      }
    } else {
      bsymm_ = VectorType::Zero(alpha_);
    }

    for (int i = 0; i < nv_; i++) {
      for (int j = 0; j < alpha_; j++) {
        Wsymm_(i, j) = pars(k);
        k++;
      }
    }

    SetBareParameters();
  }

  void SetBareParameters() {
    // Setting the bare values of the RBM parameters
    for (int i = 0; i < nv_; i++) {
      a_(i) = asymm_;
    }

    for (int j = 0; j < nh_; j++) {
      int jsymm = std::floor(double(j) / double(permsize_));
      b_(j) = bsymm_(jsymm);
    }

    for (int i = 0; i < nv_; i++) {
      for (int j = 0; j < nh_; j++) {
        int jsymm = std::floor(double(j) / double(permsize_));
        W_(i, j) = Wsymm_(permtable_[j % permsize_][i], jsymm);
      }
    }
  }

  // Value of the logarithm of the wave-function
  T LogVal(const Eigen::VectorXd &v) override {
    RbmSpin<T>::lncosh(W_.transpose() * v + b_, lnthetas_);

    return (v.dot(a_) + lnthetas_.sum());
  }

  // Value of the logarithm of the wave-function
  // using pre-computed look-up tables for efficiency
  T LogVal(const Eigen::VectorXd &v, const LookupType &lt) override {
    RbmSpin<T>::lncosh(lt.V(0), lnthetas_);

    return (v.dot(a_) + lnthetas_.sum());
  }

  // Difference between logarithms of values, when one or more visible variables
  // are being flipped
  VectorType LogValDiff(
      const Eigen::VectorXd &v, const std::vector<std::vector<int>> &tochange,
      const std::vector<std::vector<double>> &newconf) override {
    const std::size_t nconn = tochange.size();
    VectorType logvaldiffs = VectorType::Zero(nconn);

    thetas_ = (W_.transpose() * v + b_);
    RbmSpin<T>::lncosh(thetas_, lnthetas_);

    T logtsum = lnthetas_.sum();

    for (std::size_t k = 0; k < nconn; k++) {
      if (tochange[k].size() != 0) {
        thetasnew_ = thetas_;

        for (std::size_t s = 0; s < tochange[k].size(); s++) {
          const int sf = tochange[k][s];

          logvaldiffs(k) += a_(sf) * (newconf[k][s] - v(sf));

          thetasnew_ += W_.row(sf) * (newconf[k][s] - v(sf));
        }

        RbmSpin<T>::lncosh(thetasnew_, lnthetasnew_);
        logvaldiffs(k) += lnthetasnew_.sum() - logtsum;
      }
    }
    return logvaldiffs;
  }

  // Difference between logarithms of values, when one or more visible variables
  // are being flipped Version using pre-computed look-up tables for efficiency
  // on a small number of spin flips
  T LogValDiff(const Eigen::VectorXd &v, const std::vector<int> &tochange,
               const std::vector<double> &newconf,
               const LookupType &lt) override {
    T logvaldiff = 0.;

    if (tochange.size() != 0) {
      RbmSpin<T>::lncosh(lt.V(0), lnthetas_);

      thetasnew_ = lt.V(0);

      for (std::size_t s = 0; s < tochange.size(); s++) {
        const int sf = tochange[s];

        logvaldiff += a_(sf) * (newconf[s] - v(sf));

        thetasnew_ += W_.row(sf) * (newconf[s] - v(sf));
      }

      RbmSpin<T>::lncosh(thetasnew_, lnthetasnew_);
      logvaldiff += (lnthetasnew_.sum() - lnthetas_.sum());
    }
    return logvaldiff;
  }

  const Hilbert &GetHilbert() const { return hilbert_; }

  void to_json(json &j) const override {
    j["Machine"]["Name"] = "RbmSpinSymm";
    j["Machine"]["Nvisible"] = nv_;
    j["Machine"]["Alpha"] = alpha_;
    j["Machine"]["UseVisibleBias"] = usea_;
    j["Machine"]["UseHiddenBias"] = useb_;
    j["Machine"]["asymm"] = asymm_;
    j["Machine"]["bsymm"] = bsymm_;
    j["Machine"]["Wsymm"] = Wsymm_;
  }

  void from_json(const json &pars) override {
    if (pars.at("Machine").at("Name") != "RbmSpinSymm") {
      throw InvalidInputError(
          "Error while constructing RbmSpinSymm from Json input");
    }

    if (FieldExists(pars["Machine"], "Nvisible")) {
      nv_ = pars["Machine"]["Nvisible"];
    }
    if (nv_ != hilbert_.Size()) {
      throw InvalidInputError(
          "Number of visible units is incompatible with given "
          "Hilbert space");
    }

    alpha_ = FieldVal(pars["Machine"], "Alpha", "Machine");

    usea_ = FieldOrDefaultVal(pars["Machine"], "UseVisibleBias", true);
    useb_ = FieldOrDefaultVal(pars["Machine"], "UseHiddenBias", true);

    Init(graph_);

    // Loading parameters, if defined in the input
    if (FieldExists(pars["Machine"], "asymm")) {
      asymm_ = pars["Machine"]["asymm"].get<T>();
    } else {
      asymm_ = 0;
    }

    if (FieldExists(pars["Machine"], "bsymm")) {
      bsymm_ = pars["Machine"]["bsymm"];
    } else {
      bsymm_.setZero();
    }
    if (FieldExists(pars["Machine"], "Wsymm")) {
      Wsymm_ = pars["Machine"]["Wsymm"];
    }

    SetBareParameters();
  }
};

}  // namespace netket

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_SQR_ABS_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_SQR_ABS_HPP_INCLUDED

#include <boost/simd/arithmetic/functions/sqr_abs.hpp>
#include <boost/simd/include/functions/scalar/sqr.hpp>
#include <boost/simd/include/functions/scalar/abss.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/dispatch/meta/upgrade.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::sqr_abs_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename dispatch::meta::upgrade<A0>::type uptype;
      uptype tmp = sqr(static_cast<uptype>(a0));
      return (tmp > Valmax<result_type>()) ? Valmax<result_type>()
                                           : static_cast<result_type>(tmp);
    }
  };
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::sqr_abs_, tag::cpu_
                            , (A0)
                            , (scalar_< ints64_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      if (a0 == 0) return Zero<A0>();
      A0 aa0 =  abss(a0);
      A0 z = Valmax<A0>()/aa0;
      return (z < aa0) ? Valmax<A0>() : sqr(a0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::sqr_abs_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return sqr(a0);
    }
  };
} } }

#endif


#define BOOST_TEST_MODULE "test_PDNS_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/utility.hpp>

#include <mjolnir/forcefield/PDNS/ProteinDNANonSpecificInteraction.hpp>
#include <mjolnir/core/VerletList.hpp>
#include <mjolnir/core/SpatialPartitionBase.hpp>
#include <mjolnir/core/NaivePairCalculation.hpp>

BOOST_AUTO_TEST_CASE(PDNS_Interaction)
{
    namespace test = mjolnir::test;
    mjolnir::LoggerManager::set_default_logger("test_pdns_interaction.log");

    using real_type         = double;
    using traits_type       = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type         = traits_type::real_type;
    using coord_type        = traits_type::coordinate_type;
    using boundary_type     = traits_type::boundary_type;
    using system_type       = mjolnir::System<traits_type>;
    using topology_type     = mjolnir::Topology;

    using interaction_type       = mjolnir::ProteinDNANonSpecificInteraction<traits_type>;
    using potential_type         = typename interaction_type::potential_type;
    using parameter_list_type    = typename interaction_type::parameter_list_type;
    using contact_parameter_type = typename parameter_list_type::contact_parameter_type;
    using dna_index_type         = typename parameter_list_type::dna_index_type;
    using ignore_molecule_type   = typename parameter_list_type::ignore_molecule_type;
    using ignore_group_type      = typename parameter_list_type::ignore_group_type;
    using partition_type         = mjolnir::NaivePairCalculation<traits_type, potential_type>;

    //  0 o              |
    //     \             |
    //    1 o === o 3    |
    //     /       \     |
    //  2 o         o 4  |

    constexpr auto pi  = mjolnir::math::constants<real_type>::pi();

    const real_type r0     = 5.0;
    const real_type theta0 = pi * 0.5;
    const real_type phi0   = pi * 2.0 / 3.0;

    const real_type sigma  = 1.0;
    const real_type delta  = pi / 18.0;

    const contact_parameter_type p_pro{1, 0, 2, -1.2, r0, theta0, phi0, r0+5.0, (r0+5.0)*(r0+5.0)};
    const dna_index_type p_dna{3, 4};

    parameter_list_type parameter_list(
        sigma, delta, 5.0, {p_pro}, {p_dna}, {/*exclusion*/},
        ignore_molecule_type("Nothing"), ignore_group_type({}));

    interaction_type interaction(potential_type{},
        parameter_list_type(parameter_list),
        mjolnir::SpatialPartition<traits_type, potential_type>(
            mjolnir::make_unique<partition_type>()));

    topology_type topol(5);
    topol.add_connection(0, 1, "bond");
    topol.add_connection(1, 2, "bond");
    topol.add_connection(3, 4, "bond");
    topol.construct_molecules();

    system_type sys(5, boundary_type{});
    test::clear_everything(sys);

    interaction.initialize(sys, topol);

    std::mt19937 rng(123456789);

    for(const auto r     : {r0 - 0.2, r0 + 0.5})
    {
    for(const auto theta : {theta0 + delta * 0.2,
                            theta0 + delta * 0.7,
                            theta0 + delta * 1.2,
                            theta0 - delta * 1.2,
                            theta0 - delta * 0.7,
                            theta0 - delta * 0.2})
    {
    for(const auto phi   : {phi0 + delta * 0.2,
                            phi0 + delta * 0.7,
                            phi0 + delta * 1.2,
                            phi0 - delta * 1.2,
                            phi0 - delta * 0.7,
                            phi0 - delta * 0.2})
    {
        BOOST_TEST_MESSAGE("=======================================================");
        BOOST_TEST_MESSAGE("r = " << r << ", theta = " << theta << ", phi = " << phi);
        BOOST_TEST_MESSAGE("theta0 = " << theta0 << ", delta = " << delta);

        test::clear_everything(sys);

        //  2 o              |
        //     \             |
        //  ^ 1 o === o 3    |
        //     /       \     |
        //  0 o         o 4  |

        sys.position(1) = mjolnir::math::make_coordinate<coord_type>(0.0, 0.0, 0.0);
        sys.position(0) = mjolnir::math::make_coordinate<coord_type>(
                -std::cos(theta) - 1.0, -std::sin(theta), 0.0);
        sys.position(2) = mjolnir::math::make_coordinate<coord_type>(
                 std::cos(theta) - 1.0,  std::sin(theta), 0.0);

        sys.position(3) = mjolnir::math::make_coordinate<coord_type>(
                r, 0.0, 0.0);
        sys.position(4) = mjolnir::math::make_coordinate<coord_type>(
                r + std::cos(pi-phi), -std::sin(pi-phi), 0.0);

        test::apply_random_rotation(sys, rng);

        // check configuration is okay
        {
            // XXX here it does not use boundary condition.
            const auto v13 = sys.position(3) - sys.position(1);
            BOOST_TEST_REQUIRE(mjolnir::math::length(v13) == r,
                               boost::test_tools::tolerance(1e-3));

            const auto v02 = sys.position(2) - sys.position(0);
            const auto dot_1302 = mjolnir::math::dot_product(v02, v13);
            const auto cos_1302 = mjolnir::math::clamp<real_type>(dot_1302 *
                mjolnir::math::rlength(v13) * mjolnir::math::rlength(v02), -1, 1);
            BOOST_TEST_REQUIRE(std::acos(cos_1302) == theta,
                               boost::test_tools::tolerance(1e-3));

            const auto v43 = sys.position(3) - sys.position(4);
            const auto dot_134 = mjolnir::math::dot_product(v13, v43);
            const auto cos_134 = mjolnir::math::clamp<real_type>(dot_134 *
                mjolnir::math::rlength(v13) * mjolnir::math::rlength(v43), -1, 1);
            BOOST_TEST_REQUIRE(std::acos(cos_134) == phi,
                               boost::test_tools::tolerance(1e-3));
        }

        test::apply_random_perturbation(sys, rng, 0.01);

        constexpr real_type tol = 1e-4;
        constexpr real_type dr  = 1e-5;

        test::check_force(sys, interaction, tol, dr);
        test::check_virial(sys, interaction, tol);
        test::check_force_and_virial(sys, interaction, tol);
        test::check_force_and_energy(sys, interaction, tol);
        test::check_force_energy_virial(sys, interaction, tol);

    } // theta2
    } // theta1
    } // rbp
}


#pragma once

#include <crest/util/eigen_extensions.hpp>

#include <Eigen/Sparse>

#include <stdexcept>

namespace crest
{
    namespace wave
    {
        class ConvergenceError : std::runtime_error {
        public:
            explicit ConvergenceError(const std::string & what) : std::runtime_error(what) {}
        };

        template <typename Scalar>
        struct InitialConditions
        {
            VectorX<Scalar> u0_h;
            VectorX<Scalar> v0_h;
            VectorX<Scalar> u0_tt_h;
        };

        template <typename Scalar>
        class Initializer
        {
        public:
            virtual ~Initializer() {}

            virtual VectorX<Scalar> initialize(const InitialConditions<Scalar> & initial_conditions,
                                               Scalar dt) const = 0;
        };

        template <typename Scalar>
        class Integrator
        {
        public:
            virtual ~Integrator() {}

            /**
             * Tolerance for iterative solver if the integrator uses an iterative solver, otherwise ignored.
             */
            void set_tolerance(Scalar tol) { _tol = tol; }
            Scalar tolerance() const { return _tol; }

            virtual void setup(Scalar dt, Eigen::SparseMatrix<Scalar> stiffness, Eigen::SparseMatrix<Scalar> mass) = 0;

            virtual VectorX<Scalar> next(int step_index,
                                         Scalar dt,
                                         const VectorX<Scalar> & current_solution,
                                         const VectorX<Scalar> & previous_solution,
                                         const VectorX<Scalar> & load_next,
                                         const VectorX<Scalar> & load_current,
                                         const VectorX<Scalar> & load_previous) = 0;

        protected:
            Integrator() : _tol(Scalar(10) * std::numeric_limits<Scalar>::epsilon()) {}

        private:
            Scalar _tol;
        };

        /**
         * Computes u_h^1 = u_h^0 + dt * u_h_t^0 + 0.5 * dt^2 u_h_tt^0
         */
        template <typename Scalar>
        class SeriesExpansionInitializer : public Initializer<Scalar>
        {
        public:
            explicit SeriesExpansionInitializer() {}

            virtual VectorX<Scalar> initialize(const InitialConditions<Scalar> & initial_conditions,
                                               Scalar dt) const override
            {
                const auto & ic = initial_conditions;
                return ic.u0_h + dt * ic.v0_h + 0.5 * dt * dt * ic.u0_tt_h;
            }
        };

        template <typename Scalar>
        class DirectCrankNicolson : public Integrator<Scalar>
        {
        public:
            explicit DirectCrankNicolson() {}

            virtual void setup(Scalar dt, Eigen::SparseMatrix<Scalar> stiffness, Eigen::SparseMatrix<Scalar> mass)
            {
                _mass = std::move(mass);
                _stiffness = std::move(stiffness);

                const auto C = mass + Scalar(0.25) * dt * dt * stiffness;
                _c_factor.compute(C);
            }

            virtual VectorX<Scalar> next(int step_index,
                                         Scalar dt,
                                         const VectorX<Scalar> & x_curr,
                                         const VectorX<Scalar> & x_prev,
                                         const VectorX<Scalar> & b_prev,
                                         const VectorX<Scalar> & b_curr,
                                         const VectorX<Scalar> & b_next)
            {
                (void) step_index;

                const auto & M = _mass;
                const auto & A = _stiffness;
                const auto dt2 = dt * dt;

                const auto rhs = M * (Scalar(2.0) * x_curr - x_prev)
                                 - 0.25 * dt2 * A * (Scalar(2.0) * x_curr + x_prev)
                                 + 0.25 * dt2 * (b_next + Scalar(2.0) * b_curr + b_prev);

                const VectorX<Scalar> x_next = _c_factor.solve(rhs);

                if (_c_factor.info() != Eigen::Success)
                {
                    throw ConvergenceError("Integration did not converge.");
                }
                return x_next;
            }

        private:
            Eigen::SparseMatrix<Scalar> _stiffness;
            Eigen::SparseMatrix<Scalar> _mass;

            typedef Eigen::SimplicialLDLT<Eigen::SparseMatrix<Scalar>> Factorization;
            Factorization _c_factor;
        };

        template <typename Scalar>
        class IterativeCrankNicolson : public Integrator<Scalar>
        {
        public:
            explicit IterativeCrankNicolson() {}

            virtual void setup(Scalar dt, Eigen::SparseMatrix<Scalar> stiffness, Eigen::SparseMatrix<Scalar> mass)
            {
                _mass = std::move(mass);
                _stiffness = std::move(stiffness);

                const auto C = mass + Scalar(0.25) * dt * dt * stiffness;

                // Eigen's Conjugate gradient applies diagonal preconditioning by default
                _cg.compute(C);
                _cg.setTolerance(Integrator<Scalar>::tolerance());
            }

            virtual VectorX<Scalar> next(int step_index,
                                         Scalar dt,
                                         const VectorX<Scalar> & x_curr,
                                         const VectorX<Scalar> & x_prev,
                                         const VectorX<Scalar> & b_prev,
                                         const VectorX<Scalar> & b_curr,
                                         const VectorX<Scalar> & b_next)
            {
                (void) step_index;

                const auto & M = _mass;
                const auto & A = _stiffness;
                const auto dt2 = dt * dt;

                const auto rhs = M * (Scalar(2.0) * x_curr - x_prev)
                                 - 0.25 * dt2 * A * (Scalar(2.0) * x_curr + x_prev)
                                 + 0.25 * dt2 * (b_next + Scalar(2.0) * b_curr + b_prev);

                // Use x_curr as starting guess for CG
                const VectorX<Scalar> x_next = _cg.solveWithGuess(rhs, x_curr);

                if (_cg.info() != Eigen::Success)
                {
                    throw ConvergenceError("Integration did not converge.");
                }
                return x_next;
            }

        private:
            Eigen::SparseMatrix<Scalar> _stiffness;
            Eigen::SparseMatrix<Scalar> _mass;

            Eigen::ConjugateGradient<Eigen::SparseMatrix<Scalar>> _cg;
        };

        template <typename Scalar>
        class IterativeLeapfrog : public Integrator<Scalar>
        {
        public:
            explicit IterativeLeapfrog() {}

            virtual void setup(Scalar dt, Eigen::SparseMatrix<Scalar> stiffness, Eigen::SparseMatrix<Scalar> mass)
            {
                (void) dt;

                _mass = std::move(mass);
                _stiffness = std::move(stiffness);

                // Eigen's Conjugate gradient applies diagonal preconditioning by default
                _cg.compute(_mass);
                _cg.setTolerance(Integrator<Scalar>::tolerance());
            }

            virtual VectorX<Scalar> next(int step_index,
                                         Scalar dt,
                                         const VectorX<Scalar> & x_curr,
                                         const VectorX<Scalar> & x_prev,
                                         const VectorX<Scalar> & b_prev,
                                         const VectorX<Scalar> & b_curr,
                                         const VectorX<Scalar> & b_next)
            {
                (void) step_index;
                (void) b_prev;
                (void) b_next;

                const auto & M = _mass;
                const auto & A = _stiffness;
                const auto dt2 = dt * dt;

                const auto rhs = M * (Scalar(2.0) * x_curr - x_prev)
                                 + dt2 * (b_curr - A * x_curr);

                // Use x_curr as starting guess for CG
                const VectorX<Scalar> x_next = _cg.solveWithGuess(rhs, x_curr);

                if (_cg.info() != Eigen::Success)
                {
                    throw ConvergenceError("Integration did not converge.");
                }
                return x_next;
            }

        private:
            Eigen::SparseMatrix<Scalar> _stiffness;
            Eigen::SparseMatrix<Scalar> _mass;

            Eigen::ConjugateGradient<Eigen::SparseMatrix<Scalar>> _cg;
        };

        namespace detail
        {
            template <typename Scalar>
            Eigen::DiagonalMatrix<Scalar, Eigen::Dynamic> lump_matrix(const Eigen::SparseMatrix<Scalar> & mat)
            {
                VectorX<Scalar> row_wise_sum = mat * VectorX<Scalar>::Ones(mat.cols());
                return Eigen::DiagonalMatrix<Scalar, Eigen::Dynamic>(std::move(row_wise_sum));
            };
        }

        template <typename Scalar>
        class LumpedLeapfrog : public Integrator<Scalar>
        {
        public:
            explicit LumpedLeapfrog() {}

            virtual void setup(Scalar dt,
                               Eigen::SparseMatrix<Scalar> stiffness,
                               Eigen::SparseMatrix<Scalar> mass) override
            {
                (void) dt;

                _diagonal_mass = detail::lump_matrix(mass);
                _stiffness = std::move(stiffness);
            }

            virtual VectorX<Scalar> next(int step_index,
                                         Scalar dt,
                                         const VectorX<Scalar> & x_curr,
                                         const VectorX<Scalar> & x_prev,
                                         const VectorX<Scalar> & b_prev,
                                         const VectorX<Scalar> & b_curr,
                                         const VectorX<Scalar> & b_next) override
            {
                (void) step_index;
                (void) b_prev;
                (void) b_next;

                const auto & M = _diagonal_mass;
                const auto & A = _stiffness;
                const auto dt2 = dt * dt;

                const auto rhs = M * (Scalar(2.0) * x_curr - x_prev)
                                 + dt2 * (b_curr - A * x_curr);

                // Inverting M is just a diagonal matrix inversion
                const VectorX<Scalar> x_next = M.inverse() * rhs;

                return x_next;
            }

        private:
            Eigen::SparseMatrix<Scalar> _stiffness;
            Eigen::DiagonalMatrix<Scalar, Eigen::Dynamic> _diagonal_mass;
        };

    }
}


#ifndef LAGRANGEROOTNODE_HPP
#define LAGRANGEROOTNODE_HPP

#include "stochasticInput.hpp"
#include <boost/scoped_ptr.hpp>

template <typename LagrangeSolver, typename RecourseSolver> void lagrangeRootNode(stochasticInput &input,
	MPI_Comm comm = MPI_COMM_WORLD) {
	using namespace std;
	using boost::scoped_ptr;

	BAContext ctx(comm);
	ctx.initializeAssignment(input.nScenarios());

	int nvar1 = input.nFirstStageVars();
	int nscen = input.nScenarios();
	int mype = ctx.mype();

	const vector<int> &localScen = ctx.localScenarios();
	vector<double> lagrangeObjs;
	vector<vector<double> > lagrangeSolutions;
	double objsum_local = 0.0;
	for (unsigned i = 1; i < localScen.size(); i++) {
		int scen = localScen[i];
		LagrangeSolver lsol(input, scen, vector<double>(nvar1,0.0));
		lsol.go();
		printf("Objective from scen %d: %f %f\n",scen,lsol.getBestPossibleObjective(),lsol.getBestFeasibleObjective());
		objsum_local += lsol.getBestPossibleObjective();
		lagrangeObjs.push_back(lsol.getBestPossibleObjective());
		lagrangeSolutions.push_back(lsol.getBestFirstStageSolution());
	}
	double lagrangelb;
	MPI_Allreduce(&objsum_local,&lagrangelb,1,MPI_DOUBLE,MPI_SUM,comm);

	const vector<double> &obj1 = input.getFirstStageObj();

	// evaluate solutions
	vector<double> bestSolution;
	double bestObj = COIN_DBL_MAX;

	int nvar2 = input.nSecondStageVars(0);
	int ncons2 = input.nSecondStageCons(0);

	vector<variableState> rowSave(ncons2), colSave(nvar2);
	bool havesave = false;

	vector<vector<double> > uniqueSolutions;
	vector<double> objs;
	vector<int> freqCount;

	vector<double> unionSolution(nvar1,0.0);
	double unionobj;

	for (int scen_ = 0; scen_ < nscen; scen_++) {
		vector<double> curSolution(nvar1);

		if (mype == ctx.owner(scen_)) {
			int idx = find(localScen.begin(),localScen.end(),scen_)-localScen.begin()-1;
			curSolution = lagrangeSolutions[idx];
		}
		MPI_Bcast(&curSolution[0],nvar1,MPI_DOUBLE,ctx.owner(scen_),comm);
		for (int i =0 ; i < nvar1; i++) cout <<curSolution[i] << " "; cout << endl;

		bool diff = true;
		for (unsigned r = 0; r < uniqueSolutions.size(); r++) {
			const vector<double>& v = uniqueSolutions[r];
			diff = false;
			for (int i = 0; i < nvar1; i++) {
				if (fabs(v[i]-1.0) < 1e-5) unionSolution[i] = 1.0;
				if (fabs(v[i]-curSolution[i]) > 1e-5) {
					diff = true;
					break;
				}
			}
			if (!diff) {
				freqCount[r]++;
				break;
			} else {
				diff = true;
			}
		}
		if (!diff) continue;

		uniqueSolutions.push_back(curSolution);
		freqCount.push_back(1);

		double sum = 0.0;
		bool infeas = false;
		for (unsigned q = 1; q < localScen.size(); q++) {
			int scen = localScen[q];
			RecourseSolver rsol(input, scen, curSolution);
			rsol.setDualObjectiveLimit(1e7);

			if (input.scenarioDimensionsEqual() && input.continuousRecourse() && havesave) {
				for (int r = 0; r < nvar2; r++) {
					rsol.setSecondStageColState(r,colSave[r]);
				}
				for (int r = 0; r < ncons2; r++) {
					rsol.setSecondStageRowState(r,rowSave[r]);
				}
			}
			rsol.go();
			sum += rsol.getObjective();

			if (rsol.getStatus() == ProvenInfeasible) {
				printf("got infeasible 1st stage\n");
				infeas = true; break;
			}
			assert(rsol.getStatus() == Optimal);
			if (input.scenarioDimensionsEqual() && input.continuousRecourse() && !havesave) {
				for (int r = 0; r < nvar2; r++) {
					colSave[r] = rsol.getSecondStageColState(r);
				}
				for (int r = 0; r < ncons2; r++) {
					rowSave[r] = rsol.getSecondStageRowState(r);
				}
				havesave = true;
			}

		}
		if (infeas) { sum += COIN_DBL_MAX; }
		double sum_all;
		MPI_Allreduce(&sum,&sum_all,1,MPI_DOUBLE,MPI_SUM,comm);
		for (int k = 0; k < nvar1; k++) sum_all += curSolution[k]*obj1[k];

		objs.push_back(sum_all);

		if (sum_all < bestObj) {
			bestObj = sum_all;
			bestSolution = curSolution;
		}

	}

	{
		assert(input.scenarioDimensionsEqual());
		double sum = 0.0;
		bool infeas = false;
		for (unsigned q = 1; q < localScen.size(); q++) {
			int scen = localScen[q];
			RecourseSolver rsol(input, scen, unionSolution);
			rsol.setDualObjectiveLimit(1e7);

			if (input.continuousRecourse() && havesave) {
				for (int r = 0; r < nvar2; r++) {
					rsol.setSecondStageColState(r,colSave[r]);
				}
				for (int r = 0; r < ncons2; r++) {
					rsol.setSecondStageRowState(r,rowSave[r]);
				}
			}
			rsol.go();
			sum += rsol.getObjective();

			if (rsol.getStatus() == ProvenInfeasible) {
				printf("got infeasible 1st stage\n");
				infeas = true; break;
			}
			assert(rsol.getStatus() == Optimal);
			if (input.continuousRecourse() && !havesave) {
				for (int r = 0; r < nvar2; r++) {
					colSave[r] = rsol.getSecondStageColState(r);
				}
				for (int r = 0; r < ncons2; r++) {
					rowSave[r] = rsol.getSecondStageRowState(r);
				}
				havesave = true;
			}

		}
		if (infeas) { sum += COIN_DBL_MAX; }
		double sum_all;
		MPI_Allreduce(&sum,&sum_all,1,MPI_DOUBLE,MPI_SUM,comm);
		for (int k = 0; k < nvar1; k++) sum_all += unionSolution[k]*obj1[k];
		unionobj = sum_all;
	}
	if (mype == 0) {
		printf("%lu unique solutions\nCount\tObj\n", uniqueSolutions.size());
		for (unsigned r = 0; r < uniqueSolutions.size(); r++) {
			printf("%d\t", freqCount[r]);
			if (objs[r] > 1e20) {
				printf("inf\n");
			} else {
				printf("%f\n",objs[r]);
			}
		}
		printf("Lagrange LB: %f\n",lagrangelb);
		printf("Best UB: %f\n",bestObj);
		printf("Union UB: %f\n",unionobj);
		// formula for positive values
		printf("Gap: %f%%\n",100*fabs(bestObj-lagrangelb)/fabs(lagrangelb));
	}

}

#endif


#include <feature_stack.h>
#include <Eigen/Eigenvalues>

void recap(const std::string s, int n, int dim)
{
  if(n)
    std::cout << s << ": " << n << " (dim " << dim*n << ")" << std::endl;
}

void FeatureStack::summary() const
{
  std::cout << "Total feature dimension: " << dim_s << std::endl;
  recap(" - Points XY", pointsXY.size(), 2);
  recap(" - Points Polar", pointsPolar.size(), 2);
  recap(" - Points depths", depths.size(), 1);
  if(translation.des != TranslationDescriptor::NONE)
    recap(" - 3D translations", 1, 3);
  if(rotation.des != RotationDescriptor::NONE)
    recap(" - 3D rotations", 1, 3);
}

vpColVector FeatureStack::sd()
{
  if(sd_.size() == 0)
  {
    sd_.resize(dim_s);
    computeFeatures(cdMo, false);
  }
  return sd_;
}

void FeatureStack::updateFeatures(const vpHomogeneousMatrix &cMo)
{
  if(!init_done)
    initLog();
  computeFeatures(cMo, true);
  e_ = s_-sd();
  if(eigvals_.size())
  {
    static Eigen::MatrixXd LLp(dim_s, dim_s);
    static vpMatrix LLp_visp;
    LLp_visp = L_true_*L_.pseudoInverse(1e-15);

    for(uint row=0; row < dim_s; ++row)
    {
      for(uint col = 0; col < dim_s; ++col)
      {
        LLp(row, col) = LLp_visp[row][col];
      }
    }

    const auto eigv{LLp.eigenvalues()};

    for(uint row = 0; row < dim_s; ++row)
      eigvals_[row] = eigv(row).real();
    std::sort(eigvals_.begin(), eigvals_.end());
  }
}

void FeatureStack::computeFeatures(const vpHomogeneousMatrix &cMo, bool current)
{
  uint row(0);
  if(translation.des != TranslationDescriptor::NONE)
  {
    if(translation.des == TranslationDescriptor::cTo)
      translation.feature.buildFrom(cMo);
    else
      translation.feature.buildFrom(cdMo*cMo.inverse());
    row = update(row, translation.feature, current);
  }

  if(rotation.des != RotationDescriptor::NONE)
  {
    if(rotation.des == RotationDescriptor::cRcd)
      rotation.feature.buildFrom(cMo*cdMo.inverse());
    else
      rotation.feature.buildFrom(cdMo*cMo.inverse());
    row = update(row, rotation.feature, current);
  }

  auto xy(pointsXY.begin());
  auto rt(pointsPolar.begin());
  auto d(depths.begin());
  for(auto &[P, descriptor, zd]: points3D)
  {
    P.track(cMo);
    const double z(P.get_Z());
    const auto P_true{P};
    // Z-estimation
    if(current)
    {
      if(z_estim > 0)
        P.set_Z(z_estim);
      else if(z_estim == 0)
        P.set_Z(zd);
    }
    if(descriptor == PointDescriptor::XY)
    {
      xy->buildFrom(P.get_x(), P.get_y(), P.get_Z());
      static vpFeaturePoint xy_true;
      vpFeatureBuilder::create(xy_true, P_true);
      row = update(row, *xy, current, &xy_true);
      xy++;
    }
    else if(descriptor == PointDescriptor::Polar)
    {
      vpFeatureBuilder::create(*rt, P);
      static vpFeaturePointPolar rt_true;
      vpFeatureBuilder::create(rt_true, P_true);
      row = update(row, *rt, current, &rt_true);
      rt++;
    }
    else
    {
      d->buildFrom(P.get_x(), P.get_y(), P.get_Z(), log(z/zd));
      static vpFeatureDepth d_true;
      d_true.buildFrom(P_true.get_x(), P_true.get_y(), P_true.get_Z(), log(P_true.get_Z()/zd));
      row = update(row, *d, current, &d_true);
      d++;
    }
  }
}

void FeatureStack::addFeaturePoint(vpPoint P, PointDescriptor descriptor)
{
  if(descriptor == PointDescriptor::XY)
  {
    pointsXY.push_back(vpFeaturePoint());
    dim_s += 2;
  }
  else if(descriptor == PointDescriptor::Polar)
  {
    pointsPolar.push_back(vpFeaturePointPolar());
    dim_s += 2;
  }
  else
  {
    depths.push_back(vpFeatureDepth());
    dim_s += 1;
  }
  P.track(cdMo);
  points3D.push_back({P, descriptor, P.get_Z()});
}

void FeatureStack::setTranslation3D(std::string descriptor)
{
  if(descriptor == "cTo")
  {
    translation.feature.setFeatureTranslationType(vpFeatureTranslation::cMo);
    translation.des = TranslationDescriptor::cTo;
    dim_s += 3;
  }
  else if(descriptor == "cdTc")
  {
    translation.feature.setFeatureTranslationType(vpFeatureTranslation::cdMc);
    translation.des = TranslationDescriptor::cdTc;
    dim_s += 3;
  }
}

void FeatureStack::setRotation3D(std::string descriptor)
{
  if(descriptor == "cdRc")
  {
    rotation.feature.setFeatureThetaURotationType(vpFeatureThetaU::cdRc);
    rotation.des = RotationDescriptor::cdRc;
    dim_s += 3;
  }
  else if(descriptor == "cRcd")
  {
    rotation.feature.setFeatureThetaURotationType(vpFeatureThetaU::cRcd);
    rotation.des = RotationDescriptor::cRcd;
    dim_s += 3;
  }
}

void FeatureStack::initLog()
{
  init_done = true;

  // build sub-dir and legend
  std::stringstream ss;
  std::string base_path, legend3D;
  auto updatePath([&]()
  {
    const auto key(ss.str());
    auto legend_key(key);
    /* if(legend_key == "cdRc")
                          legend_key = "{}^{c*}\\mathbf{R}_c";
                        else if(legend_key == "cRcd")
                          legend_key = "{}^c\\mathbf{R}_{c*}";
                        else if(legend_key == "cTo")
                            legend_key = "{}^c\\mathbf{T}_o";
                        else if(legend_key == "cdTc")
                          legend_key = "{}^{c*}\\mathbf{T}_c";*/
    if(base_path == "")
    {
      base_path = key;
      legend3D = key;
    }
    else
    {
      base_path += "_" + key;
      legend3D += "+" + legend_key;
    }
    ss.str("");
  });

  std::stringstream legend;
  legend << "[";
  if(pointsXY.size())
  {
    ss << "XY" << pointsXY.size();
    updatePath();
    for(size_t i = 0; i < pointsXY.size(); ++i)
      legend << "'x_{" << i+1 << "}', 'y_{" << i+1 << "}',";
  }
  if(pointsPolar.size())
  {
    ss << "Polar" << pointsPolar.size();
    updatePath();
    for(size_t i = 0; i < pointsPolar.size(); ++i)
      legend << "'\\rho_{" << i+1 << "}', '\\theta_{" << i+1 << "}',";
  }
  if(depths.size())
  {
    ss << "Depth" << depths.size();
    updatePath();
    for(size_t i = 0; i < depths.size(); ++i)
      legend << "'\\log Z_{" << i+1 << "}/Z^*',";
  }
  if(translation.des != TranslationDescriptor::NONE)
  {
    ss << (translation.des == TranslationDescriptor::cTo ? "cTo" : "cdTc");
    updatePath();
    legend << "t_x, t_y, t_z,";
  }
  if(rotation.des != RotationDescriptor::NONE)
  {
    ss << (rotation.des == RotationDescriptor::cRcd ? "cRcd" : "cdRc");
    updatePath();
    legend << "\\theta u_x, \\theta u_y, \\theta u_z,";
  }

  // build ID for this experiment
  if(pointsXY.size() || pointsPolar.size() || depths.size())
  {
    if(z_estim == 0)
      ss << "Zd";
    else if(z_estim>0)
      ss << "Z" << z_estim;
    else
      ss << "Zc";
    ss << "_";
  }
  ss << "lambda" << simulator.config_manager.read<double>("lambda");

  simulator.initLog(base_path, ss.str(), legend3D);

  // plot feature error
  s_.resize(dim_s, false);
  e_.resize(dim_s, false);
  L_.resize(dim_s, 6, false);

  if(e_.size())
  {
    std::string legend_s(legend.str());
    legend_s.back() = ']';
    simulator.logger.save(e_, "_err", legend_s, "Feature error");

    if(dim_s > 6 || ((pointsXY.size() + pointsPolar.size() || depths.size()) && z_estim >= 0))
    {
      L_true_.resize(dim_s, 6, false);
      eigvals_.resize(dim_s);
      simulator.logger.save(eigvals_, "_eigvals", "\\lambda_", "<Eigen values of >L\\hat{L}^+");
    }
  }
}


#include "worldengine/images/scatter_plot_image.h"

#include <boost/log/trivial.hpp>

namespace WorldEngine
{
ScatterPlotImage::ScatterPlotImage(const World& world, uint32_t size) :
    Image(world, Size(size, size)), size_(size)
{
}
ScatterPlotImage::~ScatterPlotImage() {}

void ScatterPlotImage::DrawImage(boost::gil::rgb8_image_t::view_t& target)
{
   const HumidityArrayType&    humidity    = world_.GetHumidityData();
   const TemperatureArrayType& temperature = world_.GetTemperatureData();

   // Find minimum and maximum values of humidity and temperature on land so we
   // can normalize temperature and humidity to the char.
   float minHumidity    = std::numeric_limits<float>::max();
   float maxHumidity    = std::numeric_limits<float>::lowest();
   float minTemperature = std::numeric_limits<float>::max();
   float maxTemperature = std::numeric_limits<float>::lowest();

   for (uint32_t y = 0; y < world_.height(); y++)
   {
      for (uint32_t x = 0; x < world_.width(); x++)
      {
         if (world_.IsLand(x, y))
         {
            if (humidity[y][x] < minHumidity)
            {
               minHumidity = humidity[y][x];
            }
            if (humidity[y][x] > maxHumidity)
            {
               maxHumidity = humidity[y][x];
            }
            if (temperature[y][x] < minTemperature)
            {
               minTemperature = temperature[y][x];
            }
            if (temperature[y][x] > maxTemperature)
            {
               maxTemperature = temperature[y][x];
            }
         }
      }
   }

   const float temperatureDelta = maxTemperature - minTemperature;
   const float humidityDelta    = maxHumidity - minHumidity;

   // Set all pixels white
   for (uint32_t y = 0; y < size_; y++)
   {
      for (uint32_t x = 0; x < size_; x++)
      {
         target(x, y) = boost::gil::rgb8_pixel_t(255, 255, 255);
      }
   }

   // Fill in "bad" boxes with gray
   const std::vector<HumidityLevel>    hValues({HumidityLevel::Arid,
                                             HumidityLevel::Semiarid,
                                             HumidityLevel::Subhumid,
                                             HumidityLevel::Humid,
                                             HumidityLevel::Perhumid});
   const std::vector<TemperatureLevel> tValues({TemperatureLevel::Polar,
                                                TemperatureLevel::Alpine,
                                                TemperatureLevel::Boreal,
                                                TemperatureLevel::Cool,
                                                TemperatureLevel::Subtropical});

   for (size_t i = 0; i < 5; i++)
   {
      int32_t hMin = std::max(
         static_cast<int32_t>((size_ - 1) *
                              (world_.GetThreshold(hValues[i]) - minHumidity) /
                              humidityDelta),
         0);
      int32_t hMax;
      if (i != 4)
      {
         hMax =
            std::min(static_cast<int32_t>(
                        (size_ - 1) *
                        (world_.GetThreshold(hValues[i + 1]) - minHumidity) /
                        humidityDelta),
                     static_cast<int32_t>(size_));
      }
      else
      {
         hMax = size_;
      }
      int32_t vMax = std::clamp<int32_t>(
         static_cast<int32_t>(
            (size_ - 1) * (world_.GetThreshold(tValues[i]) - minTemperature) /
            temperatureDelta),
         0,
         size_ - 1);

      BOOST_LOG_TRIVIAL(trace) << "i, hMin, hMax, vMax: (" << i << ", " << hMin
                               << ", " << hMax << ", " << vMax << ")";

      for (int32_t y = hMin; y < hMax; y++)
      {
         for (int32_t x = 0; x < vMax; x++)
         {
            target(x, (size_ - 1) - y) =
               boost::gil::rgb8_pixel_t(128, 128, 128);
         }
      }
   }

   // Draw lines based on thresholds
   for (TemperatureLevel t : TemperatureIterator())
   {
      int32_t v = static_cast<int32_t>(
         (size_ - 1) * (world_.GetThreshold(t) - minTemperature) /
         temperatureDelta);
      if (0 < v && v < static_cast<int32_t>(size_))
      {
         for (uint32_t y = 0; y < size_; y++)
         {
            target(v, (size_ - 1) - y) = boost::gil::rgb8_pixel_t(0, 0, 0);
         }
      }
   }

   for (HumidityLevel p : HumidityIterator())
   {
      int32_t h = static_cast<int32_t>(
         (size_ - 1) * (world_.GetThreshold(p) - minHumidity) / humidityDelta);
      if (0 < h && h < static_cast<int32_t>(size_))
      {
         for (uint32_t x = 0; x < size_; x++)
         {
            target(x, (size_ - 1) - h) = boost::gil::rgb8_pixel_t(0, 0, 0);
         }
      }
   }

   // Draw gamma curve
   const float curveGamma = world_.gammaCurve();
   const float curveBonus = world_.curveOffset();

   for (uint32_t x = 0; x < size_; x++)
   {
      uint32_t y = static_cast<uint32_t>(
         (size_ - 1) *
         ((std::pow(static_cast<float>(x) / (size_ - 1), curveGamma) *
           (1 - curveBonus)) +
          curveBonus));
      target(x, (size_ - 1) - y) = boost::gil::rgb8_pixel_t(255, 0, 0);
   }

   // Examine all cells in the map and if it is land get the temperature and
   // humidity for the cell.
   for (uint32_t y = 0; y < world_.height(); y++)
   {
      for (uint32_t x = 0; x < world_.width(); x++)
      {
         if (world_.IsLand(x, y))
         {
            float t = temperature[y][x];
            float p = humidity[y][x];

            // Get red and blue values depending on temperature and humidity
            uint8_t r = 0;
            uint8_t b = 0;

            switch (world_.GetTemperatureLevel(x, y))
            {
            case TemperatureLevel::Polar:
               r = 0; //
               break;

            case TemperatureLevel::Alpine:
               r = 42; //
               break;

            case TemperatureLevel::Boreal:
               r = 85; //
               break;

            case TemperatureLevel::Cool:
               r = 128; //
               break;

            case TemperatureLevel::Warm:
               r = 170; //
               break;

            case TemperatureLevel::Subtropical:
               r = 213; //
               break;

            case TemperatureLevel::Tropical:
            default:
               r = 255; //
               break;
            }

            switch (world_.GetHumidityLevel(x, y))
            {
            case HumidityLevel::Superarid:
               b = 32; //
               break;

            case HumidityLevel::Perarid:
               b = 64; //
               break;

            case HumidityLevel::Arid:
               b = 96; //
               break;

            case HumidityLevel::Semiarid:
               b = 128; //
               break;

            case HumidityLevel::Subhumid:
               b = 160; //
               break;

            case HumidityLevel::Humid:
               b = 192; //
               break;

            case HumidityLevel::Perhumid:
               b = 224; //
               break;

            case HumidityLevel::Superhumid:
            default:
               b = 255; //
               break;
            }

            // Calculate x and y position based on normalized temperature and
            // humidity
            uint32_t nx = static_cast<uint32_t>(
               (size_ - 1) * (t - minTemperature) / temperatureDelta);
            uint32_t ny = static_cast<uint32_t>(
               (size_ - 1) * (p - minHumidity) / humidityDelta);

            target(nx, (size_ - 1) - ny) = boost::gil::rgb8_pixel_t(r, 128, b);
         }
      }
   }
}
} // namespace WorldEngine


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT license.
//
// Generalized Pose-and-Scale Estimation using 4-Point Congruence Constraints
//
// Victor Fragoso and Sudipta Sinha.
// In Proc. of the IEEE International Conf. on 3D Vision (3DV) 2020.
//
// Please contact the author of this library if you have any questions.
// Author: Victor Fragoso (victor.fragoso@microsoft.com)

#include <gp4pc/pinhole_camera.h>

#include <cmath>
#include <limits>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>

namespace msft {

PinholeCamera::PinholeCamera(const double focal_length,
                             const Eigen::Vector2d& principal_point,
                             const Eigen::Quaterniond& world_to_camera_rotation,
                             const Eigen::Vector3d& world_to_camera_translation)
    : rotation_(world_to_camera_rotation.normalized()),
      translation_(world_to_camera_translation) {
  intrinsics_mat_ <<
      focal_length, 0.0, principal_point.x(),
      0.0, focal_length, principal_point.y(),
      0.0, 0.0, 1.0;
}

PinholeCamera::PinholeCamera(const double focal_length,
                             const Eigen::Vector2d& principal_point,
                             const Eigen::Vector4d& world_to_camera_rotation,
                             const Eigen::Vector3d& world_to_camera_translation)
    : PinholeCamera(focal_length,
                    principal_point,
                    Eigen::Quaterniond(world_to_camera_rotation[0],
                                       world_to_camera_rotation[1],
                                       world_to_camera_rotation[2],
                                       world_to_camera_rotation[3]),
                    world_to_camera_translation) {
  CHECK(std::abs(world_to_camera_rotation.norm() - 1.0) <
        std::numeric_limits<double>::epsilon())
      << "Quaternion has not unit norm";
}

double PinholeCamera::ProjectPoint(const Eigen::Vector3d& point3d,
                                   Eigen::Vector2d* point2d) const {
  const Eigen::Vector3d point_in_camera = rotation_ * point3d + translation_;
  *CHECK_NOTNULL(point2d) = (intrinsics_mat_ * point_in_camera).hnormalized();
  return point_in_camera.z();
}

Eigen::Vector3d
PinholeCamera::PixelToUnitRay(const Eigen::Vector2d& pixel) const {
  const Eigen::Vector3d ray =
      rotation_.conjugate() *
      (intrinsics_mat_.inverse() * pixel.homogeneous()).normalized();
  return ray;
}

Eigen::Vector3d PinholeCamera::GetPosition() const {
  const Eigen::Vector3d camera_position = rotation_.conjugate() * -translation_;
  return camera_position;
}

}  // namespace msft


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================

#include "test_utils/sample_graph.hpp"
#include "test_utils/logger.hpp"

#include "paal/data_structures/voronoi/capacitated_voronoi.hpp"
#include "paal/data_structures/object_with_copy.hpp"
#include "paal/data_structures/voronoi/voronoi_traits.hpp"

#include <boost/test/unit_test.hpp>

using namespace paal::data_structures;

std::vector<long long> fcapv{ 7, 7, 3, 1, 2, 2, 2, 1 };
int fcap(int i) { return fcapv[i]; }

std::vector<long long> cdemv{ 2, 2, 1, 3, 3, 1, 1, 2 };
int cdem(int i) { return cdemv[i]; }

BOOST_AUTO_TEST_SUITE(capacitated_voronoi_test_suite)

BOOST_AUTO_TEST_CASE(GeneratorLocationvoronoiutionWithClientAssignmentsTest) {
    LOGLN("Test 0");
    typedef sample_graphs_metrics SGM;
    auto gm = SGM::get_graph_metric_small();

    typedef capacitated_voronoi<decltype(gm), decltype(fcap), decltype(cdem)>
        voronoi;
    typedef voronoi::Generators GSet;
    typedef voronoi::Vertices VSet;
    voronoi vor(GSet{}, VSet{ SGM::A, SGM::B, SGM::C, SGM::D, SGM::E }, gm,
                fcap, cdem);

    /*for(int i = 0; i < 5; ++i)
        LOGLN(gm(SGM::A, i));

    for(int i = 0; i < 5; ++i)
        LOGLN(gm(SGM::B, i));*/

    vor.add_generator(SGM::A);
    auto ab_min_a = vor.add_generator(SGM::B);
    auto b_min_ab = vor.rem_generator(SGM::A);
    BOOST_CHECK_EQUAL(vor.add_generator(SGM::A), -b_min_ab);
    BOOST_CHECK_EQUAL(vor.rem_generator(SGM::B), -ab_min_a);
}

BOOST_AUTO_TEST_CASE(test_1) {
    LOGLN("Test 1");
    typedef sample_graphs_metrics SGM;
    auto gm = SGM::get_graph_metric_small();

    typedef capacitated_voronoi<decltype(gm), decltype(fcap), decltype(cdem)>
        voronoi;
    typedef paal::data_structures::voronoi_traits<voronoi> VT;
    typedef VT::GeneratorsSet GSet;
    typedef VT::VerticesSet VSet;
    voronoi vor(GSet{ SGM::A, SGM::B },
                VSet{ SGM::A, SGM::B, SGM::C, SGM::D, SGM::E }, gm, fcap, cdem);

    vor.add_generator(SGM::C);
    vor.rem_generator(SGM::A);
    vor.add_generator(SGM::A);
    vor.rem_generator(SGM::C);
    vor.add_generator(SGM::D);
    vor.rem_generator(SGM::A);
}

template <typename voronoi> void rem_add(voronoi &v, int g) {
    auto back = v.rem_generator(g);
    auto ret = v.add_generator(g);
    assert(-back == ret);
}

template <typename voronoi>
void rem_add_owc(paal::data_structures::object_with_copy<voronoi> &v, int g) {
    auto back = v.invoke_on_copy(&voronoi::rem_generator, g);
    auto ret = v.invoke_on_copy(&voronoi::add_generator, g);
    assert(-back == ret);
    back = v.invoke(&voronoi::rem_generator, g);
    ret = v.invoke(&voronoi::add_generator, g);
    assert(-back == ret);
}

BOOST_AUTO_TEST_CASE(test_2) {
    LOGLN("Test 2");
    typedef sample_graphs_metrics SGM;
    auto gm = SGM::get_graph_metric_medium();

    typedef capacitated_voronoi<decltype(gm), decltype(fcap), decltype(cdem)>
        voronoi;
    typedef paal::data_structures::voronoi_traits<voronoi> VT;
    typedef VT::GeneratorsSet GSet;
    typedef VT::VerticesSet VSet;
    voronoi vor(
        GSet{ SGM::A, SGM::B, SGM::C, SGM::D, SGM::E, SGM::F, SGM::G, SGM::H },
        VSet{ SGM::A, SGM::B, SGM::C, SGM::D, SGM::E, SGM::F, SGM::G, SGM::H },
        gm, fcap, cdem);
    for (int i = 0; i < 8; ++i) {
        rem_add(vor, i);
    }
}

BOOST_AUTO_TEST_CASE(test_3) {
    LOGLN("Test 3");
    typedef sample_graphs_metrics SGM;
    auto gm = SGM::get_graph_metric_small();

    typedef capacitated_voronoi<decltype(gm), decltype(fcap), decltype(cdem)>
        voronoi;
    typedef paal::data_structures::voronoi_traits<voronoi> VT;
    typedef VT::GeneratorsSet GSet;
    typedef VT::VerticesSet VSet;
    voronoi vor(GSet{ SGM::A /*, SGM::B*/ /*, SGM::C, SGM::D, SGM::E*/ },
                VSet{ SGM::A, SGM::B, SGM::C, SGM::D, SGM::E }, gm, fcap, cdem);

    voronoi vCopy(vor);

    for (int i = 0; i < 1 /*5*/; ++i) {
        rem_add(vCopy, i);
    }
}

BOOST_AUTO_TEST_CASE(test_4) {
    LOGLN("Test 4");
    typedef sample_graphs_metrics SGM;
    auto gm = SGM::get_graph_metric_medium();

    typedef capacitated_voronoi<decltype(gm), decltype(fcap), decltype(cdem)>
        voronoi;
    typedef paal::data_structures::voronoi_traits<voronoi> VT;
    typedef VT::GeneratorsSet GSet;
    typedef VT::VerticesSet VSet;
    voronoi vor(
        GSet{ SGM::A, SGM::B, SGM::C, SGM::D, SGM::E, SGM::F, SGM::G, SGM::H },
        VSet{ SGM::A, SGM::B, SGM::C, SGM::D, SGM::E, SGM::F, SGM::G, SGM::H },
        gm, fcap, cdem);
    for (int i = 0; i < 8; ++i) {
        paal::data_structures::object_with_copy<voronoi> owc(vor);
        rem_add_owc(owc, i);
    }
}
BOOST_AUTO_TEST_SUITE_END()