#ifndef HIDDEN_MARKOV_MODEL
#define HIDDEN_MARKOV_MODEL

#include "util/integer_range.hpp"

#include <boost/assert.hpp>
#include <boost/math/constants/constants.hpp>

#include <cmath>

#include <limits>
#include <vector>

namespace osrm
{
namespace engine
{
namespace map_matching
{

static const double log_2_pi = std::log(2. * boost::math::constants::pi<double>());
static const double IMPOSSIBLE_LOG_PROB = -std::numeric_limits<double>::infinity();
static const double MINIMAL_LOG_PROB = std::numeric_limits<double>::lowest();
static const std::size_t INVALID_STATE = std::numeric_limits<std::size_t>::max();

// closures to precompute log -> only simple floating point operations
struct EmissionLogProbability
{
    double sigma_z;
    double log_sigma_z;

    EmissionLogProbability(const double sigma_z) : sigma_z(sigma_z), log_sigma_z(std::log(sigma_z))
    {
    }

    double operator()(const double distance) const
    {
        return -0.5 * (log_2_pi + (distance / sigma_z) * (distance / sigma_z)) - log_sigma_z;
    }
};

struct TransitionLogProbability
{
    double beta;
    double log_beta;
    TransitionLogProbability(const double beta) : beta(beta), log_beta(std::log(beta)) {}

    double operator()(const double d_t) const { return -log_beta - d_t / beta; }
};

template <class CandidateLists> struct HiddenMarkovModel
{
    std::vector<std::vector<double>> viterbi;
    std::vector<std::vector<std::pair<unsigned, unsigned>>> parents;
    std::vector<std::vector<float>> path_distances;
    std::vector<std::vector<bool>> pruned;
    std::vector<bool> breakage;

    const CandidateLists &candidates_list;
    const std::vector<std::vector<double>> &emission_log_probabilities;

    HiddenMarkovModel(const CandidateLists &candidates_list,
                      const std::vector<std::vector<double>> &emission_log_probabilities)
        : breakage(candidates_list.size()), candidates_list(candidates_list),
          emission_log_probabilities(emission_log_probabilities)
    {
        viterbi.resize(candidates_list.size());
        parents.resize(candidates_list.size());
        path_distances.resize(candidates_list.size());
        pruned.resize(candidates_list.size());
        breakage.resize(candidates_list.size());
        for (const auto i : util::irange<std::size_t>(0UL, candidates_list.size()))
        {
            const auto &num_candidates = candidates_list[i].size();
            // add empty vectors
            if (num_candidates > 0)
            {
                viterbi[i].resize(num_candidates);
                parents[i].resize(num_candidates);
                path_distances[i].resize(num_candidates);
                pruned[i].resize(num_candidates);
            }
        }

        Clear(0);
    }

    void Clear(std::size_t initial_timestamp)
    {
        BOOST_ASSERT(viterbi.size() == parents.size() && parents.size() == path_distances.size() &&
                     path_distances.size() == pruned.size() && pruned.size() == breakage.size());

        for (const auto t : util::irange(initial_timestamp, viterbi.size()))
        {
            std::fill(viterbi[t].begin(), viterbi[t].end(), IMPOSSIBLE_LOG_PROB);
            std::fill(parents[t].begin(), parents[t].end(), std::make_pair(0U, 0U));
            std::fill(path_distances[t].begin(), path_distances[t].end(), 0.0);
            std::fill(pruned[t].begin(), pruned[t].end(), true);
        }
        std::fill(breakage.begin() + initial_timestamp, breakage.end(), true);
    }

    std::size_t initialize(std::size_t initial_timestamp)
    {
        auto num_points = candidates_list.size();
        do
        {
            BOOST_ASSERT(initial_timestamp < num_points);

            for (const auto s : util::irange<std::size_t>(0UL, viterbi[initial_timestamp].size()))
            {
                viterbi[initial_timestamp][s] = emission_log_probabilities[initial_timestamp][s];
                parents[initial_timestamp][s] = std::make_pair(initial_timestamp, s);
                pruned[initial_timestamp][s] = viterbi[initial_timestamp][s] < MINIMAL_LOG_PROB;

                breakage[initial_timestamp] =
                    breakage[initial_timestamp] && pruned[initial_timestamp][s];
            }

            ++initial_timestamp;
        } while (initial_timestamp < num_points && breakage[initial_timestamp - 1]);

        if (initial_timestamp >= num_points)
        {
            return INVALID_STATE;
        }

        BOOST_ASSERT(initial_timestamp > 0);
        --initial_timestamp;

        BOOST_ASSERT(breakage[initial_timestamp] == false);

        return initial_timestamp;
    }
};
} // namespace map_matching
} // namespace engine
} // namespace osrm

#endif // HIDDEN_MARKOV_MODEL


/*
auther: wanyouwen
date: 2018.6.24

双目示例:
双目相机app程序框架：

1. 读取相机配置文件(内参数 畸变矫正参数 双目对齐变换矩阵) =======================
   cv::FileStorage fsSettings(setting_filename, cv::FileStorage::READ);
   fsSettings["LEFT.K"] >> K_l;//内参数
   fsSettings["LEFT.D"] >> D_l;// 畸变矫正
   fsSettings["LEFT.P"] >> P_l;// P_l,P_r --左右相机在校准后坐标系中的投影矩阵 3×4
   fsSettings["LEFT.R"] >> R_l;// R_l,R_r --左右相机校准变换（旋转）矩阵  3×3
2. 计算双目矫正映射矩阵========================================================
   cv::Mat M1l,M2l,M1r,M2r;
       cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l);
       cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r);
3. 创建双目系统===============================================================
   ORB_SLAM2::System SLAM(vocabulary_filepath, setting_filename, ORB_SLAM2::System::STEREO, true);
4. 从双目设备捕获图像,设置分辨率捕获图像========================================
   cv::VideoCapture CapAll(deviceid); //打开相机设备
   //设置分辨率   1280*480  分成两张 640*480  × 2 左右相机
       CapAll.set(CV_CAP_PROP_FRAME_WIDTH,1280);
       CapAll.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
    5. 获取左右相机图像===========================================================
   CapAll.read(src_img);
   imLeft  = src_img(cv::Range(0, 480), cv::Range(0, 640));
       imRight = src_img(cv::Range(0, 480), cv::Range(640, 1280));
6. 使用2步获取的双目矫正映射矩阵 矫正 左右相机图像==============================
   cv::remap(imLeft,imLeftRect,M1l,M2l,cv::INTER_LINEAR);
       cv::remap(imRight,imRightRect,M1r,M2r,cv::INTER_LINEAR);
7. 记录时间戳 time ，并计时===================================================
	#ifdef COMPILEDWITHC11
	   std::chrono::steady_clock::time_point    t1 = std::chrono::steady_clock::now();
	#else
	   std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
	#endif
    8. 把左右图像和时间戳 传给 SLAM系统===========================================
    SLAM.TrackStereo(imLeftRect, imRightRect, time);
9. 计时结束，计算时间差，处理时间=============================================
10.循环执行 5-9步===========================================================
11.结束，关闭slam系统，关闭所有线程===========================================
12.保存相机轨迹==============================================================

*/
#include<iostream>
#include<algorithm>
#include<fstream>
#include<iomanip>
#include<chrono>//chrono是一个time library, 源于boost，现在已经是C++标准。

#include<opencv2/core/core.hpp>
#include<System.h>

using namespace std;
using namespace cv;

#include <boost/format.hpp>  // 格式化字符串 for formating strings 处理图像文件格式
#include <boost/thread/thread.hpp>

static void print_help()
{
    printf("\n orbslam2 stereo test : \n");//生成视差和点云图
    printf("\n Usage: my_stereo [-v=<path_to_vocabulary>] [-s=<path_to_settings>]\n"
           "\n [--d=<camera_id>] \n");
}

int main(int argc, char **argv)
{
    //std::uint32_t timestamp;
    double time=0.0, ttrack=0;

    //vector<IMUData> imudatas;
    std::string setting_filename = "";    //配置文件
    std::string vocabulary_filepath = ""; //关键帧数据库 词典 重定位
    int deviceid = 1;                     //相机设备id
    cv::CommandLineParser parser(argc, argv,
        "{help h||}{d|1|}{v|../../Vocabulary/ORBvoc.bin|}{s|my_stereo.yaml|}");
//=======打印帮助信息============
    if(parser.has("help"))
    {
        print_help();
        return 0;
    }
    if( parser.has("d") )
        deviceid = parser.get<int>("d");//相机设备id
    if( parser.has("s") )
        setting_filename = parser.get<std::string>("s");//
    if( parser.has("v") )
        vocabulary_filepath = parser.get<std::string>("v");//
    if (!parser.check()) {
        parser.printErrors();
        return 1;
    }
//if(setting_filename.empty())//{

 //1.  读取相机配置文件(内参数 畸变矫正参数 双目对齐变换矩阵 ) ====================
    cv::FileStorage fsSettings(setting_filename, cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        cerr << "ERROR: Wrong path to setting file : " << setting_filename << endl;
        return -1;
    }

    cv::Mat K_l, K_r, D_l, D_r, P_l, P_r, R_l, R_r;
    fsSettings["LEFT.K"] >> K_l;//内参数
    if(K_l.empty())  cout << "K_l missing "<<endl;
    fsSettings["RIGHT.K"] >> K_r;//
    if(K_r.empty()) cout << "K_r missing "<<endl;
    fsSettings["LEFT.D"] >> D_l;// 畸变矫正
    if(D_l.empty()) cout << "D_l missing "<<endl;
    fsSettings["RIGHT.D"] >> D_r;
    if(D_r.empty()) cout << "D_r missing "<<endl;

    fsSettings["LEFT.P"] >> P_l;// P_l,P_r --左右相机在校准后坐标系中的投影矩阵 3×4
    if(P_l.empty()) cout << "P_l missing "<<endl;
    fsSettings["RIGHT.P"] >> P_r;
    if(K_r.empty()) cout << "P_r missing "<<endl;

    fsSettings["LEFT.R"] >> R_l;// R_l,R_r --左右相机校准变换（旋转）矩阵  3×3
    if(R_l.empty()) cout << "R_l missing "<<endl;
    fsSettings["RIGHT.R"] >> R_r;
    if(R_r.empty()) cout << "R_r missing "<<endl;

    int rows_l = fsSettings["LEFT.height"];
    int cols_l = fsSettings["LEFT.width"];
    int rows_r = fsSettings["RIGHT.height"];
    int cols_r = fsSettings["RIGHT.width"];

    if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() || rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0)
    {
        cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;
        return -1;
    }
//}

//外参数
// Mat R, T, R1, P1, R2, P2;
//fs["R"] >> R;
//fs["T"] >> T;
//图像矫正摆正 映射计算
//stereoRectify( K_l, D_l, K_r, D_r, cv::Size(cols_l,rows_l), R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, -1, cv::Size(cols_l,rows_l), &roi1, &roi2 );

// 2. 计算双目矫正映射矩阵================================================
    cv::Mat M1l,M2l,M1r,M2r;
    cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l);
    cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r);

// 3. 创建双目系统 ORB_SLAM2::System======================================
    ORB_SLAM2::System SLAM(vocabulary_filepath, setting_filename, ORB_SLAM2::System::STEREO, true);

    cout << endl << "-------" << endl;
    cout << "Start processing sequence ..." << endl;
// 4. 从双目设备捕获图像设置分辨率捕获图像 ===================================================
    cv::VideoCapture CapAll(deviceid); //打开相机设备
    if( !CapAll.isOpened() )
    {
       printf("打开摄像头失败\r\n");
       printf("再试一次\r\n");
       //sleep(1);//延时1秒
       cv::VideoCapture CapAll(1); //打开相机设备
       if( !CapAll.isOpened() ) {
         printf("打开摄像头失败\r\n");
	 printf("再试一次..\r\n");
          //sleep(1);//延时1秒
          cv::VideoCapture CapAll(1); //打开相机设备
          if( !CapAll.isOpened() ) {
	  printf("打开摄像头失败\r\n");
	   return -1;
         }
       }
   }
    //设置分辨率   1280*480  分成两张 640*480  × 2 左右相机
    CapAll.set(CV_CAP_PROP_FRAME_WIDTH,1280);
    CapAll.set(CV_CAP_PROP_FRAME_HEIGHT, 480);

    cv::Mat src_img, imLeft, imRight, imLeftRect, imRightRect;

    while(CapAll.read(src_img))
     {
// 5. 获取左右相机图像====================================================
        imLeft  = src_img(cv::Range(0, 480), cv::Range(0, 640));
        imRight = src_img(cv::Range(0, 480), cv::Range(640, 1280));

// 6. 矫正左右相机图像====================================================
        cv::remap(imLeft,imLeftRect,M1l,M2l,cv::INTER_LINEAR);
        cv::remap(imRight,imRightRect,M1r,M2r,cv::INTER_LINEAR);

// 7. 记录时间戳 tframe ，并计时==========================================
#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point        t1 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif
        time += ttrack ;

// 8. 把左右图像和时间戳 传给 SLAM系统====================================
        SLAM.TrackStereo(imLeftRect, imRightRect, time);

// 9. 计时结束，计算时间差，处理时间======================================
#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point        t2 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif

        ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();

  //  if(ttrack<T)
//   usleep((T-ttrack)*1e6); //sleep

    }

// 10. 结束，关闭slam系统，关闭所有线程===================================
    SLAM.Shutdown();
