//          Copyright Gavin Band 2008 - 2012.
// 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 <string>
#include <deque>
#include <iostream>
#include <algorithm>
#include <boost/math/distributions/chi_squared.hpp>
#include "genfile/string_utils.hpp"
#include "appcontext/OptionProcessor.hpp"
#include "appcontext/ApplicationContext.hpp"
#include "appcontext/get_current_time_as_string.hpp"
#include "appcontext/ProgramFlow.hpp"
#include "config/qctool_version_autogenerated.hpp"
#include "config/config.hpp"
#if HAVE_MGL
	#include "mgl/mgl.h"
	#include "mgl/mgl_zb.h"
#endif

namespace globals {
	std::string const program_name = "inflation" ;
	std::string const program_version = qctool_revision ;
}

struct InflationApplication: public appcontext::ApplicationContext
{
public:
	static void declare_options( appcontext::OptionProcessor& options ) {
		options.set_help_option( "-help" ) ;
		options[ "-analysis-name" ]
			.set_description( "Specify a name to label results from this analysis with.  (This applies to modules which store their results in a qcdb file.)" )
			.set_takes_single_value()
			.set_default_value( "inflation analysis, started " + appcontext::get_current_time_as_string() ) ;
		options [ "-qq-plot" ]
			.set_description( "Specify that " + globals::program_name + " should make a qq-plot in the file with the given name." )
			.set_takes_single_value() ;
		options [ "-log" ]
			.set_description( "Specify that " + globals::program_name + " should write a log file to the given file." )
			.set_takes_single_value() ;
	}

public:
	InflationApplication( appcontext::OptionProcessor::UniquePtr options, int argc, char** argv ):
		appcontext::ApplicationContext( globals::program_name, globals::program_version , options, argc, argv, "-log" )
	{
	}

	void process() {
		unsafe_process();
	}
private:

	void unsafe_process() {
		get_ui_context().logger() << application_name() << ": reading..." ;

		std::string line ;
		std::deque< float > numbers ;
		std::size_t missing_count = 0 ;
		while( std::getline( std::cin, line )) {
			if( line.empty() || line.find_first_not_of( "0123456789.E+-" ) != std::string::npos ) {
				++missing_count ;
			} else {
				numbers.push_back( genfile::string_utils::to_repr< float >( line ) ) ;
			}

			if( ( numbers.size() + missing_count ) % 1000000 == 0 ) {
				get_ui_context().logger() << ( numbers.size() + missing_count ) << ".." ;
			}
		}

		get_ui_context().logger() << ( numbers.size() / 1000000 ) << ".\n" ;
		get_ui_context().logger() << "inflation.cpp: read " << numbers.size() << " P-values from std::cin.\n" ;
		get_ui_context().logger() << "inflation.cpp: converting to chi-squared statistics...\n" ;
		using namespace boost::math ;
		chi_squared_distribution< float > chi_square( 1 ) ;
		for( std::size_t i = 0; i < numbers.size(); ++i ) {
			numbers[i] = quantile( complement( chi_square, numbers[i] )) ;
		}

		get_ui_context().logger() << "inflation.cpp: finding median...\n" ;

		std::cout << "analysis,N,missing,median,lambda,1pc_adjusted_median,1pc_adjusted_lambda\n" ;
		std::cout
			<< options().get< std::string >( "-analysis-name" ) << ","
			<< numbers.size() << ","
			<< missing_count ;
		// Compute basic lambda
		{
			std::size_t const mid = numbers.size() / 2 ;
			std::nth_element( numbers.begin(), numbers.begin() + mid, numbers.end() ) ;
			double const median = numbers[ mid ] ;
			std::cout << "," << median << "," << (median / quantile( complement( chi_square, 0.5 ))) ;
		}

		if( options().check( "-qq-plot" )) {
#if HAVE_MGL
			get_ui_context().logger() << "inflation.cpp: making qq-plot...\n" ;
			plot( numbers, options().get< std::string >( "-qq-plot" ), options().get< std::string >( "-analysis-name" ) ) ;
#else
			get_ui_context().logger() << "!! InflationApplication::unsafe_process(): you must build with MathGL support for -qq-plot to be supported.  Skipping plots...\n" ;
#endif
		}

		// Compute lambda after removing top 99% of signal
		{
			std::size_t const top_one_percent = numbers.size() * 0.99 ;
			std::deque< float >::iterator top_one_percent_i = numbers.begin() + top_one_percent ;
			numbers.erase( top_one_percent_i, numbers.end() ) ;
		}

		{
			std::size_t const mid = numbers.size() / 2 ;
			std::nth_element( numbers.begin(), numbers.begin() + mid, numbers.end() ) ;
			double const median = numbers[ mid ] ;
			std::cout << "," << median << "," << (median / quantile( complement( chi_square, 0.5 ))) ;
		}
		std::cout << "\n" ;
	}

#if HAVE_MGL
	void plot( std::deque< float >& numbers, std::string const& filename, std::string const& title ) {
		std::sort( numbers.begin(), numbers.end() ) ;

		mglData
			observed( numbers.size() ),
			expected( numbers.size() )
		;

		using namespace boost::math ;
		{
			chi_squared_distribution< float > chi_squared( 1 ) ;
			for( std::size_t i = 0; i < numbers.size(); ++i ) {
				observed.a[i] = numbers[i] ;
				expected.a[i] = quantile( complement( chi_squared, float( numbers.size() - i ) / ( numbers.size() + 1 ) ) ) ;
			}
		}
		mglGraphZB graph( 800, 800 ) ;
		graph.Light( true ) ;
		graph.Clf( mglColor( 1.0, 1.0, 1.0 ) ) ;
		graph.Title( title.c_str(), 0, 4 ) ;
		graph.SetTickLen( 0.04 ) ;
		graph.SetRanges(
			0.0, 30,
			0.0, 30
		) ;
		graph.Axis( "x", true ) ;
		graph.Axis( "y", true ) ;
		graph.CAxis( 0.0, 3.0 ) ;

		graph.Plot( expected, observed ) ; //, observed ) ;
		graph.WritePNG( filename.c_str(), "", false ) ;
	}
#endif
} ;

int main( int argc, char** argv ) {
	try {
		appcontext::OptionProcessor::UniquePtr options( new appcontext::OptionProcessor ) ;
		InflationApplication::declare_options( *options ) ;
		InflationApplication app( options, argc, argv ) ;
		app.process() ;
	} catch ( appcontext::HaltProgramWithReturnCode const& e ) {
		return e.return_code() ;
	}
	return 0 ;
}


#include "focal_grid.h"

#include <string>
#include <stdint.h>
#include <iostream>
#include <iomanip>
#include <math.h>
#include <chrono>
#include <vector>
#include <map>
#include <unordered_map>
#include <iterator>
#include <future>
#include <thread>
#include <algorithm>
#include <dirent.h>
#include <armadillo>
#include <boost/python/call.hpp>

using namespace std;

FocalGrid::FocalGrid() {
	// empty
}

bool FocalGrid::SetGridParameters(session_data &session, frame_data &frame, int NX, int NY, int NZ, double DS, double X0, double Y0, double Z0) {

	bool grid_loaded = true;

	// Set the focal grid parameters:
	try {

		// grid parameters:
		nx = NX;
		ny = NY;
		nz = NZ;
		ds = DS;
		x0 = X0;
		y0 = Y0;
		z0 = Z0;

		// load calibration file:
		string calib_file = session.session_loc + "/" + session.cal_loc + "/" + session.cal_name;

		arma::Mat<double> CalibMatrix;

		CalibMatrix.load(calib_file);

		// Clear vectors:
		image_size.clear();
		calib_mat.clear();
		X_xyz.clear();
		X_uv.clear();
		uv_offset.clear();

		// Set N_cam
		N_cam = session.N_cam;

		for (int i=0; i<N_cam; i++) {
			image_size.push_back(make_tuple(get<0>(frame.image_size[i]),get<1>(frame.image_size[i])));
			calib_mat.push_back(CalibMatrix.col(i));
			X_xyz.push_back(FocalGrid::Camera2WorldMatrix(CalibMatrix.col(i)));
			X_uv.push_back(FocalGrid::World2CameraMatrix(CalibMatrix.col(i)));
			arma::Col<double> uv_off_i = {CalibMatrix(10,i)/2.0-CalibMatrix(12,i)/2.0, CalibMatrix(9,i)/2.0-CalibMatrix(11,i)/2.0, 0.0};
			uv_offset.push_back(uv_off_i);
		}

	}
	catch (...) {
		cout << "Could not set focal grid parameters." << endl;
		grid_loaded = false;
	}

	return grid_loaded;
}

bool FocalGrid::ConstructFocalGrid(PyObject* progress_func) {

	bool grid_build = true;

	int voxel_ind = 0;

	int progress = 0;

	try {

		pix2vox.clear();
		vox2pix.clear();

		for (int k=0; k<nz; k++) {
			progress = (int) 100.0*(((k+1)*1.0)/(nz*1.0));
			boost::python::call<void>(progress_func,progress);
			for (int j=0; j<ny; j++) {
				for (int i=0; i<nx; i++) {
					vector<int> uv_voxel = FocalGrid::CheckVoxel(i, j, k);
					if (uv_voxel.size()==N_cam) {
						voxel_ind = k*nx*ny+j*nx+i;
						pix2vox.insert(pair<int,int>(uv_voxel[0],voxel_ind));
						vox2pix.insert(pair<int,vector<int>>(voxel_ind,uv_voxel));
					}
				}
			}
		}
		cout << "focal grid size: " << pix2vox.size() << endl;
		cout << "focal grid has been constructed" << endl;
	}
	catch (...) {
		grid_build = false;
		cout << "could not construct focal grid" << endl;
	}

	return grid_build;
}

vector<int> FocalGrid::CheckVoxel(int i, int j, int k) {

	vector<int> uv_out;

	int uv_ind = -1;

	arma::Col<double> xyz(4);

	xyz = {x0-((nx-1)/2.0)*ds+i*ds,
		y0-((ny-1)/2.0)*ds+j*ds,
		z0-((nz-1)/2.0)*ds+k*ds,
		1.0};

	arma::Col<double> uv(3);

	for (int n=0; n<N_cam; n++) {
		uv = X_uv[n]*xyz-uv_offset[n];
		if (uv(0)>=0.0 && uv(0)<(1.0*get<1>(image_size[n]))) {
			if (uv(1)>=0.0 && uv(1)<(1.0*get<0>(image_size[n]))) {
				uv_ind = ((int) uv(1))*get<1>(image_size[n])+((int) uv(0));
				uv_out.push_back(uv_ind);
			}
		}
	}

	return uv_out;
}

vector<arma::Col<int>> FocalGrid::ProjectCloud2Frames(arma::Mat<double> &cloud_in) {

	vector<arma::Col<int>> frame_now;

	int N_vox = cloud_in.n_cols;

	for (int n=0; n<N_cam; n++) {
		arma::Col<int> frame_n;
		frame_n.zeros(get<0>(image_size[n])*get<1>(image_size[n]));
		frame_now.push_back(frame_n);
	}

	arma::Col<double> uv;
	arma::Col<double> xyz;
	int u = 0;
	int v = 0;

	for (int i=0; i<N_vox; i++) {
		for (int j=0; j<N_cam; j++) {
			xyz = {cloud_in(0,i),cloud_in(1,i),cloud_in(2,i),1.0};
			uv = X_uv[j]*xyz-uv_offset[j];
			if (uv(0)>=0 && uv(0)<(get<1>(image_size[j]))) {
				if (uv(1)>=0 && uv(1)<(get<0>(image_size[j]))) {
					u = (int) uv(0);
					v = (int) uv(1);
					if (frame_now[j](get<1>(image_size[j])*v+u)==0) {
						frame_now[j](get<1>(image_size[j])*v+u) = (int) cloud_in(3,i);
					}
					else {
						if (frame_now[j](get<1>(image_size[j])*v+u) > cloud_in(3,i)) {
							frame_now[j](get<1>(image_size[j])*v+u) = (int) cloud_in(3,i);
						}
					}
				}
			}
		}
	}
	return frame_now;
}

arma::Mat<double> FocalGrid::ProjectCloud2UVdouble(arma::Mat<double> &cloud_in, int cam_nr) {

	int N_vox = cloud_in.n_cols;

	arma::Mat<double> uv_doubles(3,N_vox);

	arma::Mat<double> xyz_coords = cloud_in;
	xyz_coords.row(3).fill(1.0);
	arma::Mat<double> M_mat =  X_uv[cam_nr];
	M_mat.col(3) += -uv_offset[cam_nr];
	uv_doubles = M_mat*xyz_coords;

	return uv_doubles;
}

