/*Copyright (c) 2021 James Gayvert

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

/*
 * molcas_interface.cpp
 */

#include "molcas_interface.h"

#include <h5pp/h5pp.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <algorithm>
#include <Eigen/Dense>
#include <fstream>
#include <iostream>
#include <unsupported/Eigen/CXX11/Tensor>

#include "BasisSet.h"
#include "gto_ordering.h"
#include "opencap_exception.h"
#include "utils.h"

template<typename T>
using  MatrixType = Eigen::Matrix<T,Eigen::Dynamic, Eigen::Dynamic>;

template<typename Scalar,int rank, typename sizeType>
auto Tensor_to_Matrix(const Eigen::Tensor<Scalar,rank> &tensor,const sizeType rows,const sizeType cols)
{
    return Eigen::Map<const MatrixType<Scalar>> (tensor.data(), rows,cols);
}

Eigen::Map<const Eigen::MatrixXd> reshape (const Eigen::VectorXd& b, const uint n, const uint m) {
    return Eigen::Map<const Eigen::MatrixXd>(b.data(), n, m);
}

void read_rassi_tdms(std::vector<std::vector<Eigen::MatrixXd>> &alpha_opdms,
		std::vector<std::vector<Eigen::MatrixXd>> &beta_opdms,
		std::string filename, BasisSet bs,size_t nstates)
{
    alpha_opdms = std::vector< std::vector<Eigen::MatrixXd>>(nstates, std::vector<Eigen::MatrixXd> (nstates));
    beta_opdms = std::vector< std::vector<Eigen::MatrixXd>>(nstates, std::vector<Eigen::MatrixXd> (nstates));
	h5pp::File file(filename, h5pp::FilePermission::READONLY);
	//first lets check if the dimensions are correct
	std::vector<long> nbas_vec = file.readAttribute<std::vector<long>>("NBAS","/");
	long nbas=0;
	for(auto num_bas:nbas_vec)
		nbas+=num_bas;
	if(nbas!=bs.Nbasis)
		opencap_throw("Error: dimensions of RASSI basis set does not match specified basis set.");
	auto nsym = file.readAttribute<long>("NSYM", "/");
	//now lets load the densities
	Eigen::Tensor<double,3> rass_data, spin_dens;
	try
	{
		file.readDataset(rass_data,"SFS_TRANSITION_DENSITIES");
		file.readDataset(spin_dens,"SFS_TRANSITION_SPIN_DENSITIES");
	}
	catch(exception &e)
	{
		opencap_throw("Error: SFS_TRANSITION_DENSITIES dataset not found. Use the TRD1 keyword"
				" in the RASSI module to activate transition densities.")
	}
    const auto& d = rass_data.dimensions();

    if (nsym>1)
    {
    	//Step 1: get the desymmetrization matrix
		Eigen::VectorXd desym_vec;
		file.readDataset(desym_vec,"DESYM_MATRIX");
		Eigen::MatrixXd desym_mat;
		std::vector<long> nbas_vec = file.readAttribute<std::vector<long>>("NBAS","/");
		desym_mat = reshape(desym_vec,nbas,nbas);
		long mat_size=0;
		for(auto num_bas:nbas_vec)
			mat_size+=num_bas*num_bas;
		//now lets loop over the matrices
		for(long i=0;i<d[0];i++)
		{
            if(d[0]!=nstates)
                opencap_throw("Error: Found " + std::to_string(d[0]) + " states in RASSI file, but "
                          + std::to_string(nstates) + " states were specified.");
			std::vector<Eigen::MatrixXd> alpha_state_row;
			std::vector<Eigen::MatrixXd> beta_state_row;
			for (long j=0;j<d[1];j++)
			{
				//step 2: get the raw matrices
				Eigen::array<long,3> offset = {i,j,0};    //Starting point
				Eigen::array<long,3> extent = {1,1,1}; //end point
				Eigen::Tensor<double, 2> dmt_slice = rass_data.slice(offset, extent).reshape(Eigen::array<long,2>{mat_size,(long)1});
				Eigen::MatrixXd  dmt_vec =  Tensor_to_Matrix(dmt_slice,mat_size,(long)1);
				Eigen::Tensor<double, 2> spin_slice = spin_dens.slice(offset, extent).reshape(Eigen::array<long,2>{mat_size,(long)1});
				Eigen::MatrixXd  spin_vec =  Tensor_to_Matrix(dmt_slice,mat_size,(long)1);

				//step 3: turn into bigger matrix by adding zeros
				Eigen::MatrixXd dmat,spin;
				dmat = Eigen::MatrixXd::Zero(nbas,nbas);
				spin = Eigen::MatrixXd::Zero(nbas,nbas);
				//fill the blocks
				size_t elements_index = 0;
				size_t bf_index = 0;
				for(size_t isym=0;isym<nsym;isym++)
				{
					size_t n_elements = nbas_vec[isym] * nbas_vec[isym];
					Eigen::VectorXd dmat_block_vec(n_elements),spin_block_vec(n_elements);
					Eigen::MatrixXd dmat_block,spin_block;
					//grab the elements we need
					for(size_t k=0;k<n_elements;k++)
					{
						dmat_block_vec(k) = dmt_vec(elements_index+k);
						spin_block_vec(k) = spin_vec(elements_index+k);
					}
					dmat_block = reshape(dmat_block_vec,nbas_vec[isym],nbas_vec[isym]);
					spin_block = reshape(spin_block_vec,nbas_vec[isym],nbas_vec[isym]);
					dmat.block(bf_index,bf_index,nbas_vec[isym],nbas_vec[isym]) = dmat_block;
					spin.block(bf_index,bf_index,nbas_vec[isym],nbas_vec[isym]) = spin_block;
					//update our index counters
					elements_index+=n_elements;
					bf_index+= nbas_vec[isym];
				}

				//step 4: desymmetrize
				Eigen::MatrixXd desym_dmat = desym_mat * dmat * desym_mat.transpose();
				Eigen::MatrixXd desym_spin = desym_mat * spin * desym_mat.transpose();

				//step 5: save
				Eigen::MatrixXd alpha_opdm,beta_opdm;
				alpha_opdm= 0.5*(desym_dmat+desym_spin);
				beta_opdm = 0.5*(desym_dmat-desym_spin);
				to_opencap_ordering(alpha_opdm,bs,get_molcas_ids(bs,filename));
				to_opencap_ordering(beta_opdm,bs,get_molcas_ids(bs,filename));
                alpha_opdms[i][j] = alpha_opdm;
                beta_opdms[i][j] = beta_opdm;
			}
		}

    }
    else
    {
		//now lets loop over the matrices
		for(long i=0;i<d[0];i++)
		{
            if(d[0]!=nstates)
            opencap_throw("Error: Found " + std::to_string(d[0]) + " states in RASSI file, but "
                          + std::to_string(nstates) + " states were specified.");
			for (long j=0;j<d[1];j++)
			{
				Eigen::array<long,3> offset = {i,j,0};    //Starting point
				Eigen::array<long,3> extent = {1,1,1}; //end point
				Eigen::Tensor<double, 2> dmt_slice = rass_data.slice(offset, extent).reshape(Eigen::array<long,2>{nbas,nbas});
				Eigen::MatrixXd  dmt_mat =  Tensor_to_Matrix(dmt_slice,nbas,nbas);
				Eigen::Tensor<double, 2> spin_slice = spin_dens.slice(offset, extent).reshape(Eigen::array<long,2>{nbas,nbas});
				Eigen::MatrixXd  spin_mat =  Tensor_to_Matrix(dmt_slice,nbas,nbas);
				Eigen::MatrixXd alpha_opdm,beta_opdm;
				alpha_opdm= 0.5*(dmt_mat+spin_mat);
				beta_opdm = 0.5*(dmt_mat-spin_mat);
				to_opencap_ordering(alpha_opdm,bs,get_molcas_ids(bs,filename));
				to_opencap_ordering(beta_opdm,bs,get_molcas_ids(bs,filename));
                alpha_opdms[i][j] = alpha_opdm;
                beta_opdms[i][j] = beta_opdm;
			}
		}
    }

    std::cout << "Warning: TDM M-->N is assumed to be conjugate transpose of "
    << "TDM N-->M where M>N" << std::endl;
    for (size_t i=0;i<nstates;i++)
    {
    	for(size_t j=i+1;j<nstates;j++)
    	{
    		alpha_opdms[j][i]= alpha_opdms[i][j].adjoint();
    		beta_opdms[j][i]= beta_opdms[i][j].adjoint();
    	}
    }
}

Eigen::MatrixXd read_rassi_overlap(std::string filename,BasisSet bs)
{
	Eigen::VectorXd overlap_vec;
	h5pp::File file(filename, h5pp::FilePermission::READONLY);
	file.readDataset(overlap_vec,"AO_OVERLAP_MATRIX");
	auto nsym = file.readAttribute<long>("NSYM", "/");
	if(nsym >1)
	{
		Eigen::VectorXd desym_vec;
		file.readDataset(desym_vec,"DESYM_MATRIX");
		Eigen::MatrixXd desym_mat;
		std::vector<long> nbas_vec = file.readAttribute<std::vector<long>>("NBAS","/");
		long nbas=0;
		for(auto num_bas:nbas_vec)
			nbas+=num_bas;
		desym_mat = reshape(desym_vec,nbas,nbas);
		Eigen::MatrixXd smat;
		smat = Eigen::MatrixXd::Zero(nbas,nbas);
		//fill the blocks
		size_t elements_index = 0;
		size_t bf_index = 0;
		for(size_t isym=0;isym<nsym;isym++)
		{
			size_t n_elements = nbas_vec[isym] * nbas_vec[isym];
			Eigen::VectorXd block_vec(n_elements);
			Eigen::MatrixXd block_mat;
			//grab the elements we need
			for(size_t i=0;i<n_elements;i++)
				block_vec(i) = overlap_vec(elements_index+i);
			block_mat = reshape(block_vec,nbas_vec[isym],nbas_vec[isym]);
			smat.block(bf_index,bf_index,nbas_vec[isym],nbas_vec[isym]) = block_mat;
			//update our index counters
			elements_index+=n_elements;
			bf_index+= nbas_vec[isym];
		}
		Eigen::MatrixXd desym_overlap = desym_mat * smat * desym_mat.transpose();
		to_opencap_ordering(desym_overlap,bs,get_molcas_ids(bs,filename));
		return desym_overlap;
	}
	else
	{
		int nbas = sqrt(overlap_vec.size());
		Eigen::MatrixXd smat;
		smat = reshape(overlap_vec,nbas,nbas);
		to_opencap_ordering(smat,bs,get_molcas_ids(bs,filename));
		return smat;
	}
}

Eigen::MatrixXd read_rotation_matrix(size_t nstates, std::ifstream &is)
{
	Eigen::MatrixXd rotation_matrix(nstates,nstates);
	rotation_matrix = Eigen::MatrixXd::Zero(nstates,nstates);
	std::string line, rest;
	size_t num_groups = nstates%5==0 ? nstates/5 : nstates/5+1;
	for (size_t i=1;i<=num_groups;i++)
	{
		std::getline(is,line);
        if(i>1)
            std::getline(is,line);
		for (size_t j=1;j<=nstates;j++)
		{
			std::getline(is,line);
			std::vector<std::string> tokens = split(line,' ');
			size_t row_idx = std::stoul(tokens[0]);
			for(size_t k=1;k<tokens.size();k++)
			{
				size_t col_idx = k-1+(i-1)*5;
				if(col_idx>=rotation_matrix.cols() || row_idx-1 >= rotation_matrix.rows())
					opencap_throw("Error: State index of out bounds. There is a problem with the OpenMolcas output file. Exiting...");
				rotation_matrix(row_idx-1,col_idx)=std::stod(tokens[k]);
			}
		}
	}
	return rotation_matrix;
}

Eigen::MatrixXd read_mscaspt2_heff(size_t nstates, std::string filename, Eigen::MatrixXd &rotation_matrix)
{
	Eigen::MatrixXd ZERO_ORDER_H(nstates,nstates);
	ZERO_ORDER_H= Eigen::MatrixXd::Zero(nstates,nstates);
	std::ifstream is(filename);
	if (is.good())
	{
		std::string line, rest;
		std::getline(is,line);
		while (line.find("Number of CI roots used")== std::string::npos && is.peek()!=EOF)
			std::getline(is,line);
		size_t num_states = stoi(split(line,' ').back());
		if (num_states!=nstates)
			opencap_throw("Error: "+std::to_string(num_states)+ " roots were found in the OpenMolcas "
					"output file, but " + std::to_string(nstates) +" states were specified in the input. "
							"Exiting...");
		while (line.find("MULTI-STATE CASPT2 SECTION")== std::string::npos && is.peek()!=EOF)
		{
			std::getline(is,line);
			if (line.find("H0 eigenvectors:")!= std::string::npos)
				rotation_matrix = read_rotation_matrix(nstates,is);
		}
		if (is.peek()==EOF)
			opencap_throw("Error: Reached end of file before MULTI-STATE CASPT2 SECTION.");
		//get diagonal shift
		for (size_t i=1;i<=3;i++)
			std::getline(is,line);
        double E_shift = 0.0;
        if(line.find("Output diagonal")!=std::string::npos)
        {
            std::vector<std::string> split_line = split(line,' ');
            E_shift = std::stod(split_line[split_line.size()-1]);
            for(size_t i=1;i<=2;i++)
                std::getline(is,line);
        }
		size_t num_groups = nstates%5==0 ? nstates/5 : nstates/5+1;
		for (size_t i=1;i<=num_groups;i++)
		{
			for (size_t j=1;j<=2;j++)
				std::getline(is,line);
			//now time to start reading in the matrix elements
			size_t states_in_group = nstates - (i-1)*5;
			for (size_t j=1;j<=states_in_group;j++)
			{
				std::getline(is,line);
				std::vector<std::string> tokens = split(line,' ');
				size_t row_idx = std::stoul(tokens[0]);
				for(size_t k=1;k<tokens.size();k++)
				{
					size_t col_idx = k-1+(i-1)*5;
					if(col_idx>=ZERO_ORDER_H.cols() || row_idx-1 >= ZERO_ORDER_H.rows())
						opencap_throw("Error: State index of out bounds. There is a problem with the OpenMolcas output file. Exiting...");
					ZERO_ORDER_H(row_idx-1,col_idx)=std::stod(tokens[k]);
					ZERO_ORDER_H(col_idx,row_idx-1)=std::stod(tokens[k]);
				}
			}
		}
		for (size_t i=0;i<nstates;i++)
			ZERO_ORDER_H(i,i)+=E_shift;
	}
	else
	{
    	opencap_throw("Error: I couldn't read:" + filename);
	}
	return ZERO_ORDER_H;
}