// 11. 保存相机轨迹======================================================
    SLAM.SaveTrajectoryKITTI("myCameraTrajectory.txt");

    CapAll.release();
    cv::destroyAllWindows();
    return 0;
}


#include <iostream>
#include <string>
#include <fstream>
#include <algorithm>
#include <vector>
#include <iterator>
#include <numeric>
#include <memory>
#include <chrono>

#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

#include "VNS.hpp"
#include "Solution.hpp"
#include "Helper.hpp"

using namespace std;
namespace ublas = boost::numeric::ublas;

/*********************************************************************************/
/*                   CONSTRUCTOR AND MEMBER FUNCTIONS OF VNS                     */
/*********************************************************************************/

VNS::VNS(const string &directory,
	const unsigned int &kMin,
	const unsigned int &kMax,
	const unsigned int &tMax,
	const unsigned int &shaking_mode,
	const unsigned int &local_search_mode,
	const unsigned int &update_xij_mode,
	const unsigned int &initial_mode,
	const unsigned int &time):
	// Init VNS and TestSet
		TestSet(directory), m_k(kMin - 1), m_kMax(kMax), m_kMin(kMin), m_tMax(tMax),
		m_shaking_mode(shaking_mode), m_local_search_mode(local_search_mode), m_update_xij_mode(update_xij_mode),
		m_incumbent_solution(m_locationNumber, 1), m_perturbed_solution(m_locationNumber, 1),
		m_fx(DBL_MAX), m_best_fx(DBL_MAX), m_sum_dj(accumulate(m_dj.begin(), m_dj.end(), 0.0)),
		m_initial_mode(initial_mode), m_maxNH(pow(m_kMax, 2)),m_timeMax(time), m_rMax(0),
		m_neighborhood_k(m_maxNH, vector<bool>(m_locationNumber, 1)), m_flow_tpl(0), m_initial_fx(DBL_MAX)
{
}

void VNS::initialSolution(
	vector <bool> &incumbent_solution,
	const vector<double> &bi,
	const vector<double> &fi,
	const vector<double> &dj,
	ublas::matrix<double> &cij,
	const double &sum_dj,
	const unsigned int &init_mode)
{
	if (init_mode == 0)
		initialGreedy(incumbent_solution, bi, fi, dj, cij, sum_dj);
	else
		initialRVNS(incumbent_solution, bi,  dj, cij);

	if (m_best_fx <= m_fx)
		m_initial_fx = m_best_fx;
	else
		m_initial_fx = m_fx;
}

// Move or not
void VNS::neighborhoodChange(
	vector<bool> &incumbent_solution,
	const vector<bool> &local_solution,
	unsigned int &k,
	const double &fx,
	double &best_fx,
	const double &kMin)
{
	if (fx < best_fx)
	{
		incumbent_solution = local_solution;
		k = kMin - 1;
		best_fx = fx;
	}
	else
		k++;
}

// Creates Perturbed Solution S'
vector<bool> VNS::shaking(
	const vector<bool> &incumbent_solution,
	const vector<double> &bi,
	const vector<double> &dj,
	ublas::matrix<double> &cij,
	const double &k,
	double &fx,
	const unsigned int &shaking_mode)
{
	if (shaking_mode == 0)
		return shakingKOperations(incumbent_solution, bi, m_dj, m_cij, m_k, fx);
	else if (shaking_mode == 1)
		return shakingKMaxOperations(incumbent_solution, bi, m_dj, m_cij, m_k, fx);
	else if (shaking_mode == 2)
		return shakingAssignments(incumbent_solution, bi, m_dj, m_cij, m_k, fx);
	else
		return shakingCosts(incumbent_solution, bi, m_dj, m_cij, m_k, fx);
}

// Getter f(x)
const double VNS::getFx() const
{
	return m_best_fx;
}

// Getter initial f(x)
const double VNS::getInitialFx() const
{
	return m_initial_fx;
}


/* ----------------------------------------------------------------------------
 * Copyright 2018, Ross Hartley <m.ross.hartley@gmail.com>
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 *  @file   kinematics.cpp
 *  @author Ross Hartley
 *  @brief  Example of invariant filtering for contact-aided inertial navigation
 *  @date   September 25, 2018
 **/

#include <iostream>
#include <fstream>
#include <string>
#include <cstdlib>
#include <Eigen/Dense>
#include <boost/algorithm/string.hpp>
#include <vector>
#include "InEKF.h"

#define DT_MIN 1e-6
#define DT_MAX 1

using namespace std;
using namespace inekf;

double stod98(const std::string &s) {
    return atof(s.c_str());
}

int stoi98(const std::string &s) {
    return atoi(s.c_str());
}