vector<tuple<double,double,double,int>> FocalGrid::ProjectFrames2Cloud(vector<arma::Col<int>> &frame_in) {

	vector<tuple<double,double,double,int>> pcl_now;

	unordered_map<int,int> pcl_voxels;

	int N_row = get<0>(image_size[0]);
	int N_col = get<1>(image_size[1]);

	pair<multimap<int,int>::iterator, multimap<int,int>::iterator> voxels_i;

	int vox_now;
	vector<int> uv_now;

	int n = 0;
	bool is_voxel = true;

	int frame_val_0 = 0;
	int frame_val_n = 0;

	int code_now = 0;

	int count = 0;

	// Insert voxels into an unordered map and give them a segment code:
	for (int i=0; i<(N_row*N_col); i++) {
		frame_val_0 = frame_in[0](i);
		if (frame_val_0>0) {
			voxels_i = pix2vox.equal_range(i);
			for (multimap<int,int>::iterator it=voxels_i.first; it != voxels_i.second; ++it) {
				vox_now = it->second;
				uv_now = vox2pix[vox_now];
				n = 1;
				is_voxel = true;
				code_now = frame_val_0;
				int body_view_count = 0;
				if (frame_val_0 == 1) {
					body_view_count = 1;
				}
				while (is_voxel==true && n < N_cam) {
					frame_val_n = frame_in[n](uv_now[n]);
					if (frame_val_n>0) {
						code_now = code_now+pow(max_n_seg,n)*frame_val_n;
					}
					else {
						is_voxel = false;
					}
					// Reject voxels which have N_cam-1 views formed by the body:
					if (frame_val_n==1) {
						body_view_count++;
					}
					n++;
				}
				if (body_view_count == (N_cam-1)) {
					is_voxel = false;
				}
				if (is_voxel==true) {
					count++;
					pcl_voxels.insert(pair<int,int>(vox_now,code_now));
				}
			}
		}
	}

	// For each voxel in the pcl_voxels map:
	// -> find the neighboring voxels
	// -> add the voxel to pcl_now if it has more than 1 neighbor and less tan 6 neighbors

	unordered_map<int,int>::iterator nb_it;

	arma::Col<int> neighbors(27);

	int nb_sum;

	arma::Col<double> xyz_pos(3);

	for (nb_it = pcl_voxels.begin(); nb_it != pcl_voxels.end(); nb_it++) {
		vox_now = nb_it->first;
		code_now = nb_it->second;
		neighbors = FocalGrid::FindNeighbors(vox_now);
		if (neighbors(13)>0) {
			nb_sum = 0;
			for (int m=0; m<27; m++) {
				if (pcl_voxels.find(neighbors(m)) != pcl_voxels.end()) {
					// Do nothing
				}
				else {
					nb_sum++;
				}
			}
			if (nb_sum > 2 && nb_sum < 27) {
				xyz_pos = FocalGrid::CalculatePosition(vox_now);
				pcl_now.push_back(make_tuple(xyz_pos(0),xyz_pos(1),xyz_pos(2),code_now));
			}
		}
	}

	if (pcl_now.size() == 0) {
		pcl_now.push_back(make_tuple(0.0,0.0,0.0,0));
	}

	return pcl_now;
}

arma::Mat<int> FocalGrid::FindConnectedPointClouds(arma::Mat<double> &pcl_in) {

	int N_pts = pcl_in.n_cols;

	arma::Row<double> seg_ids = arma::unique(pcl_in.row(3));
	int N_seg = seg_ids.n_cols;

	arma::Mat<int> connectivity_mat;

	// Set the diagonals of the connectivity mat to the unique codes:
	if (N_seg>1) {

		connectivity_mat.zeros(N_seg,N_seg);

		for (int j=0; j<N_seg; j++) {
			connectivity_mat(j,j) = seg_ids(j);
		}

		double central_x;
		double central_y;
		double central_z;
		double central_code;

		arma::uvec dx_ids;
		arma::Mat<double> dx_points;

		arma::uvec dxy_ids;
		arma::Mat<double> dxy_points;

		arma::uvec dxyz_ids;
		arma::Mat<double> dxyz_points;

		arma::Row<double> neighbor_codes;
		int N_codes;

		arma::uvec central_code_ind;
		arma::uvec neighbor_code_ind;

		for (int i=0; i<N_pts; i++) {
			// Iterate through the voxels, find any points which are at (+dx,-dx) subsequently (+dy,-dy) and finally (+dz,-dz).
			// Points which show up in all 3 conditions are neighboring points.
			// Than check the codes of the neighboring points and add scores to the connectivity mat if their codes differ from the central point.
			central_x = pcl_in(0,i);
			central_y = pcl_in(1,i);
			central_z = pcl_in(2,i);
			central_code = pcl_in(3,i);

			dx_ids = arma::find((pcl_in.row(0)>(central_x-(1.1*ds))) && (pcl_in.row(0)<(central_x+(1.1*ds))));
			if (dx_ids.n_rows>1) {
				dx_points = pcl_in.cols(dx_ids);
				dxy_ids = arma::find((dx_points.row(1)>(central_y-(1.1*ds))) && (dx_points.row(1)<(central_y+(1.1*ds))));
				if (dxy_ids.n_rows>1) {
					dxy_points = dx_points.cols(dxy_ids);
					dxyz_ids = arma::find((dxy_points.row(2)>(central_z-(1.1*ds))) && (dxy_points.row(2)<(central_z+(1.1*ds))));
					if (dxyz_ids.n_rows>1) {
						dxyz_points = dxy_points.cols(dxyz_ids);
						neighbor_codes = arma::unique(dxyz_points.row(3));
						N_codes = neighbor_codes.n_cols;
						central_code_ind = arma::find(seg_ids == central_code);
						for (int k=0; k<N_codes; k++) {
							neighbor_code_ind = arma::find(seg_ids == neighbor_codes(k));
							if (central_code_ind(0) != neighbor_code_ind(0)) {
								connectivity_mat(central_code_ind,neighbor_code_ind) += 1;
								connectivity_mat(neighbor_code_ind,central_code_ind) += 1;
							}
						}
					}
				}
			}
		}
	}
	else {
		connectivity_mat.zeros(1,1);
	}
	return connectivity_mat;
}

arma::Mat<double> FocalGrid::ConvertVector2Mat(vector<tuple<double,double,double,int>> pcl_in) {
	int N_pts = pcl_in.size();
	arma::Mat<double> pcl_out(4,N_pts);
	for (int j=0; j<N_pts; j++) {
		pcl_out(0,j) = get<0>(pcl_in[j]);
		pcl_out(1,j) = get<1>(pcl_in[j]);
		pcl_out(2,j) = get<2>(pcl_in[j]);
		pcl_out(3,j) = get<3>(pcl_in[j]);
	}
	return pcl_out;
}

arma::Col<int> FocalGrid::FindNeighbors(int vox_ind) {

	// find the neighboring voxels:

	arma::Col<int> vox_int_mat(27);

	int i = vox_ind % nx;
	int j = ((vox_ind-i)/nx) % ny;
	int k = (vox_ind-i-j*nx)/(nx*ny);

	if ((i>0 && i<(nx-1)) && (j>0 && j<(ny-1)) && (k>0 && k<(nz-1))) {

		vox_int_mat(0) = vox_ind-nx*ny-nx-1;
		vox_int_mat(1) = vox_ind-nx*ny-nx;
		vox_int_mat(2) = vox_ind-nx*ny-nx+1;
		vox_int_mat(3) = vox_ind-nx*ny-1;
		vox_int_mat(4) = vox_ind-nx*ny;
		vox_int_mat(5) = vox_ind-nx*ny+1;
		vox_int_mat(6) = vox_ind-nx*ny+nx-1;
		vox_int_mat(7) = vox_ind-nx*ny+nx;
		vox_int_mat(8) = vox_ind-nx*ny+nx+1;
		vox_int_mat(9) = vox_ind-nx-1;
		vox_int_mat(10) = vox_ind-nx;
		vox_int_mat(11) = vox_ind-nx+1;
		vox_int_mat(12) = vox_ind-1;
		vox_int_mat(13) = vox_ind;
		vox_int_mat(14) = vox_ind+1;
		vox_int_mat(15) = vox_ind+nx-1;
		vox_int_mat(16) = vox_ind+nx;
		vox_int_mat(17) = vox_ind+nx+1;
		vox_int_mat(18) = vox_ind+nx*ny-nx-1;
		vox_int_mat(19) = vox_ind+nx*ny-nx;
		vox_int_mat(20) = vox_ind+nx*ny-nx+1;
		vox_int_mat(21) = vox_ind+nx*ny-1;
		vox_int_mat(22) = vox_ind+nx*ny;
		vox_int_mat(23) = vox_ind+nx*ny+1;
		vox_int_mat(24) = vox_ind+nx*ny+nx-1;
		vox_int_mat(25) = vox_ind+nx*ny+nx;
		vox_int_mat(26) = vox_ind+nx*ny+nx+1;

	}
	else {
		vox_int_mat.zeros();
	}

	return vox_int_mat;
}

arma::Col<double> FocalGrid::CalculatePosition(int vox_ind) {

	int i = vox_ind % nx;
	int j = ((vox_ind-i)/nx) % ny;
	int k = (vox_ind-i-j*nx)/(nx*ny);

	arma::Col<double> xyz_pos(3);

	xyz_pos(0) = x0-((nx-1)/2.0)*ds+i*ds;
	xyz_pos(1) = y0-((ny-1)/2.0)*ds+j*ds;
	xyz_pos(2) = z0-((nz-1)/2.0)*ds+k*ds;

	return xyz_pos;
}

arma::Mat<int> FocalGrid::TransformXYZ2UV(arma::Col<double> xyz_pos) {

	arma::Mat<int> uv_mat(2,N_cam);

	for (int n=0; n<N_cam; n++) {
		arma::Col<double> uv = X_uv[n]*xyz_pos-uv_offset[n];
		uv_mat(0,n) = int(uv(0));
		uv_mat(1,n) = int(uv(1));
	}

	return uv_mat;
}

tuple<arma::Mat<int>, arma::Col<double>> FocalGrid::RayCasting(int cam_nr, arma::Col<double> xyz_pos_prev, arma::Col<double> uv_pos_prev, arma::Col<double> uv_pos_now) {

	// Calculate the 3D translation vector:
	arma::Col<double> xyz_uv_prev = X_xyz[cam_nr]*(uv_pos_prev+uv_offset[cam_nr]);
	arma::Col<double> xyz_uv_now = X_xyz[cam_nr]*(uv_pos_now+uv_offset[cam_nr]);
	arma::Col<double> trans_vec = xyz_uv_now-xyz_uv_prev;
	trans_vec(3) = 0.0;

	// Add the translation to the xyz position:
	arma::Col<double> xyz_pos_now = xyz_pos_prev + trans_vec;

	// Project the new position back to the camera views:

	arma::Mat<int> uv_mat(3,N_cam);

	for (int n=0; n<N_cam; n++) {
		arma::Col<double> uv = X_uv[n]*xyz_pos_now-uv_offset[n];
		if (uv(0)>=0 && uv(0)<(get<1>(image_size[n])) && uv(1)>=0 && uv(1)<(get<0>(image_size[n]))) {
			uv_mat(0,n) = int(uv(0));
			uv_mat(1,n) = int(uv(1));
			uv_mat(2,n) = int(uv(2));
		}
		else {
			arma::Col<double> uv_old = X_uv[n]*xyz_pos_prev-uv_offset[n];
			uv_mat(0,n) = int(uv_old(0));
			uv_mat(1,n) = int(uv_old(1));
			uv_mat(2,n) = int(uv_old(2));
		}
	}

	return make_tuple(uv_mat,xyz_pos_now);
}

arma::Col<double> FocalGrid::CalculateViewVector(int cam_nr) {

	arma::Col<double> u_vec = {1.0,0.0,0.0};
	arma::Col<double> v_vec = {0.0,1.0,0.0};

	arma::Col<double> u_vec_world = X_xyz[cam_nr]*u_vec;
	arma::Col<double> v_vec_world = X_xyz[cam_nr]*v_vec;

	arma::Col<double> view_vec = arma::cross(v_vec_world.rows(0,2),u_vec_world.rows(0,2))/(arma::norm(u_vec_world.rows(0,2))*arma::norm(v_vec_world.rows(0,2)));

	return arma::normalise(view_vec);

}

arma::Mat<double> FocalGrid::Camera2WorldMatrix(arma::Col<double> calib_param) {

	// return the world to camera projection matrix

	arma::Mat<double> C = {{calib_param(0), calib_param(2), 0, 0},
		 			{0, calib_param(1), 0, 0},
		 			{0, 0, 0, 1}};

	double theta = sqrt(pow(calib_param(3),2)+pow(calib_param(4),2)+pow(calib_param(5),2));

	arma::Mat<double> omega = {{0, -calib_param(5), calib_param(4)},
			 			{calib_param(5), 0, -calib_param(3)},
			 			{-calib_param(4), calib_param(3), 0}};

	arma::Mat<double> R(3,3); R.eye();

	R = R+(sin(theta)/theta)*omega+((1-cos(theta))/pow(theta,2))*(omega*omega);

	arma::Col<double> T = {calib_param(6), calib_param(7), calib_param(8)};

	arma::Mat<double> K = {{R(0,0), R(0,1), R(0,2), T(0)},
		 				{R(1,0), R(1,1), R(1,2), T(1)},
		 				{R(2,0), R(2,1), R(2,2), T(2)},
		 				{0, 0, 0, 1}};

	return arma::inv(K)*arma::pinv(C);

}