Eigen::MatrixXd read_nevpt2_heff(size_t nstates, std::string filename, std::string method)
{
	std::string line_to_find;
	if(compare_strings(method,"pc-nevpt2"))
		line_to_find = "Zero + second order effective Hamiltonian (PC)";
	else if(compare_strings(method,"sc-nevpt2"))
		line_to_find = "Zero + second order Effective Hamiltonian (SC)";
	else
		opencap_throw("Either pc-nevpt2 or sc-nevpt2 should be selected.");
	Eigen::MatrixXd ZERO_ORDER_H(nstates,nstates);
	ZERO_ORDER_H= Eigen::MatrixXd::Zero(nstates,nstates);
	std::ifstream is(filename);
	if (is.good())
	{
		std::string line, rest;
		std::getline(is,line);
		while (line.find(line_to_find)== std::string::npos && is.peek()!=EOF)
			std::getline(is,line);
		if(is.peek()==EOF)
			opencap_throw("Error: Unable to find QD-NEVPT2 effective Hamiltonian.");
		std::getline(is,line);
		std::getline(is,line);
		size_t num_groups = nstates%5==0 ? nstates/5 : nstates/5+1;
		for (size_t i=1;i<=num_groups;i++)
		{
			std::getline(is,line);
			for (size_t j=1;j<=nstates;j++)
			{
				std::getline(is,line);
				std::vector<std::string> tokens = split(line,' ');
				size_t row_idx = std::stoul(tokens[0]);
				for(size_t k=1;k<tokens.size();k++)
				{
					size_t col_idx = k-1+(i-1)*5;
					if(col_idx>=ZERO_ORDER_H.cols() || row_idx-1 >= ZERO_ORDER_H.rows())
						opencap_throw("Error: State index of out bounds. There is a problem with the OpenMolcas output file. Exiting...");
					ZERO_ORDER_H(row_idx-1,col_idx)=std::stod(tokens[k]);
				}
			}
		}
	}
	else
	{
    	opencap_throw("Error: I couldn't read:" + filename);
	}
	return ZERO_ORDER_H;
}

std::vector<Atom> read_geometry_from_rassi(std::string filename)
{
	h5pp::File file(filename, h5pp::FilePermission::READONLY);
	auto nsym = file.readAttribute<long>("NSYM", "/");
	std::string atom_nums_tag,coords_tag;
	if (nsym>1)
	{
		atom_nums_tag="DESYM_CENTER_ATNUMS";
		coords_tag = "DESYM_CENTER_COORDINATES";
	}
	else
	{
		atom_nums_tag="CENTER_ATNUMS";
		coords_tag = "CENTER_COORDINATES";
	}
	std::vector<Atom> atoms;
	std::vector<long> atomic_nums;
	Eigen::MatrixXd coords_mat;
	file.readDataset(atomic_nums,atom_nums_tag);
	file.readDataset(coords_mat,coords_tag);
	for(size_t i=0;i<coords_mat.rows();i++)
		atoms.push_back(Atom(atomic_nums[i],coords_mat(i,0),coords_mat(i,1),coords_mat(i,2)));
	return atoms;
}

BasisSet read_basis_from_rassi(std::string filename,std::vector<Atom> atoms)
{
	h5pp::File file(filename, h5pp::FilePermission::READONLY);
	Eigen::Matrix<long,Eigen::Dynamic,Eigen::Dynamic> prim_ids,basis_ids;
	Eigen::MatrixXd prims;
	auto nsym = file.readAttribute<long>("NSYM", "/");
	file.readDataset(prims,"PRIMITIVES");
	file.readDataset(prim_ids,"PRIMITIVE_IDS");
	std::string bf_ids_tag;
	if(nsym>1)
		bf_ids_tag="DESYM_BASIS_FUNCTION_IDS";
	else
		bf_ids_tag ="BASIS_FUNCTION_IDS";
	file.readDataset(basis_ids,bf_ids_tag);
	BasisSet bs;
	for(auto atm:atoms)
		bs.centers.push_back(atm.coords);
	for(size_t i=0;i<basis_ids.rows();i++)
	{
		long ctr = basis_ids(i,0);
		long shell_num = basis_ids(i,1);
		int l = basis_ids(i,2);
		shell_id id(ctr,shell_num,l);
		int bs_idx = bs.get_index_of_shell_id(id);
		if (bs_idx==-1)
		{
			Shell new_shell(l,bs.centers[ctr-1]);
			new_shell.l=abs(l);
			if(l<0 && abs(l)>1)
				new_shell.pure=false;
			bs.add_shell(new_shell);
		}
	}
	for(size_t i=0;i<prim_ids.rows();i++)
	{
		long ctr = prim_ids(i,0);
		int l = prim_ids(i,1);
		long shell_num = prim_ids(i,2);
		double exp = prims(i,0);
		double coeff = prims(i,1);
		shell_id id(ctr,shell_num,l);
		int bs_idx = bs.get_index_of_shell_id(id);
		if (coeff!=0)
			bs.basis[bs_idx].add_primitive(exp,coeff);
	}
	bs.normalize();
	return bs;
}


/**
* This file is part of ORB-SLAM3
*
* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
*
* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along with ORB-SLAM3.
* If not, see <http://www.gnu.org/licenses/>.
*/

#include "KannalaBrandt8.h"

#include <boost/serialization/export.hpp>

//BOOST_CLASS_EXPORT_IMPLEMENT(ORB_SLAM3::KannalaBrandt8)

namespace ORB_SLAM3 {
//BOOST_CLASS_EXPORT_GUID(KannalaBrandt8, "KannalaBrandt8")

    cv::Point2f KannalaBrandt8::project(const cv::Point3f &p3D) {
        const float x2_plus_y2 = p3D.x * p3D.x + p3D.y * p3D.y;
        const float theta = atan2f(sqrtf(x2_plus_y2), p3D.z);
        const float psi = atan2f(p3D.y, p3D.x);

        const float theta2 = theta * theta;
        const float theta3 = theta * theta2;
        const float theta5 = theta3 * theta2;
        const float theta7 = theta5 * theta2;
        const float theta9 = theta7 * theta2;
        const float r = theta + mvParameters[4] * theta3 + mvParameters[5] * theta5
                        + mvParameters[6] * theta7 + mvParameters[7] * theta9;

        return cv::Point2f(mvParameters[0] * r * cos(psi) + mvParameters[2],
                           mvParameters[1] * r * sin(psi) + mvParameters[3]);

    }

    Eigen::Vector2d KannalaBrandt8::project(const Eigen::Vector3d &v3D) {
        const double x2_plus_y2 = v3D[0] * v3D[0] + v3D[1] * v3D[1];
        const double theta = atan2f(sqrtf(x2_plus_y2), v3D[2]);
        const double psi = atan2f(v3D[1], v3D[0]);

        const double theta2 = theta * theta;
        const double theta3 = theta * theta2;
        const double theta5 = theta3 * theta2;
        const double theta7 = theta5 * theta2;
        const double theta9 = theta7 * theta2;
        const double r = theta + mvParameters[4] * theta3 + mvParameters[5] * theta5
                        + mvParameters[6] * theta7 + mvParameters[7] * theta9;

        Eigen::Vector2d res;
        res[0] = mvParameters[0] * r * cos(psi) + mvParameters[2];
        res[1] = mvParameters[1] * r * sin(psi) + mvParameters[3];

        return res;

    }

    Eigen::Vector2f KannalaBrandt8::project(const Eigen::Vector3f &v3D) {
        const float x2_plus_y2 = v3D[0] * v3D[0] + v3D[1] * v3D[1];
        const float theta = atan2f(sqrtf(x2_plus_y2), v3D[2]);
        const float psi = atan2f(v3D[1], v3D[0]);

        const float theta2 = theta * theta;
        const float theta3 = theta * theta2;
        const float theta5 = theta3 * theta2;
        const float theta7 = theta5 * theta2;
        const float theta9 = theta7 * theta2;
        const float r = theta + mvParameters[4] * theta3 + mvParameters[5] * theta5
                         + mvParameters[6] * theta7 + mvParameters[7] * theta9;

        Eigen::Vector2f res;
        res[0] = mvParameters[0] * r * cos(psi) + mvParameters[2];
        res[1] = mvParameters[1] * r * sin(psi) + mvParameters[3];

        return res;

        /*cv::Point2f cvres = this->project(cv::Point3f(v3D[0],v3D[1],v3D[2]));

        Eigen::Vector2d res;
        res[0] = cvres.x;
        res[1] = cvres.y;

        return res;*/
    }

    Eigen::Vector2f KannalaBrandt8::projectMat(const cv::Point3f &p3D) {
        cv::Point2f point = this->project(p3D);
        return Eigen::Vector2f(point.x, point.y);
    }

    float KannalaBrandt8::uncertainty2(const Eigen::Matrix<double,2,1> &p2D)
    {
        /*Eigen::Matrix<double,2,1> c;
        c << mvParameters[2], mvParameters[3];
        if ((p2D-c).squaredNorm()>57600) // 240*240 (256)
            return 100.f;
        else
            return 1.0f;*/
        return 1.f;
    }

    Eigen::Vector3f KannalaBrandt8::unprojectEig(const cv::Point2f &p2D) {
        cv::Point3f ray = this->unproject(p2D);
        return Eigen::Vector3f(ray.x, ray.y, ray.z);
    }

    cv::Point3f KannalaBrandt8::unproject(const cv::Point2f &p2D) {
        //Use Newthon method to solve for theta with good precision (err ~ e-6)
        cv::Point2f pw((p2D.x - mvParameters[2]) / mvParameters[0], (p2D.y - mvParameters[3]) / mvParameters[1]);
        float scale = 1.f;
        float theta_d = sqrtf(pw.x * pw.x + pw.y * pw.y);
        theta_d = fminf(fmaxf(-CV_PI / 2.f, theta_d), CV_PI / 2.f);

        if (theta_d > 1e-8) {
            //Compensate distortion iteratively
            float theta = theta_d;

            for (int j = 0; j < 10; j++) {
                float theta2 = theta * theta, theta4 = theta2 * theta2, theta6 = theta4 * theta2, theta8 =
                        theta4 * theta4;
                float k0_theta2 = mvParameters[4] * theta2, k1_theta4 = mvParameters[5] * theta4;
                float k2_theta6 = mvParameters[6] * theta6, k3_theta8 = mvParameters[7] * theta8;
                float theta_fix = (theta * (1 + k0_theta2 + k1_theta4 + k2_theta6 + k3_theta8) - theta_d) /
                                  (1 + 3 * k0_theta2 + 5 * k1_theta4 + 7 * k2_theta6 + 9 * k3_theta8);
                theta = theta - theta_fix;
                if (fabsf(theta_fix) < precision)
                    break;
            }
            //scale = theta - theta_d;
            scale = std::tan(theta) / theta_d;
        }

        return cv::Point3f(pw.x * scale, pw.y * scale, 1.f);
    }

    Eigen::Matrix<double, 2, 3> KannalaBrandt8::projectJac(const Eigen::Vector3d &v3D) {
        double x2 = v3D[0] * v3D[0], y2 = v3D[1] * v3D[1], z2 = v3D[2] * v3D[2];
        double r2 = x2 + y2;
        double r = sqrt(r2);
        double r3 = r2 * r;
        double theta = atan2(r, v3D[2]);

        double theta2 = theta * theta, theta3 = theta2 * theta;
        double theta4 = theta2 * theta2, theta5 = theta4 * theta;
        double theta6 = theta2 * theta4, theta7 = theta6 * theta;
        double theta8 = theta4 * theta4, theta9 = theta8 * theta;

        double f = theta + theta3 * mvParameters[4] + theta5 * mvParameters[5] + theta7 * mvParameters[6] +
                  theta9 * mvParameters[7];
        double fd = 1 + 3 * mvParameters[4] * theta2 + 5 * mvParameters[5] * theta4 + 7 * mvParameters[6] * theta6 +
                   9 * mvParameters[7] * theta8;

        Eigen::Matrix<double, 2, 3> JacGood;
        JacGood(0, 0) = mvParameters[0] * (fd * v3D[2] * x2 / (r2 * (r2 + z2)) + f * y2 / r3);
        JacGood(1, 0) =
                mvParameters[1] * (fd * v3D[2] * v3D[1] * v3D[0] / (r2 * (r2 + z2)) - f * v3D[1] * v3D[0] / r3);

        JacGood(0, 1) =
                mvParameters[0] * (fd * v3D[2] * v3D[1] * v3D[0] / (r2 * (r2 + z2)) - f * v3D[1] * v3D[0] / r3);
        JacGood(1, 1) = mvParameters[1] * (fd * v3D[2] * y2 / (r2 * (r2 + z2)) + f * x2 / r3);

        JacGood(0, 2) = -mvParameters[0] * fd * v3D[0] / (r2 + z2);
        JacGood(1, 2) = -mvParameters[1] * fd * v3D[1] / (r2 + z2);

        return JacGood;
    }