int main() {
    //  ---- Initialize invariant extended Kalman filter ----- //
    RobotState initial_state;

    // Initialize state mean
    Eigen::Matrix3d R0;
    Eigen::Vector3d v0, p0, bg0, ba0;
    R0 << 1, 0, 0, // initial orientation
          0, -1, 0, // IMU frame is rotated 90deg about the x-axis
          0, 0, -1;
    v0 << 0,0,0; // initial velocity
    p0 << 0,0,0; // initial position
    bg0 << 0,0,0; // initial gyroscope bias
    ba0 << 0,0,0; // initial accelerometer bias
    initial_state.setRotation(R0);
    initial_state.setVelocity(v0);
    initial_state.setPosition(p0);
    initial_state.setGyroscopeBias(bg0);
    initial_state.setAccelerometerBias(ba0);

    // Initialize state covariance
    NoiseParams noise_params;
    noise_params.setGyroscopeNoise(0.01);
    noise_params.setAccelerometerNoise(0.1);
    noise_params.setGyroscopeBiasNoise(0.00001);
    noise_params.setAccelerometerBiasNoise(0.0001);
    noise_params.setContactNoise(0.01);

    // Initialize filter
    InEKF filter(initial_state, noise_params);
    cout << "Noise parameters are initialized to: \n";
    cout << filter.getNoiseParams() << endl;
    cout << "Robot's state is initialized to: \n";
    cout << filter.getState() << endl;

    // Open data file
    ifstream infile("../src/data/imu_kinematic_measurements.txt");
    string line;
    Eigen::Matrix<double,6,1> imu_measurement = Eigen::Matrix<double,6,1>::Zero();
    Eigen::Matrix<double,6,1> imu_measurement_prev = Eigen::Matrix<double,6,1>::Zero();
    double t = 0;
    double t_prev = 0;

    // ---- Loop through data file and read in measurements line by line ---- //
    while (getline(infile, line)){
        vector<string> measurement;
        boost::split(measurement,line,boost::is_any_of(" "));
        // // Handle measurements
        if (measurement[0].compare("IMU")==0){
            cout << "Received IMU Data, propagating state\n";
            assert((measurement.size()-2) == 6);
            t = atof(measurement[1].c_str());
            // Read in IMU data
            imu_measurement << stod98(measurement[2]),
                               stod98(measurement[3]),
                               stod98(measurement[4]),
                               stod98(measurement[5]),
                               stod98(measurement[6]),
                               stod98(measurement[7]);

            // Propagate using IMU data
            double dt = t - t_prev;
            if (dt > DT_MIN && dt < DT_MAX) {
                filter.Propagate(imu_measurement_prev, dt);
            }

        }
        else if (measurement[0].compare("CONTACT")==0){
            cout << "Received CONTACT Data, setting filter's contact state\n";
            assert((measurement.size()-2)%2 == 0);
            vector<pair<int,bool> > contacts;
            int id;
            bool indicator;
            t = stod98(measurement[1]);
            // Read in contact data
            for (int i=2; i<measurement.size(); i+=2) {
                id = stoi98(measurement[i]);
                indicator = bool(stod98(measurement[i+1]));
                contacts.push_back(pair<int,bool> (id, indicator));
            }
            // Set filter's contact state
            filter.setContacts(contacts);
        }
        else if (measurement[0].compare("KINEMATIC")==0){
            cout << "Received KINEMATIC observation, correcting state\n";
            assert((measurement.size()-2)%44 == 0);
            int id;
            Eigen::Quaternion<double> q;
            Eigen::Vector3d p;
            Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
            Eigen::Matrix<double,6,6> covariance;
            vectorKinematics measured_kinematics;
            t = stod98(measurement[1]);
            // Read in kinematic data
            for (int i=2; i<measurement.size(); i+=44) {
                id = stoi98(measurement[i]);
                q = Eigen::Quaternion<double> (stod98(measurement[i+1]),stod98(measurement[i+2]),stod98(measurement[i+3]),stod98(measurement[i+4]));
                q.normalize();
                p << stod98(measurement[i+5]),stod98(measurement[i+6]),stod98(measurement[i+7]);
                pose.block<3,3>(0,0) = q.toRotationMatrix();
                pose.block<3,1>(0,3) = p;
                for (int j=0; j<6; ++j) {
                    for (int k=0; k<6; ++k) {
                        covariance(j,k) = stod98(measurement[i+8 + j*6+k]);
                    }
                }
                Kinematics frame(id, pose, covariance);
                measured_kinematics.push_back(frame);
            }
            // Correct state using kinematic measurements
            filter.CorrectKinematics(measured_kinematics);
        }

        // Store previous timestamp
        t_prev = t;
        imu_measurement_prev = imu_measurement;
    }

    // Print final state
    cout << filter.getState() << endl;
    return 0;
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2015 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "outer_hull_legacy.h"
#include "extract_cells.h"
#include "remesh_self_intersections.h"
#include "assign.h"
#include "../../remove_unreferenced.h"

#include <CGAL/AABB_tree.h>
#include <CGAL/AABB_traits.h>
#include <CGAL/AABB_triangle_primitive.h>
#include <CGAL/intersections.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>

#include "points_inside_component.h"
#include "order_facets_around_edges.h"
#include "outer_facet.h"
#include "../../sortrows.h"
#include "../../facet_components.h"
#include "../../winding_number.h"
#include "../../triangle_triangle_adjacency.h"
#include "../../unique_edge_map.h"
#include "../../barycenter.h"
#include "../../per_face_normals.h"
#include "../../sort_angles.h"
#include <Eigen/Geometry>
#include <vector>
#include <map>
#include <queue>
#include <iostream>
#include <type_traits>
#include <CGAL/number_utils.h>
//#define IGL_OUTER_HULL_DEBUG

template <
  typename DerivedV,
  typename DerivedF,
  typename DerivedG,
  typename DerivedJ,
  typename Derivedflip>
IGL_INLINE void igl::copyleft::cgal::outer_hull_legacy(
  const Eigen::PlainObjectBase<DerivedV> & V,
  const Eigen::PlainObjectBase<DerivedF> & F,
  Eigen::PlainObjectBase<DerivedG> & G,
  Eigen::PlainObjectBase<DerivedJ> & J,
  Eigen::PlainObjectBase<Derivedflip> & flip)
{
#ifdef IGL_OUTER_HULL_DEBUG
  std::cerr << "Extracting outer hull" << std::endl;
#endif
  using namespace Eigen;
  using namespace std;
  typedef typename DerivedF::Index Index;
  Matrix<Index,DerivedF::RowsAtCompileTime,1> C;
  typedef Matrix<typename DerivedV::Scalar,Dynamic,DerivedV::ColsAtCompileTime> MatrixXV;
  //typedef Matrix<typename DerivedF::Scalar,Dynamic,DerivedF::ColsAtCompileTime> MatrixXF;
  typedef Matrix<typename DerivedG::Scalar,Dynamic,DerivedG::ColsAtCompileTime> MatrixXG;
  typedef Matrix<typename DerivedJ::Scalar,Dynamic,DerivedJ::ColsAtCompileTime> MatrixXJ;
  const Index m = F.rows();

  // UNUSED:
  //const auto & duplicate_simplex = [&F](const int f, const int g)->bool
  //{
  //  return
  //    (F(f,0) == F(g,0) && F(f,1) == F(g,1) && F(f,2) == F(g,2)) ||
  //    (F(f,1) == F(g,0) && F(f,2) == F(g,1) && F(f,0) == F(g,2)) ||
  //    (F(f,2) == F(g,0) && F(f,0) == F(g,1) && F(f,1) == F(g,2)) ||
  //    (F(f,0) == F(g,2) && F(f,1) == F(g,1) && F(f,2) == F(g,0)) ||
  //    (F(f,1) == F(g,2) && F(f,2) == F(g,1) && F(f,0) == F(g,0)) ||
  //    (F(f,2) == F(g,2) && F(f,0) == F(g,1) && F(f,1) == F(g,0));
  //};

#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"outer hull..."<<endl;
#endif

#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"edge map..."<<endl;
#endif
  typedef Matrix<typename DerivedF::Scalar,Dynamic,2> MatrixX2I;
  typedef Matrix<typename DerivedF::Index,Dynamic,1> VectorXI;
  //typedef Matrix<typename DerivedV::Scalar, 3, 1> Vector3F;
  MatrixX2I E,uE;
  VectorXI EMAP;
  vector<vector<typename DerivedF::Index> > uE2E;
  unique_edge_map(F,E,uE,EMAP,uE2E);
#ifdef IGL_OUTER_HULL_DEBUG
  for (size_t ui=0; ui<uE.rows(); ui++) {
      std::cout << ui << ": " << uE2E[ui].size() << " -- (";
      for (size_t i=0; i<uE2E[ui].size(); i++) {
          std::cout << uE2E[ui][i] << ", ";
      }
      std::cout << ")" << std::endl;
  }
#endif

  std::vector<std::vector<typename DerivedF::Index> > uE2oE;
  std::vector<std::vector<bool> > uE2C;
  order_facets_around_edges(V, F, uE, uE2E, uE2oE, uE2C);
  uE2E = uE2oE;
  VectorXI diIM(3*m);
  for (auto ue : uE2E) {
      for (size_t i=0; i<ue.size(); i++) {
          auto fe = ue[i];
          diIM[fe] = i;
      }
  }

  vector<vector<vector<Index > > > TT,_1;
  triangle_triangle_adjacency(E,EMAP,uE2E,false,TT,_1);
  VectorXI counts;
#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"facet components..."<<endl;
#endif
  facet_components(TT,C,counts);
  assert(C.maxCoeff()+1 == counts.rows());
  const size_t ncc = counts.rows();
  G.resize(0,F.cols());
  J.resize(0,1);
  flip.setConstant(m,1,false);

#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"reindex..."<<endl;
#endif
  // H contains list of faces on outer hull;
  vector<bool> FH(m,false);
  vector<bool> EH(3*m,false);
  vector<MatrixXG> vG(ncc);
  vector<MatrixXJ> vJ(ncc);
  vector<MatrixXJ> vIM(ncc);
  //size_t face_count = 0;
  for(size_t id = 0;id<ncc;id++)
  {
    vIM[id].resize(counts[id],1);
  }
  // current index into each IM
  vector<size_t> g(ncc,0);
  // place order of each face in its respective component
  for(Index f = 0;f<m;f++)
  {
    vIM[C(f)](g[C(f)]++) = f;
  }

#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"barycenters..."<<endl;
#endif
  // assumes that "resolve" has handled any coplanar cases correctly and nearly
  // coplanar cases can be sorted based on barycenter.
  MatrixXV BC;
  barycenter(V,F,BC);

#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"loop over CCs (="<<ncc<<")..."<<endl;
#endif
  for(Index id = 0;id<(Index)ncc;id++)
  {
    auto & IM = vIM[id];
    // starting face that's guaranteed to be on the outer hull and in this
    // component
    int f;
    bool f_flip;
#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"outer facet..."<<endl;
#endif
  igl::copyleft::cgal::outer_facet(V,F,IM,f,f_flip);
#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"outer facet: "<<f<<endl;
  //cout << V.row(F(f, 0)) << std::endl;
  //cout << V.row(F(f, 1)) << std::endl;
  //cout << V.row(F(f, 2)) << std::endl;
#endif
    int FHcount = 1;
    FH[f] = true;
    // Q contains list of face edges to continue traversing upong
    queue<int> Q;
    Q.push(f+0*m);
    Q.push(f+1*m);
    Q.push(f+2*m);
    flip(f) = f_flip;
    //std::cout << "face " << face_count++ << ": " << f << std::endl;
    //std::cout << "f " << F.row(f).array()+1 << std::endl;
    //cout<<"flip("<<f<<") = "<<(flip(f)?"true":"false")<<endl;
#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"BFS..."<<endl;
#endif
    while(!Q.empty())
    {
      // face-edge
      const int e = Q.front();
      Q.pop();
      // face
      const int f = e%m;
      // corner
      const int c = e/m;
#ifdef IGL_OUTER_HULL_DEBUG
      std::cout << "edge: " << e << ", ue: " << EMAP(e) << std::endl;
      std::cout << "face: " << f << std::endl;
      std::cout << "corner: " << c << std::endl;
      std::cout << "consistent: " << uE2C[EMAP(e)][diIM[e]] << std::endl;
#endif
      // Should never see edge again...
      if(EH[e] == true)
      {
        continue;
      }
      EH[e] = true;
      // source of edge according to f
      const int fs = flip(f)?F(f,(c+2)%3):F(f,(c+1)%3);
      // destination of edge according to f
      const int fd = flip(f)?F(f,(c+1)%3):F(f,(c+2)%3);
      // edge valence
      const size_t val = uE2E[EMAP(e)].size();
#ifdef IGL_OUTER_HULL_DEBUG
      //std::cout << "vd: " << V.row(fd) << std::endl;
      //std::cout << "vs: " << V.row(fs) << std::endl;
      //std::cout << "edge: " << V.row(fd) - V.row(fs) << std::endl;
      for (size_t i=0; i<val; i++) {
          if (i == diIM(e)) {
              std::cout << "* ";
          } else {
              std::cout << "  ";
          }
          std::cout << i << ": "
              << " (e: " << uE2E[EMAP(e)][i] << ", f: "
              << uE2E[EMAP(e)][i] % m * (uE2C[EMAP(e)][i] ? 1:-1) << ")" << std::endl;
      }
#endif

      // is edge consistent with edge of face used for sorting
      const int e_cons = (uE2C[EMAP(e)][diIM(e)] ? 1: -1);
      int nfei = -1;
      // Loop once around trying to find suitable next face
      for(size_t step = 1; step<val+2;step++)
      {
        const int nfei_new = (diIM(e) + 2*val + e_cons*step*(flip(f)?-1:1))%val;
        const int nf = uE2E[EMAP(e)][nfei_new] % m;
        {
#ifdef IGL_OUTER_HULL_DEBUG
        //cout<<"Next facet: "<<(f+1)<<" --> "<<(nf+1)<<", |"<<
        //  di[EMAP(e)][diIM(e)]<<" - "<<di[EMAP(e)][nfei_new]<<"| = "<<
        //    abs(di[EMAP(e)][diIM(e)] - di[EMAP(e)][nfei_new])
        //    <<endl;
#endif

          // Only use this face if not already seen
          if(!FH[nf])
          {
            nfei = nfei_new;
          //} else {
          //    std::cout << "skipping face " << nfei_new << " because it is seen before"
          //        << std::endl;
          }
          break;
        //} else {
        //    std::cout << di[EMAP(e)][diIM(e)].transpose() << std::endl;
        //    std::cout << di[EMAP(e)][diIM(nfei_new)].transpose() << std::endl;
        //    std::cout << "skipping face " << nfei_new << " with identical dihedral angle"
        //        << std::endl;
        }
//#ifdef IGL_OUTER_HULL_DEBUG
//        cout<<"Skipping co-planar facet: "<<(f+1)<<" --> "<<(nf+1)<<endl;
//#endif
      }

      int max_ne = -1;
      if(nfei >= 0)
      {
        max_ne = uE2E[EMAP(e)][nfei];
      }

      if(max_ne>=0)
      {
        // face of neighbor
        const int nf = max_ne%m;
#ifdef IGL_OUTER_HULL_DEBUG
        if(!FH[nf])
        {
          // first time seeing face
          cout<<(f+1)<<" --> "<<(nf+1)<<endl;
        }
#endif
        FH[nf] = true;
        //std::cout << "face " << face_count++ << ": " << nf << std::endl;
        //std::cout << "f " << F.row(nf).array()+1 << std::endl;
        FHcount++;
        // corner of neighbor
        const int nc = max_ne/m;
        const int nd = F(nf,(nc+2)%3);
        const bool cons = (flip(f)?fd:fs) == nd;
        flip(nf) = (cons ? flip(f) : !flip(f));
        //cout<<"flip("<<nf<<") = "<<(flip(nf)?"true":"false")<<endl;
        const int ne1 = nf+((nc+1)%3)*m;
        const int ne2 = nf+((nc+2)%3)*m;
        if(!EH[ne1])
        {
          Q.push(ne1);
        }
        if(!EH[ne2])
        {
          Q.push(ne2);
        }
      }
    }

    {
      vG[id].resize(FHcount,3);
      vJ[id].resize(FHcount,1);
      //nG += FHcount;
      size_t h = 0;
      assert(counts(id) == IM.rows());
      for(int i = 0;i<counts(id);i++)
      {
        const size_t f = IM(i);
        //if(f_flip)
        //{
        //  flip(f) = !flip(f);
        //}
        if(FH[f])
        {
          vG[id].row(h) = (flip(f)?F.row(f).reverse().eval():F.row(f));
          vJ[id](h,0) = f;
          h++;
        }
      }
      assert((int)h == FHcount);
    }
  }

  // Is A inside B? Assuming A and B are consistently oriented but closed and
  // non-intersecting.
  const auto & has_overlapping_bbox = [](
    const Eigen::PlainObjectBase<DerivedV> & V,
    const MatrixXG & A,
    const MatrixXG & B)->bool
  {
    const auto & bounding_box = [](
      const Eigen::PlainObjectBase<DerivedV> & V,
      const MatrixXG & F)->
        DerivedV
    {
      DerivedV BB(2,3);
      BB<<
         1e26,1e26,1e26,
        -1e26,-1e26,-1e26;
      const size_t m = F.rows();
      for(size_t f = 0;f<m;f++)
      {
        for(size_t c = 0;c<3;c++)
        {
          const auto & vfc = V.row(F(f,c)).eval();
          BB(0,0) = std::min(BB(0,0), vfc(0,0));
          BB(0,1) = std::min(BB(0,1), vfc(0,1));
          BB(0,2) = std::min(BB(0,2), vfc(0,2));
          BB(1,0) = std::max(BB(1,0), vfc(0,0));
          BB(1,1) = std::max(BB(1,1), vfc(0,1));
          BB(1,2) = std::max(BB(1,2), vfc(0,2));
        }
      }
      return BB;
    };
    // A lot of the time we're dealing with unrelated, distant components: cull
    // them.
    DerivedV ABB = bounding_box(V,A);
    DerivedV BBB = bounding_box(V,B);
    if( (BBB.row(0)-ABB.row(1)).maxCoeff()>0  ||
        (ABB.row(0)-BBB.row(1)).maxCoeff()>0 )
    {
      // bounding boxes do not overlap
      return false;
    } else {
      return true;
    }
  };

  // Reject components which are completely inside other components
  vector<bool> keep(ncc,true);
  size_t nG = 0;
  // This is O( ncc * ncc * m)
  for(size_t id = 0;id<ncc;id++)
  {
    if (!keep[id]) continue;
    std::vector<size_t> unresolved;
    for(size_t oid = 0;oid<ncc;oid++)
    {
      if(id == oid || !keep[oid])
      {
        continue;
      }
      if (has_overlapping_bbox(V, vG[id], vG[oid])) {
          unresolved.push_back(oid);
      }
    }
    const size_t num_unresolved_components = unresolved.size();
    DerivedV query_points(num_unresolved_components, 3);
    for (size_t i=0; i<num_unresolved_components; i++) {
        const size_t oid = unresolved[i];
        DerivedF f = vG[oid].row(0);
        query_points(i,0) = (V(f(0,0), 0) + V(f(0,1), 0) + V(f(0,2), 0))/3.0;
        query_points(i,1) = (V(f(0,0), 1) + V(f(0,1), 1) + V(f(0,2), 1))/3.0;
        query_points(i,2) = (V(f(0,0), 2) + V(f(0,1), 2) + V(f(0,2), 2))/3.0;
    }
    Eigen::VectorXi inside;
    igl::copyleft::cgal::points_inside_component(V, vG[id], query_points, inside);
    assert((size_t)inside.size() == num_unresolved_components);
    for (size_t i=0; i<num_unresolved_components; i++) {
        if (inside(i, 0)) {
            const size_t oid = unresolved[i];
            keep[oid] = false;
        }
    }
  }
  for (size_t id = 0; id<ncc; id++) {
      if (keep[id]) {
          nG += vJ[id].rows();
      }
  }

  // collect G and J across components
  G.resize(nG,3);
  J.resize(nG,1);
  {
    size_t off = 0;
    for(Index id = 0;id<(Index)ncc;id++)
    {
      if(keep[id])
      {
        assert(vG[id].rows() == vJ[id].rows());
        G.block(off,0,vG[id].rows(),vG[id].cols()) = vG[id];
        J.block(off,0,vJ[id].rows(),vJ[id].cols()) = vJ[id];
        off += vG[id].rows();
      }
    }
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
// generated by autoexplicit.sh
template void igl::copyleft::cgal::outer_hull_legacy<Eigen::Matrix<CGAL::Epeck::FT, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<CGAL::Epeck::FT, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&);
template void igl::copyleft::cgal::outer_hull_legacy< Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > &, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > &, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > &);
template void igl::copyleft::cgal::outer_hull_legacy<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&);
#ifdef WIN32
#endif
#endif


/******************************************************************************

  This source file is part of the Avogadro project.

  Copyright 2010 Eric C. Brown

  This source code is released under the New BSD License, (the "License").

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.

******************************************************************************/

#include "qtaimcriticalpointlocator.h"
#include "qtaimwavefunction.h"
#include "qtaimodeintegrator.h"
#include "qtaimlsodaintegrator.h"
#include "qtaimmathutilities.h"

#include <Eigen/Core>

#include <QList>

#include <QtConcurrentMap>

#include <QTemporaryFile>
#include <QFile>
#include <QDataStream>
#include <QDir>

#include <QVariant>

#include <QProgressDialog>
#include <QFutureWatcher>
#include <QFuture>

using namespace std;
using namespace Eigen;

#define HUGE_REAL_NUMBER 1.e20
#define SMALL_GRADIENT_NORM 1.e-4

namespace Avogadro {
namespace QtPlugins {