arma::Mat<double> FocalGrid::World2CameraMatrix(arma::Col<double> calib_param) {

	// return the world to camera projection matrix

	arma::Mat<double> C = {{calib_param(0), calib_param(2), 0, 0},
		 			{0, calib_param(1), 0, 0},
		 			{0, 0, 0, 1}};

	double theta = sqrt(pow(calib_param(3),2)+pow(calib_param(4),2)+pow(calib_param(5),2));

	arma::Mat<double> omega = {{0, -calib_param(5), calib_param(4)},
			 			{calib_param(5), 0, -calib_param(3)},
			 			{-calib_param(4), calib_param(3), 0}};

	arma::Mat<double> R(3,3); R.eye();

	R = R+(sin(theta)/theta)*omega+((1-cos(theta))/pow(theta,2))*(omega*omega);

	arma::Col<double> T = {calib_param(6), calib_param(7), calib_param(8)};

	arma::Mat<double> K = {{R(0,0), R(0,1), R(0,2), T(0)},
		 				{R(1,0), R(1,1), R(1,2), T(1)},
		 				{R(2,0), R(2,1), R(2,2), T(2)},
		 				{0, 0, 0, 1}};

	return C*K;

}


////////////////////////////////////////////////////////////////////////////////
// Copyright 2019 FZI Research Center for Information Technology
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// 2. 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.
//
// 3. Neither the name of the copyright holder nor the names of its
// contributors may be used to endorse or promote products derived from this
// software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
////////////////////////////////////////////////////////////////////////////////

//-----------------------------------------------------------------------------
/*!\file    ForwardDynamicsSolver.cpp
 *
 * \author  Stefan Scherzinger <scherzin@fzi.de>
 * \date    2016/02/14
 *
 */
//-----------------------------------------------------------------------------

// this package
#include <cartesian_controller_base/ForwardDynamicsSolver.h>

// other
#include <map>
#include <sstream>
#include <boost/algorithm/clamp.hpp>
#include <eigen_conversions/eigen_kdl.h>

// KDL
#include <kdl/jntarrayvel.hpp>
#include <kdl/framevel.hpp>

// DEBUG

namespace cartesian_controller_base{

  ForwardDynamicsSolver::ForwardDynamicsSolver()
  {
  }

  ForwardDynamicsSolver::~ForwardDynamicsSolver(){}

  trajectory_msgs::JointTrajectoryPoint ForwardDynamicsSolver::getJointControlCmds(
        ros::Duration period,
        const ctrl::Vector6D& net_force)
  {

    // Compute joint space inertia matrix
    m_jnt_space_inertia_solver->JntToMass(m_current_positions,m_jnt_space_inertia);

    // Compute joint jacobian
    m_jnt_jacobian_solver->JntToJac(m_current_positions,m_jnt_jacobian);

    // Compute joint accelerations according to: \f$ \ddot{q} = H^{-1} ( J^T f) \f$
    m_current_accelerations.data = m_jnt_space_inertia.data.inverse() * m_jnt_jacobian.data.transpose() * net_force;

    // Integrate once, starting with zero motion
    m_current_velocities.data = 0.5 * m_current_accelerations.data * period.toSec();

    // Integrate twice, starting with zero motion
    m_current_positions.data = m_last_positions.data + 0.5 * m_current_velocities.data * period.toSec();

    // Make sure positions stay in allowed margins
    for (int i = 0; i < m_number_joints; ++i)
    {
      m_current_positions(i) = boost::algorithm::clamp(
          m_current_positions(i),m_lower_pos_limits(i),m_upper_pos_limits(i));
    }

    // Apply results
    trajectory_msgs::JointTrajectoryPoint control_cmd;
    for (int i = 0; i < m_number_joints; ++i)
    {
      control_cmd.positions.push_back(m_current_positions(i));
      control_cmd.velocities.push_back(m_current_velocities(i));

      // Accelerations should be left empty. Those values will be interpreted
      // by most hardware joint drivers as max. tolerated values. As a
      // consequence, the robot will move very slowly.
    }
    control_cmd.time_from_start = period; // valid for this duration

    return control_cmd;
  }

  const KDL::Frame& ForwardDynamicsSolver::getEndEffectorPose() const
  {
    return m_end_effector_pose;
  }

  const ctrl::Vector6D& ForwardDynamicsSolver::getEndEffectorVel() const
  {
    return m_end_effector_vel;
  }

  const KDL::JntArray& ForwardDynamicsSolver::getPositions() const
  {
    return m_current_positions;
  }

  bool ForwardDynamicsSolver::setStartState(
      const std::vector<hardware_interface::JointHandle>& joint_handles)
  {
    // Copy into internal buffers.
    for (int i = 0; i < joint_handles.size(); ++i)
    {
      m_current_positions(i)      = joint_handles[i].getPosition();
      m_current_velocities(i)     = joint_handles[i].getVelocity();
      m_current_accelerations(i)  = 0.0;
      m_last_positions(i)         = m_current_positions(i);
    }
    return true;
  }

  bool ForwardDynamicsSolver::init(
      const KDL::Chain& chain,
      const KDL::JntArray& upper_pos_limits,
      const KDL::JntArray& lower_pos_limits)
  {
    if (!buildGenericModel(chain))
    {
      ROS_ERROR("ForwardDynamicsSolver: Something went wrong in setting up the internal model.");
      return false;
    }

    // Initialize
    m_number_joints              = m_chain.getNrOfJoints();
    m_current_positions.data     = ctrl::VectorND::Zero(m_number_joints);
    m_current_velocities.data    = ctrl::VectorND::Zero(m_number_joints);
    m_current_accelerations.data = ctrl::VectorND::Zero(m_number_joints);
    m_last_positions.data        = ctrl::VectorND::Zero(m_number_joints);
    m_upper_pos_limits           = upper_pos_limits;
    m_lower_pos_limits           = lower_pos_limits;

    // Forward kinematics
    m_fk_pos_solver.reset(new KDL::ChainFkSolverPos_recursive(m_chain));
    m_fk_vel_solver.reset(new KDL::ChainFkSolverVel_recursive(m_chain));

    // Forward dynamics
    m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
    m_jnt_space_inertia_solver.reset(new KDL::ChainDynParam(m_chain,KDL::Vector::Zero()));
    m_jnt_jacobian.resize(m_number_joints);
    m_jnt_space_inertia.resize(m_number_joints);

    ROS_INFO("Forward dynamics solver initialized");
    ROS_INFO("Forward dynamics solver has control over %i joints", m_number_joints);

    return true;
  }

  bool ForwardDynamicsSolver::buildGenericModel(const KDL::Chain& input_chain)
  {
    m_chain = input_chain;

    // Set all masses and inertias to minimal (yet stable) values.
    double m_min = 0.001;
    double ip_min = 0.000001;
    for (size_t i = 0; i < m_chain.segments.size(); ++i)
    {
      // Fixed joint segment
      if (m_chain.segments[i].getJoint().getType() == KDL::Joint::None)
      {
        m_chain.segments[i].setInertia(
            KDL::RigidBodyInertia::Zero());
      }
      else  // relatively moving segment
      {
        m_chain.segments[i].setInertia(
            KDL::RigidBodyInertia(
              m_min,                // mass
              KDL::Vector::Zero(),  // center of gravity
              KDL::RotationalInertia(
                ip_min,             // ixx
                ip_min,             // iyy
                ip_min              // izz
                // ixy, ixy, iyz default to 0.0
                )));
      }
    }

    // Only give the last segment a generic mass and inertia.
    // See https://arxiv.org/pdf/1908.06252.pdf for a motivation for this setting.
    double m = 1;
    double ip = 1;
    m_chain.segments[m_chain.segments.size()-1].setInertia(
        KDL::RigidBodyInertia(
          m,
          KDL::Vector::Zero(),
          KDL::RotationalInertia(ip, ip, ip)));

    return true;
  }

} // namespace


/* Copyright 2022 Zuru Tech HK Limited.
 *
 * 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 <chrono>
#include <stdexcept>

#include <Eigen/Dense>

#include <superlu_dist/superlu_ddefs.h>

#include <solvers/SparseSystem.hpp>
#include <solvers/SuperLUSolver.hpp>

namespace solvers {

SuperLUSolver::SuperLUSolver(const SuperLUReorder reorder)
{
    /* Set the default input options:
        options.Fact = DOFACT;
        options.Equil = YES;
        options.ColPerm = METIS_AT_PLUS_A;
        options.RowPerm = LargeDiag_MC64;
        options.ReplaceTinyPivot = YES;
        options.Trans = NOTRANS;
        options.IterRefine = DOUBLE;
        options.SolveInitialized = NO;
        options.RefineInitialized = NO;
        options.PrintStat = YES;
     */
    set_default_options_dist(&_options);
    _options.PrintStat = yes_no_t::NO;
    _options.ColPerm = static_cast<colperm_t>(reorder);
    _options.IterRefine = IterRefine_t::NOREFINE;
    _options.SymPattern = yes_no_t::YES;
}

Eigen::VectorXd SuperLUSolver::solve(const SparseSystem& system,
                                     double& duration) const
{
    auto [ccol_index, row_index, values, b] = system.toStdCSR();
    // Transfer ownership from vectors to pointers
    auto* ccol_index_ptr = new int[ccol_index.size()];
    auto* row_index_ptr = new int[row_index.size()];
    auto* values_ptr = new double[values.size()];
    std::copy(ccol_index.begin(), ccol_index.end(), ccol_index_ptr);
    std::copy(row_index.begin(), row_index.end(), row_index_ptr);
    std::copy(values.begin(), values.end(), values_ptr);

    const auto n = static_cast<int>(system.dim());
    const auto nnz = static_cast<int>(system.nnz());

    // Choose a GPU device
    int rank;
    int devs;
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);
    cudaGetDeviceCount(&devs);
    cudaSetDevice(rank % devs);

    gridinfo_t grid;
    constexpr int nprow = 1;
    constexpr int npcol = 1;
    superlu_gridinit(MPI_COMM_WORLD, nprow, npcol, &grid);

    // Create A in CSC format
    SuperMatrix A;
    dCreate_CompCol_Matrix_dist(&A, n, n, nnz, values_ptr, row_index_ptr,
                                ccol_index_ptr, Stype_t::SLU_NC, Dtype_t::SLU_D,
                                Mtype_t::SLU_GE);

    // Initialize variables
    dScalePermstruct_t scale_perm;
    dScalePermstructInit(n, n, &scale_perm);
    dLUstruct_t lu;
    dLUstructInit(n, &lu);
    SuperLUStat_t stat;
    PStatInit(&stat);

    // Call the linear equation solver
    double error;
    int info;
    superlu_dist_options_t options(_options);
    Eigen::VectorXd result = Eigen::Map<Eigen::VectorXd>(b.data(), n);
    pdgssvx_ABglobal(&options, &A, &scale_perm, result.data(), n, 1, &grid, &lu,
                     &error, &stat, &info);

    // Measure execution time in seconds
    duration = 0.;
    for (size_t phase = 0; phase < PhaseType::NPHASES; ++phase) {
        duration += stat.utime[phase];
    }

    // Destroy MPI objects
    PStatFree(&stat);
    Destroy_CompCol_Matrix_dist(&A);
    dDestroy_LU(n, &grid, &lu);
    dScalePermstructFree(&scale_perm);
    dLUstructFree(&lu);
    superlu_gridexit(&grid);
    cudaDeviceReset();
    return result;
}

}    // namespace solvers


#include <Configuration.h> // Package Path
#include <avatar_locomanipulation/enable_pinocchio_with_hpp_fcl.h> // Enable HPP FCL
// Multibody
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/multibody/geometry.hpp"
// Algorithms
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/geometry.hpp"
#include "pinocchio/algorithm/model.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/frames.hpp" // Jacobian Frame Computation
// Parsers
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"
// Spatial
#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"

// Standard
#include <math.h>
#include <map>
#include <iostream>
#include <boost/shared_ptr.hpp>