    bool KannalaBrandt8::ReconstructWithTwoViews(const std::vector<cv::KeyPoint>& vKeys1, const std::vector<cv::KeyPoint>& vKeys2, const std::vector<int> &vMatches12,
                                          Sophus::SE3f &T21, std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated){
        if(!tvr){
            Eigen::Matrix3f K = this->toK_();
            tvr = new TwoViewReconstruction(K);
        }

        //Correct FishEye distortion
        std::vector<cv::KeyPoint> vKeysUn1 = vKeys1, vKeysUn2 = vKeys2;
        std::vector<cv::Point2f> vPts1(vKeys1.size()), vPts2(vKeys2.size());

        for(size_t i = 0; i < vKeys1.size(); i++) vPts1[i] = vKeys1[i].pt;
        for(size_t i = 0; i < vKeys2.size(); i++) vPts2[i] = vKeys2[i].pt;

        cv::Mat D = (cv::Mat_<float>(4,1) << mvParameters[4], mvParameters[5], mvParameters[6], mvParameters[7]);
        cv::Mat R = cv::Mat::eye(3,3,CV_32F);
        cv::Mat K = this->toK();
        cv::fisheye::undistortPoints(vPts1,vPts1,K,D,R,K);
        cv::fisheye::undistortPoints(vPts2,vPts2,K,D,R,K);

        for(size_t i = 0; i < vKeys1.size(); i++) vKeysUn1[i].pt = vPts1[i];
        for(size_t i = 0; i < vKeys2.size(); i++) vKeysUn2[i].pt = vPts2[i];

        return tvr->Reconstruct(vKeysUn1,vKeysUn2,vMatches12,T21,vP3D,vbTriangulated);
    }

    cv::Mat KannalaBrandt8::toK() {
        cv::Mat K = (cv::Mat_<float>(3, 3)
                << mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f);
        return K;
    }
    Eigen::Matrix3f KannalaBrandt8::toK_() {
        Eigen::Matrix3f K;
        K << mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f;
        return K;
    }

    bool KannalaBrandt8::epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2,
                                           const Eigen::Matrix3f& R12, const Eigen::Vector3f& t12, const float sigmaLevel, const float unc) {
        Eigen::Vector3f p3D;
        return this->TriangulateMatches(pCamera2,kp1,kp2,R12,t12,sigmaLevel,unc,p3D) > 0.0001f;
    }

    bool KannalaBrandt8::matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, GeometricCamera* pOther,
                                             Sophus::SE3f& Tcw1, Sophus::SE3f& Tcw2,
                                             const float sigmaLevel1, const float sigmaLevel2,
                                             Eigen::Vector3f& x3Dtriangulated){
        Eigen::Matrix<float,3,4> eigTcw1 = Tcw1.matrix3x4();
        Eigen::Matrix3f Rcw1 = eigTcw1.block<3,3>(0,0);
        Eigen::Matrix3f Rwc1 = Rcw1.transpose();
        Eigen::Matrix<float,3,4> eigTcw2 = Tcw2.matrix3x4();
        Eigen::Matrix3f Rcw2 = eigTcw2.block<3,3>(0,0);
        Eigen::Matrix3f Rwc2 = Rcw2.transpose();

        cv::Point3f ray1c = this->unproject(kp1.pt);
        cv::Point3f ray2c = pOther->unproject(kp2.pt);

        Eigen::Vector3f r1(ray1c.x, ray1c.y, ray1c.z);
        Eigen::Vector3f r2(ray2c.x, ray2c.y, ray2c.z);

        //Check parallax between rays
        Eigen::Vector3f ray1 = Rwc1 * r1;
        Eigen::Vector3f ray2 = Rwc2 * r2;

        const float cosParallaxRays = ray1.dot(ray2)/(ray1.norm() * ray2.norm());

        //If parallax is lower than 0.9998, reject this match
        if(cosParallaxRays > 0.9998){
            return false;
        }

        //Parallax is good, so we try to triangulate
        cv::Point2f p11,p22;

        p11.x = ray1c.x;
        p11.y = ray1c.y;

        p22.x = ray2c.x;
        p22.y = ray2c.y;

        Eigen::Vector3f x3D;

        Triangulate(p11,p22,eigTcw1,eigTcw2,x3D);

        //Check triangulation in front of cameras
        float z1 = Rcw1.row(2).dot(x3D)+Tcw1.translation()(2);
        if(z1<=0){  //Point is not in front of the first camera
            return false;
        }

        float z2 = Rcw2.row(2).dot(x3D)+Tcw2.translation()(2);
        if(z2<=0){ //Point is not in front of the first camera
            return false;
        }

        //Check reprojection error in first keyframe
        //  -Transform point into camera reference system
        Eigen::Vector3f x3D1 = Rcw1 * x3D + Tcw1.translation();
        Eigen::Vector2f uv1 = this->project(x3D1);

        float errX1 = uv1(0) - kp1.pt.x;
        float errY1 = uv1(1) - kp1.pt.y;

        if((errX1*errX1+errY1*errY1)>5.991*sigmaLevel1){   //Reprojection error is high
            return false;
        }

        //Check reprojection error in second keyframe;
        //  -Transform point into camera reference system
        Eigen::Vector3f x3D2 = Rcw2 * x3D + Tcw2.translation(); // avoid using q
        Eigen::Vector2f uv2 = pOther->project(x3D2);

        float errX2 = uv2(0) - kp2.pt.x;
        float errY2 = uv2(1) - kp2.pt.y;

        if((errX2*errX2+errY2*errY2)>5.991*sigmaLevel2){   //Reprojection error is high
            return false;
        }

        //Since parallax is big enough and reprojection errors are low, this pair of points
        //can be considered as a match
        x3Dtriangulated = x3D;

        return true;
    }

    float KannalaBrandt8::TriangulateMatches(GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const Eigen::Matrix3f& R12, const Eigen::Vector3f& t12, const float sigmaLevel, const float unc, Eigen::Vector3f& p3D) {

        Eigen::Vector3f r1 = this->unprojectEig(kp1.pt);
        Eigen::Vector3f r2 = pCamera2->unprojectEig(kp2.pt);

        //Check parallax
        Eigen::Vector3f r21 = R12 * r2;

        const float cosParallaxRays = r1.dot(r21)/(r1.norm() *r21.norm());

        if(cosParallaxRays > 0.9998){
            return -1;
        }

        //Parallax is good, so we try to triangulate
        cv::Point2f p11,p22;

        p11.x = r1[0];
        p11.y = r1[1];

        p22.x = r2[0];
        p22.y = r2[1];

        Eigen::Vector3f x3D;
        Eigen::Matrix<float,3,4> Tcw1;
        Tcw1 << Eigen::Matrix3f::Identity(), Eigen::Vector3f::Zero();

        Eigen::Matrix<float,3,4> Tcw2;

        Eigen::Matrix3f R21 = R12.transpose();
        Tcw2 << R21, -R21 * t12;

        Triangulate(p11,p22,Tcw1,Tcw2,x3D);
        // cv::Mat x3Dt = x3D.t();

        float z1 = x3D(2);
        if(z1 <= 0){
            return -2;
        }

        float z2 = R21.row(2).dot(x3D)+Tcw2(2,3);
        if(z2<=0){
            return -3;
        }

        //Check reprojection error
        Eigen::Vector2f uv1 = this->project(x3D);

        float errX1 = uv1(0) - kp1.pt.x;
        float errY1 = uv1(1) - kp1.pt.y;

        if((errX1*errX1+errY1*errY1)>5.991 * sigmaLevel){   //Reprojection error is high
            return -4;
        }

        Eigen::Vector3f x3D2 = R21 * x3D + Tcw2.col(3);
        Eigen::Vector2f uv2 = pCamera2->project(x3D2);

        float errX2 = uv2(0) - kp2.pt.x;
        float errY2 = uv2(1) - kp2.pt.y;

        if((errX2*errX2+errY2*errY2)>5.991 * unc){   //Reprojection error is high
            return -5;
        }

        p3D = x3D;

        return z1;
    }

    std::ostream & operator<<(std::ostream &os, const KannalaBrandt8 &kb) {
        os << kb.mvParameters[0] << " " << kb.mvParameters[1] << " " << kb.mvParameters[2] << " " << kb.mvParameters[3] << " "
           << kb.mvParameters[4] << " " << kb.mvParameters[5] << " " << kb.mvParameters[6] << " " << kb.mvParameters[7];
        return os;
    }

    std::istream & operator>>(std::istream &is, KannalaBrandt8 &kb) {
        float nextParam;
        for(size_t i = 0; i < 8; i++){
            assert(is.good());  //Make sure the input stream is good
            is >> nextParam;
            kb.mvParameters[i] = nextParam;

        }
        return is;
    }

    void KannalaBrandt8::Triangulate(const cv::Point2f &p1, const cv::Point2f &p2, const Eigen::Matrix<float,3,4> &Tcw1,
                                     const Eigen::Matrix<float,3,4> &Tcw2, Eigen::Vector3f &x3D)
    {
        Eigen::Matrix<float,4,4> A;
        A.row(0) = p1.x*Tcw1.row(2)-Tcw1.row(0);
        A.row(1) = p1.y*Tcw1.row(2)-Tcw1.row(1);
        A.row(2) = p2.x*Tcw2.row(2)-Tcw2.row(0);
        A.row(3) = p2.y*Tcw2.row(2)-Tcw2.row(1);

        Eigen::JacobiSVD<Eigen::Matrix4f> svd(A, Eigen::ComputeFullV);
        Eigen::Vector4f x3Dh = svd.matrixV().col(3);
        x3D = x3Dh.head(3)/x3Dh(3);
    }

    bool KannalaBrandt8::IsEqual(GeometricCamera* pCam)
    {
        if(pCam->GetType() != GeometricCamera::CAM_FISHEYE)
            return false;

        KannalaBrandt8* pKBCam = (KannalaBrandt8*) pCam;

        if(abs(precision - pKBCam->GetPrecision()) > 1e-6)
            return false;

        if(size() != pKBCam->size())
            return false;

        bool is_same_camera = true;
        for(size_t i=0; i<size(); ++i)
        {
            if(abs(mvParameters[i] - pKBCam->getParameter(i)) > 1e-6)
            {
                is_same_camera = false;
                break;
            }
        }
        return is_same_camera;
    }

}


#ifndef DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP
#define DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP

#include <descartes_light/bgl/bgl_dijkstra_solver.h>
#include <descartes_light/bgl/impl/event_visitors.hpp>

#include <descartes_light/descartes_macros.h>
DESCARTES_IGNORE_WARNINGS_PUSH
#include <boost/graph/dijkstra_shortest_paths.hpp>
DESCARTES_IGNORE_WARNINGS_POP

namespace descartes_light
{
template <typename FloatType, typename Visitors>
static VertexDesc<FloatType> solveDijkstra(BGLGraph<FloatType>& graph,
                                           std::vector<VertexDesc<FloatType>>& predecessors,
                                           const VertexDesc<FloatType>& source,
                                           const Visitors& event_visitors,
                                           const std::vector<std::vector<VertexDesc<FloatType>>>& ladder_rungs)
{
  // Internal properties
  auto index_prop_map = boost::get(boost::vertex_index, graph);
  auto weight_prop_map = boost::get(boost::edge_weight, graph);
  auto color_prop_map = boost::get(&Vertex<FloatType>::color, graph);
  auto distance_prop_map = boost::get(&Vertex<FloatType>::distance, graph);

  predecessors.resize(boost::num_vertices(graph), std::numeric_limits<std::size_t>::max());

  typedef typename boost::property_map<BGLGraph<FloatType>, boost::vertex_index_t>::type IndexMap;
  typedef boost::iterator_property_map<typename std::vector<VertexDesc<FloatType>>::iterator, IndexMap> PredecessorMap;
  PredecessorMap predecessor_it_map = boost::make_iterator_property_map(predecessors.begin(), index_prop_map);

  auto visitor = boost::make_dijkstra_visitor(event_visitors);

  // Perform the search
  try
  {
    boost::dijkstra_shortest_paths(graph,
                                   source,
                                   predecessor_it_map,
                                   distance_prop_map,
                                   weight_prop_map,
                                   index_prop_map,
                                   std::less<>(),
                                   std::plus<>(),
                                   std::numeric_limits<FloatType>::max(),
                                   static_cast<FloatType>(0.0),
                                   visitor,
                                   color_prop_map);

    // In the case that the visitor does not throw the target vertex descriptor, find the lowest cost vertex in last
    // rung of the ladder graph
    auto target = std::min_element(ladder_rungs.back().begin(),
                                   ladder_rungs.back().end(),
                                   [&](const VertexDesc<FloatType>& a, const VertexDesc<FloatType>& b) {
                                     return graph[a].distance < graph[b].distance;
                                   });

    // Check that the identified lowest cost vertex is valid and has a cost less than inf
    if (target != ladder_rungs.back().end() && graph[*target].distance < std::numeric_limits<FloatType>::max())
      throw *target;
  }
  catch (const VertexDesc<FloatType>& target)
  {
    return target;
  }

  // If the visitor never threw the vertex descriptor, there was an issue with the search
  throw std::runtime_error("Search failed to encounter vertex associated with the last waypoint in the trajectory");
}

template <typename FloatType, typename Visitors>
BGLDijkstraSVSESolver<FloatType, Visitors>::BGLDijkstraSVSESolver(Visitors event_visitors, unsigned num_threads)
  : BGLSolverBaseSVSE<FloatType>(num_threads), event_visitors_(std::move(event_visitors))
{
}

template <typename FloatType, typename Visitors>
SearchResult<FloatType> BGLDijkstraSVSESolver<FloatType, Visitors>::search()
{
  // Convenience aliases
  auto& graph_ = BGLSolverBase<FloatType>::graph_;
  const auto& source_ = BGLSolverBase<FloatType>::source_;
  auto& predecessors_ = BGLSolverBase<FloatType>::predecessors_;
  const auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;

  VertexDesc<FloatType> target =
      solveDijkstra<FloatType, Visitors>(graph_, predecessors_, source_, event_visitors_, ladder_rungs_);

  SearchResult<FloatType> result;

  // Reconstruct the path from the predecesor map; remove the artificial start state
  const auto vd_path = BGLSolverBase<FloatType>::reconstructPath(source_, target);
  result.trajectory = BGLSolverBase<FloatType>::toStates(vd_path);
  result.trajectory.erase(result.trajectory.begin());

  result.cost = graph_[target].distance;

  return result;
}

template <typename FloatType, typename Visitors>
BGLDijkstraSVDESolver<FloatType, Visitors>::BGLDijkstraSVDESolver(Visitors event_visitors, unsigned num_threads)
  : BGLSolverBaseSVDE<FloatType>(num_threads), event_visitors_(std::move(event_visitors))
{
}

template <typename FloatType, typename Visitors>
SearchResult<FloatType> BGLDijkstraSVDESolver<FloatType, Visitors>::search()
{
  // Convenience aliases
  auto& graph_ = BGLSolverBase<FloatType>::graph_;
  const auto& source_ = BGLSolverBase<FloatType>::source_;
  auto& predecessors_ = BGLSolverBase<FloatType>::predecessors_;
  const auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;

  // Create the dynamic edge adding event visitor
  const auto& edge_eval_ = BGLSolverBaseSVDE<FloatType>::edge_eval_;
  auto vis = std::make_pair(add_all_edges_dynamically<FloatType, boost::on_examine_vertex>(edge_eval_, ladder_rungs_),
                            event_visitors_);

  VertexDesc<FloatType> target = solveDijkstra(graph_, predecessors_, source_, vis, ladder_rungs_);

  SearchResult<FloatType> result;

  // Reconstruct the path from the predecesor map; remove the artificial start state
  const auto vd_path = BGLSolverBase<FloatType>::reconstructPath(source_, target);
  result.trajectory = BGLSolverBase<FloatType>::toStates(vd_path);
  result.trajectory.erase(result.trajectory.begin());

  result.cost = graph_[target].distance;

  return result;
}

}  // namespace descartes_light