  QList<QVariant> QTAIMLocateNuclearCriticalPoint( QList<QVariant> input  )
  {
    const QString fileName=input.at(0).toString();
    const qint64 nucleus=input.at(1).toInt();
    const QVector3D x0y0z0(
        input.at(2).toReal(),
        input.at(3).toReal(),
        input.at(4).toReal()
        );

    QTAIMWavefunction wfn;
    wfn.loadFromBinaryFile(fileName);

    QTAIMWavefunctionEvaluator eval(wfn);

    QVector3D result;

    if( wfn.nuclearCharge(nucleus) < 4 )
    {
      //      QTAIMODEIntegrator ode(eval,QTAIMODEIntegrator::CMBPMinusThreeGradientInElectronDensity);
      QTAIMLSODAIntegrator ode(eval,QTAIMLSODAIntegrator::CMBPMinusThreeGradientInElectronDensity);
      result=ode.integrate(x0y0z0);
    }
    else
    {
      result=x0y0z0;
    }

    bool correctSignature;
    Matrix<qreal,3,1> xyz; xyz << result.x(), result.y(), result.z();

    if(
        QTAIMMathUtilities::signatureOfASymmetricThreeByThreeMatrix(
            eval.hessianOfElectronDensity(xyz)
            ) == -3
        )
    {
      correctSignature=true;
    }
    else
    {
      correctSignature=false;
    }

    QList<QVariant> value;

    if( correctSignature )
    {
      value.append(correctSignature);
      value.append(result.x());
      value.append(result.y());
      value.append(result.z());
    }
    else
    {
      value.append(false);
    }

    return value;

  }

  QList<QVariant> QTAIMLocateBondCriticalPoint( QList<QVariant> input  )
  {

    QList<QVariant> value;
    value.clear();

    const QString wfnFileName=input.at(0).toString();
    const QString nuclearCriticalPointsFileName=input.at(1).toString();
    const qint64 nucleusA=input.at(2).toInt();
    const qint64 nucleusB=input.at(3).toInt();
    const QVector3D x0y0z0(
        input.at(4).toReal(),
        input.at(5).toReal(),
        input.at(6).toReal()
        );

    QTAIMWavefunction wfn;
    wfn.loadFromBinaryFile(wfnFileName);

    QList<QVector3D> nuclearCriticalPoints;
    QFile nuclearCriticalPointsFile(nuclearCriticalPointsFileName);
    nuclearCriticalPointsFile.open(QIODevice::ReadOnly);
    QDataStream nuclearCriticalPointsFileIn(&nuclearCriticalPointsFile);
    nuclearCriticalPointsFileIn >> nuclearCriticalPoints ;
    nuclearCriticalPointsFile.close();

    QList<QPair<QVector3D,qreal> > betaSpheres;
    for( qint64 i=0 ; i < nuclearCriticalPoints.length() ; ++i )
    {
      QPair<QVector3D,qreal> thisBetaSphere;
      thisBetaSphere.first=nuclearCriticalPoints.at(i);
      thisBetaSphere.second=0.1;
      betaSpheres.append(thisBetaSphere);
    }

    QTAIMWavefunctionEvaluator eval(wfn);

    QList<QVector3D> ncpList;

    QVector3D result;
    //    QTAIMODEIntegrator ode(eval,QTAIMODEIntegrator::CMBPMinusOneGradientInElectronDensity);
    QTAIMLSODAIntegrator ode(eval,QTAIMLSODAIntegrator::CMBPMinusOneGradientInElectronDensity);
    result=ode.integrate(x0y0z0);
    Matrix<qreal,3,1> xyz; xyz << result.x(), result.y(), result.z();

    if(
        !( QTAIMMathUtilities::signatureOfASymmetricThreeByThreeMatrix(
            eval.hessianOfElectronDensity(xyz)
            ) == -1 )
        || (eval.gradientOfElectronDensity(xyz)).norm() > SMALL_GRADIENT_NORM
        )
    {
      value.append(false);
      value.append(result.x());
      value.append(result.y());
      value.append(result.z());
      return value;
    }

    Matrix<qreal,3,3> eigenvectorsOfHessian;
    eigenvectorsOfHessian=QTAIMMathUtilities::eigenvectorsOfASymmetricThreeByThreeMatrix(
        eval.hessianOfElectronDensity(xyz)
        );
    Matrix<qreal,3,1> highestEigenvectorOfHessian;
    highestEigenvectorOfHessian <<
        eigenvectorsOfHessian(0,2),
        eigenvectorsOfHessian(1,2),
        eigenvectorsOfHessian(2,2);

    const qreal smallStep=0.01;

    QVector3D forwardStartingPoint( result.x() + smallStep*highestEigenvectorOfHessian(0),
                                    result.y() + smallStep*highestEigenvectorOfHessian(1),
                                    result.z() + smallStep*highestEigenvectorOfHessian(2) );

    QVector3D backwardStartingPoint( result.x() - smallStep*highestEigenvectorOfHessian(0),
                                     result.y() - smallStep*highestEigenvectorOfHessian(1),
                                     result.z() - smallStep*highestEigenvectorOfHessian(2) );

    //    QTAIMODEIntegrator forwardODE(eval,QTAIMODEIntegrator::SteepestAscentPathInElectronDensity);
    QTAIMLSODAIntegrator forwardODE(eval,QTAIMLSODAIntegrator::SteepestAscentPathInElectronDensity);
    forwardODE.setBetaSpheres( betaSpheres );
    QVector3D forwardEndpoint=forwardODE.integrate(forwardStartingPoint);
    QList<QVector3D> forwardPath=forwardODE.path();

    //    QTAIMODEIntegrator backwardODE(eval,QTAIMODEIntegrator::SteepestAscentPathInElectronDensity);
    QTAIMLSODAIntegrator backwardODE(eval,QTAIMLSODAIntegrator::SteepestAscentPathInElectronDensity);
    backwardODE.setBetaSpheres( betaSpheres );
    QVector3D backwardEndpoint=backwardODE.integrate(backwardStartingPoint);
    QList<QVector3D> backwardPath=backwardODE.path();

    qreal smallestDistance=HUGE_REAL_NUMBER;
    qint64 smallestDistanceIndex=0;

    for( qint64 n=0 ; n < wfn.numberOfNuclei()  ; ++n )
    {
      Matrix<qreal,3,1> a(forwardEndpoint.x(),forwardEndpoint.y(),forwardEndpoint.z());
      Matrix<qreal,3,1> b(wfn.xNuclearCoordinate(n), wfn.yNuclearCoordinate(n), wfn.zNuclearCoordinate(n));

      qreal distance=QTAIMMathUtilities::distance(a,b);

      if( distance < smallestDistance )
      {
        smallestDistance = distance;
        smallestDistanceIndex=n;
      }
    }
    qint64 forwardNucleusIndex=smallestDistanceIndex;

    smallestDistance=HUGE_REAL_NUMBER;
    smallestDistanceIndex=0;

    for( qint64 n=0 ; n < wfn.numberOfNuclei()  ; ++n )
    {
      Matrix<qreal,3,1> a(backwardEndpoint.x(),backwardEndpoint.y(),backwardEndpoint.z());
      Matrix<qreal,3,1> b(wfn.xNuclearCoordinate(n), wfn.yNuclearCoordinate(n), wfn.zNuclearCoordinate(n));

      qreal distance=QTAIMMathUtilities::distance(a,b);

      if( distance < smallestDistance )
      {
        smallestDistance = distance;
        smallestDistanceIndex=n;
      }
    }
    qint64 backwardNucleusIndex=smallestDistanceIndex;

    bool bondPathConnectsPair;
    if( (forwardNucleusIndex == nucleusA && backwardNucleusIndex == nucleusB) ||
        (forwardNucleusIndex == nucleusB && backwardNucleusIndex == nucleusA) )
    {
      bondPathConnectsPair=true;
    }
    else
    {
      bondPathConnectsPair=false;
    }

    if( bondPathConnectsPair )
    {
      value.append(true);
      value.append(nucleusA);
      value.append(nucleusB);
      value.append(result.x());
      value.append(result.y());
      value.append(result.z());
      Matrix<qreal,3,1> xyz_ ; xyz_ << result.x(),result.y(),result.z();
      value.append( eval.laplacianOfElectronDensity(xyz_) );
      value.append( QTAIMMathUtilities::ellipticityOfASymmetricThreeByThreeMatrix(
          eval.hessianOfElectronDensity(xyz_)
          )
                    );
      value.append( 1 + forwardPath.length() + 1 + backwardPath.length() + 1);
      value.append( forwardEndpoint.x() );
      for(qint64 i=forwardPath.length() - 1 ; i >= 0 ; --i)
      {
        value.append( forwardPath.at(i).x() );
      }
      value.append(result.x());
      for(qint64 i=0; i < backwardPath.length() ; ++i)
      {
        value.append( backwardPath.at(i).x() );
      }
      value.append( backwardEndpoint.x() );
      value.append( forwardEndpoint.y() );
      for(qint64 i=forwardPath.length() - 1 ; i >= 0 ; --i)
      {
        value.append( forwardPath.at(i).y() );
      }
      value.append(result.y());
      for(qint64 i=0; i < backwardPath.length() ; ++i)
      {
        value.append( backwardPath.at(i).y() );
      }
      value.append( backwardEndpoint.y() );
      value.append( forwardEndpoint.z() );
      for(qint64 i=forwardPath.length() - 1 ; i >= 0 ; --i)
      {
        value.append( forwardPath.at(i).z() );
      }
      value.append(result.z());
      for(qint64 i=0; i < backwardPath.length() ; ++i)
      {
        value.append( backwardPath.at(i).z() );
      }
      value.append( backwardEndpoint.z() );

    }
    else
    {
      value.append(false);
      // for debugging
      value.append(result.x());
      value.append(result.y());
      value.append(result.z());
    }

    return value;
  }

  QList<QVariant> QTAIMLocateElectronDensitySink( QList<QVariant> input  )
  {
    qint64 counter=0;
    const QString fileName=input.at(counter).toString(); counter++;
    //    const qint64 nucleus=input.at(counter).toInt(); counter++
    qreal x0=input.at(counter).toReal(); counter++;
    qreal y0=input.at(counter).toReal(); counter++;
    qreal z0=input.at(counter).toReal(); counter++;

    const QVector3D x0y0z0(x0,y0,z0);

    QTAIMWavefunction wfn;
    wfn.loadFromBinaryFile(fileName);

    QTAIMWavefunctionEvaluator eval(wfn);

    bool correctSignature;
    QVector3D result;

    Matrix<qreal,3,1> xyz; xyz << x0, y0, z0;
    if( eval.electronDensity( xyz ) < 1.e-1 )
    {
      correctSignature=false;
    }
    else
    {
      //      QTAIMODEIntegrator ode(eval,QTAIMODEIntegrator::CMBPMinusThreeGradientInElectronDensityLaplacian);
      QTAIMLSODAIntegrator ode(eval,QTAIMLSODAIntegrator::CMBPMinusThreeGradientInElectronDensityLaplacian);
      result=ode.integrate(x0y0z0);

      Matrix<qreal,3,1> xyz_; xyz_ << result.x(), result.y(), result.z();

      if( eval.electronDensity(xyz_) > 1.e-1 &&
          eval.gradientOfElectronDensityLaplacian(xyz_).norm() < 1.e-3 )
      {
        if(
            QTAIMMathUtilities::signatureOfASymmetricThreeByThreeMatrix(
                eval.hessianOfElectronDensityLaplacian(xyz_)
                ) == -3
            )
        {
          correctSignature=true;
        }
        else
        {
          correctSignature=false;
        }
      }
      else
      {
        correctSignature=false;
      }
    }

    QList<QVariant> value;
    if( correctSignature )
    {
      value.append(correctSignature);
      value.append(result.x());
      value.append(result.y());
      value.append(result.z());
    }
    else
    {
      value.append(false);
    }

    return value;

  }

  QList<QVariant> QTAIMLocateElectronDensitySource( QList<QVariant> input  )
  {
    qint64 counter=0;
    const QString fileName=input.at(counter).toString(); counter++;
    //    const qint64 nucleus=input.at(counter).toInt(); counter++
    qreal x0=input.at(counter).toReal(); counter++;
    qreal y0=input.at(counter).toReal(); counter++;
    qreal z0=input.at(counter).toReal(); counter++;

    const QVector3D x0y0z0(x0,y0,z0);

    QTAIMWavefunction wfn;
    wfn.loadFromBinaryFile(fileName);

    QTAIMWavefunctionEvaluator eval(wfn);

    bool correctSignature;
    QVector3D result;

    Matrix<qreal,3,1> xyz; xyz << x0, y0, z0;
    if( eval.electronDensity( xyz ) < 1.e-1 )
    {
      correctSignature=false;
    }
    else
    {
      //      QTAIMODEIntegrator ode(eval,QTAIMODEIntegrator::CMBPPlusThreeGradientInElectronDensityLaplacian);
      QTAIMLSODAIntegrator ode(eval,QTAIMLSODAIntegrator::CMBPPlusThreeGradientInElectronDensityLaplacian);
      result=ode.integrate(x0y0z0);

      Matrix<qreal,3,1> xyz_; xyz_ << result.x(), result.y(), result.z();

      if( eval.electronDensity(xyz_) > 1.e-1 &&
          eval.gradientOfElectronDensityLaplacian(xyz_).norm() < 1.e-3 )
      {
        if(
            QTAIMMathUtilities::signatureOfASymmetricThreeByThreeMatrix(
                eval.hessianOfElectronDensityLaplacian(xyz_)
                ) == 3
            )
        {
          correctSignature=true;
        }
        else
        {
          correctSignature=false;
        }
      }
      else
      {
        correctSignature=false;
      }
    }

    QList<QVariant> value;
    if( correctSignature )
    {
      value.append(correctSignature);
      value.append(result.x());
      value.append(result.y());
      value.append(result.z());
    }
    else
    {
      value.append(false);
    }

    return value;

  }