int main(int argc, char ** argv){
	// models and geomModels
	pinocchio::Model robot_model, cart_model, appended_model;
  pinocchio::GeometryModel robot_geomModel, cart_geomModel, appended_geomModel;
  // data and geomdata
  std::unique_ptr<pinocchio::Data> robot_data, cart_data, appended_data;
  std::unique_ptr<pinocchio::GeometryData> robot_geomData, cart_geomData, appended_geomData;

  // Define the Robot--------
	std::string filename = THIS_PACKAGE_PATH"models/valkyrie_simplified_collisions.urdf";
  std::string srdf_filename = THIS_PACKAGE_PATH"models/valkyrie_disable_collisions.srdf";
  std::string meshDir  = THIS_PACKAGE_PATH"../val_model/";
  // build model and geomModel
  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),robot_model);
  pinocchio::urdf::buildGeom(robot_model, filename, pinocchio::COLLISION, robot_geomModel, meshDir );
  // Add and remove collision pairs
  robot_geomModel.addAllCollisionPairs();
  pinocchio::srdf::removeCollisionPairs(robot_model, robot_geomModel, srdf_filename, false);
  // Define the data and geomData
  robot_data = std::unique_ptr<pinocchio::Data>(new pinocchio::Data(robot_model));
	robot_geomData = std::unique_ptr<pinocchio::GeometryData>(new pinocchio::GeometryData(robot_geomModel));
	// Define the configuration of Robot
  Eigen::VectorXd q_robot;
  q_robot = Eigen::VectorXd::Zero(robot_model.nq);

  double theta = 0;//M_PI/4.0;
  Eigen::AngleAxis<double> aa(theta, Eigen::Vector3d(0.0, 0.0, 1.0));
  Eigen::Quaternion<double> init_quat(1.0, 0.0, 0.0, 0.0); //Initialized to remember the w component comes first
  init_quat = aa;

  q_robot[3] = init_quat.x(); q_robot[4] = init_quat.y(); q_robot[5] = init_quat.z(); q_robot[6] = init_quat.w(); // Set up the quaternion in q

  q_robot[2] = 1.0; // set z value to 1.0, this is the pelvis location

  q_robot[7 + robot_model.getJointId("leftHipPitch") - 2] = -0.3;
  q_robot[7 + robot_model.getJointId("rightHipPitch") - 2] = -0.3;
  q_robot[7 + robot_model.getJointId("leftKneePitch") - 2] = 0.6;
  q_robot[7 + robot_model.getJointId("rightKneePitch") - 2] = 0.6;
  q_robot[7 + robot_model.getJointId("leftAnklePitch") - 2] = -0.3;
  q_robot[7 + robot_model.getJointId("rightAnklePitch") - 2] = -0.3;

  q_robot[7 + robot_model.getJointId("rightShoulderPitch") - 2] = -0.2;
  q_robot[7 + robot_model.getJointId("rightShoulderRoll") - 2] = 1.1;
  q_robot[7 + robot_model.getJointId("rightElbowPitch") - 2] = 0.4;
  q_robot[7 + robot_model.getJointId("rightForearmYaw") - 2] = 1.5;

  q_robot[7 + robot_model.getJointId("leftShoulderPitch") - 2] = -0.2;
  q_robot[7 + robot_model.getJointId("leftShoulderRoll") - 2] = -1.1;
  q_robot[7 + robot_model.getJointId("leftElbowPitch") - 2] = -0.4;
  q_robot[7 + robot_model.getJointId("leftForearmYaw") - 2] = 1.5;

	// Define the Cart--------
	filename = THIS_PACKAGE_PATH"models/test_cart.urdf";
  meshDir  = THIS_PACKAGE_PATH"models/cart/";
  // build model and geomModel
  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),cart_model);
  pinocchio::urdf::buildGeom(cart_model, filename, pinocchio::COLLISION, cart_geomModel, meshDir );

  //------ Suggested Fix
  // Apply a prefix
  std::string prefix ("cart/");
  for (pinocchio::JointIndex i = 1; i < cart_model.joints.size(); ++i) {
    cart_model.names[i] = prefix + cart_model.names[i];
  }
  for (pinocchio::FrameIndex i = 0; i < cart_model.frames.size(); ++i) {
    ::pinocchio::Frame& f = cart_model.frames[i];
    f.name = prefix + f.name;
  }
  //----------------------
  // Add and remove collision pairs
  cart_geomModel.addAllCollisionPairs();
  // Define the data and geomData
  cart_data = std::unique_ptr<pinocchio::Data>(new pinocchio::Data(cart_model));
	cart_geomData = std::unique_ptr<pinocchio::GeometryData>(new pinocchio::GeometryData(cart_geomModel));
	// Define the configuration of the cart
  Eigen::VectorXd q_cart;
  q_cart = Eigen::VectorXd::Zero(cart_model.nq);
  q_cart[0] = -0.05;  q_cart[1] = 0.0;  q_cart[2] = 0.0;
  // q_cart[0] = -0.06;  q_cart[1] = -0.0085;  q_cart[2] = -0.04;
  double theta1 = 0;//M_PI/4.0;
  Eigen::AngleAxis<double> bb(theta1, Eigen::Vector3d(0.0, 0.0, 1.0)); // yaw pi/4 to the left
  Eigen::Quaternion<double> quat_init; quat_init =  bb;
  q_cart[3] = quat_init.x();// 0.0;
  q_cart[4] = quat_init.y(); //0.0;
  q_cart[5] = quat_init.z(); //sin(theta/2.0);
  q_cart[6] = quat_init.w(); //cos(theta/2.0);

  // Perform initial forward kinematics
  pinocchio::forwardKinematics(robot_model, *robot_data, q_robot);
  // Compute Joint Jacobians
  pinocchio::computeJointJacobians(robot_model,*robot_data, q_robot);
  // Update Frame Placements
  pinocchio::updateFramePlacements(robot_model, *robot_data);
  // Perform initial forward kinematics
  pinocchio::forwardKinematics(cart_model, *cart_data, q_cart);
  // Compute Joint Jacobians
  pinocchio::computeJointJacobians(cart_model,*cart_data, q_cart);
  // Update Frame Placements
  pinocchio::updateFramePlacements(cart_model, *cart_data);

  // Append the object onto the robot, and fill appended RobotModel
  pinocchio::appendModel(robot_model, cart_model, robot_geomModel, cart_geomModel, 0, pinocchio::SE3::Identity(), appended_model, appended_geomModel);

  appended_data = std::unique_ptr<pinocchio::Data>(new pinocchio::Data(appended_model));
  appended_geomData = std::unique_ptr<pinocchio::GeometryData>(new pinocchio::GeometryData(appended_geomModel));

  Eigen::VectorXd q_appended;
  q_appended = Eigen::VectorXd::Zero(appended_model.nq);

  for(int i=0; i<cart_model.nq; ++i){
  	q_appended[i] = q_cart[i];
  }
  int i = cart_model.nq;
  for(int j=0; j<robot_model.nq; ++j){
  	q_appended[i] = q_robot[j];
  	++i;
  }

  // Perform initial forward kinematics
  pinocchio::forwardKinematics(appended_model, *appended_data, q_appended);
  // Compute Joint Jacobians
  pinocchio::computeJointJacobians(appended_model,*appended_data, q_appended);
  // Update Frame Placements
  pinocchio::updateFramePlacements(appended_model, *appended_data);
  // Update geometry Placements
  pinocchio::updateGeometryPlacements(appended_model, *appended_data, appended_geomModel, *appended_geomData, q_appended);

  std::cout << "appended_model: \n" << appended_model << std::endl;
  std::cout << "appended_geomModel: \n" << appended_geomModel << std::endl;
  // List Operational Space Frames
  for (int k=0 ; k<appended_model.frames.size() ; ++k){
    std::cout << "frame:" << k << " " << appended_model.frames[k].name << " : " << appended_data->oMf[k].translation().transpose() << std::endl;
  }
}


/*******************************************************************************
 *
 * Data structures for the symbolic manipulation of linear constraints.
 *
 * Author: Arnaud J. Venet (arnaud.j.venet@nasa.gov)
 * Contributor: Jorge A. Navas (jorge.navas@sri.com)
 *
 * Notices:
 *
 * Copyright (c) 2011 United States Government as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All Rights Reserved.
 *
 * Disclaimers:
 *
 * No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF
 * ANY KIND, EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED
 * TO, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS,
 * ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE,
 * OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL BE
 * ERROR FREE, OR ANY WARRANTY THAT DOCUMENTATION, IF PROVIDED, WILL CONFORM TO
 * THE SUBJECT SOFTWARE. THIS AGREEMENT DOES NOT, IN ANY MANNER, CONSTITUTE AN
 * ENDORSEMENT BY GOVERNMENT AGENCY OR ANY PRIOR RECIPIENT OF ANY RESULTS,
 * RESULTING DESIGNS, HARDWARE, SOFTWARE PRODUCTS OR ANY OTHER APPLICATIONS
 * RESULTING FROM USE OF THE SUBJECT SOFTWARE.  FURTHER, GOVERNMENT AGENCY
 * DISCLAIMS ALL WARRANTIES AND LIABILITIES REGARDING THIRD-PARTY SOFTWARE,
 * IF PRESENT IN THE ORIGINAL SOFTWARE, AND DISTRIBUTES IT "AS IS."
 *
 * Waiver and Indemnity:  RECIPIENT AGREES TO WAIVE ANY AND ALL CLAIMS AGAINST
 * THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS, AS WELL
 * AS ANY PRIOR RECIPIENT.  IF RECIPIENT'S USE OF THE SUBJECT SOFTWARE RESULTS
 * IN ANY LIABILITIES, DEMANDS, DAMAGES, EXPENSES OR LOSSES ARISING FROM SUCH
 * USE, INCLUDING ANY DAMAGES FROM PRODUCTS BASED ON, OR RESULTING FROM,
 * RECIPIENT'S USE OF THE SUBJECT SOFTWARE, RECIPIENT SHALL INDEMNIFY AND HOLD
 * HARMLESS THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS,
 * AS WELL AS ANY PRIOR RECIPIENT, TO THE EXTENT PERMITTED BY LAW.
 * RECIPIENT'S SOLE REMEDY FOR ANY SUCH MATTER SHALL BE THE IMMEDIATE,
 * UNILATERAL TERMINATION OF THIS AGREEMENT.
 *
 ******************************************************************************/

#pragma once

#include <memory>
#include <optional>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#include <boost/container/flat_map.hpp>
#include <boost/functional/hash.hpp>

#include "crab/patricia_trees.hpp"
#include "crab/types.hpp"

namespace crab {

class linear_expression_t final {

  public:
    using component_t = std::pair<number_t, variable_t>;
    using variable_set_t = patricia_tree_set<variable_t>;

  private:
    using map_t = boost::container::flat_map<variable_t, number_t>;
    using map_ptr = std::shared_ptr<map_t>;
    using pair_t = typename map_t::value_type;

    map_ptr _map;
    number_t _cst;

    linear_expression_t(map_ptr map, number_t cst) : _map(std::move(map)), _cst(std::move(cst)) {}

    linear_expression_t(const map_t& map, number_t cst) : _map(std::make_shared<map_t>()), _cst(std::move(cst)) {
        *this->_map = map;
    }

    void add(variable_t x, const number_t& n) {
        typename map_t::iterator it = this->_map->find(x);
        if (it != this->_map->end()) {
            number_t r = it->second + n;
            if (r == 0) {
                this->_map->erase(it);
            } else {
                it->second = r;
            }
        } else {
            if (n != 0) {
                this->_map->insert(pair_t(x, n));
            }
        }
    }

  public:
    using iterator = typename map_t::iterator;
    using const_iterator = typename map_t::const_iterator;

    linear_expression_t() : _map(std::make_shared<map_t>()), _cst(0) {}

    linear_expression_t(linear_expression_t&& other) = default;
    linear_expression_t(const linear_expression_t& other) = default;

    explicit linear_expression_t(number_t n) : _map(std::make_shared<map_t>()), _cst(std::move(n)) {}

    linear_expression_t(signed long long int n) : _map(std::make_shared<map_t>()), _cst(number_t(n)) {}

    linear_expression_t(variable_t x) : _map(std::make_shared<map_t>()), _cst(0) {
        this->_map->insert(pair_t(x, number_t(1)));
    }

    linear_expression_t(const number_t& n, variable_t x) : _map(std::make_shared<map_t>()), _cst(0) {
        this->_map->insert(pair_t(x, n));
    }

    linear_expression_t& operator=(const linear_expression_t& e) {
        if (this != &e) {
            this->_map = e._map;
            this->_cst = e._cst;
        }
        return *this;
    }

    const_iterator begin() const { return this->_map->begin(); }

    const_iterator end() const { return this->_map->end(); }

    iterator begin() { return this->_map->begin(); }

    iterator end() { return this->_map->end(); }

    size_t hash() const {
        size_t res = 0;
        for (const auto& p : *this) {
            boost::hash_combine(res, p);
        }
        boost::hash_combine(res, _cst);
        return res;
    }