#endif  // DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP


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

#include <boost/simd/arithmetic/functions/subs.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/is_less.hpp>
#include <boost/simd/include/functions/simd/is_less_equal.hpp>
#include <boost/simd/include/functions/simd/shri.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/bitwise_xor.hpp>
#include <boost/simd/include/functions/simd/if_else_zero.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/dispatch/meta/as_unsigned.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::subs_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return boost::simd::sub(a0, a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::subs_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<uint_<A0>,X>))((simd_<uint_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      A0 a0ma1 = a0-a1;
      return if_else_zero(le(a0ma1, a0), a0ma1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::subs_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<int_<A0>,X>))((simd_<int_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::as_unsigned<A0>::type utype;
      typedef typename meta::scalar_of<A0>::type stype;

      utype ux = bitwise_cast<utype>(a0);
      utype uy = bitwise_cast<utype>(a1);
      utype res = ux - uy;

      ux = shri(ux, sizeof(stype)*CHAR_BIT-1) + Valmax<stype>();

      return bitwise_cast<A0>(if_else(bitwise_cast<A0>((ux ^ uy) & (ux ^ res)) < Zero<A0>(), ux, res));
    }
  };
} } }

#endif


#include "tracer.hpp"
#include "initial_conditions/initial_conditions.hpp"
#include "ode_state.hpp"
#include "dynamics/ray_dynamics.hpp"
#include <future>
#include <chrono>

#include <boost/numeric/odeint/integrate/integrate_const.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>

#include <boost/range/combine.hpp>
#include <boost/range/algorithm/min_element.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/numeric/odeint/stepper/euler.hpp>
#include "observers/energy_error_observer.hpp"
#include <utility>

Tracer::Tracer( const Potential& pot, std::shared_ptr<RayDynamics> dynamics ) :
	mDimension( pot.getDimension() ),
	mSupport( pot.getSupport() ),
	mExtents( pot.getExtents() ),
	mDynamics( std::move(dynamics) ),
    mMasterObserver( pot.getDimension(), mDynamics ),
	mEnergyErrorObs(std::make_shared<EnergyErrorObserver>())
{
	using namespace boost::range;
	using namespace boost::adaptors;

	// we choose initial step according to the potential length
	// scaling = size / support, i.e. dt = support/size

	// cannot use a lambda as older boost looks for member result_type
	struct ratio
	{
		typedef double result_type;
		double operator()(const boost::tuple<double, std::size_t>& t) const { return boost::get<0>(t) / boost::get<1>(t); }
	};

	auto ratios = combine(pot.getSupport(), pot.getExtents()) | transformed(ratio());
	mInitialDeltaT = *min_element(ratios);

	mMasterObserver.addObserverObject(mEnergyErrorObs);
}

Tracer::~Tracer() = default;

void Tracer::setErrorBounds( double abs_err, double rel_err )
{
	mAbsErrorBound = abs_err;
	mRelErrorBound = rel_err;
}

void Tracer::addObserver( obs_type observer )
{
	mMasterObserver.addObserverObject(std::move(observer));
}

TraceResult Tracer::trace(InitCondGenPtr& incoming_wave, InitialConditionConfiguration config)
{
	// fix coordinate transformation for initial condition
	auto support = mSupport;
	gen_vect offset(mDimension);
	for(unsigned i = 0; i < mDimension; ++i)
	{
		offset[i] = mSupport[i] / mExtents[i];
		support[i] -= 2*offset[i];
	}
    config.setDynamics( mDynamics ).setSupport( support ).setOffset(offset);
	incoming_wave->init( config );

	// set up master observer
	mMasterObserver.setPeriodicBoundaries( mDynamics->hasPeriodicBoundary() );
	mMasterObserver.startTracing( );

	unsigned int threadcount = std::min(mMaxThreads, (std::size_t)std::thread::hardware_concurrency());
	#ifndef NDEBUG
	std::cout << "distribute computation to " << threadcount << " threads\n";
	#endif

	std::vector<std::future<void>> threads;
	auto tf = [this](InitCondGenPtr w, bool is_printer)
	{
		return this->traceThreadFunction( std::move(w), is_printer );
	};

	for(unsigned i = 0; i < threadcount; ++i)
	{
		// start threads, only thread zero prints progress
		threads.push_back( std::async ( std::launch::async, tf, incoming_wave, i == 0) );
	}

	for( auto& f : threads)
		f.get();

	mMasterObserver.finishTracing();

	return TraceResult{mEnergyErrorObs->getMaximumError(), mEnergyErrorObs->getMeanError(), getTracedParticleCount()};
}

void Tracer::traceThreadFunction( InitCondGenPtr incoming_wave, bool printer )
{
	// check that dynamcis are set
	if(!mDynamics)
		THROW_EXCEPTION( std::runtime_error, "Cannot perform tracing when no dynamics are set!" );

	// setup types for boost odeint solver
	using namespace boost::numeric::odeint;

	if(mIntegrator == Integrator::RUNGE_KUTTA_CASH_KARP_54_ADAPTIVE) {
		typedef runge_kutta_cash_karp54<GState> error_stepper_type;
		typedef controlled_runge_kutta<error_stepper_type> controlled_stepper_type;
		typedef default_error_checker<double, range_algebra, default_operations> error_checker_type;

		// setup the integrator once!
		controlled_stepper_type stepper(error_checker_type(mAbsErrorBound, mRelErrorBound));

		traceThreadFunction_imp(stepper, incoming_wave, printer);
	} else if (mIntegrator == Integrator::EULER_CONST) {
		typedef euler<GState> stepper_type;

		// setup the integrator once!
		stepper_type stepper{};

		traceThreadFunction_imp(stepper, incoming_wave, printer);
	}
}

template<class T>
void Tracer::traceThreadFunction_imp( T&& stepper, InitCondGenPtr incoming_wave, bool printer )
{
	MasterObserver thread_observer( mMasterObserver.clone() );
	InitialCondition incoming = incoming_wave->next();

	GState p(mDimension, mDynamics->hasMonodromy());
	auto last_time = std::chrono::steady_clock::now();

	for(std::size_t i = 0; incoming; ++i, ++incoming)
	{

		if(printer && (std::chrono::steady_clock::now() - last_time) > std::chrono::seconds(10))
		{
		    last_time = std::chrono::steady_clock::now();
			std::cout << "integrate " << thread_observer.getTracedParticleCount() <<  " \n";
		}

		// generate initial condition and set up state
		p.position() = incoming.getState().getPosition();
		p.velocity() = incoming.getState().getVelocity();
		if( mDynamics->hasMonodromy() )
			p.init_monodromy();

		// notify the observer
		thread_observer.startTrajectory( incoming );

		try
		{
			/// \todo this can return... do we want to do sth with the return value?
			boost::numeric::odeint::integrate_const(
					std::ref(stepper),
					[this](const GState& s, GState& d, double t) { mDynamics->stateUpdate(s, d, t); },
					p,
					0.0, 				// start time
					mEndTime, 			// end time
					mInitialDeltaT, 	// initial time step
					std::ref(thread_observer)
			);
		} catch(int& i) {};

		thread_observer.finishTrajectory( incoming );
	}
}

void Tracer::setTimeStep(double dt)
{
	mInitialDeltaT = dt;
}

std::size_t Tracer::getDimension() const
{
	return mDimension;
}

void Tracer::setMaxThreads( std::size_t threads )
{
	if( threads == 0 )
		threads = 1;
	mMaxThreads = threads;
}

std::size_t Tracer::getMaxThreads( ) const
{
	return mMaxThreads;
}

void Tracer::setIntegrator(Integrator integrator)
{
    mIntegrator = integrator;
}

const std::vector<std::shared_ptr<Observer>>& Tracer::getObservers() const
{
	return mMasterObserver.getObservers();
}

void Tracer::setEndTime( double et )
{
	mEndTime = et;
}

double Tracer::getEndTime() const
{
	return mEndTime;
}

std::size_t Tracer::getTracedParticleCount() const
{
	return mMasterObserver.getTracedParticleCount();
}


// The configure script should define NTL_FP_CONTRACT_OFF
// for icc via the NOCONTRACT variable
#ifdef NTL_FP_CONTRACT_OFF
#pragma fp_contract(off)
#endif

#include <NTL/tools.h>

#ifdef NTL_ENABLE_AVX_FFT

// The configure script tries to prevent this, but we
// double check here.  Note that while it is strongly
// discouraged, other parts of NTL probably work even with
// "fast math"; however, quad_float will definitely break.

#if (defined(__GNUC__) && __FAST_MATH__)
#error "do not compile pd_FFT.cpp with -ffast-math!!"
#endif

#include <NTL/PD.h>
#include <NTL/pd_FFT.h>
#include <NTL/FFT_impl.h>

#if (defined(__GNUC__) && __FAST_MATH__)
#error "do not compile pd_FFT.cpp with -ffast-math!!"
#endif

#if (NTL_FMA_DETECTED && !defined(NTL_CONTRACTION_FIXED))
#error "contraction not fixed"
#endif

NTL_START_IMPL

#define NTL_CSR_NEAREST (0x00000000)
#define NTL_CSR_DOWN    (0x00002000)
#define NTL_CSR_UP      (0x00004000)
#define NTL_CSR_TRUNC   (0x00006000)
#define NTL_CSR_MASK    (0x00006000)

CSRPush::CSRPush()
{
   // save current register value
   reg = _mm_getcsr();
   // set rounding mode to "down"
   _mm_setcsr((reg & ~NTL_CSR_MASK) | NTL_CSR_DOWN);
}

CSRPush::~CSRPush()
{
   _mm_setcsr(reg);
}

void
pd_LazyPrepMulModPrecon_impl(double *bninv, const double *b, double n, long len)
{
   for (long i = 0; i < len; i++) bninv[i] = b[i]/n;
}

template<class pd> pd
pd_LazyReduce1(pd a, double q)
{
   return correct_excess(a, q);
}

template<class pd> pd
pd_LazyReduce2(pd a, double q)
{
   return correct_excess(a, 2*q);
}

// inputs in [0, 2*n), output in [0, 4*n)
template<class pd> pd
pd_LazyAddMod(pd a, pd b, double n)
{
   return a+b;
}

// inputs in [0, 2*n), output in [0, 4*n)
template<class pd> pd
pd_LazySubMod(pd a, pd b, double n)
{
   return a-b+2*n;
}

// inputs in [0, 2*n), output in [0, 2*n)
template<class pd> pd
pd_LazyAddMod2(pd a, pd b, double n)
{
   pd r = a+b;
   return correct_excess(r, 2*n);
}

// inputs in [0, 2*n), output in [0, 2*n)
template<class pd> pd
pd_LazySubMod2(pd a, pd b, double n)
{
   pd r = a-b;
   return correct_deficit(r, 2*n);
}

// inputs in [0, 4*n), output in [0, 4*n)
template<class pd> pd
pd_LazyAddMod4(pd a, pd b, double n)
{
   pd r = a+b;
   return correct_excess(r, 4*n);
}

// inputs in [0, 4*n), output in [0, 4*n)
template<class pd> pd
pd_LazySubMod4(pd a, pd b, double n)
{
   pd r = a-b;
   return correct_deficit(r, 4*n);
}

// Input and output in [0, 4*n)
template<class pd> pd
pd_LazyDoubleMod4(pd a, double n)
{
   return 2 * pd_LazyReduce2(a, n);
}

// Input and output in [0, 2*n)
template<class pd> pd
pd_LazyDoubleMod2(pd a, double n)
{
   return 2 * pd_LazyReduce1(a, n);
}

// n in [0,2^50), b in [0,n), a in [0,4*n), bninv = RoundDown(b/n)
// returns a*b mod n in [0, 2*n)
template<class pd> pd
pd_LazyMulModPrecon(pd a, pd b, double n, pd bninv)
{
   pd hi = a*b;
   pd lo = fused_mulsub(a, b, hi);  // hi+lo == a*b (exactly)
   pd q =  fused_muladd(a, bninv, 1L << 52);
   q -= (1L << 52);             // q is the correct quotient, or one too small
   pd d = fused_negmuladd(q, n, hi);   // d == hi - q*n (exactly)
   pd r = d + lo;           // r is the remainder, or the remainder plus n

   return r;
}

// return (a[0] + a[1], a[0] - a[1], a[2] + a[3], a[2] - a[3], ...)
// all inputs and outputs in [0, 2*n)
template<class pd> pd
pd_fwd_butterfly_packed2(pd a, double n)
{
   pd b = swap2(a);
   pd sum = pd_LazyAddMod(a, b, n);
   pd diff = pd_LazySubMod(b, a, n);
   pd res = blend2(sum, diff);
   res = pd_LazyReduce2(res, n);
   return res;
}