  QTAIMCriticalPointLocator::QTAIMCriticalPointLocator( QTAIMWavefunction &wfn)
  {
    m_wfn=&wfn;

    m_nuclearCriticalPoints.empty();
    m_bondCriticalPoints.empty();
    m_ringCriticalPoints.empty();
    m_cageCriticalPoints.empty();

    m_laplacianAtBondCriticalPoints.empty();
    m_ellipticityAtBondCriticalPoints.empty();

    m_bondPaths.empty();
    m_bondedAtoms.empty();

    m_electronDensitySources.empty();
    m_electronDensitySinks.empty();

  }

  void QTAIMCriticalPointLocator::locateNuclearCriticalPoints()
  {

    QString tempFileName=QTAIMCriticalPointLocator::temporaryFileName();

    QList<QList<QVariant> > inputList;

    const qint64 numberOfNuclei = m_wfn->numberOfNuclei();

    for( qint64 n=0 ; n < numberOfNuclei ; ++n)
    {
      QList<QVariant> input;
      input.append( tempFileName );
      input.append( n );
      input.append( m_wfn->xNuclearCoordinate(n) );
      input.append( m_wfn->yNuclearCoordinate(n) );
      input.append( m_wfn->zNuclearCoordinate(n) );

      inputList.append(input);
    }

    m_wfn->saveToBinaryFile(tempFileName);

    QProgressDialog dialog;
    dialog.setWindowTitle("QTAIM");
    dialog.setLabelText(QString("Nuclear Critical Points Search"));

    QFutureWatcher<void> futureWatcher;
    QObject::connect(&futureWatcher, SIGNAL(finished()), &dialog, SLOT(reset()));
    QObject::connect(&dialog, SIGNAL(canceled()), &futureWatcher, SLOT(cancel()));
    QObject::connect(&futureWatcher, SIGNAL(progressRangeChanged(int,int)), &dialog, SLOT(setRange(int,int)));
    QObject::connect(&futureWatcher, SIGNAL(progressValueChanged(int)), &dialog, SLOT(setValue(int)));

    QFuture<QList<QVariant> > future=QtConcurrent::mapped(inputList, QTAIMLocateNuclearCriticalPoint);
    futureWatcher.setFuture(future);
    dialog.exec();
    futureWatcher.waitForFinished();

    QList<QList<QVariant> > results;
    if( futureWatcher.future().isCanceled() )
    {
      results.clear();
    }
    else
    {
      results=future.results();
    }

    QFile file;
    file.remove(tempFileName);

    for( qint64 n=0 ; n < results.length() ; ++n )
    {

      bool correctSignature = results.at(n).at(0).toBool();

      if (correctSignature)
      {

        QVector3D result(
          results.at(n).at(1).toReal(),
          results.at(n).at(2).toReal(),
          results.at(n).at(3).toReal()
        );

        m_nuclearCriticalPoints.append( result );
      }

    }

  }

  void QTAIMCriticalPointLocator::locateBondCriticalPoints()
  {

    if( m_nuclearCriticalPoints.length() < 1 )
    {
      return;
    }

    const qint64 numberOfNuclei = m_wfn->numberOfNuclei();

    if( numberOfNuclei < 2)
    {
      return;
    }

    QString tempFileName=QTAIMCriticalPointLocator::temporaryFileName();

    QString nuclearCriticalPointsFileName=QTAIMCriticalPointLocator::temporaryFileName();
    QFile nuclearCriticalPointsFile(nuclearCriticalPointsFileName);
    nuclearCriticalPointsFile.open(QIODevice::WriteOnly);
    QDataStream nuclearCriticalPointsOut(&nuclearCriticalPointsFile);
    nuclearCriticalPointsOut << m_nuclearCriticalPoints;
    nuclearCriticalPointsFile.close();

    QList<QList<QVariant> > inputList;

    for( qint64 M=0 ; M < numberOfNuclei - 1 ; ++M )
    {
      for( qint64 N=M+1 ; N < numberOfNuclei ; ++N )
      {

        const qreal distanceCutoff = 8.0 ;

        Matrix<qreal,3,1> a;
        Matrix<qreal,3,1> b;

        a << m_wfn->xNuclearCoordinate(M), m_wfn->yNuclearCoordinate(M), m_wfn->zNuclearCoordinate(M) ;
        b << m_wfn->xNuclearCoordinate(N), m_wfn->yNuclearCoordinate(N), m_wfn->zNuclearCoordinate(N) ;

        if( QTAIMMathUtilities::distance(a,b) < distanceCutoff )
        {
          QVector3D x0y0z0( ( m_wfn->xNuclearCoordinate(M) + m_wfn->xNuclearCoordinate(N) ) / 2.0 ,
                            ( m_wfn->yNuclearCoordinate(M) + m_wfn->yNuclearCoordinate(N) ) / 2.0,
                            ( m_wfn->zNuclearCoordinate(M) + m_wfn->zNuclearCoordinate(N) ) / 2.0 );

          QList<QVariant> input;
          input.append( tempFileName );
          input.append( nuclearCriticalPointsFileName );
          input.append( M );
          input.append( N );
          input.append( x0y0z0.x() );
          input.append( x0y0z0.y() );
          input.append( x0y0z0.z() );

          inputList.append(input);
        }
      } // end N
    } // end M

    m_wfn->saveToBinaryFile(tempFileName);

    QProgressDialog dialog;
    dialog.setWindowTitle("QTAIM");
    dialog.setLabelText(QString("Bond Critical Points Search"));

    QFutureWatcher<void> futureWatcher;
    QObject::connect(&futureWatcher, SIGNAL(finished()), &dialog, SLOT(reset()));
    QObject::connect(&dialog, SIGNAL(canceled()), &futureWatcher, SLOT(cancel()));
    QObject::connect(&futureWatcher, SIGNAL(progressRangeChanged(int,int)), &dialog, SLOT(setRange(int,int)));
    QObject::connect(&futureWatcher, SIGNAL(progressValueChanged(int)), &dialog, SLOT(setValue(int)));

    QFuture<QList<QVariant> > future=QtConcurrent::mapped(inputList, QTAIMLocateBondCriticalPoint);;
    futureWatcher.setFuture(future);
    dialog.exec();
    futureWatcher.waitForFinished();

    QList<QList<QVariant> > results;
    if( futureWatcher.future().isCanceled() )
    {
      results.clear();
    }
    else
    {
      results=future.results();
    }

    QFile file;
    file.remove(tempFileName);
    file.remove(nuclearCriticalPointsFileName);

    for( qint64 i=0 ; i < results.length() ; ++i )
    {
      QList<QVariant> thisCriticalPoint=results.at(i);

      bool success=thisCriticalPoint.at(0).toBool();

      if(success)
      {
        QPair<qint64,qint64> bondedAtoms_;
        bondedAtoms_.first=thisCriticalPoint.at(1).toInt();
        bondedAtoms_.second=thisCriticalPoint.at(2).toInt();
        m_bondedAtoms.append( bondedAtoms_ );

        QVector3D coordinates(thisCriticalPoint.at(3).toReal(),
                              thisCriticalPoint.at(4).toReal(),
                              thisCriticalPoint.at(5).toReal());

        m_bondCriticalPoints.append( coordinates );

        m_laplacianAtBondCriticalPoints.append(thisCriticalPoint.at(6).toReal());
        m_ellipticityAtBondCriticalPoints.append(thisCriticalPoint.at(7).toReal());
        qint64 pathLength=thisCriticalPoint.at(8).toInt();

        QList<QVector3D> bondPath;
        for( qint64 j=0 ; j < pathLength ; ++j )
        {
          QVector3D pathPoint(thisCriticalPoint.at(9 + j                ).toReal(),
                              thisCriticalPoint.at(9 + j +   pathLength ).toReal(),
                              thisCriticalPoint.at(9 + j + 2*pathLength ).toReal());

          bondPath.append(pathPoint);
        }

        m_bondPaths.append(bondPath);
      }

    }

  }

  void QTAIMCriticalPointLocator::locateElectronDensitySources()
  {

    QString tempFileName=QTAIMCriticalPointLocator::temporaryFileName();

    QList<QList<QVariant> > inputList;

    qreal xmin,ymin,zmin;
    qreal xmax,ymax,zmax;
    qreal xstep,ystep,zstep;

    // TODO: if only we were using Eigen data structures...
    QList<qreal> xNuclearCoordinates;
    QList<qreal> yNuclearCoordinates;
    QList<qreal> zNuclearCoordinates;

    for( qint64 i=0; i < m_wfn->numberOfNuclei() ; ++i )
    {
      xNuclearCoordinates.append( m_wfn->xNuclearCoordinate(i) );
      yNuclearCoordinates.append( m_wfn->yNuclearCoordinate(i) );
      zNuclearCoordinates.append( m_wfn->zNuclearCoordinate(i) );
    }

    xmin=xNuclearCoordinates.first();
    xmax=xNuclearCoordinates.first();
    for( qint64 i=1 ; i < m_wfn->numberOfNuclei() ; ++i)
    {
      if( xNuclearCoordinates.at(i) < xmin )
      {
        xmin=xNuclearCoordinates.at(i);
      }
      if( xNuclearCoordinates.at(i) > xmax )
      {
        xmax=xNuclearCoordinates.at(i);
      }
    }

    ymin=yNuclearCoordinates.first();
    ymax=yNuclearCoordinates.first();
    for( qint64 i=1 ; i < yNuclearCoordinates.count() ; ++i)
    {
      if( yNuclearCoordinates.at(i) < ymin )
      {
        ymin=yNuclearCoordinates.at(i);
      }
      if( yNuclearCoordinates.at(i) > ymax )
      {
        ymax=yNuclearCoordinates.at(i);
      }
    }

    zmin=zNuclearCoordinates.first();
    zmax=zNuclearCoordinates.first();
    for( qint64 i=1 ; i < zNuclearCoordinates.count() ; ++i)
    {
      if( zNuclearCoordinates.at(i) < zmin )
      {
        zmin=zNuclearCoordinates.at(i);
      }
      if( zNuclearCoordinates.at(i) > zmax )
      {
        zmax=zNuclearCoordinates.at(i);
      }
    }

    xmin= -2.0 + xmin;
    ymin= -2.0 + ymin;
    zmin= -2.0 + zmin;

    xmax = 2.0 + xmax;
    ymax = 2.0 + ymax;
    zmax = 2.0 + zmax;

    xstep=ystep=zstep= 0.5;

    for( qreal x=xmin ; x < xmax+xstep ; x=x+xstep)
    {
      for( qreal y=ymin ; y < ymax+ystep ; y=y+ystep)
      {
        for( qreal z=zmin ; z < zmax+zstep ; z=z+zstep)
        {
          QList<QVariant> input;
          input.append( tempFileName );
//          input.append( n );
          input.append( x );
          input.append( y );
          input.append( z );

          inputList.append(input);
        }
      }
    }

    m_wfn->saveToBinaryFile(tempFileName);

    QProgressDialog dialog;
    dialog.setWindowTitle("QTAIM");
    dialog.setLabelText(QString("Electron Density Sources Search"));

    QFutureWatcher<void> futureWatcher;
    QObject::connect(&futureWatcher, SIGNAL(finished()), &dialog, SLOT(reset()));
    QObject::connect(&dialog, SIGNAL(canceled()), &futureWatcher, SLOT(cancel()));
    QObject::connect(&futureWatcher, SIGNAL(progressRangeChanged(int,int)), &dialog, SLOT(setRange(int,int)));
    QObject::connect(&futureWatcher, SIGNAL(progressValueChanged(int)), &dialog, SLOT(setValue(int)));

    QFuture<QList<QVariant> > future=QtConcurrent::mapped(inputList, QTAIMLocateElectronDensitySource );
    futureWatcher.setFuture(future);
    dialog.exec();
    futureWatcher.waitForFinished();

    QList<QList<QVariant> > results;
    if( futureWatcher.future().isCanceled() )
    {
      results.clear();
    }
    else
    {
      results=future.results();
    }

    QFile file;
    file.remove(tempFileName);

    for( qint64 n=0 ; n < results.length() ; ++n )
    {

      qint64 counter=0;
      bool correctSignature = results.at(n).at(counter).toBool(); counter++;

      if( correctSignature )
      {
        qreal x=results.at(n).at(counter).toReal(); counter++;
        qreal y=results.at(n).at(counter).toReal(); counter++;
        qreal z=results.at(n).at(counter).toReal(); counter++;

        if( (xmin < x && x < xmax) &&
            (ymin < y && y < ymax) &&
            (zmin < z && z < zmax) )
        {
          QVector3D result(x,y,z);

          qreal smallestDistance=HUGE_REAL_NUMBER;

          for(qint64 i=0 ; i < m_electronDensitySources.length() ; ++i )
          {

            Matrix<qreal,3,1> a(x,y,z);
            Matrix<qreal,3,1> b(m_electronDensitySources.at(i).x(),
                                m_electronDensitySources.at(i).y(),
                                m_electronDensitySources.at(i).z());

            qreal distance=QTAIMMathUtilities::distance(a,b);

            if( distance < smallestDistance )
            {
              smallestDistance=distance;
            }

          }

          if( smallestDistance > 1.e-2 )
          {
            m_electronDensitySources.append( result );
          }
        }
      }
    }
//    qDebug() << "SOURCES" << m_electronDensitySources;
  }