    // syntactic equality
    bool equal(const linear_expression_t& o) const {
        if (is_constant()) {
            if (!o.is_constant()) {
                return false;
            } else {
                return (constant() == o.constant());
            }
        } else {
            if (constant() != o.constant()) {
                return false;
            }

            if (size() != o.size()) {
                return false;
            } else {
                for (const_iterator it = begin(), jt = o.begin(), et = end(); it != et; ++it, ++jt) {
                    if (((*it).first != (*jt).first) || ((*it).second != (*jt).second)) {
                        return false;
                    }
                }
                return true;
            }
        }
    }

    bool is_constant() const { return (this->_map->empty()); }

    number_t constant() const { return this->_cst; }

    std::size_t size() const { return this->_map->size(); }

    number_t operator[](variable_t x) const {
        typename map_t::const_iterator it = this->_map->find(x);
        if (it != this->_map->end()) {
            return it->second;
        } else {
            return 0;
        }
    }

    template <typename RenamingMap>
    linear_expression_t rename(const RenamingMap& map) const {
        number_t cst(this->_cst);
        linear_expression_t new_exp(cst);
        for (auto v : this->variables()) {
            auto const it = map.find(v);
            if (it != map.end()) {
                variable_t v_out((*it).second);
                new_exp = new_exp + linear_expression_t(this->operator[](v), v_out);
            } else {
                new_exp = new_exp + linear_expression_t(this->operator[](v), v);
            }
        }
        return new_exp;
    }

    linear_expression_t operator+(number_t n) const {
        linear_expression_t r(this->_map, this->_cst + std::move(n));
        return r;
    }

    linear_expression_t operator+(int n) const { return this->operator+(number_t(n)); }

    linear_expression_t operator+(variable_t x) const {
        linear_expression_t r(*this->_map, this->_cst);
        r.add(x, number_t(1));
        return r;
    }

    linear_expression_t operator+(const linear_expression_t& e) const {
        linear_expression_t r(*this->_map, this->_cst + e._cst);
        for (typename map_t::const_iterator it = e._map->begin(); it != e._map->end(); ++it) {
            r.add(it->first, it->second);
        }
        return r;
    }

    linear_expression_t operator-(const number_t& n) const { return this->operator+(-n); }

    linear_expression_t operator-(int n) const { return this->operator+(-number_t(n)); }

    linear_expression_t operator-(variable_t x) const {
        linear_expression_t r(*this->_map, this->_cst);
        r.add(x, number_t(-1));
        return r;
    }

    linear_expression_t operator-() const { return this->operator*(number_t(-1)); }

    linear_expression_t operator-(const linear_expression_t& e) const {
        linear_expression_t r(*this->_map, this->_cst - e._cst);
        for (typename map_t::const_iterator it = e._map->begin(); it != e._map->end(); ++it) {
            r.add(it->first, -it->second);
        }
        return r;
    }

    linear_expression_t operator*(const number_t& n) const {
        if (n == 0) {
            return linear_expression_t();
        } else {
            map_ptr map = std::make_shared<map_t>();
            for (typename map_t::const_iterator it = this->_map->begin(); it != this->_map->end(); ++it) {
                number_t c = n * it->second;
                if (c != 0) {
                    map->insert(pair_t(it->first, c));
                }
            }
            return linear_expression_t(map, n * this->_cst);
        }
    }

    linear_expression_t operator*(int n) const { return operator*(number_t(n)); }

    variable_set_t variables() const {
        variable_set_t variables;
        for (const auto& v_c : *this) {
            variables += v_c.first;
        }
        return variables;
    }

    void write(std::ostream& o) const {
        bool start = true;
        for (auto [v, n] : *this) {
            if (n > 0 && !start) {
                o << "+";
            }
            if (n == -1) {
                o << "-";
            } else if (n != 1) {
                o << n << "*";
            }
            o << v;
            start = false;
        }
        if (this->_cst > 0 && !this->_map->empty()) {
            o << "+";
        }
        if (this->_cst != 0 || this->_map->empty()) {
            o << this->_cst;
        }
    }

    // for dgb
    void dump() { write(std::cout); }

}; // class linear_expression_t

inline std::ostream& operator<<(std::ostream& o, const linear_expression_t& e) {
    e.write(o);
    return o;
}

inline std::size_t hash_value(const linear_expression_t& e) { return e.hash(); }

struct linear_expression_hasher_t {
    size_t operator()(const linear_expression_t& e) const { return e.hash(); }
};

struct linear_expression_equal_t {
    bool operator()(const linear_expression_t& e1, const linear_expression_t& e2) const { return e1.equal(e2); }
};

using linear_expression_unordered_set =
    std::unordered_set<linear_expression_t, linear_expression_hasher_t, linear_expression_equal_t>;

template <typename Value>
using linear_expression_unordered_map =
    std::unordered_map<linear_expression_t, Value, linear_expression_hasher_t, linear_expression_equal_t>;

class linear_constraint_t final {

  public:
    using variable_set_t = patricia_tree_set<variable_t>;
    using constraint_kind_t = enum { EQUALITY, DISEQUATION, INEQUALITY, STRICT_INEQUALITY };
    using iterator = typename linear_expression_t::iterator;
    using const_iterator = typename linear_expression_t::const_iterator;

  private:
    constraint_kind_t _kind;
    linear_expression_t _expr;
    // This flag has meaning only if _kind == INEQUALITY or STRICT_INEQUALITY.
    // If true the inequality is signed otherwise unsigned.
    // By default all constraints are signed.
    bool _signedness;

  public:
    linear_constraint_t() : _kind(EQUALITY), _signedness(true) {}
    linear_constraint_t(linear_constraint_t&& other) = default;
    linear_constraint_t(const linear_constraint_t& other) = default;

    linear_constraint_t(linear_expression_t expr, constraint_kind_t kind)
        : _kind(kind), _expr(std::move(expr)), _signedness(true) {}

    linear_constraint_t(linear_expression_t expr, constraint_kind_t kind, bool signedness)
        : _kind(kind), _expr(std::move(expr)), _signedness(signedness) {
        if (_kind != INEQUALITY && _kind != STRICT_INEQUALITY) {
            CRAB_ERROR("Only inequalities can have signedness information");
        }
    }

    static linear_constraint_t get_true() {
        linear_constraint_t res(linear_expression_t(number_t(0)), EQUALITY);
        return res;
    }

    static linear_constraint_t get_false() {
        linear_constraint_t res(linear_expression_t(number_t(0)), DISEQUATION);
        return res;
    }

    bool is_tautology() const {
        switch (this->_kind) {
        case DISEQUATION: return (this->_expr.is_constant() && this->_expr.constant() != 0);
        case EQUALITY: return (this->_expr.is_constant() && this->_expr.constant() == 0);
        case INEQUALITY: return (this->_expr.is_constant() && this->_expr.constant() <= 0);
        case STRICT_INEQUALITY: return (this->_expr.is_constant() && this->_expr.constant() < 0);
        default: CRAB_ERROR("Unreachable");
        }
    }

    bool is_contradiction() const {
        switch (this->_kind) {
        case DISEQUATION: return (this->_expr.is_constant() && this->_expr.constant() == 0);
        case EQUALITY: return (this->_expr.is_constant() && this->_expr.constant() != 0);
        case INEQUALITY: return (this->_expr.is_constant() && this->_expr.constant() > 0);
        case STRICT_INEQUALITY: return (this->_expr.is_constant() && this->_expr.constant() >= 0);
        default: CRAB_ERROR("Unreachable");
        }
    }

    bool is_inequality() const { return (this->_kind == INEQUALITY); }

    bool is_strict_inequality() const { return (this->_kind == STRICT_INEQUALITY); }

    bool is_equality() const { return (this->_kind == EQUALITY); }

    bool is_disequation() const { return (this->_kind == DISEQUATION); }

    const linear_expression_t& expression() const { return this->_expr; }

    constraint_kind_t kind() const { return this->_kind; }

    bool is_signed() const {
        if (_kind != INEQUALITY && _kind != STRICT_INEQUALITY) {
            CRAB_WARN("Only inequalities have signedness");
        }
        return _signedness;
    }

    bool is_unsigned() const { return (!is_signed()); }

    const_iterator begin() const { return this->_expr.begin(); }

    const_iterator end() const { return this->_expr.end(); }

    iterator begin() { return this->_expr.begin(); }

    iterator end() { return this->_expr.end(); }

    std::size_t size() const { return this->_expr.size(); }

    // syntactic equality
    bool equal(const linear_constraint_t& o) const {
        return (_kind == o._kind && _signedness == o._signedness && _expr.equal(o._expr));
    }

    size_t hash() const {
        size_t res = 0;
        boost::hash_combine(res, _expr);
        boost::hash_combine(res, _kind);
        if (_kind == INEQUALITY || _kind == STRICT_INEQUALITY) {
            boost::hash_combine(res, _signedness);
        }
        return res;
    }

    index_t index() const {
        // XXX: to store linear constraints in patricia trees
        return (index_t)hash();
    }

    number_t operator[](variable_t x) const { return this->_expr.operator[](x); }

    linear_constraint_t negate() const {

        if (is_tautology()) {
            return get_false();
        } else if (is_contradiction()) {
            return get_true();
        } else {
            switch (kind()) {
            case INEQUALITY: {
                // try to take advantage if we use z_number_t.
                // negate(e <= 0) = e >= 1
                return linear_constraint_t(-(expression() - 1), INEQUALITY, is_signed());
            }
            case STRICT_INEQUALITY: {
                // negate(x + y < 0)  <-->  x + y >= 0 <--> -x -y <= 0
                linear_expression_t e = -this->_expr;
                return linear_constraint_t(e, INEQUALITY, is_signed());
            }
            case EQUALITY: return linear_constraint_t(this->_expr, DISEQUATION);
            case DISEQUATION: return linear_constraint_t(this->_expr, EQUALITY);
            default: CRAB_ERROR("Cannot negate linear constraint");
            }
        }
    }

    template <typename RenamingMap>
    linear_constraint_t rename(const RenamingMap& map) const {
        linear_expression_t e = this->_expr.rename(map);
        return linear_constraint_t(e, this->_kind, is_signed());
    }

    void write(std::ostream& o) const {
        if (this->is_contradiction()) {
            o << "false";
        } else if (this->is_tautology()) {
            o << "true";
        } else {
            linear_expression_t e = this->_expr - this->_expr.constant();
            o << e;
            switch (this->_kind) {
            case INEQUALITY: {
                if (is_signed()) {
                    o << " <= ";
                } else {
                    o << " <=_u ";
                }
                break;
            }
            case STRICT_INEQUALITY: {
                if (is_signed()) {
                    o << " < ";
                } else {
                    o << " <_u ";
                }
                break;
            }
            case EQUALITY: {
                o << " = ";
                break;
            }
            case DISEQUATION: {
                o << " != ";
                break;
            }
            }
            number_t c = -this->_expr.constant();
            o << c;
        }
    }

    // for dgb
    void dump() { write(std::cout); }

}; // class linear_constraint_t

inline std::ostream& operator<<(std::ostream& o, const linear_constraint_t& c) {
    c.write(o);
    return o;
}

inline std::size_t hash_value(const linear_constraint_t& e) { return e.hash(); }

inline linear_expression_t var_sub(variable_t x, const number_t& n) { return linear_expression_t(x).operator-(n); }
inline linear_expression_t var_sub(variable_t x, variable_t y) { return linear_expression_t(x).operator-(y); }
inline linear_expression_t var_add(variable_t x, number_t n) { return linear_expression_t(x).operator+(std::move(n)); }
inline linear_expression_t var_add(variable_t x, variable_t y) { return linear_expression_t(x).operator+(y); }
inline linear_expression_t var_mul(const number_t& n, variable_t x) { return linear_expression_t(n, x); }
} // namespace crab


#include "graph_cut.h"

#include <unordered_map>

#include <boost/graph/stoer_wagner_min_cut.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/typeof/typeof.hpp>

extern "C" {
    #include "graclus/metisLib/metis.h"
}

#include <glog/logging.h>