// return (a[0] + a[2], a[1] + a[3], (a[0] - a[2]), (a[1] - a[3])*root, ...)
// all inputs and outputs in [0, 2*n)
// it is also assumed that w = (1,1,1,root,...) and wninv = RoundDown(w/n)
template<class pd> pd
pd_fwd_butterfly_packed4(pd a, pd w, double n, pd wninv)
{
   pd b = swap4(a);
   pd sum = pd_LazyAddMod(a, b, n);
   pd diff = pd_LazySubMod(b, a, n);
   pd res = blend4(sum, diff);
   res = pd_LazyMulModPrecon(res, w, n, wninv);
   return res;
}

static double
pd_LazyPrepMulModPrecon(long b, long n)
{
   return double(b)/double(n);
}

//===================================

#define NTL_PD_FFT_THRESH (11)

#define PDLGSZ NTL_LG2_PDSZ
#define PDSZ NTL_PDSZ

#if (PDSZ == 8)
typedef PD<8> pd_full;
typedef PD<4> pd_half;
typedef PD<2> pd_qrtr;
#else
typedef PD<4> pd_full;
typedef PD<2> pd_half;
#endif

#define PDLD pd_full::load

// this assumes xx0, xx1, w, qinv are pd_half's
#define fwd_butterfly_half(xx0, xx1, w, q, wqinv)  \
do \
{ \
   pd_half x0_ = xx0; \
   pd_half x1_ = xx1; \
   pd_half t_  = pd_LazySubMod(x0_, x1_, q); \
   xx0 = pd_LazyAddMod2(x0_, x1_, q); \
   xx1 = pd_LazyMulModPrecon(t_, w, q, wqinv); \
}  \
while (0)

// this assumes xx0, xx1, w, qinv are pd_full's
#define fwd_butterfly_full(xx0, xx1, w, q, wqinv)  \
do \
{ \
   pd_full x0_ = xx0; \
   pd_full x1_ = xx1; \
   pd_full t_  = pd_LazySubMod(x0_, x1_, q); \
   xx0 = pd_LazyAddMod2(x0_, x1_, q); \
   xx1 = pd_LazyMulModPrecon(t_, w, q, wqinv); \
}  \
while (0)

// this assumes xx0_ptr, xx1_ptr, w_ptr, wqinv_ptr are double pointers
// which are read/written as pd_full's.
// In gcc, restrict keyword will help code gen.
#define fwd_butterfly(xx0_ptr, xx1_ptr, w_ptr, q, wqinv_ptr)  \
do \
{ \
   pd_full x0_     = PDLD(xx0_ptr); \
   pd_full x1_     = PDLD(xx1_ptr); \
   pd_full w_      = PDLD(w_ptr); \
   pd_full wqinv_  = PDLD(wqinv_ptr); \
   pd_full t_      = pd_LazySubMod(x0_, x1_, q); \
   store(xx0_ptr, pd_LazyAddMod2(x0_, x1_, q)); \
   store(xx1_ptr, pd_LazyMulModPrecon(t_, w_, q, wqinv_)); \
}  \
while (0)

#if 0
#define fwd_butterfly_x4(xx0_ptr, xx1_ptr, w_ptr, q, wqinv_ptr)  \
do  \
{  \
   pd_full xx0_0_ = PDLD(xx0_ptr+0*PDSZ);  pd_full xx1_0_ = PDLD(xx1_ptr+0*PDSZ);  \
   pd_full xx0_1_ = PDLD(xx0_ptr+1*PDSZ);  pd_full xx1_1_ = PDLD(xx1_ptr+1*PDSZ);  \
   pd_full xx0_2_ = PDLD(xx0_ptr+2*PDSZ);  pd_full xx1_2_ = PDLD(xx1_ptr+2*PDSZ);  \
   pd_full xx0_3_ = PDLD(xx0_ptr+3*PDSZ);  pd_full xx1_3_ = PDLD(xx1_ptr+3*PDSZ);  \
   fwd_butterfly_full(xx0_0_, xx1_0_, PDLD(w_ptr+0*PDSZ), q, PDLD(wqinv_ptr+0*PDSZ));  \
   fwd_butterfly_full(xx0_1_, xx1_1_, PDLD(w_ptr+1*PDSZ), q, PDLD(wqinv_ptr+1*PDSZ));  \
   fwd_butterfly_full(xx0_2_, xx1_2_, PDLD(w_ptr+2*PDSZ), q, PDLD(wqinv_ptr+2*PDSZ));  \
   fwd_butterfly_full(xx0_3_, xx1_3_, PDLD(w_ptr+3*PDSZ), q, PDLD(wqinv_ptr+3*PDSZ));  \
   store(xx0_ptr+0*PDSZ, xx0_0_);  store(xx1_ptr+0*PDSZ, xx1_0_);  \
   store(xx0_ptr+1*PDSZ, xx0_1_);  store(xx1_ptr+1*PDSZ, xx1_1_);  \
   store(xx0_ptr+2*PDSZ, xx0_2_);  store(xx1_ptr+2*PDSZ, xx1_2_);  \
   store(xx0_ptr+3*PDSZ, xx0_3_);  store(xx1_ptr+3*PDSZ, xx1_3_);  \
}  \
while(0)
#else
#define fwd_butterfly_x4(xx0_ptr, xx1_ptr, w_ptr, q, wqinv_ptr)  \
do  \
{  \
   fwd_butterfly(xx0_ptr+0*PDSZ, xx1_ptr+0*PDSZ, w_ptr+0*PDSZ, q, wqinv_ptr+0*PDSZ);  \
   fwd_butterfly(xx0_ptr+1*PDSZ, xx1_ptr+1*PDSZ, w_ptr+1*PDSZ, q, wqinv_ptr+1*PDSZ);  \
   fwd_butterfly(xx0_ptr+2*PDSZ, xx1_ptr+2*PDSZ, w_ptr+2*PDSZ, q, wqinv_ptr+2*PDSZ);  \
   fwd_butterfly(xx0_ptr+3*PDSZ, xx1_ptr+3*PDSZ, w_ptr+3*PDSZ, q, wqinv_ptr+3*PDSZ);  \
}  \
while(0)
#endif

static inline NTL_ALWAYS_INLINE void
pd_fft_layer_inner_loop(double* NTL_RESTRICT xp0,
                        double* NTL_RESTRICT xp1,
                        long size,
                        const double* NTL_RESTRICT wtab,
                        const double* NTL_RESTRICT wqinvtab,
                        double q)

{
   long j = 0;
   do {
     fwd_butterfly_x4(xp0+j, xp1+j, wtab+j, q, wqinvtab+j);
     j += 4*PDSZ;
   } while (j < size);
}

// assumes size >= 8*PDSZ
static inline NTL_ALWAYS_INLINE void
pd_fft_layer(double* xp, long blocks, long size,
	     const double* wtab,
	     const double* wqinvtab,
	     double q)
{
   size /= 2;

   do {
      pd_fft_layer_inner_loop(xp, xp+size, size, wtab, wqinvtab, q);
      xp += 2 * size;
   } while (--blocks != 0);
}

// size == 8*PDSZ
static inline NTL_ALWAYS_INLINE void
pd_fft_layer_size8(double* NTL_RESTRICT xp, long blocks,
		  const double* NTL_RESTRICT wtab,
		  const double* NTL_RESTRICT wqinvtab,
		  double q)
{
   do {
      fwd_butterfly_x4(xp+0*PDSZ, xp+4*PDSZ, wtab, q, wqinvtab);
      xp += 8*PDSZ;
   } while (--blocks != 0);
}

// size == 4*PDSZ
static inline NTL_ALWAYS_INLINE void
pd_fft_layer_size4(double* NTL_RESTRICT xp, long blocks,
		   const double* NTL_RESTRICT wtab,
		   const double* NTL_RESTRICT wqinvtab,
		   double q)
{
   do {
      fwd_butterfly(xp+0*PDSZ, xp+2*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);
      fwd_butterfly(xp+1*PDSZ, xp+3*PDSZ, wtab+1*PDSZ, q, wqinvtab+1*PDSZ);

      fwd_butterfly(xp+4*PDSZ, xp+6*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);
      fwd_butterfly(xp+5*PDSZ, xp+7*PDSZ, wtab+1*PDSZ, q, wqinvtab+1*PDSZ);

      xp += 8*PDSZ;
      blocks -= 2;
   } while (blocks != 0);
}

// size == 2*PDSZ
static inline NTL_ALWAYS_INLINE void
pd_fft_layer_size2(double* NTL_RESTRICT xp, long blocks,
		   const double* NTL_RESTRICT wtab,
		   const double* NTL_RESTRICT wqinvtab,
		   double q)
{
   do {
      fwd_butterfly(xp+0*PDSZ, xp+1*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);
      fwd_butterfly(xp+2*PDSZ, xp+3*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);
      fwd_butterfly(xp+4*PDSZ, xp+5*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);
      fwd_butterfly(xp+6*PDSZ, xp+7*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);

      xp += 8*PDSZ;
      blocks -= 4;
   } while (blocks != 0);
}

#if (PDSZ == 8)
static inline NTL_ALWAYS_INLINE void
pd_fft_layer_size1_one_block(double* x,
                   pd_half w8, pd_half w8qinv,
                   pd_full w4, pd_full w4qinv,
		   double q)
{
   pd_half x0 = pd_half::load(x);
   pd_half x1 = pd_half::load(x+PDSZ/2);
   fwd_butterfly_half(x0, x1, w8, q, w8qinv);
   pd_full y = join(x0, x1);

   y = pd_fwd_butterfly_packed4(y, w4, q, w4qinv);
   y = pd_fwd_butterfly_packed2(y, q);

   store(x, y);
}

// size == PDSZ == 8
// processes last three levels, of size 8, 4, and 2.
static inline NTL_ALWAYS_INLINE void
pd_fft_layer_size1(double* xp, long blocks,
                   const double **w_pp, const double **wqinv_pp,
		   double q)
{
   const double *w8_ptr = *w_pp;
   const double *w8qinv_ptr = *wqinv_pp;

   const double *w4_ptr = *(w_pp-1);
   const double *w4qinv_ptr = *(wqinv_pp-1);

   pd_half w8 = pd_half::load(w8_ptr);

   pd_half w8qinv = pd_half::load(w8qinv_ptr);

   pd_qrtr w4_qrtr = pd_qrtr::load(w4_ptr);
   pd_half w4_half = join(w4_qrtr, w4_qrtr);
   pd_full w4      = join(w4_half, w4_half);
   w4 = blend4(dup2even(w4), w4);

   pd_qrtr w4qinv_qrtr = pd_qrtr::load(w4qinv_ptr);
   pd_half w4qinv_half = join(w4qinv_qrtr, w4qinv_qrtr);
   pd_full w4qinv      = join(w4qinv_half, w4qinv_half);
   w4qinv = blend4(dup2even(w4qinv), w4qinv);

   do {
      pd_fft_layer_size1_one_block(xp+0*PDSZ, w8, w8qinv, w4, w4qinv, q);
      pd_fft_layer_size1_one_block(xp+1*PDSZ, w8, w8qinv, w4, w4qinv, q);
      pd_fft_layer_size1_one_block(xp+2*PDSZ, w8, w8qinv, w4, w4qinv, q);
      pd_fft_layer_size1_one_block(xp+3*PDSZ, w8, w8qinv, w4, w4qinv, q);

      xp += 4*PDSZ;
      blocks -= 4;
   } while (blocks != 0);
}
#else
// PDSZ == 4

static inline NTL_ALWAYS_INLINE void
pd_fft_layer_size1_one_block(double* x,
                   pd_half w4, pd_half w4qinv,
		   double q)
{
   pd_half x0 = pd_half::load(x);
   pd_half x1 = pd_half::load(x+PDSZ/2);
   fwd_butterfly_half(x0, x1, w4, q, w4qinv);
   pd_full y = join(x0, x1);

   y = pd_fwd_butterfly_packed2(y, q);

   store(x, y);
}

// size == PDSZ == 4
// processes last two levels, of size 4 and 2.
static inline NTL_ALWAYS_INLINE void
pd_fft_layer_size1(double* xp, long blocks,
                   const double **w_pp, const double **wqinv_pp,
		   double q)
{
   const double *w4_ptr = *w_pp;
   const double *w4qinv_ptr = *wqinv_pp;

   pd_half w4 = pd_half::load(w4_ptr);
   pd_half w4qinv = pd_half::load(w4qinv_ptr);

   do {
      pd_fft_layer_size1_one_block(xp+0*PDSZ, w4, w4qinv, q);
      pd_fft_layer_size1_one_block(xp+1*PDSZ, w4, w4qinv, q);
      pd_fft_layer_size1_one_block(xp+2*PDSZ, w4, w4qinv, q);
      pd_fft_layer_size1_one_block(xp+3*PDSZ, w4, w4qinv, q);

      xp += 4*PDSZ;
      blocks -= 4;
   } while (blocks != 0);
}

#endif

void
pd_fft_base(double* xp, long lgN, const pd_mod_t& mod)
{
  double q = mod.q;
  const double** wtab = mod.wtab;
  const double** wqinvtab = mod.wqinvtab;

  long N = 1L << lgN;

  long j, size, blocks;
  for (j = lgN, size = N, blocks = 1;
       size > 8*PDSZ; j--, blocks <<= 1, size >>= 1)
    pd_fft_layer(xp, blocks, size, wtab[j], wqinvtab[j], q);

  pd_fft_layer_size8(xp, blocks, wtab[j], wqinvtab[j], q);
  j--, blocks <<= 1, size >>= 1;

  pd_fft_layer_size4(xp, blocks, wtab[j], wqinvtab[j], q);
  j--, blocks <<= 1, size >>= 1;

  pd_fft_layer_size2(xp, blocks, wtab[j], wqinvtab[j], q);
  j--, blocks <<= 1, size >>= 1;

  pd_fft_layer_size1(xp, blocks, wtab+j, wqinvtab+j, q);

}

static inline NTL_ALWAYS_INLINE void
pd_move(double *x, const long *a)
{
   pd_full r;
   loadu(r, a);
   store(x, r);
}

static inline NTL_ALWAYS_INLINE void
pd_move(long *x, const double *a)
{
   pd_full r;
   load(r, a);
   storeu(x, r);
}

static inline NTL_ALWAYS_INLINE void
pd_reduce1_move(long *x, const double *a, double q)
{
   pd_full r;
   load(r, a);
   r = pd_LazyReduce1(r, q);
   storeu(x, r);
}