  void QTAIMCriticalPointLocator::locateElectronDensitySinks()
  {

    QString tempFileName=QTAIMCriticalPointLocator::temporaryFileName();

    QList<QList<QVariant> > inputList;

    qreal xmin,ymin,zmin;
    qreal xmax,ymax,zmax;
    qreal xstep,ystep,zstep;

    // TODO: if only we were using Eigen data structures...
    QList<qreal> xNuclearCoordinates;
    QList<qreal> yNuclearCoordinates;
    QList<qreal> zNuclearCoordinates;

    for( qint64 i=0; i < m_wfn->numberOfNuclei() ; ++i )
    {
      xNuclearCoordinates.append( m_wfn->xNuclearCoordinate(i) );
      yNuclearCoordinates.append( m_wfn->yNuclearCoordinate(i) );
      zNuclearCoordinates.append( m_wfn->zNuclearCoordinate(i) );
    }

    xmin=xNuclearCoordinates.first();
    xmax=xNuclearCoordinates.first();
    for( qint64 i=1 ; i < m_wfn->numberOfNuclei() ; ++i)
    {
      if( xNuclearCoordinates.at(i) < xmin )
      {
        xmin=xNuclearCoordinates.at(i);
      }
      if( xNuclearCoordinates.at(i) > xmax )
      {
        xmax=xNuclearCoordinates.at(i);
      }
    }

    ymin=yNuclearCoordinates.first();
    ymax=yNuclearCoordinates.first();
    for( qint64 i=1 ; i < yNuclearCoordinates.count() ; ++i)
    {
      if( yNuclearCoordinates.at(i) < ymin )
      {
        ymin=yNuclearCoordinates.at(i);
      }
      if( yNuclearCoordinates.at(i) > ymax )
      {
        ymax=yNuclearCoordinates.at(i);
      }
    }

    zmin=zNuclearCoordinates.first();
    zmax=zNuclearCoordinates.first();
    for( qint64 i=1 ; i < zNuclearCoordinates.count() ; ++i)
    {
      if( zNuclearCoordinates.at(i) < zmin )
      {
        zmin=zNuclearCoordinates.at(i);
      }
      if( zNuclearCoordinates.at(i) > zmax )
      {
        zmax=zNuclearCoordinates.at(i);
      }
    }

    xmin= -2.0 + xmin;
    ymin= -2.0 + ymin;
    zmin= -2.0 + zmin;

    xmax = 2.0 + xmax;
    ymax = 2.0 + ymax;
    zmax = 2.0 + zmax;

    xstep=ystep=zstep= 0.5;

    for( qreal x=xmin ; x < xmax+xstep ; x=x+xstep)
    {
      for( qreal y=ymin ; y < ymax+ystep ; y=y+ystep)
      {
        for( qreal z=zmin ; z < zmax+zstep ; z=z+zstep)
        {
          QList<QVariant> input;
          input.append( tempFileName );
//          input.append( n );
          input.append( x );
          input.append( y );
          input.append( z );

          inputList.append(input);
        }
      }
    }

    m_wfn->saveToBinaryFile(tempFileName);

    QProgressDialog dialog;
    dialog.setWindowTitle("QTAIM");
    dialog.setLabelText(QString("Electron Density Sinks Search"));

    QFutureWatcher<void> futureWatcher;
    QObject::connect(&futureWatcher, SIGNAL(finished()), &dialog, SLOT(reset()));
    QObject::connect(&dialog, SIGNAL(canceled()), &futureWatcher, SLOT(cancel()));
    QObject::connect(&futureWatcher, SIGNAL(progressRangeChanged(int,int)), &dialog, SLOT(setRange(int,int)));
    QObject::connect(&futureWatcher, SIGNAL(progressValueChanged(int)), &dialog, SLOT(setValue(int)));

    QFuture<QList<QVariant> > future=QtConcurrent::mapped(inputList, QTAIMLocateElectronDensitySink );
    futureWatcher.setFuture(future);
    dialog.exec();
    futureWatcher.waitForFinished();

    QList<QList<QVariant> > results;
    if( futureWatcher.future().isCanceled() )
    {
      results.clear();
    }
    else
    {
      results=future.results();
    }

    QFile file;
    file.remove(tempFileName);

    for( qint64 n=0 ; n < results.length() ; ++n )
    {

      qint64 counter=0;
      bool correctSignature = results.at(n).at(counter).toBool(); counter++;

      if( correctSignature )
      {
        qreal x=results.at(n).at(counter).toReal(); counter++;
        qreal y=results.at(n).at(counter).toReal(); counter++;
        qreal z=results.at(n).at(counter).toReal(); counter++;

        if( (xmin < x && x < xmax) &&
            (ymin < y && y < ymax) &&
            (zmin < z && z < zmax) )
        {
          QVector3D result(x,y,z);

          qreal smallestDistance=HUGE_REAL_NUMBER;

          for(qint64 i=0 ; i < m_electronDensitySinks.length() ; ++i )
          {

            Matrix<qreal,3,1> a(x,y,z);
            Matrix<qreal,3,1> b(m_electronDensitySinks.at(i).x(),
                                m_electronDensitySinks.at(i).y(),
                                m_electronDensitySinks.at(i).z());

            qreal distance=QTAIMMathUtilities::distance(a,b);

            if( distance < smallestDistance )
            {
              smallestDistance=distance;
            }

          }

          if( smallestDistance > 1.e-2 )
          {
            m_electronDensitySinks.append( result );
          }
        }
      }
    }
//    qDebug() << "SINKS" << m_electronDensitySinks;
  }

  QString QTAIMCriticalPointLocator::temporaryFileName()
  {
    QTemporaryFile temporaryFile;
    temporaryFile.open();
    QString tempFileName=temporaryFile.fileName();
    temporaryFile.close();
    temporaryFile.remove();

    // wait for temporary file to be deleted
    QDir dir;
    do
    {
      // Nothing
    } while ( dir.exists(tempFileName) );

    return tempFileName;
  }

} // namespace QtPlugins
} // namespace Avogadro


#include <iostream>
#include <random>

#include <boost/tuple/tuple.hpp>

#include "pca.h"
#include "utils.h"

std::string word_vector_file;
std::vector<std::string> search_words;

// main function arguments

void printHelp() {
  std::cout << "c++ pca implementation \n\n";
  std::cout << "Options:\n";
  std::cout << "Parameters for drawing:\n";
  std::cout << "\t-word-vector <file>\n";
  std::cout << "\t\tUse word vector stored in <file>\n";
  std::cout << "\t-draw-words <words>\n";
  std::cout << "\t\tChoose <words> to be drawed\n";
  std::cout << "\nExamples:\n";
  std::cout << "./pca -word-vector data.txt -draw-words king queen prince princess\n";

}

int ArgPos(char *str, int argc, char **argv) {
  int i;
  std::string s_str;
  std::string s_argv[argc];

  for(i = 1; i < argc; i++) s_argv[i] = argv[i];
  s_str = str;

  for(i = 1; i < argc; i++) if(s_str == s_argv[i]) {
      if(i == argc - 1) {
	std::cout << "Argument missing for " << s_str << std::endl;;
	exit(1);
      }
      return i;
  }
  return -1;
}

void ArgPass(int argc, char **argv) {
  int i;
  if ((i = ArgPos((char *)"-word-vector", argc, argv)) > 0) word_vector_file = argv[i + 1];
  if ((i = ArgPos((char *)"-draw-words", argc, argv)) > 0) {
    for(int j = 0; j < argc - 4; j++)
      {
	search_words.push_back(argv[i + 1 + j]);
      }
  }
}

// main function arguments

int main(int argc, char **argv) {
  std::ofstream fout;

  std::string line;
  std::vector<std::string> word;

  std::vector< std::vector<double> > data;
  std::vector<std::string> label;

  fout.open("temp.dat", std::ofstream::out);

  if(argc == 1) {
    printHelp();
    return 0;
  }

  ArgPass(argc, argv);

  std::ifstream inFile;
  inFile.open(word_vector_file, std::ifstream::in);
  if(inFile.fail()) {
    std::cout << "Word vector file not found!\n";
    exit(1);
  }

  std::getline(inFile, line);
  split(line, word);
  int num_records = atoi(word[0].c_str());
  int num_variables = atoi(word[1].c_str());
  stats::pca pca(num_variables);
  pca.set_do_bootstrap(true, 100);

  for(int i = 0; i < num_records; ++i) {
    word.clear();
    std::getline(inFile,line);
    split(line, word);
    std::vector<double> record(num_variables);
    for(int j = 0; j < num_variables; j++) record[j] = atof(word[j+1].c_str());

    pca.add_record(record);
    label.push_back(word[0]);
    data.push_back(record);
  }

  pca.solve();

  /*
  std::cout<<"Energy = "<<pca.get_energy()<<" ("<<
    stats::utils::get_sigma(pca.get_energy_boot())<<")"<<std::endl;;

  const auto eigenvalues = pca.get_eigenvalues();
  std::cout<<"First three eigenvalues = "<<eigenvalues[0]<<", "
      <<eigenvalues[1]<<", "
	   <<eigenvalues[2]<<std::endl;;

  std::cout<<"Orthogonal Check = "<<pca.check_eigenvectors_orthogonal()<<std::endl;;
  std::cout<<"Projection Check = "<<pca.check_projection_accurate()<<std::endl;;
  */

  //pca.save("pca_results");

  std::vector<double> principal_1 = pca.get_principal(0);
  std::vector<double> principal_2 = pca.get_principal(1);

  // Very simple use case of gnuplot

  std::vector< boost::tuple<std::string, double, double> > xy_pts_A;

  double x, y;

  for(int i = 0; i < num_records; i++) {
    x = 0;
    y = 0;
    for(int j = 0; j < num_variables; j++) {
      x += principal_1[j] * data[i][j];
      y += principal_2[j] * data[i][j];
    }
    //xy_pts_A.push_back(boost::make_tuple(label[i],x,y));
  }

  for(int i = 0; i < num_records; i++) {
    for(auto& k: search_words) {
      if( label[i] == (k) ) {
	x = 0;
	y = 0;
	for(int k = 0; k < num_variables; k++) {
	  x += principal_1[k] * data[i][k];
	  y += principal_2[k] * data[i][k];
	}
	xy_pts_A.push_back(boost::make_tuple(label[i],x,y));
      }
    }
  }

  for(const auto& i : xy_pts_A) {
    fout << boost::get<0>(i) << " " << boost::get<1>(i)<< " " << boost::get<2>(i) << std::endl;
  }

  fout.open("script.gp",std::ofstream::out);

  fout << "set term png" << std::endl;
  fout << "set output \"sample.png\"" << std::endl;
  fout << "plot 'temp.dat' using 2:3:1 with labels offset 1 title 'word vectors'" << std::endl;

  fout.close();
  return 0;
}


//#define CGAL_INTERSECTION_MAP_FOR_SUPPORTING_CIRCLES

#include <CGAL/Cartesian.h>
#include <CGAL/Handle_for.h>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/MP_Float.h>
#include <CGAL/Gmpz.h>
#include <CGAL/Gmpq.h>
#include <CGAL/Algebraic_kernel_for_circles_2_2.h>
#include <CGAL/intersections.h>
#include <CGAL/Circular_kernel_2.h>
#include <CGAL/Arr_circular_arc_traits_2.h>
#include <CGAL/Lazy_circular_kernel_2.h>
#include <CGAL/Filtered_bbox_circular_kernel_2.h>
#include <CGAL/Arrangement_2.h>
#include <CGAL/Arr_naive_point_location.h>
#include <CGAL/Arr_circular_line_arc_traits_2.h>
#include <CGAL/Timer.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <CGAL/IO/Dxf_variant_reader.h>
#include <fstream>
#include <iomanip>

#include <boost/type_traits.hpp>

// CIRCULAR KERNEL TYPEDEFS
//typedef CGAL::MP_Float RT;
//typedef CGAL::Quotient<RT> NT1;
typedef CGAL::Gmpz RT;
typedef CGAL::Gmpq NT1;
typedef CGAL::Cartesian<NT1> Linear_k1;
typedef CGAL::Algebraic_kernel_for_circles_2_2<NT1> Algebraic_k1;
typedef CGAL::Circular_kernel_2<Linear_k1, Algebraic_k1> CircularKernel;
typedef CGAL::Arr_circular_arc_traits_2<CircularKernel> CircularK_CA_Traits;
typedef CircularKernel::Circular_arc_2 CircularKArc;
typedef std::vector<CircularKArc> CircularKArcContainer;
typedef CircularKernel::Circular_arc_2 Circular_arc_2;
typedef CircularKernel::Line_arc_2 Line_arc_2;
typedef CGAL::Arr_circular_line_arc_traits_2<CircularKernel>   CircularK_Variant_Traits;
typedef boost::variant< Circular_arc_2, Line_arc_2 > CircularKVarArc;
typedef std::vector<CircularKVarArc> CircularKVarArcContainer;