namespace GraphSfM {
namespace graph {

// Wrapper class for weighted, undirected Graclus graph.
class GraclusGraph {
public:
    GraclusGraph(const std::vector<std::pair<int, int>>& edges,
                 const std::vector<int>& weights)
    {
        CHECK_EQ(edges.size(), weights.size());

        std::unordered_map<int, std::vector<std::pair<int, int>>> adjacency_list;
        for (size_t i = 0; i < edges.size(); ++i) {
            const auto& edge = edges[i];
            const auto weight = weights[i];
            const int vertex_idx1 = GetVertexIdx(edge.first);
            const int vertex_idx2 = GetVertexIdx(edge.second);
            adjacency_list[vertex_idx1].emplace_back(vertex_idx2, weight);
            adjacency_list[vertex_idx2].emplace_back(vertex_idx1, weight);
        }

        xadj_.reserve(vertex_id_to_idx_.size() + 1);
        adjncy_.reserve(2 * edges.size());
        adjwgt_.reserve(2 * edges.size());

        idxtype edge_idx = 0;
        for (size_t i = 0; i < vertex_id_to_idx_.size(); ++i) {
            xadj_.push_back(edge_idx);

            if (adjacency_list.count(i) == 0) { continue; }

            for (const auto& edge : adjacency_list[i]) {
                edge_idx += 1;
                adjncy_.push_back(edge.first);
                adjwgt_.push_back(edge.second);
            }
        }

        xadj_.push_back(edge_idx);

        CHECK_EQ(edge_idx, 2 * edges.size());
        CHECK_EQ(xadj_.size(), vertex_id_to_idx_.size() + 1);
        CHECK_EQ(adjncy_.size(), 2 * edges.size());
        CHECK_EQ(adjwgt_.size(), 2 * edges.size());

        data.gdata = data.rdata = nullptr;

        data.nvtxs = vertex_id_to_idx_.size();
        data.nedges = 2 * edges.size();
        data.mincut = data.minvol = -1;

        data.xadj = xadj_.data();
        data.adjncy = adjncy_.data();

        data.vwgt = nullptr;
        data.adjwgt = adjwgt_.data();

        data.adjwgtsum = nullptr;
        data.label = nullptr;
        data.cmap = nullptr;

        data.where = data.pwgts = nullptr;
        data.id = data.ed = nullptr;
        data.bndptr = data.bndind = nullptr;
        data.rinfo = nullptr;
        data.vrinfo = nullptr;
        data.nrinfo = nullptr;

        data.ncon = 1;
        data.nvwgt = nullptr;
        data.npwgts = nullptr;

        data.vsize = nullptr;

        data.coarser = data.finer = nullptr;
    }

    int GetVertexIdx(const int id) {
        const auto it = vertex_id_to_idx_.find(id);
        if (it == vertex_id_to_idx_.end()) {
            const int idx = vertex_id_to_idx_.size();
            vertex_id_to_idx_.emplace(id, idx);
            vertex_idx_to_id_.emplace(idx, id);
            return idx;
        } else {
            return it->second;
        }
    }

    int GetVertexId(const int idx) { return vertex_idx_to_id_.at(idx); }

    GraphType data;

private:
    std::unordered_map<int, int> vertex_id_to_idx_;
    std::unordered_map<int, int> vertex_idx_to_id_;
    std::vector<idxtype> xadj_;
    std::vector<idxtype> adjncy_;
    std::vector<idxtype> adjwgt_;
};

void ComputeMinGraphCutStoerWagner(
    const std::vector<std::pair<int, int>>& edges,
    const std::vector<int>& weights, int* cut_weight,
    std::vector<char>* cut_labels)
{
    CHECK_EQ(edges.size(), weights.size());
    CHECK_GE(edges.size(), 2);

    typedef boost::property<boost::edge_weight_t, int> edge_weight_t;
    typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
                                boost::no_property, edge_weight_t>
        undirected_graph_t;

    int max_vertex_index = 0;
    for (const auto& edge : edges) {
        CHECK_GE(edge.first, 0);
        CHECK_GE(edge.second, 0);
        max_vertex_index = std::max(max_vertex_index, edge.first);
        max_vertex_index = std::max(max_vertex_index, edge.second);
    }

    const undirected_graph_t graph(edges.begin(), edges.end(), weights.begin(),
                                   max_vertex_index + 1, edges.size());

    const auto parities = boost::make_one_bit_color_map(
        boost::num_vertices(graph), boost::get(boost::vertex_index, graph));

    *cut_weight = boost::stoer_wagner_min_cut(graph, boost::get(boost::edge_weight, graph),
                                  boost::parity_map(parities));

    cut_labels->resize(boost::num_vertices(graph));
    for (size_t i = 0; i < boost::num_vertices(graph); ++i) {
        (*cut_labels)[i] = boost::get(parities, i);
    }
}

std::unordered_map<int, int> ComputeNormalizedMinGraphCut(
    const std::vector<std::pair<int, int>>& edges,
    const std::vector<int>& weights, const int num_parts)
{
    GraclusGraph graph(edges, weights);

    const int levels =
        amax((graph.data.nvtxs) / (40 * log2_metis(num_parts)), 20 * (num_parts));

    std::vector<idxtype> cut_labels(graph.data.nvtxs);

    int options[11];
    options[0] = 0;
    int wgtflag = 1;
    int numflag = 0;
    int chain_length = 0;
    int edgecut;
    int var_num_parts = num_parts;

    MLKKM_PartGraphKway(&graph.data.nvtxs, graph.data.xadj, graph.data.adjncy,
                        graph.data.vwgt, graph.data.adjwgt, &wgtflag, &numflag,
                        &var_num_parts, &chain_length, options, &edgecut,
                        cut_labels.data(), levels);

    float lbvec[MAXNCON];
    ComputePartitionBalance(&graph.data, num_parts, cut_labels.data(), lbvec);
    ComputeNCut(&graph.data, &cut_labels[0], num_parts);
    std::unordered_map<int, int> labels;
    for (size_t idx = 0; idx < cut_labels.size(); ++idx) {
        labels.emplace(graph.GetVertexId(idx), cut_labels[idx]);
    }

    return labels;
}

}  // namespace graph
}  // namespace GraphSfM


#pragma once

#include <boost/mpl/at.hpp>
#include <cassert>
#include <cstdio>
#include <functional>
#include <iostream>
#include "aux/filtered_range.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"
#include "storage/multi_slice.hpp"

namespace boltzmann {
namespace ct_dense {

class multi_slices_factory
{
 public:
  typedef MultiSlice::index_type index_type;
  // typedef std::tuple<enum TRIG, index_type> key_t;
  /// key: (angular frequency, _sin_ or _cos_)
  typedef std::tuple<index_type, enum TRIG> key_t;
  typedef unsigned int size_type;
  typedef std::map<key_t, MultiSlice> container_t;

 public:
  template <typename BASIS>
  static void create(container_t& data, const BASIS& basis);
};

template <typename BASIS>
void
multi_slices_factory::create(container_t& data, const BASIS& basis)
{
  typedef std::map<key_t, MultiSlice> container_t;
  typedef typename BASIS::elem_t elem_t;
  typedef typename boost::mpl::at_c<typename elem_t::types_t, 0>::type fa_type;
  typedef typename boost::mpl::at_c<typename elem_t::types_t, 1>::type fr_type;
  // typename elem_t::Acc::template get<fr_type> fr_accessor;
  typename elem_t::Acc::template get<fa_type> fa_accessor;

  const index_type L = spectral::get_max_l(basis);
  const size_type N = basis.n_dofs();

  for (auto elem = basis.begin(); elem != basis.end(); elem++) {
    auto ang_elem = fa_accessor(*elem);

    const int l = ang_elem.get_id().l;
    const enum TRIG t = (TRIG)ang_elem.get_id().t;
    // key_t key(t, l);
    key_t key(l, t);

    if (data.find(key) != data.end()) continue;  // this element is already done

    typedef std::vector<std::pair<index_type, index_type> > line_t;
    // line 1 (first line)
    line_t line1;
    for (index_type l1 = 0; l1 < l + 1; ++l1) {
      assert(l - l1 >= 0);
      line1.push_back(std::make_pair(l1, l - l1));
    }
    // line 2 (lower diagonal)
    line_t line2;
    for (index_type l1 = l + 1; l1 <= L; ++l1) {
      line2.push_back(std::make_pair(l1, l1 - l));
    }
    // line 3 (upper diagonal)
    line_t line3;

    if (l > 0) {
      // hint: if l==0, line2 and line3 are the same
      for (index_type l1 = 1; l1 <= L - l; ++l1) {
        line3.push_back(std::make_pair(l1, l1 + l));
      }
    }

    auto cmp = [&fa_accessor](const elem_t& e, index_type l, enum TRIG t) {
      auto id = fa_accessor(e).get_id();
      return (id.l == l && TRIG(id.t) == t);
    };
    // ---------- add lines to data, compute offsets and block sizes ----------
    // find all elements with given (l, t)-values
    auto range_z =
        filtered_range(basis.begin(), basis.end(), std::bind(cmp, std::placeholders::_1, l, t));
    std::vector<elem_t> elemsz(std::get<0>(range_z), std::get<1>(range_z));
    size_type size_z = elemsz.size();

    MultiSlice current_mslice(t, l, N);

    auto add_line = [&](const line_t& line) {
      // process lines
      for (auto pair : line) {
        // l1 <-> row
        const index_type l1 = pair.first;
        // l2 <-> column
        const index_type l2 = pair.second;
        // enum TRIG t1;
        // enum TRIG t2;

        auto add_range = [&](const std::vector<elem_t>& x, const std::vector<elem_t>& y) {
          if (x.size() == 0 || y.size() == 0) return;  // nothing to do
          size_type offset_x = basis.get_dof_index(x.begin()->get_id());
          size_type size_x = basis.get_dof_index(x.rbegin()->get_id()) - offset_x + 1;
          assert(size_x == x.size());
          size_type offset_y = basis.get_dof_index(y.begin()->get_id());
          size_type size_y = basis.get_dof_index(y.rbegin()->get_id()) - offset_y + 1;
          assert(size_y == y.size());
          assert(size_x > 0 && size_y > 0);
          enum TRIG t1 = TRIG(fa_accessor(x[0]).get_id().t);
          enum TRIG t2 = TRIG(fa_accessor(y[0]).get_id().t);
          current_mslice.add_block(l1, t1, l2, t2, offset_x, offset_y, size_x, size_y, size_z);
        };

        if (t == TRIG::COS) {
          // Note: range should be contiguous in ordering of elements (this is not ensured here!!)
          // add range1
          auto range1x = filteredv(
              basis.begin(), basis.end(), std::bind(cmp, std::placeholders::_1, l1, TRIG::COS));
          auto range1y = filteredv(
              basis.begin(), basis.end(), std::bind(cmp, std::placeholders::_1, l2, TRIG::COS));
          // aka add block (l1, cos) x (l2, cos) to multi_slice
          add_range(range1x, range1y);

          auto range2x = filteredv(
              basis.begin(), basis.end(), std::bind(cmp, std::placeholders::_1, l1, TRIG::SIN));
          auto range2y = filteredv(
              basis.begin(), basis.end(), std::bind(cmp, std::placeholders::_1, l2, TRIG::SIN));
          add_range(range2x, range2y);
        } else if (t == TRIG::SIN) {
          // add range1
          auto range1x = filteredv(
              basis.begin(), basis.end(), std::bind(cmp, std::placeholders::_1, l1, TRIG::COS));
          auto range1y = filteredv(
              basis.begin(), basis.end(), std::bind(cmp, std::placeholders::_1, l2, TRIG::SIN));
          add_range(range1x, range1y);
          // add range2
          auto range2x = filteredv(
              basis.begin(), basis.end(), std::bind(cmp, std::placeholders::_1, l1, TRIG::SIN));
          auto range2y = filteredv(
              basis.begin(), basis.end(), std::bind(cmp, std::placeholders::_1, l2, TRIG::COS));
          add_range(range2x, range2y);
        }
      }
    };

    add_line(line1);
    add_line(line2);
    add_line(line3);

    current_mslice.finalize();

#ifdef DEBUG
    std::printf("MultiSlice finished\n\tStats: (l=%3d, t=%3d), size: %6d, blocks: %3d\n",
                l,
                int(t),
                current_mslice.size(),
                current_mslice.nblocks());
#endif
    // insert mslice into map
    data[key] = current_mslice;
  }
}
}  // ct_dense
}  // end namespace boltzmann


/*
 symmetry_core.cpp

 Copyright (c) 2014, 2015, 2016 Terumasa Tadano

 This file is distributed under the terms of the MIT license.
 Please see the file 'LICENCE.txt' in the root directory
 or http://opensource.org/licenses/mit-license.php for information.
*/

#include "mpi_common.h"
#include "symmetry_core.h"
#include "memory.h"
#include "constants.h"
#include "error.h"
#include "system.h"
#include <iomanip>
#include <fstream>
#include <algorithm>
#include <set>
#include "mathfunctions.h"

#ifdef _USE_EIGEN
#include <Eigen/Core>
#endif

using namespace PHON_NS;

Symmetry::Symmetry(PHON *phon): Pointers(phon){
    file_sym = "SYMM_INFO_PRIM";
    time_reversal_sym = false;
}
Symmetry::~Symmetry(){}