static inline NTL_ALWAYS_INLINE void
pd_reduce2_move(long *x, const double *a, double q)
{
   pd_full r;
   load(r, a);
   r = pd_LazyReduce2(r, q);
   r = pd_LazyReduce1(r, q);
   storeu(x, r);
}

static inline NTL_ALWAYS_INLINE void
pd_mul_move(long *x, const double *a, pd_full b, double q, pd_full bqinv)
{
   pd_full r;
   load(r, a);
   r = pd_LazyMulModPrecon(r, b, q, bqinv);
   r = pd_LazyReduce1(r, q);
   storeu(x, r);
}

static
void pd_fft_short(double* xp, long yn, long xn, long lgN,
                   const pd_mod_t& mod)
{
  long N = 1L << lgN;

  if (yn == N)
    {
      if (xn == N && lgN <= NTL_PD_FFT_THRESH)
	{
	  // no truncation
	  pd_fft_base(xp, lgN, mod);
	  return;
	}
    }

  // divide-and-conquer algorithm

  long half = N >> 1;
  double q = mod.q;

  if (yn <= half)
    {
      if (xn <= half)
	{
	  pd_fft_short(xp, yn, xn, lgN - 1, mod);
	}
      else
	{
	  xn -= half;

	  // (X, Y) -> X + Y
	  for (long j = 0; j < xn; j+=PDSZ)
	    store(xp+j, pd_LazyAddMod2(PDLD(xp+j), PDLD(xp+j+half), q));

	  pd_fft_short(xp, yn, half, lgN - 1, mod);
	}
    }
  else
    {
      yn -= half;

      double* xp0 = xp;
      double* xp1 = xp + half;
      const double* wtab = mod.wtab[lgN];
      const double* wqinvtab = mod.wqinvtab[lgN];

      if (xn <= half)
	{
	  // X -> (X, w*X)
	  for (long j = 0; j < xn; j+=PDSZ)
	    store(xp1+j, pd_LazyMulModPrecon(PDLD(xp0+j), PDLD(wtab+j), q, PDLD(wqinvtab+j)));

	  pd_fft_short(xp0, half, xn, lgN - 1, mod);
	  pd_fft_short(xp1, yn, xn, lgN - 1, mod);
	}
      else
	{
	  xn -= half;

	  // (X, Y) -> (X + Y, w*(X - Y))
          pd_fft_layer_inner_loop(xp0, xp1, xn, wtab, wqinvtab, q);

	  // X -> (X, w*X)
	  for (long j = xn; j < half; j+=PDSZ)
	    store(xp1+j, pd_LazyMulModPrecon(PDLD(xp0+j), PDLD(wtab+j), q, PDLD(wqinvtab+j)));

	  pd_fft_short(xp0, half, half, lgN - 1, mod);
	  pd_fft_short(xp1, yn, half, lgN - 1, mod);
	}
    }
}

void
pd_fft_trunc_impl(long* A, const long* a, double* xp, long lgN, const pd_mod_t& mod,
                  long yn, long xn)

{
   for (long i = 0; i < xn; i += 4*PDSZ) {
      pd_move(xp+i+0*PDSZ, a+i+0*PDSZ);
      pd_move(xp+i+1*PDSZ, a+i+1*PDSZ);
      pd_move(xp+i+2*PDSZ, a+i+2*PDSZ);
      pd_move(xp+i+3*PDSZ, a+i+3*PDSZ);
   }

   pd_fft_short(xp, yn, xn, lgN, mod);

   double q = mod.q;
   for (long i = 0; i < yn; i += 4*PDSZ) {
      pd_reduce1_move(A+i+0*PDSZ, xp+i+0*PDSZ, q);
      pd_reduce1_move(A+i+1*PDSZ, xp+i+1*PDSZ, q);
      pd_reduce1_move(A+i+2*PDSZ, xp+i+2*PDSZ, q);
      pd_reduce1_move(A+i+3*PDSZ, xp+i+3*PDSZ, q);
   }
}

void
pd_fft_trunc_impl(long* A, const long* a, double* xp, long lgN, const pd_mod_t& mod,
                  long yn, long xn, double fac)

{
   for (long i = 0; i < xn; i += 4*PDSZ) {
      pd_move(xp+i+0*PDSZ, a+i+0*PDSZ);
      pd_move(xp+i+1*PDSZ, a+i+1*PDSZ);
      pd_move(xp+i+2*PDSZ, a+i+2*PDSZ);
      pd_move(xp+i+3*PDSZ, a+i+3*PDSZ);
   }

   pd_fft_short(xp, yn, xn, lgN, mod);

   double q = mod.q;
   double facqinv = fac/q;
   for (long i = 0; i < yn; i += 4*PDSZ) {
      pd_mul_move(A+i+0*PDSZ, xp+i+0*PDSZ, fac, q, facqinv);
      pd_mul_move(A+i+1*PDSZ, xp+i+1*PDSZ, fac, q, facqinv);
      pd_mul_move(A+i+2*PDSZ, xp+i+2*PDSZ, fac, q, facqinv);
      pd_mul_move(A+i+3*PDSZ, xp+i+3*PDSZ, fac, q, facqinv);
   }
}

//================ ifft ==============

// return (a[0] + a[1], a[0] - a[1], a[2] + a[3], a[2] - a[3], ...)
// all inputs and outputs in [0, 4*n)
template<class pd> pd
pd_inv_butterfly_packed2(pd a, double n)
{
   a = pd_LazyReduce2(a, n);
   pd b = swap2(a);
   pd sum = pd_LazyAddMod(a, b, n);
   pd diff = pd_LazySubMod(b, a, n);
   pd res = blend2(sum, diff);
   return res;
}

// return (a[0] + a[2], a[1] + a[3]*root, a[0] - a[2], a[1] - a[3]*root, ...)
// all inputs and outputs in [0, 4*n)
// it is also assumed that w = (1,1,1,root,...) and wninv = RoundDown(w/n)
template<class pd> pd
pd_inv_butterfly_packed4(pd a, pd w, double n, pd wninv)
{
   a = pd_LazyMulModPrecon(a, w, n, wninv);
   pd b = swap4(a);
   pd sum = pd_LazyAddMod(a, b, n);
   pd diff = pd_LazySubMod(b, a, n);
   pd res = blend4(sum, diff);
   return res;
}

#define inv_butterfly_half(xx0, xx1, w, q, wqinv)  \
do  \
{  \
   pd_half x0_ = pd_LazyReduce2(xx0, q);  \
   pd_half x1_ = xx1;  \
   pd_half t_ = pd_LazyMulModPrecon(x1_, w, q, wqinv);   \
   xx0 = pd_LazyAddMod(x0_, t_, q);    \
   xx1 = pd_LazySubMod(x0_, t_, q);    \
} while (0)

#define inv_butterfly(xx0_ptr, xx1_ptr, w_ptr, q, wqinv_ptr)  \
do  \
{  \
   pd_full x0_ = pd_LazyReduce2(PDLD(xx0_ptr), q);  \
   pd_full x1_ = PDLD(xx1_ptr);  \
   pd_full t_ = pd_LazyMulModPrecon(x1_, PDLD(w_ptr), q, PDLD(wqinv_ptr));   \
   store(xx0_ptr, pd_LazyAddMod(x0_, t_, q));    \
   store(xx1_ptr, pd_LazySubMod(x0_, t_, q));    \
} while (0)

#define inv_butterfly_x4(xx0_ptr, xx1_ptr, w_ptr, q, wqinv_ptr)  \
do  \
{  \
   inv_butterfly(xx0_ptr+0*PDSZ, xx1_ptr+0*PDSZ, w_ptr+0*PDSZ, q, wqinv_ptr+0*PDSZ);  \
   inv_butterfly(xx0_ptr+1*PDSZ, xx1_ptr+1*PDSZ, w_ptr+1*PDSZ, q, wqinv_ptr+1*PDSZ);  \
   inv_butterfly(xx0_ptr+2*PDSZ, xx1_ptr+2*PDSZ, w_ptr+2*PDSZ, q, wqinv_ptr+2*PDSZ);  \
   inv_butterfly(xx0_ptr+3*PDSZ, xx1_ptr+3*PDSZ, w_ptr+3*PDSZ, q, wqinv_ptr+3*PDSZ);  \
}  \
while(0)

static inline NTL_ALWAYS_INLINE void
pd_ifft_layer_inner_loop(double* NTL_RESTRICT xp0,
                         double* NTL_RESTRICT xp1,
                         long size,
                         const double* NTL_RESTRICT wtab,
                         const double* NTL_RESTRICT wqinvtab,
                         double q)

{
   long j = 0;
   do {
     inv_butterfly_x4(xp0+j, xp1+j, wtab+j, q, wqinvtab+j);
     j += 4*PDSZ;
   } while (j < size);
}

// assumes size >= 8*PDSZ
static inline NTL_ALWAYS_INLINE void
pd_ifft_layer(double* xp, long blocks, long size,
	      const double* wtab,
	      const double* wqinvtab,
	      double q)
{
   size /= 2;

   do {
      pd_ifft_layer_inner_loop(xp, xp+size, size, wtab, wqinvtab, q);
      xp += 2 * size;
   } while (--blocks != 0);
}

// size == 8*PDSZ
static inline NTL_ALWAYS_INLINE void
pd_ifft_layer_size8(double* NTL_RESTRICT xp, long blocks,
		    const double* NTL_RESTRICT wtab,
		    const double* NTL_RESTRICT wqinvtab,
		    double q)
{
   do {
      inv_butterfly_x4(xp+0*PDSZ, xp+4*PDSZ, wtab, q, wqinvtab);
      xp += 8*PDSZ;
   } while (--blocks != 0);
}

// size == 4*PDSZ
static inline NTL_ALWAYS_INLINE void
pd_ifft_layer_size4(double* NTL_RESTRICT xp, long blocks,
		    const double* NTL_RESTRICT wtab,
		    const double* NTL_RESTRICT wqinvtab,
		    double q)
{
   do {
      inv_butterfly(xp+0*PDSZ, xp+2*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);
      inv_butterfly(xp+1*PDSZ, xp+3*PDSZ, wtab+1*PDSZ, q, wqinvtab+1*PDSZ);

      inv_butterfly(xp+4*PDSZ, xp+6*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);
      inv_butterfly(xp+5*PDSZ, xp+7*PDSZ, wtab+1*PDSZ, q, wqinvtab+1*PDSZ);

      xp += 8*PDSZ;
      blocks -= 2;
   } while (blocks != 0);
}

// size == 2*PDSZ
static inline NTL_ALWAYS_INLINE void
pd_ifft_layer_size2(double* NTL_RESTRICT xp, long blocks,
		    const double* NTL_RESTRICT wtab,
		    const double* NTL_RESTRICT wqinvtab,
		    double q)
{
   do {
      inv_butterfly(xp+0*PDSZ, xp+1*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);
      inv_butterfly(xp+2*PDSZ, xp+3*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);
      inv_butterfly(xp+4*PDSZ, xp+5*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);
      inv_butterfly(xp+6*PDSZ, xp+7*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);

      xp += 8*PDSZ;
      blocks -= 4;
   } while (blocks != 0);
}

#if (PDSZ == 8)
static inline NTL_ALWAYS_INLINE void
pd_ifft_layer_size1_one_block(double* x,
                   pd_half w8, pd_half w8qinv,
                   pd_full w4, pd_full w4qinv,
		   double q)
{
   pd_full y = PDLD(x);
   y = pd_inv_butterfly_packed2(y, q);
   y = pd_inv_butterfly_packed4(y, w4, q, w4qinv);

   pd_half x0 = get_lo(y);
   pd_half x1 = get_hi(y);
   inv_butterfly_half(x0, x1, w8, q, w8qinv);

   store(x, x0);
   store(x+PDSZ/2, x1);
}

// size == PDSZ == 8
// processes last three levels, of size 8, 4, and 2.
static inline NTL_ALWAYS_INLINE void
pd_ifft_layer_size1(double* xp, long blocks,
                   const double **w_pp, const double **wqinv_pp,
		   double q)
{
   const double *w8_ptr = *w_pp;
   const double *w8qinv_ptr = *wqinv_pp;

   const double *w4_ptr = *(w_pp-1);
   const double *w4qinv_ptr = *(wqinv_pp-1);

   pd_half w8 = pd_half::load(w8_ptr);

   pd_half w8qinv = pd_half::load(w8qinv_ptr);

   pd_qrtr w4_qrtr = pd_qrtr::load(w4_ptr);
   pd_half w4_half = join(w4_qrtr, w4_qrtr);
   pd_full w4      = join(w4_half, w4_half);
   w4 = blend4(dup2even(w4), w4);

   pd_qrtr w4qinv_qrtr = pd_qrtr::load(w4qinv_ptr);
   pd_half w4qinv_half = join(w4qinv_qrtr, w4qinv_qrtr);
   pd_full w4qinv      = join(w4qinv_half, w4qinv_half);
   w4qinv = blend4(dup2even(w4qinv), w4qinv);

   do {
      pd_ifft_layer_size1_one_block(xp+0*PDSZ, w8, w8qinv, w4, w4qinv, q);
      pd_ifft_layer_size1_one_block(xp+1*PDSZ, w8, w8qinv, w4, w4qinv, q);
      pd_ifft_layer_size1_one_block(xp+2*PDSZ, w8, w8qinv, w4, w4qinv, q);
      pd_ifft_layer_size1_one_block(xp+3*PDSZ, w8, w8qinv, w4, w4qinv, q);

      xp += 4*PDSZ;
      blocks -= 4;
   } while (blocks != 0);
}
#else
// PDSZ == 4

static inline NTL_ALWAYS_INLINE void
pd_ifft_layer_size1_one_block(double* x,
                   pd_half w4, pd_half w4qinv,
		   double q)
{
   pd_full y = PDLD(x);
   y = pd_inv_butterfly_packed2(y, q);

   pd_half x0 = get_lo(y);
   pd_half x1 = get_hi(y);
   inv_butterfly_half(x0, x1, w4, q, w4qinv);

   store(x, x0);
   store(x+PDSZ/2, x1);
}