// LAZY KERNEL TYPEDEFS
typedef CGAL::Interval_nt_advanced NT3;
typedef CGAL::Cartesian<NT3> Linear_k3;
typedef CGAL::Algebraic_kernel_for_circles_2_2<NT3> Algebraic_k3;
typedef CGAL::Circular_kernel_2 <Linear_k3,Algebraic_k3> CK3_;
typedef CGAL::Lazy_circular_kernel_2<CircularKernel,CK3_> LazyCurvedK;
typedef CGAL::Arr_circular_arc_traits_2<LazyCurvedK> LazyCurvedK_CA_Traits;
typedef LazyCurvedK::Circular_arc_2 LazyArc;
typedef std::vector<LazyArc> LazyArcContainer;
typedef LazyCurvedK::Circular_arc_2 Circular_arc_3;
typedef LazyCurvedK::Line_arc_2 Line_arc_3;
typedef boost::variant<Circular_arc_3,Line_arc_3 > LazyVarArc;
typedef std::vector<LazyVarArc> LazyVarContainer;
//~ typedef CGAL::Arr_circular_line_arc_traits_2<LazyCurvedK,Line_arc_3,Circular_arc_3> LazyCurvedK_Variant_Traits;
typedef CGAL::Arr_circular_line_arc_traits_2<LazyCurvedK> LazyCurvedK_Variant_Traits;

// BBOX TYPEDEFS
typedef CGAL::Filtered_bbox_circular_kernel_2<CircularKernel>
  BBCircularKernel ;
typedef CGAL::Arr_circular_arc_traits_2<BBCircularKernel>
  BBCircularKernel_CA_Traits;
typedef BBCircularKernel::Circular_arc_2
  BBCircularKernelArc;
typedef std::vector<BBCircularKernelArc>
  BBCircularKernelArcContainer;
typedef BBCircularKernel::Circular_arc_2
  Circular_arc_6;
typedef BBCircularKernel::Line_arc_2
  Line_arc_6;
typedef boost::variant<Circular_arc_6,Line_arc_6 >
  BBCircVarArc;
typedef std::vector<BBCircVarArc>
  BBCircVarContainer;
typedef CGAL::Arr_circular_line_arc_traits_2<BBCircularKernel>  BBCircVariantTraits;

// BBOX(LAZY)
typedef CGAL::Filtered_bbox_circular_kernel_2<LazyCurvedK>
  BBLazyKernel ;
typedef CGAL::Arr_circular_arc_traits_2<BBLazyKernel>
  BBLazyKernel_CA_Traits;
typedef BBLazyKernel::Circular_arc_2
  BBLazyKernelArc;
typedef std::vector<BBLazyKernelArc>
  BBLazyKernelArcContainer;
typedef BBLazyKernel::Circular_arc_2
  Circular_arc_lazybb;
typedef BBLazyKernel::Line_arc_2
  Line_arc_lazybb;
typedef boost::variant<Circular_arc_lazybb,Line_arc_lazybb >
  BBLazyVarArc;
typedef std::vector<BBLazyVarArc>
  BBLazyVarContainer;
typedef CGAL::Arr_circular_line_arc_traits_2<BBLazyKernel>  BBLazyVariantTraits;

template <class CK,class Traits,class ArcContainer>
void do_main(const char *s) {

  // TYPEDEFS
  typedef typename CK::Circular_arc_2      C2;
  typedef typename CK::Line_arc_2          L2;
  typedef typename CGAL::Arrangement_2<Traits>          Pmwx;
  typedef typename CGAL::Arr_naive_point_location<Pmwx> Point_location;

  // LOADING CURVES
  ArcContainer ac;
  std::ifstream fin;
  fin.open (s);
  CGAL::variant_load<CK, C2, L2>(
    fin, std::back_inserter(ac));
  fin.close();

  std::cout << "Size:" << ac.size() << std::endl;

  // BENCHMARKING
  Pmwx _pm;
  Point_location _pl(_pm);
  struct rusage before, after;
  struct timeval utime, stime;
  getrusage(RUSAGE_SELF,&before);
  insert(_pm,ac.begin(),ac.end(),boost::false_type());
  getrusage(RUSAGE_SELF,&after);
  timersub(&(after.ru_utime),&(before.ru_utime),&utime);
  timersub(&(after.ru_stime),&(before.ru_stime),&stime);
  std::cout<<"Time="<< utime.tv_sec<<"."<< std::setw(6) <<
  std::setfill('0')<< utime.tv_usec <<std::endl;

  std::cerr << utime.tv_sec << "." << std::setw(6) <<
  std::setfill('0')<< utime.tv_usec << std::endl;

  std::cout << "The arrangement size:" << std::endl
            << "   V = " << _pm.number_of_vertices()
            << ",  E = " << _pm.number_of_edges()
            << ",  F = " << _pm.number_of_faces() << std::endl;

}

template <class CK,class Traits,class ArcContainer>
void do_main(int k) {

  // TYPEDEFS
  typedef typename CK::Circular_arc_2      C2;
  typedef typename CK::Point_2 Point_2;
  typedef typename CK::FT FT;
  typedef typename CGAL::Arrangement_2<Traits>          Pmwx;
  typedef typename CGAL::Arr_naive_point_location<Pmwx> Point_location;

  // LOADING CURVES
  ArcContainer ac;

  double cx, cy;
  const FT rft(1.0);

  // DENSE
  if(k == 0) {
    for(cx = 0.0; cx <= 10.0; cx += 0.5) {
      for(cy = 0.0; cy <= 10.0; cy += 0.5) {
        ac.push_back(typename CK::Circular_arc_2 ( typename CK::Circle_2(Point_2(cx,cy),rft) ) );
      }
    }
  }

  // VERY DENSE
  if(k == 1) {
    for(cx = 0.0; cx <= 0.2; cx += 0.01) {
      for(cy = 0.0; cy <= 0.2; cy += 0.01) {
        ac.push_back(typename CK::Circular_arc_2 ( typename CK::Circle_2(Point_2(cx,cy),rft) ));
      }
    }
  }

  // ONE CIRCLE
  if(k == 2) {
    ac.push_back(typename CK::Circular_arc_2 ( typename CK::Circle_2(Point_2(0,0),5) ));
  }

  // RANDOM CASE
  if(k == 3) {
    CGAL::Random generatorOfgenerator;
    int random_seed = generatorOfgenerator.get_int(0, 123456);
    CGAL::Random theRandom(random_seed);
    for(int i=0; i<100; i++) {
      double x = theRandom.get_double(0.0,1.0);
      double y = theRandom.get_double(0.0,1.0);
      double r = theRandom.get_double(0.00001,1.0);
      ac.push_back(typename CK::Circular_arc_2 ( typename CK::Circle_2(Point_2(x,y),FT(r)) ));
    }
  }

  // SPARSE
  if(k == 4) {
    double h = (std::sqrt(3.0)+0.01);
    for(cx = 0.0; cx <= 40.4; cx += 2.01) {
      for(cy = h; cy <= h*20; cy += h*2) {
        ac.push_back(typename CK::Circular_arc_2 ( typename CK::Circle_2(Point_2(cx+1.0,cy),rft) ));
      }
      for(cy = 0.0; cy <= h*20.0; cy += h*2) {
        ac.push_back(typename CK::Circular_arc_2 ( typename CK::Circle_2(Point_2(cx,cy),rft)));
      }
    }
  }

  std::cout << "Size:" << ac.size() << std::endl;

  // BENCHMARKING
  Pmwx _pm;
  Point_location _pl(_pm);
  struct rusage before, after;
  struct timeval utime, stime;
  getrusage(RUSAGE_SELF,&before);
  insert(_pm,ac.begin(),ac.end(),boost::false_type());
  getrusage(RUSAGE_SELF,&after);
  timersub(&(after.ru_utime),&(before.ru_utime),&utime);
  timersub(&(after.ru_stime),&(before.ru_stime),&stime);
  std::cout<<"Time="<< utime.tv_sec<<"."<< std::setw(6) <<
  std::setfill('0')<< utime.tv_usec << std::endl;

  std::cerr << utime.tv_sec << "." << std::setw(6) <<
  std::setfill('0')<< utime.tv_usec << std::endl;

  std::cout << "The arrangement size:" << std::endl
            << "   V = " << _pm.number_of_vertices()
            << ",  E = " << _pm.number_of_edges()
            << ",  F = " << _pm.number_of_faces() << std::endl;
}

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

  const char* dxf_filename[] = { "DXF/51.dxf",
                                 "DXF/cad_l1.dxf",
                                 "DXF/cad_l2.dxf",
                                 "DXF/che_mod1.dxf",
                                 "DXF/CIOnZDraw.dxf",
                                 "DXF/mask1.dxf",
                                 "DXF/elekonta.dxf",
                                 "DXF/netlist_signal_1.dxf",
                                 "DXF/painttrack.dxf" };
  if(argc == 3) {
    int i = argv[1][0]-'0';
    int j = argv[2][0]-'0';
    if((j >= 0 && j < 9)) {
      if(i == 1) do_main<BBCircularKernel,BBCircVariantTraits, BBCircVarContainer>(dxf_filename[j]);
      if(i == 2) do_main<LazyCurvedK,LazyCurvedK_Variant_Traits, LazyVarContainer>(dxf_filename[j]);
      if(i == 3) do_main<CircularKernel,CircularK_Variant_Traits, CircularKVarArcContainer>(dxf_filename[j]);
      if(i == 4) do_main<BBLazyKernel,BBLazyVariantTraits, BBLazyVarContainer>(dxf_filename[j]);
      if((i >= 5) || (i <= 0)) std::cout << "INVALID PARAMETERS" << std::endl;
    } else {
      int k = -1;
      if(j == 9) k = 0;
      if(j == ('a'-'0')) k = 1;
      if(j == ('b'-'0')) k = 2;
      if(j == ('c'-'0')) k = 3;
      if(j == ('d'-'0')) k = 4;
      if(i == 1) do_main<BBCircularKernel,BBCircVariantTraits, BBCircVarContainer>(k);
      if(i == 2) do_main<LazyCurvedK,LazyCurvedK_Variant_Traits, LazyVarContainer>(k);
      if(i == 3) do_main<CircularKernel,CircularK_Variant_Traits, CircularKVarArcContainer>(k);
      if(i == 4) do_main<BBLazyKernel,BBLazyVariantTraits, BBLazyVarContainer>(k);
      if(i == 5) do_main<BBCircularKernel,BBCircularKernel_CA_Traits, BBCircularKernelArcContainer>(k);
      if(i == 6) do_main<LazyCurvedK,LazyCurvedK_CA_Traits, LazyArcContainer>(k);
      if(i == 7) do_main<CircularKernel,CircularK_CA_Traits, CircularKArcContainer>(k);
      if(i == 8) do_main<BBLazyKernel,BBLazyKernel_CA_Traits, BBLazyKernelArcContainer>(k);
    }
  } else std::cout << "INVALID PARAMETERS" << std::endl;

  return 0;
}


#include "Heuristics.hpp"
#include "DependencyHelpers.hpp"
#include "TsplpExceptions.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/topological_sort.hpp>
#include <boost/range/adaptor/reversed.hpp>

#include <unordered_set>

#include <xtensor/xmanipulation.hpp>

std::tuple<std::vector<std::vector<size_t>>, double> tsplp::ExploitFractionalSolution(
    xt::xarray<double> fractionalSolution, xt::xarray<double> weights,
    const xt::xtensor<size_t, 1>& startPositions, const xt::xtensor<size_t, 1>& endPositions,
    const DependencyGraph& dependencies, std::chrono::milliseconds timeout)
{
    const auto A = startPositions.size();
    assert(endPositions.size() == A);

    if (weights.dimension() == 2)
        weights = xt::repeat(xt::view(weights, xt::newaxis(), xt::all()), A, 0);

    const auto [heuristicPaths, _] = NearestInsertion((1 - fractionalSolution) * weights, startPositions, endPositions, dependencies, timeout);

    if (heuristicPaths.empty())
        return { heuristicPaths, 0 };

    double sum = 0.0;
    for (size_t a = 0; a < A; ++a)
        for (size_t i = 1; i < heuristicPaths[a].size(); ++i)
            sum += weights(a, heuristicPaths[a][i - 1], heuristicPaths[a][i]);

    return { heuristicPaths, sum };
}