void Symmetry::setup_symmetry()
{
    unsigned int natmin = system->natmin;
    double **xtmp;
    unsigned int *kdtmp;

    memory->allocate(xtmp, natmin, 3);
    memory->allocate(kdtmp, natmin);

    unsigned int i, j;

    for (i = 0; i < natmin; ++i){
        rotvec(xtmp[i], system->xr_s[system->map_p2s[i][0]], system->lavec_s);
        rotvec(xtmp[i], xtmp[i], system->rlavec_p);

        for (j = 0; j < 3; ++j) xtmp[i][j] /= 2.0 * pi;

        kdtmp[i] = system->kd[system->map_p2s[i][0]];
    }

    SymmList.clear();

    if (mympi->my_rank == 0) {
        std::cout << " Symmetry" << std::endl;
        std::cout << " ========" << std::endl << std::endl;
        setup_symmetry_operation(natmin, nsym, system->lavec_p, system->rlavec_p, xtmp, kdtmp);
    }

    MPI_Bcast(&nsym, 1, MPI_UNSIGNED, 0, MPI_COMM_WORLD);
    broadcast_symmlist(SymmList);

    if (mympi->my_rank == 0) {
        std::cout << "  Number of symmetry operations : " << nsym << std::endl << std::endl;
        gensym_withmap(xtmp, kdtmp);
    }
}

void Symmetry::setup_symmetry_operation(int N, unsigned int &nsym, double aa[3][3], double bb[3][3],
                                        double **x, unsigned int *kd)
{
    int i, j;
    std::ofstream ofs_sym;
    std::ifstream ifs_sym;
    SymmList.clear();

    if (nsym == 0) {

        // Automatically find symmetries.

        std::cout << "  NSYM = 0 is given: Trying to find symmetry operations." << std::endl;

        findsym(N, aa, x, SymmList);

        std::sort(SymmList.begin() + 1, SymmList.end());
        nsym = SymmList.size();

        if (printsymmetry) {
            std::cout << "  PRINTSYMM = 1: Symmetry information will be stored in SYMM_INFO_PRIM file." << std::endl << std::endl;
            ofs_sym.open(file_sym.c_str(), std::ios::out);
            ofs_sym << nsym << std::endl;

            for (std::vector<SymmetryOperation>::iterator p = SymmList.begin(); p != SymmList.end(); ++p) {
                for (i = 0; i < 3; ++i) {
                    for (j = 0; j < 3; ++j) {
                        ofs_sym << std::setw(4) << (*p).rot[i][j];
                    }
                }
                ofs_sym << "  ";
                for (i = 0; i < 3; ++i) {
                    ofs_sym << std::setprecision(15) << std::setw(20) << (*p).tran[i];
                }
                ofs_sym << std::endl;
            }

            ofs_sym.close();
        }

    } else if (nsym == 1) {

        // Identity operation only !

        std::cout << "  NSYM = 1 is given: Only the identity matrix will be considered." << std::endl << std::endl;

        int rot_tmp[3][3];
        double tran_tmp[3];

        for (i = 0; i < 3; ++i){
            for (j = 0; j < 3; ++j){
                if(i == j) {
                    rot_tmp[i][j] = 1;
                } else {
                    rot_tmp[i][j] = 0;
                }
            }
            tran_tmp[i] = 0.0;
        }

        SymmList.push_back(SymmetryOperation(rot_tmp, tran_tmp));

    } else {

        std::cout << "  NSYM > 1 is given: Symmetry operations will be read from SYMM_INFO_PRIM file" << std::endl << std::endl;

        int nsym2;
        int rot_tmp[3][3];
        double tran_tmp[3];

        ifs_sym.open(file_sym.c_str(), std::ios::in);
        ifs_sym >> nsym2;

        if(nsym != nsym2) error->exit("setup_symmetry_operation", "nsym in the given file and the input file are not consistent.");

        for (i = 0; i < nsym; ++i) {
            ifs_sym >> rot_tmp[0][0] >> rot_tmp[0][1] >> rot_tmp[0][2]
            >> rot_tmp[1][0] >> rot_tmp[1][1] >> rot_tmp[1][2]
            >> rot_tmp[2][0] >> rot_tmp[2][1] >> rot_tmp[2][2]
            >> tran_tmp[0] >> tran_tmp[1] >> tran_tmp[2];

            SymmList.push_back(SymmetryOperation(rot_tmp, tran_tmp));
        }
        ifs_sym.close();
    }
}

void Symmetry::findsym(int N, double aa[3][3], double **x, std::vector<SymmetryOperation> &symop_all) {

    std::vector<RotationMatrix> LatticeSymmList;

    // Generate rotational matrices that don't change the metric tensor
    LatticeSymmList.clear();
    find_lattice_symmetry(aa, LatticeSymmList);

    // Generate all the space group operations with translational vectors
    symop_all.clear();
    find_crystal_symmetry(N, system->nclassatom, system->atomlist_class, x,
        LatticeSymmList, symop_all);

    LatticeSymmList.clear();
}

void Symmetry::find_lattice_symmetry(double aa[3][3], std::vector<RotationMatrix> &LatticeSymmList) {

    /*
    Find the rotational matrices that leave the metric tensor invariant.

    Metric tensor G = (g)_{ij} = a_{i} * a_{j} is invariant under crystal symmetry operations T,
    i.e. T^{t}GT = G. Since G can be written as G = A^{t}A, the invariance condition is given by
    (AT)^{t}(AT) = G0 (original).
    */

    int i, j, k;
    int m11, m12, m13, m21, m22, m23, m31, m32, m33;

    int nsym_tmp = 0;
    int mat_tmp[3][3];
    double det, res;
    double rot_tmp[3][3];
    double aa_rot[3][3];

    double metric_tensor[3][3];
    double metric_tensor_rot[3][3];

    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            metric_tensor[i][j] = 0.0;
            for (k = 0; k < 3; ++k) {
                metric_tensor[i][j] += aa[k][i] * aa[k][j];
            }
        }
    }

    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            if (i == j) {
                mat_tmp[i][i] = 1;
            } else {
                mat_tmp[i][j] = 0;
            }
        }
    }

    // Identity matrix should be the first entry.
    LatticeSymmList.push_back(mat_tmp);

    for (m11 = -1; m11 <= 1; ++m11){
        for (m12 = -1; m12 <= 1; ++m12) {
            for (m13 = -1; m13 <= 1; ++m13){
                for (m21 = -1; m21 <= 1; ++m21){
                    for (m22 = -1; m22 <= 1; ++m22){
                        for (m23 = -1; m23 <= 1; ++m23){
                            for (m31 = -1; m31 <= 1; ++m31){
                                for (m32 = -1; m32 <= 1; ++m32){
                                    for (m33 = -1; m33 <= 1; ++m33){

                                        if (m11 == 1 && m12 == 0 && m13 == 0 &&
                                            m21 == 0 && m22 == 1 && m23 == 0 &&
                                            m31 == 0 && m32 == 0 && m33 == 1) continue;

                                        det = m11 * (m22 * m33 - m32 * m23)
                                            - m21 * (m12 * m33 - m32 * m13)
                                            + m31 * (m12 * m23 - m22 * m13);

                                        if (det != 1 && det != -1) continue;

                                        rot_tmp[0][0] = m11;
                                        rot_tmp[0][1] = m12;
                                        rot_tmp[0][2] = m13;
                                        rot_tmp[1][0] = m21;
                                        rot_tmp[1][1] = m22;
                                        rot_tmp[1][2] = m23;
                                        rot_tmp[2][0] = m31;
                                        rot_tmp[2][1] = m32;
                                        rot_tmp[2][2] = m33;

                                        // Here, aa_rot = aa * rot_tmp is correct.
                                        matmul3(aa_rot, aa, rot_tmp);

                                        for (i = 0; i < 3; ++i) {
                                            for (j = 0; j < 3; ++j) {
                                                metric_tensor_rot[i][j] = 0.0;
                                                for (k = 0; k < 3; ++k) {
                                                    metric_tensor_rot[i][j] += aa_rot[k][i] * aa_rot[k][j];
                                                }
                                            }
                                        }

                                        res = 0.0;
                                        for (i = 0; i < 3; ++i) {
                                            for (j = 0; j < 3; ++j) {
                                                res += std::pow(metric_tensor[i][j] - metric_tensor_rot[i][j], 2.0);
                                            }
                                        }

                                        // Metric tensor is invariant under symmetry operations.

                                        if (res < tolerance * tolerance) {
                                            ++nsym_tmp;
                                            for (i = 0; i < 3; ++i) {
                                                for (j = 0; j < 3; ++j) {
                                                    mat_tmp[i][j] = static_cast<int>(rot_tmp[i][j]);
                                                }
                                            }
                                            LatticeSymmList.push_back(mat_tmp);
                                        }

                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    }

    if (LatticeSymmList.size() > 48) {
        error->exit("find_lattice_symmetry", "Number of lattice symmetry is larger than 48.");
    }
}

void Symmetry::find_crystal_symmetry(int N, int nclass, std::vector<unsigned int> *atomclass, double **x,
                                     std::vector<RotationMatrix> LatticeSymmList, std::vector<SymmetryOperation> &CrystalSymmList){

    unsigned int i, j;
    unsigned int iat, jat, kat, lat;
    double x_rot[3];
    double rot[3][3], rot_tmp[3][3], rot_cart[3][3];
    double tran[3];
    double x_rot_tmp[3];
    double tmp[3];
    double mag[3], mag_rot[3];
    double diff;

    int rot_int[3][3];

    int ii, jj, kk;
    unsigned int itype;

    bool is_found;
    bool isok;
    bool mag_sym1, mag_sym2;

    bool is_identity_matrix;

    // Add identity matrix first.
    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            if (i == j) {
                rot_int[i][j] = 1;
            } else {
                rot_int[i][j] = 0;
            }
        }
        tran[i] = 0.0;
    }

    CrystalSymmList.push_back(SymmetryOperation(rot_int, tran));

    for (std::vector<RotationMatrix>::iterator it_latsym = LatticeSymmList.begin(); it_latsym != LatticeSymmList.end(); ++it_latsym) {

        iat = atomclass[0][0];

        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                rot[i][j] = static_cast<double>((*it_latsym).mat[i][j]);
            }
        }

        rotvec(x_rot, x[iat], rot);

#ifdef _OPENMP
#pragma omp parallel for private(jat, tran, isok, kat, x_rot_tmp, is_found, lat, tmp, diff, \
    i, j, itype, jj, kk, is_identity_matrix, mag, mag_rot, rot_tmp, rot_cart, mag_sym1, mag_sym2)
#endif
        for (ii = 0; ii < atomclass[0].size(); ++ii) {
            jat = atomclass[0][ii];

            for (i = 0; i < 3; ++i) {
                tran[i] = x[jat][i] - x_rot[i];
                tran[i] = tran[i] - nint(tran[i]);
            }

            isok = true;

            is_identity_matrix =
                ( std::pow(rot[0][0] - 1.0, 2) + std::pow(rot[0][1], 2) + std::pow(rot[0][2], 2)
                + std::pow(rot[1][0], 2) + std::pow(rot[1][1] - 1.0, 2) + std::pow(rot[1][2], 2)
                + std::pow(rot[2][0], 2) + std::pow(rot[2][1], 2) + std::pow(rot[2][2] - 1.0, 2)
                + std::pow(tran[0], 2) + std::pow(tran[1], 2) + std::pow(tran[2], 2) ) < eps12;

            if (is_identity_matrix) continue;

            for (itype = 0; itype < nclass; ++itype) {

                for (jj = 0; jj < atomclass[itype].size(); ++jj) {

                    kat = atomclass[itype][jj];

                    rotvec(x_rot_tmp, x[kat], rot);

                    for (i = 0; i < 3; ++i) {
                        x_rot_tmp[i] += tran[i];
                    }

                    is_found = false;

                    for (kk = 0; kk < atomclass[itype].size(); ++kk) {

                        lat = atomclass[itype][kk];

                        for (i = 0; i < 3; ++i) {
                            tmp[i] = std::fmod(std::abs(x[lat][i] - x_rot_tmp[i]), 1.0);
                            tmp[i] = std::min<double>(tmp[i], 1.0 - tmp[i]);
                        }
                        diff = tmp[0]*tmp[0] + tmp[1]*tmp[1] + tmp[2]*tmp[2];

                        if (diff < tolerance * tolerance) {
                            is_found = true;
                            break;
                        }
                    }

                    if (!is_found) isok = false;
                }
            }

            if (isok && system->lspin && system->noncollinear) {
                for (i = 0; i < 3; ++i) {
                    mag[i] = system->magmom[jat][i];
                    mag_rot[i] = system->magmom[iat][i];
                }

                matmul3(rot_tmp, rot, system->rlavec_p);
                matmul3(rot_cart, system->lavec_p, rot_tmp);

                for (i = 0; i < 3; ++i) {
                    for (j = 0; j < 3; ++j) {
                        rot_cart[i][j] /= (2.0 * pi);
                    }
                }
                rotvec(mag_rot, mag_rot, rot_cart);

                // In the case of improper rotation, the factor -1 should be multiplied
                // because the inversion operation doesn't flip the spin.
                if (!is_proper(rot_cart)) {
                    for (i = 0; i < 3; ++i) {
                        mag_rot[i] = -mag_rot[i];
                    }
                }

                mag_sym1 = (std::pow(mag[0] - mag_rot[0], 2.0)
                    + std::pow(mag[1] - mag_rot[1], 2.0)
                    + std::pow(mag[2] - mag_rot[2], 2.0) ) < eps6;

                mag_sym2 = (std::pow(mag[0] + mag_rot[0], 2.0)
                    + std::pow(mag[1] + mag_rot[1], 2.0)
                    + std::pow(mag[2] + mag_rot[2], 2.0) ) < eps6;

                if (!mag_sym1 && !mag_sym2) {
                    isok = false;
                } else if (!mag_sym1 && mag_sym2 && !trev_sym_mag) {
                    isok = false;
                }
            }

            if (isok) {
#ifdef _OPENMP
#pragma omp critical
#endif
                CrystalSymmList.push_back(SymmetryOperation((*it_latsym).mat, tran));
            }
        }

    }
}

void Symmetry::gensym_withmap(double **x, unsigned int *kd)
{
    // Generate symmetry operations in Cartesian coordinate with the atom-mapping information.

    double S[3][3], T[3][3], S_recip[3][3], mat_tmp[3][3];
    double shift[3], x_mod[3], tmp[3];
    double diff;
    unsigned int *map_tmp;
    int i, j, k;
    int num_mapped;
    unsigned int natmin = system->natmin;

    SymmListWithMap.clear();

    memory->allocate(map_tmp, natmin);

    for (std::vector<SymmetryOperation>::iterator isym = SymmList.begin(); isym != SymmList.end(); ++isym) {

        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                T[i][j] = static_cast<double>((*isym).rot[i][j]);
            }
        }

        for (i = 0; i < 3; ++i) {
            shift[i] = (*isym).tran[i];
        }

        invmat3(mat_tmp, T);
        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                S_recip[i][j] = mat_tmp[j][i];
            }
        }

        // Convert to Cartesian coordinate
        matmul3(mat_tmp, T, system->rlavec_p);
        matmul3(S, system->lavec_p, mat_tmp);
        for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
                S[i][j] /= 2.0 * pi;
            }
        }

        // Generate mapping information

        for (i = 0; i < natmin; ++i) {

            rotvec(x_mod, x[i], T);

            for (j = 0; j < 3; ++j) {
                x_mod[j] += shift[j];
            }

            num_mapped = -1;

            for (j = 0; j < natmin; ++j) {

                if (kd[j] == kd[i]) {

                    for (k = 0; k < 3; ++k) {
                        tmp[k] = std::fmod(std::abs(x_mod[k] - x[j][k]), 1.0);
                        tmp[k] = std::min<double>(tmp[k], 1.0 - tmp[k]);
                    }
                    diff = tmp[0] * tmp[0] + tmp[1] * tmp[1] + tmp[2] * tmp[2];
                    if (diff < tolerance * tolerance) {
                        num_mapped = j;
                        break;
                    }
                }
            }

            if (num_mapped == -1) {
                error->exit("gensym_withmap", "cannot find a equivalent atom");
            }
            map_tmp[i] = num_mapped;
        }

        // Add to vector

        SymmListWithMap.push_back(SymmetryOperationWithMapping(S, T, S_recip, map_tmp, natmin, shift));
    }
}