// size == PDSZ == 4
// processes last two levels, of size 4 and 2.
static inline NTL_ALWAYS_INLINE void
pd_ifft_layer_size1(double* xp, long blocks,
                   const double **w_pp, const double **wqinv_pp,
		   double q)
{
   const double *w4_ptr = *w_pp;
   const double *w4qinv_ptr = *wqinv_pp;

   pd_half w4 = pd_half::load(w4_ptr);
   pd_half w4qinv = pd_half::load(w4qinv_ptr);

   do {
      pd_ifft_layer_size1_one_block(xp+0*PDSZ, w4, w4qinv, q);
      pd_ifft_layer_size1_one_block(xp+1*PDSZ, w4, w4qinv, q);
      pd_ifft_layer_size1_one_block(xp+2*PDSZ, w4, w4qinv, q);
      pd_ifft_layer_size1_one_block(xp+3*PDSZ, w4, w4qinv, q);

      xp += 4*PDSZ;
      blocks -= 4;
   } while (blocks != 0);
}

#endif

void
pd_ifft_base(double* xp, long lgN, const pd_mod_t& mod)
{
  double q = mod.q;
  const double** wtab = mod.wtab;
  const double** wqinvtab = mod.wqinvtab;

  long N = 1L << lgN;

  long j=PDLGSZ, size=PDSZ, blocks=N/PDSZ;

  pd_ifft_layer_size1(xp, blocks, wtab+j, wqinvtab+j, q);
  j++, blocks >>= 1, size <<= 1;

  pd_ifft_layer_size2(xp, blocks, wtab[j], wqinvtab[j], q);
  j++, blocks >>= 1, size <<= 1;

  pd_ifft_layer_size4(xp, blocks, wtab[j], wqinvtab[j], q);
  j++, blocks >>= 1, size <<= 1;

  pd_ifft_layer_size8(xp, blocks, wtab[j], wqinvtab[j], q);
  j++, blocks >>= 1, size <<= 1;

  for (; size <= N; j++, blocks >>= 1, size <<= 1)
    pd_ifft_layer(xp, blocks, size, wtab[j], wqinvtab[j], q);

}

static void
pd_ifft_short2(double* xp, long yn, long lgN, const pd_mod_t& mod);

static void
pd_ifft_short1(double* xp, long yn, long lgN, const pd_mod_t& mod)

// Implements truncated inverse FFT interface, but with xn==yn.
// All computations are done in place.

{
  long N = 1L << lgN;

  if (yn == N && lgN <= NTL_PD_FFT_THRESH)
    {
      // no truncation
      pd_ifft_base(xp, lgN, mod);
      return;
    }

  // divide-and-conquer algorithm

  long half = N >> 1;
  double q = mod.q;

  if (yn <= half)
    {
      // X -> 2X
      for (long j = 0; j < yn; j+=PDSZ)
      	store(xp+j, pd_LazyDoubleMod4(PDLD(xp+j), q));

      pd_ifft_short1(xp, yn, lgN - 1, mod);
    }
  else
    {
      double* xp0 = xp;
      double* xp1 = xp + half;

      pd_ifft_short1(xp0, half, lgN - 1, mod);

      yn -= half;

      if (yn < half) {
        const double* wtab1 = mod.wtab1[lgN];
        const double* wqinvtab1 = mod.wqinvtab1[lgN];

	// X -> (2X, w*X)
	for (long j = yn; j < half; j+=PDSZ)
	  {
	    pd_full x0 = PDLD(xp0+j);
	    store(xp0+j, pd_LazyDoubleMod4(x0, q));
	    store(xp1+j, pd_LazyMulModPrecon(x0, PDLD(wtab1+j), q, PDLD(wqinvtab1+j)));
	  }
      }

      pd_ifft_short2(xp1, yn, lgN - 1, mod);

      // (X, Y) -> (X + Y/w, X - Y/w)
      pd_ifft_layer_inner_loop(xp0, xp1, yn, mod.wtab[lgN], mod.wqinvtab[lgN], q);
    }
}

static void
pd_ifft_short2(double* xp, long yn, long lgN, const pd_mod_t& mod)

// Implements truncated inverse FFT interface, but with xn==N.
// All computations are done in place.

{
  long N = 1L << lgN;

  if (yn == N && lgN <= NTL_PD_FFT_THRESH)
    {
      // no truncation
      pd_ifft_base(xp, lgN, mod);
      return;
    }

  // divide-and-conquer algorithm

  long half = N >> 1;
  double q = mod.q;

  if (yn <= half)
    {
      // X -> 2X
      for (long j = 0; j < yn; j+=PDSZ)
     	store(xp+j, pd_LazyDoubleMod4(PDLD(xp+j), q));

      // (X, Y) -> X + Y
      for (long j = yn; j < half; j+=PDSZ)
	store(xp+j, pd_LazyAddMod4(PDLD(xp+j), PDLD(xp+j+half), q));

      pd_ifft_short2(xp, yn, lgN - 1, mod);

      // (X, Y) -> X - Y
      for (long j = 0; j < yn; j+=PDSZ)
	store(xp+j, pd_LazySubMod4(PDLD(xp+j), PDLD(xp+j+half), q));
    }
  else
    {
      double* xp0 = xp;
      double* xp1 = xp + half;

      pd_ifft_short1(xp0, half, lgN - 1, mod);

      yn -= half;

      if (yn < half) {
        const double* wtab1 = mod.wtab1[lgN];
        const double* wqinvtab1 = mod.wqinvtab1[lgN];

	// (X, Y) -> (2X - Y, w*(X - Y))
	for (long j = yn; j < half; j+=PDSZ)
	  {
	    pd_full x0 = PDLD(xp0+j);
	    pd_full x1 = PDLD(xp1+j);
	    pd_full u  = pd_LazySubMod4(x0, x1, q);
	    store(xp0+j, pd_LazyAddMod4(x0, u, q));
	    store(xp1+j, pd_LazyMulModPrecon(u, PDLD(wtab1+j), q, PDLD(wqinvtab1+j)));
	  }
      }

      pd_ifft_short2(xp1, yn, lgN - 1, mod);

      // (X, Y) -> (X + Y/w, X - Y/w)
      pd_ifft_layer_inner_loop(xp0, xp1, yn, mod.wtab[lgN], mod.wqinvtab[lgN], q);
    }
}

void
pd_ifft_trunc_impl(long* A, const long* a, double* xp, long lgN, const pd_mod_t& mod,
                   long yn, double fac)
{
   long N = 1L << lgN;

   for (long i = 0; i < yn; i += 4*PDSZ) {
      pd_move(xp+i+0*PDSZ, a+i+0*PDSZ);
      pd_move(xp+i+1*PDSZ, a+i+1*PDSZ);
      pd_move(xp+i+2*PDSZ, a+i+2*PDSZ);
      pd_move(xp+i+3*PDSZ, a+i+3*PDSZ);
   }

   pd_ifft_short1(xp, yn, lgN, mod);

   double q = mod.q;
   double facqinv = fac/q;
   for (long i = 0; i < yn; i += 4*PDSZ) {
      pd_mul_move(A+i+0*PDSZ, xp+i+0*PDSZ, fac, q, facqinv);
      pd_mul_move(A+i+1*PDSZ, xp+i+1*PDSZ, fac, q, facqinv);
      pd_mul_move(A+i+2*PDSZ, xp+i+2*PDSZ, fac, q, facqinv);
      pd_mul_move(A+i+3*PDSZ, xp+i+3*PDSZ, fac, q, facqinv);
   }
}

void
pd_ifft_trunc_impl(long* A, const long* a, double* xp, long lgN, const pd_mod_t& mod,
                   long yn)
{
   long N = 1L << lgN;

   for (long i = 0; i < yn; i += 4*PDSZ) {
      pd_move(xp+i+0*PDSZ, a+i+0*PDSZ);
      pd_move(xp+i+1*PDSZ, a+i+1*PDSZ);
      pd_move(xp+i+2*PDSZ, a+i+2*PDSZ);
      pd_move(xp+i+3*PDSZ, a+i+3*PDSZ);
   }

   pd_ifft_short1(xp, yn, lgN, mod);

   double q = mod.q;
   for (long i = 0; i < yn; i += 4*PDSZ) {
      pd_reduce2_move(A+i+0*PDSZ, xp+i+0*PDSZ, q);
      pd_reduce2_move(A+i+1*PDSZ, xp+i+1*PDSZ, q);
      pd_reduce2_move(A+i+2*PDSZ, xp+i+2*PDSZ, q);
      pd_reduce2_move(A+i+3*PDSZ, xp+i+3*PDSZ, q);
   }
}

NTL_END_IMPL

#else

void _ntl_pd_FFT_dummy() { }

#endif


/**
 * \file boost/numeric/ublasx/operation/empty.hpp
 *
 * \brief Check for emptiness a ginve vector/matrix expression.
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * 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)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLAS_OPERATION_EMPTY_HPP
#define BOOST_NUMERIC_UBLAS_OPERATION_EMPTY_HPP

#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

/**
 * \brief Check for emptiness the given vector expression.
 *
 * \tparam VectorExprT The type of the input vector expression.
 * \param ve The input vector expression.
 * \return \c true if \a ve is a zero-sized vector; \c false otherwise.
 *
 * A zero-sized vector is a vector of zero length.
 */
template <typename VectorExprT>
bool empty(vector_expression<VectorExprT> const& ve)
{
	return size(ve) == 0;
}

/**
 * \brief Check for emptiness the given matrix expression.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 * \param me The input matrix expression.
 * \return \c true if \a me is a zero-sized matrix; \c false otherwise.
 *
 * A zero-sized matrix is a vector with either zero rows or zero columns or
 * both.
 */