std::tuple<std::vector<std::vector<size_t>>, double> tsplp::NearestInsertion(
    xt::xarray<double> weights, const xt::xtensor<size_t, 1>& startPositions, const xt::xtensor<size_t, 1>& endPositions,
    const DependencyGraph& dependencies, std::chrono::milliseconds timeout)
{
    const auto startTime = std::chrono::steady_clock::now();

    const auto A = startPositions.size();
    assert(endPositions.size() == A);

    if (weights.dimension() == 2)
        weights = xt::repeat(xt::view(weights, xt::newaxis(), xt::all()), A, 0);

    assert(weights.dimension() == 3);

    const auto N = weights.shape(1);
    assert(weights.shape(2) == N);

    boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS> dependencyGraph(N);
    boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> dependencyGraphUndirected(N);
    for (const auto& [u, v] : dependencies.GetArcs())
    {
        add_edge(u, v, dependencyGraph);
        add_edge(u, v, dependencyGraphUndirected);
    }

    for (size_t a = 0; a < A; ++a)
    {
        add_edge(startPositions[a], endPositions[a], dependencyGraph);
        add_edge(startPositions[a], endPositions[a], dependencyGraphUndirected);
    }

    assert(num_edges(dependencyGraph) >= size(dependencies.GetArcs()));

    std::vector<size_t> componentIds(N);
    const auto numberOfComponents = boost::connected_components(dependencyGraphUndirected, componentIds.data());

    std::vector<size_t> order;
    boost::topological_sort(dependencyGraph, std::back_inserter(order));

    std::vector<size_t> component2AgentMap(numberOfComponents, A);

    auto paths = std::vector<std::vector<size_t>>(A);
    double cost = 0;
    for (size_t a = 0; a < A; ++a)
    {
        assert(componentIds[startPositions[a]] == componentIds[endPositions[a]]);

        paths[a].push_back(startPositions[a]);
        paths[a].push_back(endPositions[a]);

        cost += weights(a, startPositions[a], endPositions[a]);

        if (component2AgentMap[componentIds[startPositions[a]]] != A)
            throw IncompatibleDependenciesException();

        component2AgentMap[componentIds[startPositions[a]]] = a;
    }

    std::vector<size_t> lastInsertPositionOfComponent(numberOfComponents, 0);

    for (const auto n : boost::adaptors::reverse(order))
    {
        if (std::chrono::steady_clock::now() >= startTime + timeout)
            return { std::vector<std::vector<size_t>>{}, 0 };

        if (std::find(startPositions.begin(), startPositions.end(), n) != startPositions.end()
            || std::find(endPositions.begin(), endPositions.end(), n) != endPositions.end())
            continue;

        const auto comp = componentIds[n];

        auto minDeltaCost = std::numeric_limits<double>::max();
        auto minA = std::numeric_limits<size_t>::max();
        auto minI = std::numeric_limits<size_t>::max();

        const auto [aRangeFirst, aRangeLast] = component2AgentMap[comp] == A
            ? std::make_pair(static_cast<size_t>(0), A)
            : std::make_pair(component2AgentMap[comp], component2AgentMap[comp] + 1);

        for (size_t a = aRangeFirst; a < aRangeLast; ++a)
        {
            for (size_t i = 1 + lastInsertPositionOfComponent[comp]; i < paths[a].size(); ++i)
            {
                const auto oldCost = weights(a, paths[a][i - 1], paths[a][i]);
                const auto newCost = weights(a, paths[a][i - 1], n) + weights(a, n, paths[a][i]);
                const auto deltaCost = newCost - oldCost;
                if (deltaCost < minDeltaCost)
                {
                    minDeltaCost = deltaCost;
                    minA = a;
                    minI = i;
                }
            }
        }

        using DiffT = decltype(paths[minA].begin())::difference_type;
        paths[minA].insert(paths[minA].begin() + static_cast<DiffT>(minI), n);
        cost += minDeltaCost;

        component2AgentMap[comp] = minA;
        lastInsertPositionOfComponent[comp] = minI;
    }

    return { paths, cost };
}

std::tuple<std::vector<std::vector<size_t>>, double> tsplp::TwoOptPaths(std::vector<std::vector<size_t>> paths,
    xt::xarray<double> weights, const DependencyGraph& dependencies)
{
    assert(weights.dimension() == 2);

    const auto A = paths.size();
    assert(A > 0);

    bool hasImproved = true;
    double improvementSum = 0.0;

    while (hasImproved)
    {
        hasImproved = false;

        for (size_t a1 = 0; a1 < A; ++a1)
        {
            for (size_t a2 = 0; a2 < A; ++a2)
            {
                for (size_t i = 1; i < paths[a1].size() - 1; ++i)
                {
                    const auto jStart = a1 == a2 ? i + 1 : size_t(1);
                    for (size_t j = jStart; j < paths[a2].size() - 1; ++j)
                    {
                        const auto u = paths[a1][i];
                        const auto v = paths[a2][j];

                        if (a1 != a2 &&
                            (!dependencies.GetIncomingSpan(u).empty() || !dependencies.GetIncomingSpan(v).empty() ||
                                !dependencies.GetOutgoingSpan(u).empty() || !dependencies.GetOutgoingSpan(v).empty()))
                            continue;

                        if (a1 == a2)
                        {
                            bool wouldBreakDependency = false;
                            for (auto k = i; k < j; ++k)
                            {
                                if (dependencies.HasArc(u, paths[a1][k + 1]) || dependencies.HasArc(paths[a1][k], v))
                                {
                                    wouldBreakDependency = true;
                                    break;
                                }
                            }

                            if (wouldBreakDependency)
                                continue;
                        }

                        double improvement = 0.0;

                        if (a1 == a2 && j == i + 1)
                        {
                            const auto before1 = weights(paths[a1][i - 1], paths[a1][i    ]);
                            const auto before2 = weights(paths[a1][i    ], paths[a1][i + 1]);
                            const auto before4 = weights(paths[a1][i + 1], paths[a1][i + 2]);
                            const auto after1  = weights(paths[a1][i - 1], paths[a1][i + 1]);
                            const auto after2  = weights(paths[a1][i + 1], paths[a1][i    ]);
                            const auto after4  = weights(paths[a1][i    ], paths[a1][i + 2]);
                            improvement = before1 + before2 + before4 - after1 - after2 - after4;
                        }
                        else
                        {
                            const auto before1 = weights(paths[a1][i - 1], paths[a1][i    ]);
                            const auto before2 = weights(paths[a1][i    ], paths[a1][i + 1]);
                            const auto before3 = weights(paths[a2][j - 1], paths[a2][j    ]);
                            const auto before4 = weights(paths[a2][j    ], paths[a2][j + 1]);
                            const auto after1  = weights(paths[a1][i - 1], paths[a2][j    ]);
                            const auto after2  = weights(paths[a2][j    ], paths[a1][i + 1]);
                            const auto after3  = weights(paths[a2][j - 1], paths[a1][i    ]);
                            const auto after4  = weights(paths[a1][i    ], paths[a2][j + 1]);
                            const auto a1Imp = before1 + before2 - after1 - after2;
                            const auto a2Imp = before3 + before4 - after3 - after4;
                            improvement = a1Imp + a2Imp;
                        }

                        assert(weights(paths[a1][i], paths[a1][i]) == 0);

                        if (improvement > 0.0)
                        {
                            hasImproved = true;
                            std::swap(paths[a1][i], paths[a2][j]);
                            improvementSum += improvement;
                        }
                    }
                }
            }
        }
    }

    return { paths, improvementSum };
}


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY 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.
//
// FlexibleSUSY 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 FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

#ifndef ROOT_FINDER_H
#define ROOT_FINDER_H

#include <iostream>
#include <cassert>
#include <string>
#include <utility>
#include <Eigen/Core>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multiroots.h>

#include "logger.hpp"
#include "error.hpp"
#include "ewsb_solver.hpp"
#include "gsl_utils.hpp"
#include "gsl_vector.hpp"
#include "wrappers.hpp"

namespace flexiblesusy {

/**
 * @class Root_finder
 * @brief Function root finder
 *
 * The user has to provide the function (of which the root should be
 * found) of the type Function_t.  This function gets as arguments an
 * Eigen vector of lenght `dimension' and returns an Eigen vector of
 * the same length.
 *
 * Example:
 * @code
 * auto parabola = [](const Eigen::Matrix<double,2,1>& x) {
 *    const double y = x(0);
 *    const double z = x(1);
 *    Eigen::Matrix<double,2,1> f;
 *    f << y*(y - 5.0), z*(z - 1.0);
 *    return f;
 * };
 *
 * Root_finder<2> root_finder(parabola, 100, 1.0e-5);
 * const double start[2] = { 10, 10 };
 * const int status = root_finder.find_root(start);
 * @endcode
 */
template <std::size_t dimension>
class Root_finder : public EWSB_solver {
public:
   using Vector_t = Eigen::Matrix<double,dimension,1>;
   using Function_t = std::function<Vector_t(const Vector_t&)>;
   enum Solver_type { GSLHybrid, GSLHybridS, GSLBroyden, GSLNewton };

   Root_finder() = default;
   template <typename F>
   Root_finder(F&&, std::size_t, double, Solver_type solver_type_ = GSLHybrid);
   virtual ~Root_finder() = default;

   template <typename F>
   void set_function(F&& f) { function = std::forward<F>(f); }
   void set_precision(double p) { precision = p; }
   void set_max_iterations(std::size_t n) { max_iterations = n; }
   void set_solver_type(Solver_type t) { solver_type = t; }
   int find_root(const Vector_t&);

   // EWSB_solver interface methods
   virtual std::string name() const override { return "Root_finder<" + solver_type_name() + ">"; }
   virtual int solve(const Eigen::VectorXd&) override;
   virtual Eigen::VectorXd get_solution() const override { return root; }

private:
   std::size_t max_iterations{100};    ///< maximum number of iterations
   double precision{1.e-2};            ///< precision goal
   Vector_t root{Vector_t::Zero()};    ///< the root
   Function_t function{nullptr};       ///< function to minimize
   Solver_type solver_type{GSLHybrid}; ///< solver type

   void print_state(const gsl_multiroot_fsolver*, std::size_t) const;
   std::string solver_type_name() const;
   const gsl_multiroot_fsolver_type* solver_type_to_gsl_pointer() const;
   static int gsl_function(const gsl_vector*, void*, gsl_vector*);
};

/**
 * Constructor
 *
 * @param function_ pointer to the function to minimize
 * @param max_iterations_ maximum number of iterations
 * @param precision_ precision goal
 * @param solver_type_ GSL multiroot solver type
 */
template <std::size_t dimension>
template <typename F>
Root_finder<dimension>::Root_finder(
   F&& function_,
   std::size_t max_iterations_,
   double precision_,
   Solver_type solver_type_
)
   : max_iterations(max_iterations_)
   , precision(precision_)
   , function(std::forward<F>(function_))
   , solver_type(solver_type_)
{
}

/**
 * Start the minimization
 *
 * @param start starting point
 *
 * @return GSL error code (GSL_SUCCESS if minimum found)
 */
template <std::size_t dimension>
int Root_finder<dimension>::find_root(const Vector_t& start)
{
   if (!function)
      throw SetupError("Root_finder: function not callable");

   int status;
   std::size_t iter = 0;
   void* parameters = &function;
   gsl_multiroot_function f = {gsl_function, dimension, parameters};

   gsl_multiroot_fsolver* solver
      = gsl_multiroot_fsolver_alloc(solver_type_to_gsl_pointer(), dimension);

   if (!solver) {
      throw OutOfMemoryError(std::string("Cannot allocate gsl_multiroot_fsolver ") +
                             gsl_multiroot_fsolver_name(solver));
   }

#ifndef ENABLE_DEBUG
   gsl_set_error_handler_off();
#endif

   GSL_vector tmp_root = to_GSL_vector(start);

   gsl_multiroot_fsolver_set(solver, &f, tmp_root.raw());

#ifdef ENABLE_VERBOSE
   print_state(solver, iter);
#endif

   do {
      iter++;
      status = gsl_multiroot_fsolver_iterate(solver);

#ifdef ENABLE_VERBOSE
      print_state(solver, iter);
#endif

      if (status)   // check if solver is stuck
         break;

      status = gsl_multiroot_test_residual(solver->f, precision);
   } while (status == GSL_CONTINUE && iter < max_iterations);

   VERBOSE_MSG("\t\t\tRoot_finder status = " << gsl_strerror(status));

   root = to_eigen_vector_fixed<dimension>(solver->x);

   gsl_multiroot_fsolver_free(solver);

   return status;
}

/**
 * Print state of the root finder
 *
 * @param solver solver
 * @param iteration iteration number
 */
template <std::size_t dimension>
void Root_finder<dimension>::print_state(const gsl_multiroot_fsolver* solver,
                                         std::size_t iteration) const
{
   VERBOSE_MSG("\t\t\tIteration " << iteration
               << ": x = " << GSL_vector(solver->x)
               << ", f(x) = " << GSL_vector(solver->f));
}

template <std::size_t dimension>
int Root_finder<dimension>::gsl_function(const gsl_vector* x, void* params, gsl_vector* f)
{
   if (!is_finite(x)) {
      gsl_vector_set_all(f, std::numeric_limits<double>::max());
      return GSL_EDOM;
   }

   Function_t* fun = static_cast<Function_t*>(params);
   int status = GSL_SUCCESS;
   const Vector_t arg(to_eigen_vector_fixed<dimension>(x));
   Vector_t result;
   result.setConstant(std::numeric_limits<double>::max());

   try {
      result = (*fun)(arg);
      // workaround for intel compiler / eigen bug that causes unexpected behavior
      // of allFinite()
      status = IsFinite(result) ? GSL_SUCCESS : GSL_EDOM;
      //status = result.allFinite() ? GSL_SUCCESS : GSL_EDOM;
   } catch (const flexiblesusy::Error&) {
      status = GSL_EDOM;
   }

   copy(result, f);

   return status;
}

template <std::size_t dimension>
std::string Root_finder<dimension>::solver_type_name() const
{
   switch (solver_type) {
   case GSLHybrid : return "GSLHybrid";
   case GSLHybridS: return "GSLHybridS";
   case GSLBroyden: return "GSLBroyden";
   case GSLNewton : return "GSLNewton";
   default:
      throw SetupError("Unknown root solver type: "
                       + std::to_string(solver_type));
   }

   return "unknown";
}

template <std::size_t dimension>
const gsl_multiroot_fsolver_type* Root_finder<dimension>::solver_type_to_gsl_pointer() const
{
   switch (solver_type) {
   case GSLHybrid : return gsl_multiroot_fsolver_hybrid;
   case GSLHybridS: return gsl_multiroot_fsolver_hybrids;
   case GSLBroyden: return gsl_multiroot_fsolver_broyden;
   case GSLNewton : return gsl_multiroot_fsolver_dnewton;
   default:
      throw SetupError("Unknown root solver type: "
                       + std::to_string(solver_type));
   }

   return nullptr;
}

template <std::size_t dimension>
int Root_finder<dimension>::solve(const Eigen::VectorXd& start)
{
   return (find_root(start) == GSL_SUCCESS ?
           EWSB_solver::SUCCESS : EWSB_solver::FAIL);
}

} // namespace flexiblesusy

#endif