void Symmetry::broadcast_symmlist(std::vector<SymmetryOperation> &sym)
{
    int i, j, k;
    int n;
    std::vector<int> sym_entry;
    int ***rot_tmp, rot[3][3];
    double **tran_tmp, tran[3];

    if (mympi->my_rank == 0) n = sym.size();
    MPI_Bcast(&n, 1, MPI_INT, 0, MPI_COMM_WORLD);

    memory->allocate(rot_tmp, n, 3, 3);
    memory->allocate(tran_tmp, n, 3);

    if (mympi->my_rank == 0) {
        for (i = 0; i < n; ++i) {
            for (j = 0; j < 3; ++j) {
                for (k = 0; k < 3; ++k) {
                    rot_tmp[i][j][k] = sym[i].rot[j][k];
                }
                tran_tmp[i][j] = sym[i].tran[j];
            }
        }
    }
    MPI_Bcast(&rot_tmp[0][0][0], 9*n, MPI_INT, 0, MPI_COMM_WORLD);
    MPI_Bcast(&tran_tmp[0][0], 3*n, MPI_DOUBLE, 0, MPI_COMM_WORLD);

    if (mympi->my_rank > 0) {
        for (i = 0; i < n; ++i) {
            for (j = 0; j < 3; ++j) {
                for (k = 0; k < 3; ++k) {
                    rot[j][k] = rot_tmp[i][j][k];
                }
                tran[j] = tran_tmp[i][j];
            }
            sym.push_back(SymmetryOperation(rot,tran));
        }
    }

    memory->deallocate(rot_tmp);
    memory->deallocate(tran_tmp);
}

bool Symmetry::is_proper(double rot[3][3])
{
    double det;
    bool ret;

    det = rot[0][0] * (rot[1][1] * rot[2][2] - rot[2][1] * rot[1][2])
        - rot[1][0] * (rot[0][1] * rot[2][2] - rot[2][1] * rot[0][2])
        + rot[2][0] * (rot[0][1] * rot[1][2] - rot[1][1] * rot[0][2]);

    if (std::abs(det - 1.0) < eps12) {
        ret = true;
    } else if (std::abs(det + 1.0) < eps12) {
        ret = false;
    } else {
        error->exit("is_proper", "This cannot happen.");
    }

    return ret;
}


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

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

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_LINE_LINE_INTERSECTION_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_LINE_LINE_INTERSECTION_HPP

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/strategies/buffer.hpp>
#include <boost/geometry/algorithms/detail/buffer/parallel_continue.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace buffer
{

// TODO: once change this to proper strategy
// It is different from current segment intersection because these are not segments but lines
// If we have the Line concept, we can create a strategy
// Assumes a convex corner
struct line_line_intersection
{

    template <typename Point>
    static inline strategy::buffer::join_selector apply(Point const& pi, Point const& pj,
        Point const& qi, Point const& qj, Point& ip)
    {
        typedef typename coordinate_type<Point>::type ct;

        // Construct lines in general form (ax + by + c = 0),
        // (will be replaced by a general_form structure in a next PR)
        ct const pa = get<1>(pi) - get<1>(pj);
        ct const pb = get<0>(pj) - get<0>(pi);
        ct const pc = -pa * get<0>(pi) - pb * get<1>(pi);

        ct const qa = get<1>(qi) - get<1>(qj);
        ct const qb = get<0>(qj) - get<0>(qi);
        ct const qc = -qa * get<0>(qi) - qb * get<1>(qi);

        ct const denominator = pb * qa - pa * qb;

        // Even if the corner was checked before (so it is convex now), that
        // was done on the original geometry. This function runs on the buffered
        // geometries, where sides are generated and might be slightly off. In
        // Floating Point, that slightly might just exceed the limit and we have
        // to check it again.

        // For round joins, it will not be used at all.
        // For miter joins, there is a miter limit
        // If segments are parallel/collinear we must be distinguish two cases:
        // they continue each other, or they form a spike
        ct const zero = ct();
        if (math::equals(denominator, zero))
        {
            return parallel_continue(qb, -qa, pb, -pa)
                ? strategy::buffer::join_continue
                : strategy::buffer::join_spike
                ;
        }

        set<0>(ip, (pc * qb - pb * qc) / denominator);
        set<1>(ip, (pa * qc - pc * qa) / denominator);

        return strategy::buffer::join_convex;
    }
};

}} // namespace detail::buffer
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_LINE_LINE_INTERSECTION_HPP


/*
 * This file is a part of the TChecker project.
 *
 * See files AUTHORS and LICENSE for copyright details.
 *
 */

#include <boost/dynamic_bitset.hpp>

#include "tchecker/dbm/refdbm.hh"
#include "tchecker/refzg/semantics.hh"

namespace tchecker {

namespace refzg {

/* standard_semantics_t */

tchecker::state_status_t standard_semantics_t::initial(tchecker::dbm::db_t * rdbm,
                                                       tchecker::reference_clock_variables_t const & r,
                                                       boost::dynamic_bitset<> const & delay_allowed,
                                                       tchecker::clock_constraint_container_t const & invariant)
{
  tchecker::refdbm::zero(rdbm, r);

  if (tchecker::refdbm::constrain(rdbm, r, invariant) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_SRC_INVARIANT_VIOLATED;

  return tchecker::STATE_OK;
}

tchecker::state_status_t standard_semantics_t::next(tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
                                                    boost::dynamic_bitset<> const & src_delay_allowed,
                                                    tchecker::clock_constraint_container_t const & src_invariant,
                                                    boost::dynamic_bitset<> const & sync_ref_clocks,
                                                    tchecker::clock_constraint_container_t const & guard,
                                                    tchecker::clock_reset_container_t const & clkreset,
                                                    boost::dynamic_bitset<> const & tgt_delay_allowed,
                                                    tchecker::clock_constraint_container_t const & tgt_invariant)
{
  if (src_delay_allowed.any()) {
    tchecker::refdbm::asynchronous_open_up(rdbm, r, src_delay_allowed);

    if (tchecker::refdbm::constrain(rdbm, r, src_invariant) == tchecker::dbm::EMPTY)
      return tchecker::STATE_CLOCKS_SRC_INVARIANT_VIOLATED; // should never occur
  }

  if (tchecker::refdbm::synchronize(rdbm, r, sync_ref_clocks) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_EMPTY_SYNC;

  if (tchecker::refdbm::constrain(rdbm, r, guard) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_GUARD_VIOLATED;

  tchecker::refdbm::reset(rdbm, r, clkreset);

  if (tchecker::refdbm::constrain(rdbm, r, tgt_invariant) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_TGT_INVARIANT_VIOLATED;

  return tchecker::STATE_OK;
}

/* elapsed_semantics_t */

tchecker::state_status_t elapsed_semantics_t::initial(tchecker::dbm::db_t * rdbm,
                                                      tchecker::reference_clock_variables_t const & r,
                                                      boost::dynamic_bitset<> const & delay_allowed,
                                                      tchecker::clock_constraint_container_t const & invariant)
{
  tchecker::refdbm::zero(rdbm, r);

  if (tchecker::refdbm::constrain(rdbm, r, invariant) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_SRC_INVARIANT_VIOLATED;

  if (delay_allowed.any()) {
    tchecker::refdbm::asynchronous_open_up(rdbm, r, delay_allowed);

    if (tchecker::refdbm::constrain(rdbm, r, invariant) == tchecker::dbm::EMPTY)
      return tchecker::STATE_CLOCKS_SRC_INVARIANT_VIOLATED;
  }

  return tchecker::STATE_OK;
}

tchecker::state_status_t elapsed_semantics_t::next(tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
                                                   boost::dynamic_bitset<> const & src_delay_allowed,
                                                   tchecker::clock_constraint_container_t const & src_invariant,
                                                   boost::dynamic_bitset<> const & sync_ref_clocks,
                                                   tchecker::clock_constraint_container_t const & guard,
                                                   tchecker::clock_reset_container_t const & clkreset,
                                                   boost::dynamic_bitset<> const & tgt_delay_allowed,
                                                   tchecker::clock_constraint_container_t const & tgt_invariant)
{
  if (tchecker::refdbm::constrain(rdbm, r, src_invariant) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_SRC_INVARIANT_VIOLATED;

  if (tchecker::refdbm::synchronize(rdbm, r, sync_ref_clocks) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_EMPTY_SYNC;

  if (tchecker::refdbm::constrain(rdbm, r, guard) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_GUARD_VIOLATED;

  tchecker::refdbm::reset(rdbm, r, clkreset);

  if (tchecker::refdbm::constrain(rdbm, r, tgt_invariant) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_TGT_INVARIANT_VIOLATED;

  if (tgt_delay_allowed.any()) {
    tchecker::refdbm::asynchronous_open_up(rdbm, r, tgt_delay_allowed);

    if (tchecker::refdbm::constrain(rdbm, r, tgt_invariant) == tchecker::dbm::EMPTY)
      return tchecker::STATE_CLOCKS_TGT_INVARIANT_VIOLATED;
  }

  return tchecker::STATE_OK;
}

/* factory */

tchecker::refzg::semantics_t * semantics_factory(enum tchecker::refzg::semantics_type_t semantics)
{
  switch (semantics) {
  case tchecker::refzg::STANDARD_SEMANTICS:
    return new tchecker::refzg::standard_semantics_t{};
  case tchecker::refzg::ELAPSED_SEMANTICS:
    return new tchecker::refzg::elapsed_semantics_t{};
  default:
    throw std::invalid_argument("Unknown semantics over zones with reference clocks");
  }
}

} // end of namespace refzg

} // end of namespace tchecker