template <typename MatrixExprT>
bool empty(matrix_expression<MatrixExprT> const& me)
{
	return num_rows(me) == 0 || num_columns(me) == 0;
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLAS_OPERATION_EMPTY_HPP


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

#ifndef MPO19_PCURE_02082020
#define MPO19_PCURE_02082020

#include <mpo19/typedefs.hpp>
#include <mpo19/random.hpp>
#include <mpo19/clustering.hpp>
#include <mpo19/linkage.hpp>
#include <mpo19/paillier_wrapper.hpp>
#include <boost/phoenix.hpp>

#include <iostream>
#include <type_traits>

namespace mpo19
{

constexpr struct calc_distance_t {
    size_t operator()(point<int64_t> const& lhs
                      , point<int64_t> const& rhs) const
    {
        using namespace boost::phoenix::placeholders;
        assert(lhs.dimension() == rhs.dimension());
        return apply_on_elements(arg1 * arg1, lhs - rhs).fold();
    }

    double operator()(point<double> const& lhs
                      , point<int64_t> const& rhs) const
    {
        using namespace boost::phoenix::placeholders;
        assert(lhs.dimension() == rhs.dimension());
        point<double> res(lhs.dimension());
        apply_on_elements(res, arg1 - arg2, lhs, rhs);
        apply_on_elements(res, arg1 * arg1, res);
        return res.fold();
    }

} calc_distance;

template<typename T, typename LinkagePolicy>
struct cluster_dist_t : public LinkagePolicy {

    template<typename... Args>
    cluster_dist_t(std::vector<point<T>> const& values, Args&&... args)
        : LinkagePolicy{std::forward<Args>(args)...}, values_{values} {}

    auto operator()(cluster const& c1, cluster const& c2)
    {
        auto c1_indices = get_indices(c1);
        auto c2_indices = get_indices(c2);

        size_t c1_indices_size = c1_indices.size();
        size_t c2_indices_size = c2_indices.size();

        assert(0 < c1_indices_size);
        assert(0 < c2_indices_size);

        auto res = calc_distance(values_[c1_indices[0]], values_[c2_indices[0]]);

        size_t i = 0, j = 1;
        for(; i != c1_indices_size; ++i, j = 0) {
            for(; j != c2_indices_size; ++j) {
                assert( !(i == 0 && j == 0) &&
                        "We already calculated the distance for (0, 0)");
                res =
                    LinkagePolicy::cluster_distance(
                        res, calc_distance(values_[c1_indices[i]], values_[c2_indices[j]]));
            }
        }

        return res;
    }

private:
    std::vector<point<T>> const& values_;
};

template<typename T>
struct pcure_clusters {
    //We use a vector<vector>, as the rows might have different sizes
    std::vector<cluster> partition_dendrograms;
    std::vector<point<T>> values;
};

template<typename T>
std::ostream& operator<<(std::ostream& os, pcure_clusters<T> const& clusters)
{
    os << '(';
    for(cluster const& c : clusters.partition_dendrograms) {
        os << c;
    }
    os << ')';
    os << '(';
    for(point<T> const& p : clusters.values) {
        os << p;
    }
    os << ')';
    return os;
}

template<typename T>
std::istream& operator>>(std::istream& is, pcure_clusters<T>& clusters)
{
    clusters.partition_dendrograms.clear();
    clusters.values.clear();

    char c = is.get();
    assert(c == '(');

    while(is.peek() != ')') {
        cluster clust;
        is >> clust;
        clusters.partition_dendrograms.emplace_back(std::move(clust));
    }

    c = is.get();
    assert(c == ')');
    c = is.get();
    assert(c == '(');

    while(is.peek() != ')') {
        point<T> p;
        is >> p;
        clusters.values.emplace_back(std::move(p));
    }

    return is;
}

template<template<class> class LinkagePolicy>
struct clear_clustering_t {

    template<typename RandomAccessRange>
    auto operator()(RandomAccessRange const& partition, size_t t)
    {
        using dist_table_t = table_t<decltype(calc_distance(partition[0], partition[0]))>;
        size_t n = partition.size();
        dist_table_t dist_matrix{boost::extents[n][n]};

        for(index_t  i = 0; i != index_t(n); ++i) {
            for(index_t j = 0; j != index_t(n); ++j) {
                dist_matrix[i][j] = calc_distance(partition[i], partition[j]);
            }
        }

        return hierarchical_clustering<LinkagePolicy<dist_table_t>> {dist_matrix}(t);
    }

    template<typename T>
    auto operator()(pcure_clusters<T> const& clusters, size_t t)
    {
        size_t n = clusters.partition_dendrograms.size();
        table_t<T> dist_matrix{boost::extents[n][n]};

        hierarchical_clustering<cluster_dist_t<T, LinkagePolicy<table_t<T>>>>
        hc{clusters.values, dist_matrix};

        cluster_dist_t<T, LinkagePolicy<table_t<T>>>& cluster_dist = hc;

        for(index_t i = 0; i != index_t(n); ++i) {
            for(index_t j = 0; j != index_t(n); ++j) {
                if(i == j) {
                    dist_matrix[i][j] = 0;
                } else {
                    dist_matrix[i][j] = cluster_dist(
                                            clusters.partition_dendrograms[i]
                                            , clusters.partition_dendrograms[j]);
                }
            }
        }

        return hc(t);
    }
};

template<typename Range>
std::vector<cluster> assign_clusters(
    Range const& input
    , std::vector<point<double>> const& centroids)
{
    assert(!centroids.empty());
    std::vector<cluster> res(centroids.size());
    size_t rank = 0;

    for(size_t i = 0; i != input.size(); ++i) {
        size_t min_index = 0;
        double min_distance = calc_distance(centroids[0], input[i]);
        for(size_t j = 1; j != centroids.size(); ++j) {
            double distance = calc_distance(centroids[j], input[i]);
            if(distance < min_distance) {
                min_index = j;
                min_distance = distance;
            }
        }
        if(res[min_index].is_cluster()) ++rank;
        cluster singleton{cluster::leaf_t(i)};
        merge(res[min_index], singleton, rank);
        assert(!singleton.is_cluster());
    }

    res.erase(std::remove_if(res.begin(), res.end(), [](cluster const& c){
        return !c.is_cluster();
    }), res.end());

    return res;
}

template<typename T>
void transform_pcure_dendrogram(
    pcure_clusters<T>& clusters
    , std::vector<cluster>& dendrogram
    , size_t base_rank)
{
    for(cluster& c : dendrogram) {
        assert(c.is_cluster());
        std::vector<index_t> indices = get_indices(c);
        assert(clusters.partition_dendrograms[indices[0]].is_cluster());
        c = std::move(clusters.partition_dendrograms[indices[0]]);

        for(size_t i = 1, rank = base_rank; i != indices.size(); ++i, ++rank) {
            index_t idx = indices[i];
            assert(idx < clusters.partition_dendrograms.size());
            assert(clusters.partition_dendrograms[idx].is_cluster());
            merge(c, clusters.partition_dendrograms[idx], rank);
        }

        assert(c.is_cluster());
    }
}

template<typename ClusteringPolicy>
struct pcure : public ClusteringPolicy {

    template<typename... Args>
    pcure(size_t s, size_t p, size_t q, size_t t1, size_t t2, size_t t, Args&&... args)
        : ClusteringPolicy{std::forward<Args>(args)...}, s{s}, p{p}, q{q}, t1{t1}, t2{t2}, t{t} {}

    template<typename T>
    pcure_clusters<T> A_clustering(std::vector<point<T>>& input)
    {
        using boost::adaptors::sliced;
        using boost::adaptors::filtered;
        using boost::range::copy;

        struct add_partition_idx_t : boost::static_visitor<void> {

            add_partition_idx_t(size_t partition_idx)
                : partition_idx_{partition_idx} {}

            result_type operator()(cluster::union_t& u) const
            {
                u.get_left().visit(*this);
                u.get_right().visit(*this);
            }

            result_type operator()(cluster::leaf_t& l) const
            {
                l += partition_idx_;
            }

        private:
            size_t partition_idx_;
        };

        assert(input.size() > s &&
               "Cannot take a sample of size s with s being bigger than the input size");
        assert(s > p &&
               "Cannot partition s into more than s partitions");
        assert(s/(p*q) > 0 &&
               "The target clusters shall be grater than zero, i.e. s/(p*q) > 0");

        size_t const target_clusters = s/(p*q);

        pcure_clusters<T> res;

        res.partition_dendrograms.reserve(s/q);

        //if s is not a multiple of p, then the last s % p items won't be included in any partition
        auto sample = random_sample(input, s);

        for(size_t i = 0; i != p; ++i) {
            size_t const slice_first = i * s/p;
            size_t const slice_last = (i+1) * s/p;
            assert(slice_first < slice_last);

            auto partition = sample | sliced(slice_first, slice_last);

            auto dendrogram = static_cast<ClusteringPolicy&>(*this)(partition, target_clusters);
            auto filtered_dendrogram = dendrogram | filtered([&](cluster const& c) -> bool {
                return get_indices(c).size() >= t1;
            });

            for(cluster& c : filtered_dendrogram) {
                if(slice_first != 0)
                    c.visit(add_partition_idx_t{slice_first});
                res.partition_dendrograms.emplace_back(std::move(c));
            }
        }
        res.values = std::move(sample);

        return res;
    }

    template<typename T>
    pcure_clusters<T> B_clustering(pcure_clusters<T>& clusters)
    {
        using boost::range::remove_if;

        assert(clusters.partition_dendrograms.size() <= s/q);

        //The result in this context is the merge history of input clusters,
        //therefore we need to convert it to a merge history of indices
        auto dendrogram = static_cast<ClusteringPolicy&>(*this)(clusters, t);
        size_t base_rank = s/p - s/(p*q) + 1;

        for(cluster& c : dendrogram) {
            assert(c.is_cluster());
            std::vector<index_t> indices = get_indices(c);
            assert(clusters.partition_dendrograms[indices[0]].is_cluster());
            c = std::move(clusters.partition_dendrograms[indices[0]]);

            for(size_t i = 1, rank = base_rank; i != indices.size(); ++i, ++rank) {
                assert(clusters.partition_dendrograms[indices[i]].is_cluster());
                merge(c, clusters.partition_dendrograms[indices[i]], rank);
            }

            assert(c.is_cluster());
        }

        dendrogram.erase(remove_if(dendrogram, [&](cluster const& c) {
            return get_indices(c).size() < t2;
        })
        , dendrogram.end());

        clusters.partition_dendrograms = std::move(dendrogram);

        return std::move(clusters);
    }

    template<typename T>
    std::vector<point<double>> get_cluster_centroids(std::vector<point<T>> const& input)
    {
        using namespace boost::phoenix::placeholders;

        std::vector<point<T>> in{input};
        auto C_A = A_clustering(in);
        auto C_B = B_clustering(C_A);

        std::vector<point<double>> centroids;
        centroids.reserve(C_B.partition_dendrograms.size());
        for(cluster const& c : C_B.partition_dendrograms) {
            std::vector<index_t> indices = get_indices(c);
            point<double> sum(C_B.values.front().dimension(), 0.0);
            for(index_t i : indices) {
                apply_on_elements(sum, arg1 + arg2, sum, C_B.values[i]);
            }
            apply_on_elements(sum, arg1/indices.size(), sum);
            centroids.emplace_back(std::move(sum));
        }

        return centroids;
    }

    template<typename T>
    std::vector<cluster> assign_clusters(
        std::vector<point<T>> const& input
        , std::vector<point<double>> const& centroids)
    {
        return ::mpo19::assign_clusters(input, centroids);
    }

private:
    size_t s, p, q, t1, t2, t;
};

}

#endif //MPO19_PCURE_02082020


// Copyright (C) 2014 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/camera/camera.h"

#include <ceres/rotation.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>

#include "theia/sfm/camera/projection_matrix_utils.h"
#include "theia/sfm/camera/project_point_to_image.h"
#include "theia/sfm/camera/radial_distortion.h"

namespace theia {

using Eigen::AngleAxisd;
using Eigen::Map;
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Vector2d;
using Eigen::Vector3d;
using Eigen::Vector4d;

Camera::Camera() {
  // Set rotation and position to zero (i.e. identity).
  Map<Matrix<double, 1, 6> >(mutable_extrinsics()).setZero();

  SetFocalLength(1.0);
  SetAspectRatio(1.0);
  SetSkew(0.0);
  SetPrincipalPoint(0.0, 0.0);
  SetRadialDistortion(0.0, 0.0);

  image_size_[0] = 0;
  image_size_[1] = 0;
}

bool Camera::InitializeFromProjectionMatrix(
      const int image_width,
      const int image_height,
      const Matrix3x4d projection_matrix) {
  DCHECK_GT(image_width, 0);
  DCHECK_GT(image_height, 0);
  image_size_[0] = image_width;
  image_size_[1] = image_height;

  Vector3d orientation, position;
  Matrix3d calibration_matrix;
  DecomposeProjectionMatrix(projection_matrix,
                            &calibration_matrix,
                            &orientation,
                            &position);

  Map<Vector3d>(mutable_extrinsics() + ORIENTATION) = orientation;
  Map<Vector3d>(mutable_extrinsics() + POSITION) = position;

  if (calibration_matrix(0, 0) == 0 || calibration_matrix(1, 1) == 0) {
    LOG(INFO) << "Cannot set focal lengths to zero!";
    return false;
  }

  CalibrationMatrixToIntrinsics(calibration_matrix,
                                mutable_intrinsics() + FOCAL_LENGTH,
                                mutable_intrinsics() + SKEW,
                                mutable_intrinsics() + ASPECT_RATIO,
                                mutable_intrinsics() + PRINCIPAL_POINT_X,
                                mutable_intrinsics() + PRINCIPAL_POINT_Y);
  return true;
}

void Camera::GetProjectionMatrix(Matrix3x4d* pmatrix) const {
  Matrix3d calibration_matrix;
  GetCalibrationMatrix(&calibration_matrix);
  ComposeProjectionMatrix(calibration_matrix,
                          GetOrientationAsAngleAxis(),
                          GetPosition(),
                          pmatrix);
}

void Camera::GetCalibrationMatrix(Matrix3d* kmatrix) const {
  IntrinsicsToCalibrationMatrix(FocalLength(),
                                Skew(),
                                AspectRatio(),
                                PrincipalPointX(),
                                PrincipalPointY(),
                                kmatrix);
}

double Camera::ProjectPoint(const Vector4d& point, Vector2d* pixel) const {
  return ProjectPointToImage(extrinsics(),
                             intrinsics(),
                             point.data(),
                             pixel->data());
}

Vector3d Camera::PixelToUnitDepthRay(const Vector2d& pixel) const {
  // Remove the effect of calibration.
  const Vector3d undistorted_point = PixelToNormalizedCoordinates(pixel);

  // Apply rotation.
  const Matrix3d& rotation = GetOrientationAsRotationMatrix();
  const Vector3d direction = rotation.transpose() * undistorted_point;
  return direction;
}

Vector3d Camera::PixelToNormalizedCoordinates(const Vector2d& pixel) const {
  // First, undo the calibration.
  const double focal_length_y = FocalLength() * AspectRatio();
  const double y_normalized = (pixel[1] - PrincipalPointY()) / focal_length_y;
  const double x_normalized =
      (pixel[0] - PrincipalPointX() - y_normalized * Skew()) / FocalLength();

  // Undo radial distortion.
  const Vector2d normalized_point(x_normalized, y_normalized);
  Vector2d undistorted_pixel;
  RadialUndistortPoint(normalized_point,
                       RadialDistortion1(),
                       RadialDistortion2(),
                       &undistorted_pixel);
  const Vector3d undistorted_point = undistorted_pixel.homogeneous();
  return undistorted_point;
}

  // ----------------------- Getter and Setter methods ---------------------- //
void Camera::SetPosition(const Vector3d& position) {
  Map<Vector3d>(mutable_extrinsics() + POSITION) = position;
}

Vector3d Camera::GetPosition() const {
  return Map<const Vector3d>(extrinsics() + POSITION);
}

void Camera::SetOrientationFromRotationMatrix(const Matrix3d& rotation) {
  ceres::RotationMatrixToAngleAxis(
      ceres::ColumnMajorAdapter3x3(rotation.data()),
      mutable_extrinsics() + ORIENTATION);
}

void Camera::SetOrientationFromAngleAxis(const Vector3d& angle_axis) {
  Map<Vector3d>(mutable_extrinsics() + ORIENTATION) = angle_axis;
}

Matrix3d Camera::GetOrientationAsRotationMatrix() const {
  Matrix3d rotation;
  ceres::AngleAxisToRotationMatrix(
      extrinsics() + ORIENTATION,
      ceres::ColumnMajorAdapter3x3(rotation.data()));
  return rotation;
}

Vector3d Camera::GetOrientationAsAngleAxis() const {
  return Map<const Vector3d>(extrinsics() + ORIENTATION);
}

void Camera::SetFocalLength(const double focal_length) {
  mutable_intrinsics()[FOCAL_LENGTH] = focal_length;
}

double Camera::FocalLength() const {
  return intrinsics()[FOCAL_LENGTH];
}

void Camera::SetAspectRatio(const double aspect_ratio) {
  mutable_intrinsics()[ASPECT_RATIO] = aspect_ratio;
}
double Camera::AspectRatio() const {
  return intrinsics()[ASPECT_RATIO];
}

void Camera::SetSkew(const double skew) {
  mutable_intrinsics()[SKEW] = skew;
}

double Camera::Skew() const {
  return intrinsics()[SKEW];
}

void Camera::SetPrincipalPoint(const double principal_point_x,
                               const double principal_point_y) {
  mutable_intrinsics()[PRINCIPAL_POINT_X] = principal_point_x;
  mutable_intrinsics()[PRINCIPAL_POINT_Y] = principal_point_y;
}

double Camera::PrincipalPointX() const {
  return intrinsics()[PRINCIPAL_POINT_X];
}

double Camera::PrincipalPointY() const {
  return intrinsics()[PRINCIPAL_POINT_Y];
}

void Camera::SetRadialDistortion(const double radial_distortion_1,
                                 const double radial_distortion_2) {
  mutable_intrinsics()[RADIAL_DISTORTION_1] = radial_distortion_1;
  mutable_intrinsics()[RADIAL_DISTORTION_2] = radial_distortion_2;
}

double Camera::RadialDistortion1() const {
  return intrinsics()[RADIAL_DISTORTION_1];
}

double Camera::RadialDistortion2() const {
  return intrinsics()[RADIAL_DISTORTION_2];
}

void Camera::SetImageSize(const int image_width, const int image_height) {
  image_size_[0] = image_width;
  image_size_[1] = image_height;
}

}  // namespace theia