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

#include "GlobalOptimization.h"

#include <vector>
#include <tuple>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Core/Utility/Console.h>
#include <Core/Utility/Timer.h>
#include <Core/Registration/PoseGraph.h>
#include <Core/Registration/GlobalOptimizationMethod.h>
#include <Core/Registration/GlobalOptimizationConvergenceCriteria.h>

namespace three{

namespace {

/// Definition of linear operators used for computing Jacobian matrix.
/// If the relative transform of the two geometry is reasonably small,
/// they can be approximated as below linearized form
/// SE(3) \approx = |     1 -gamma   beta     a |
///                 | gamma      1 -alpha     b |
///                 | -beta  alpha      1     c |
///                 |     0      0      0     1 |
/// It is from sin(x) \approx x and cos(x) \approx 1 when x is almost zero.
/// See [Choi et al 2015] for more detail. Reference list in GlobalOptimization.h
const std::vector<Eigen::Matrix4d> jacobian_operator = {
    (Eigen::Matrix4d() << /* for alpha */
    0, 0, 0, 0, 0, 0, -1, 0, 0, 1, 0, 0, 0, 0, 0, 0).finished(),
    (Eigen::Matrix4d() << /* for beta */
    0, 0, 1, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0).finished(),
    (Eigen::Matrix4d() << /* for gamma */
    0, -1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished(),
    (Eigen::Matrix4d() << /* for a */
    0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished(),
    (Eigen::Matrix4d() << /* for b */
    0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0).finished(),
    (Eigen::Matrix4d() << /* for c */
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0).finished() };

/// This function is intended for linearized form of SE(3).
/// It is an approximate form. See [Choi et al 2015] for derivation.
/// Alternatively, explicit representation that uses quaternion can be used
/// here to replace this function. Refer to linearizeOplus() in
/// https://github.com/RainerKuemmerle/g2o/blob/master/g2o/types/slam3d/edge_se3.cpp
inline Eigen::Vector6d GetLinearized6DVector(const Eigen::Matrix4d input)
{
    Eigen::Vector6d output;
    output(0) = (-input(1, 2) + input(2, 1)) / 2.0;
    output(1) = (-input(2, 0) + input(0, 2)) / 2.0;
    output(2) = (-input(0, 1) + input(1, 0)) / 2.0;
    output.block<3, 1>(3, 0) = input.block<3, 1>(0, 3);
    return std::move(output);
}

inline Eigen::Vector6d GetMisalignmentVector(const Eigen::Matrix4d &X_inv,
        const Eigen::Matrix4d &Ts, const Eigen::Matrix4d &Tt_inv)
{
    Eigen::Matrix4d temp;
    temp.noalias() = X_inv * Tt_inv * Ts;
    return GetLinearized6DVector(temp);
}

inline std::tuple<Eigen::Matrix4d, Eigen::Matrix4d, Eigen::Matrix4d>
        GetRelativePoses(const PoseGraph &pose_graph, int edge_id)
{
    const PoseGraphEdge &te = pose_graph.edges_[edge_id];
    const PoseGraphNode &ts = pose_graph.nodes_[te.source_node_id_];
    const PoseGraphNode &tt = pose_graph.nodes_[te.target_node_id_];
    Eigen::Matrix4d X_inv = te.transformation_.inverse();
    Eigen::Matrix4d Ts = ts.pose_;
    Eigen::Matrix4d Tt_inv = tt.pose_.inverse();
    return std::make_tuple(std::move(X_inv), std::move(Ts), std::move(Tt_inv));
}

std::tuple<Eigen::Matrix6d, Eigen::Matrix6d> GetJacobian(
        const Eigen::Matrix4d &X_inv, const Eigen::Matrix4d &Ts,
        const Eigen::Matrix4d &Tt_inv)
{
    Eigen::Matrix6d Js = Eigen::Matrix6d::Zero();
    for (int i = 0; i < 6; i++) {
        Eigen::Matrix4d temp = X_inv * Tt_inv *
                jacobian_operator[i] * Ts;
        Js.block<6, 1>(0, i) = GetLinearized6DVector(temp);
    }
    Eigen::Matrix6d Jt = Eigen::Matrix6d::Zero();
    for (int i = 0; i < 6; i++) {
        Eigen::Matrix4d temp = X_inv * Tt_inv *
                -jacobian_operator[i] * Ts;
        Jt.block<6, 1>(0, i) = GetLinearized6DVector(temp);
    }
    return std::make_tuple(std::move(Js), std::move(Jt));
}

/// Function to update line_process value defined in [Choi et al 2015]
/// See Eq (2). temp2 value in this function is derived from dE/dl = 0
int UpdateConfidence(
        PoseGraph &pose_graph, const Eigen::VectorXd &zeta,
        const double line_process_weight,
        const GlobalOptimizationOption &option)
{
    int n_edges = (int)pose_graph.edges_.size();
    int valid_edges_num = 0;
    for (int iter_edge = 0; iter_edge < n_edges; iter_edge++) {
        PoseGraphEdge &t = pose_graph.edges_[iter_edge];
        Eigen::Vector6d e = zeta.block<6, 1>(iter_edge * 6, 0);
        double residual_square = e.transpose() * t.information_ * e;
        double temp = line_process_weight /
                (line_process_weight + residual_square);
        double temp2 = temp * temp;
        t.confidence_ = temp2;
        if (temp2 > option.edge_prune_threshold_)
            valid_edges_num++;
    }
    return valid_edges_num;
}

/// Function to compute residual defined in [Choi et al 2015] See Eq (9).
double ComputeResidual(const PoseGraph &pose_graph, const Eigen::VectorXd &zeta,
        const double line_process_weight,
        const GlobalOptimizationOption &option)
{
    int n_edges = (int)pose_graph.edges_.size();
    double residual = 0.0;
    for (int iter_edge = 0; iter_edge < n_edges; iter_edge++) {
        const PoseGraphEdge &te = pose_graph.edges_[iter_edge];
        double line_process_iter = te.confidence_;
        Eigen::Vector6d e = zeta.block<6, 1>(iter_edge * 6, 0);
        residual += line_process_iter * e.transpose() * te.information_ * e +
                line_process_weight * pow(sqrt(line_process_iter) - 1, 2.0);
    }
    return residual;
}

/// Function to compute residual defined in [Choi et al 2015] See Eq (6).
Eigen::VectorXd ComputeZeta(const PoseGraph &pose_graph)
{
    int n_edges = (int)pose_graph.edges_.size();
    Eigen::VectorXd output(n_edges * 6);
    for (int iter_edge = 0; iter_edge < n_edges; iter_edge++) {
        Eigen::Matrix4d X_inv, Ts, Tt_inv;
        std::tie(X_inv, Ts, Tt_inv) = GetRelativePoses(pose_graph, iter_edge);
        Eigen::Vector6d e = GetMisalignmentVector(X_inv, Ts, Tt_inv);
        output.block<6, 1>(iter_edge * 6, 0) = e;
    }
    return std::move(output);
}

/// The information matrix used here is consistent with [Choi et al 2015].
/// It is [p_x | I]^T[p_x | I]. \zeta is [\alpha \beta \gamma a b c]
/// Another definition of information matrix used for [Kümmerle et al 2011] is
/// [I | p_x] ^ T[I | p_x]  so \zeta is [a b c \alpha \beta \gamma].
///
/// To see how H can be derived see [Kümmerle et al 2011].
/// Eq (9) for definition of H and b for k-th constraint.
/// To see how the covariance matrix forms H, check g2o technical note:
/// https ://github.com/RainerKuemmerle/g2o/blob/master/doc/g2o.pdf
/// Eq (20) and Eq (21). (There is a typo in the equation though. B should be J)
///
/// This function focuses the case that every edge has two nodes (not hyper graph)
/// so we have two Jacobian matrices from one constraint.
std::tuple<Eigen::MatrixXd, Eigen::VectorXd> ComputeLinearSystem(
        const PoseGraph &pose_graph, const Eigen::VectorXd &zeta)
{
    int n_nodes = (int)pose_graph.nodes_.size();
    int n_edges = (int)pose_graph.edges_.size();
    Eigen::MatrixXd H(n_nodes * 6, n_nodes * 6);
    Eigen::VectorXd b(n_nodes * 6);
    H.setZero();
    b.setZero();

    for (int iter_edge = 0; iter_edge < n_edges; iter_edge++) {
        const PoseGraphEdge &t = pose_graph.edges_[iter_edge];
        Eigen::Vector6d e = zeta.block<6, 1>(iter_edge * 6, 0);

        Eigen::Matrix4d X_inv, Ts, Tt_inv;
        std::tie(X_inv, Ts, Tt_inv) = GetRelativePoses(pose_graph, iter_edge);

        Eigen::Matrix6d Js, Jt;
        std::tie(Js, Jt) = GetJacobian(X_inv, Ts, Tt_inv);
        Eigen::Matrix6d JsT_Info =
                Js.transpose() * t.information_;
        Eigen::Matrix6d JtT_Info =
                Jt.transpose() * t.information_;
        Eigen::Vector6d eT_Info = e.transpose() * t.information_;
        double line_process_iter = t.confidence_;

        int id_i = t.source_node_id_ * 6;
        int id_j = t.target_node_id_ * 6;
        H.block<6, 6>(id_i, id_i).noalias() +=
                line_process_iter * JsT_Info * Js;
        H.block<6, 6>(id_i, id_j).noalias() +=
                line_process_iter * JsT_Info * Jt;
        H.block<6, 6>(id_j, id_i).noalias() +=
                line_process_iter * JtT_Info * Js;
        H.block<6, 6>(id_j, id_j).noalias() +=
                line_process_iter * JtT_Info * Jt;
        b.block<6, 1>(id_i, 0).noalias() -=
                line_process_iter * eT_Info.transpose() * Js;
        b.block<6, 1>(id_j, 0).noalias() -=
                line_process_iter * eT_Info.transpose() * Jt;
    }
    return std::make_tuple(std::move(H), std::move(b));
}

Eigen::VectorXd UpdatePoseVector(const PoseGraph &pose_graph)
{
    int n_nodes = (int)pose_graph.nodes_.size();
    Eigen::VectorXd output(n_nodes * 6);
    for (int iter_node = 0; iter_node < n_nodes; iter_node++) {
        Eigen::Vector6d output_iter = TransformMatrix4dToVector6d(
                pose_graph.nodes_[iter_node].pose_);
        output.block<6, 1>(iter_node * 6, 0) = output_iter;
    }
    return std::move(output);
}

std::shared_ptr<PoseGraph> UpdatePoseGraph(const PoseGraph &pose_graph,
        const Eigen::VectorXd delta)
{
    std::shared_ptr<PoseGraph> pose_graph_updated =
        std::make_shared<PoseGraph>();
    *pose_graph_updated = pose_graph;
    int n_nodes = (int)pose_graph.nodes_.size();
    for (int iter_node = 0; iter_node < n_nodes; iter_node++) {
        Eigen::Vector6d delta_iter = delta.block<6, 1>(iter_node * 6, 0);
        pose_graph_updated->nodes_[iter_node].pose_ =
                TransformVector6dToMatrix4d(delta_iter) *
                pose_graph_updated->nodes_[iter_node].pose_;
    }
    return pose_graph_updated;
}

bool CheckRightTerm(const Eigen::VectorXd &right_term,
        const GlobalOptimizationConvergenceCriteria &criteria)
{
    if (right_term.maxCoeff() < criteria.min_right_term_) {
        PrintDebug("Maximum coefficient of right term < %e\n",
                criteria.min_right_term_);
        return true;
    }
    return false;
}

bool CheckRelativeIncrement(
        const Eigen::VectorXd &delta, const Eigen::VectorXd &x,
        const GlobalOptimizationConvergenceCriteria &criteria)
{
    if (delta.norm() < criteria.min_relative_increment_ *
            (x.norm() + criteria.min_relative_increment_)) {
        PrintDebug("Delta.norm() < %e * (x.norm() + %e)\n",
                criteria.min_relative_increment_,
                criteria.min_relative_increment_);
        return true;
    }
    return false;
}

bool CheckRelativeResidualIncrement(
        double current_residual, double new_residual,
        const GlobalOptimizationConvergenceCriteria &criteria)
{
    if (current_residual - new_residual <
        criteria.min_relative_residual_increment_ * current_residual) {
        PrintDebug("Current_residual - new_residual < %e * current_residual\n",
                criteria.min_relative_residual_increment_);
        return true;
    }
    return false;
}

bool CheckResidual(double residual,
        const GlobalOptimizationConvergenceCriteria &criteria)
{
    if (residual < criteria.min_residual_) {
        PrintDebug("Current_residual < %e\n", criteria.min_residual_);
        return true;
    }
    return false;
}

bool CheckMaxIteration(int iteration,
        const GlobalOptimizationConvergenceCriteria &criteria)
{
    if (iteration >= criteria.max_iteration_) {
        PrintDebug("Reached maximum number of iterations (%d)\n",
                criteria.max_iteration_);
        return true;
    }
    return false;
}

bool CheckMaxIterationLM(int iteration,
        const GlobalOptimizationConvergenceCriteria &criteria)
{
    if (iteration >= criteria.max_iteration_lm_) {
        PrintDebug("Reached maximum number of iterations (%d)\n",
                criteria.max_iteration_lm_);
        return true;
    }
    return false;
}

double ComputeLineProcessWeight(const PoseGraph &pose_graph,
        const GlobalOptimizationOption &option)
{
    int n_edges = (int)pose_graph.edges_.size();
    double average_number_of_correspondences = 0.0;
    for (int iter_edge = 0; iter_edge < n_edges; iter_edge++) {
        double number_of_correspondences =
                pose_graph.edges_[iter_edge].information_(5,5);
        average_number_of_correspondences += number_of_correspondences;
    }
    if (n_edges > 0) {
        // see Section 5 in [Choi et al 2015]
        average_number_of_correspondences /= (double)n_edges;
        double line_process_weight = option.preference_loop_closure_ *
                pow(option.max_correspondence_distance_, 2) *
                average_number_of_correspondences;
        return line_process_weight;
    }
    else {
        return 0.0;
    }
}

void CompensateReferencePoseGraphNode(PoseGraph &pose_graph_new,
        const PoseGraph &pose_graph_orig, int reference_node)
{
    PrintDebug("CompensateReferencePoseGraphNode : reference : %d\n",
            reference_node);
    int n_nodes = (int)pose_graph_new.nodes_.size();
    if (reference_node < 0 || reference_node >= n_nodes) {
        return;
    } else {
        Eigen::Matrix4d compensation =
                pose_graph_orig.nodes_[reference_node].pose_ *
                pose_graph_new.nodes_[reference_node].pose_.inverse();
        for (int i = 0; i < n_nodes; i++)
        {
            pose_graph_new.nodes_[i].pose_ = compensation *
                    pose_graph_new.nodes_[i].pose_;
        }
    }
}

}    // unnamed namespace

std::shared_ptr<PoseGraph> CreatePoseGraphWithoutInvalidEdges(
        const PoseGraph &pose_graph,
        const GlobalOptimizationOption &option)
{
    std::shared_ptr<PoseGraph> pose_graph_pruned =
            std::make_shared<PoseGraph>();

    int n_nodes = (int)pose_graph.nodes_.size();
    for (int iter_node = 0; iter_node < n_nodes; iter_node++) {
        const PoseGraphNode &t = pose_graph.nodes_[iter_node];
        pose_graph_pruned->nodes_.push_back(t);
    }
    int n_edges = (int)pose_graph.edges_.size();
    for (int iter_edge = 0; iter_edge < n_edges; iter_edge++) {
        const PoseGraphEdge &t = pose_graph.edges_[iter_edge];
        if (t.uncertain_) {
            if (t.confidence_ > option.edge_prune_threshold_) {
                pose_graph_pruned->edges_.push_back(t);
            }
        } else {
            pose_graph_pruned->edges_.push_back(t);
        }
    }
    return pose_graph_pruned;
}

void GlobalOptimizationGaussNewton::
        OptimizePoseGraph(PoseGraph &pose_graph,
        const GlobalOptimizationConvergenceCriteria &criteria,
        const GlobalOptimizationOption &option) const
{
    int n_nodes = (int)pose_graph.nodes_.size();
    int n_edges = (int)pose_graph.edges_.size();
    double line_process_weight = ComputeLineProcessWeight(pose_graph, option);

    PrintDebug("[GlobalOptimizationGaussNewton] Optimizing PoseGraph having %d nodes and %d edges. \n",
            n_nodes, n_edges);
    PrintDebug("Line process weight : %f\n", line_process_weight);

    Eigen::VectorXd zeta = ComputeZeta(pose_graph);
    double current_residual, new_residual;
    new_residual = ComputeResidual(pose_graph, zeta,
            line_process_weight, option);
    current_residual = new_residual;

    int valid_edges_num;
    valid_edges_num = UpdateConfidence(pose_graph, zeta,
            line_process_weight, option);

    Eigen::MatrixXd H;
    Eigen::VectorXd b;
    Eigen::VectorXd x = UpdatePoseVector(pose_graph);

    std::tie(H, b) = ComputeLinearSystem(pose_graph, zeta);

    PrintDebug("[Initial     ] residual : %e\n", current_residual);

    bool stop = false;
    if (stop || CheckRightTerm(b, criteria))
        return;

    Timer timer_overall;
    timer_overall.Start();
    int iter;
    for (iter = 0; !stop; iter++) {
        Timer timer_iter;
        timer_iter.Start();

        Eigen::VectorXd delta = H.ldlt().solve(b);

        stop = stop || CheckRelativeIncrement(delta, x, criteria);
        if (stop) {
            break;
        } else {
            std::shared_ptr<PoseGraph> pose_graph_new =
                UpdatePoseGraph(pose_graph, delta);

            Eigen::VectorXd zeta_new;
            zeta_new = ComputeZeta(*pose_graph_new);
            new_residual = ComputeResidual(pose_graph, zeta_new,
                    line_process_weight, option);
            stop = stop || CheckRelativeResidualIncrement(
                    current_residual, new_residual, criteria);
            if (stop)
                break;
            current_residual = new_residual;

            zeta = zeta_new;
            pose_graph = *pose_graph_new;
            x = UpdatePoseVector(pose_graph);
            valid_edges_num = UpdateConfidence(pose_graph, zeta,
                    line_process_weight, option);
            std::tie(H, b) = ComputeLinearSystem(pose_graph, zeta);

            stop = stop || CheckRightTerm(b, criteria);
            if (stop)
                break;
        }
        timer_iter.Stop();
        PrintDebug("[Iteration %02d] residual : %e, valid edges : %d, time : %.3f sec.\n",
                iter, current_residual, valid_edges_num,
                timer_iter.GetDuration() / 1000.0);
        stop = stop || CheckResidual(current_residual, criteria)
                || CheckMaxIteration(iter, criteria);
    }    // end for
    timer_overall.Stop();
    PrintDebug("[GlobalOptimizationGaussNewton] total time : %.3f sec.\n",
            timer_overall.GetDuration() / 1000.0);
}

void GlobalOptimizationLevenbergMarquardt::
        OptimizePoseGraph(PoseGraph &pose_graph,
        const GlobalOptimizationConvergenceCriteria &criteria,
        const GlobalOptimizationOption &option) const
{
    int n_nodes = (int)pose_graph.nodes_.size();
    int n_edges = (int)pose_graph.edges_.size();
    double line_process_weight = ComputeLineProcessWeight(pose_graph, option);

    PrintDebug("[GlobalOptimizationLM] Optimizing PoseGraph having %d nodes and %d edges. \n",
            n_nodes, n_edges);
    PrintDebug("Line process weight : %f\n", line_process_weight);

    Eigen::VectorXd zeta = ComputeZeta(pose_graph);
    double current_residual, new_residual;
    new_residual = ComputeResidual(pose_graph, zeta,
            line_process_weight, option);
    current_residual = new_residual;

    int valid_edges_num = UpdateConfidence(pose_graph, zeta,
            line_process_weight, option);

    Eigen::MatrixXd H_I = Eigen::MatrixXd::Identity(n_nodes * 6, n_nodes * 6);
    Eigen::MatrixXd H;
    Eigen::VectorXd b;
    Eigen::VectorXd x = UpdatePoseVector(pose_graph);

    std::tie(H, b) = ComputeLinearSystem(pose_graph, zeta);

    Eigen::VectorXd H_diag = H.diagonal();
    double tau = 1e-5;
    double current_lambda = tau * H_diag.maxCoeff();
    double ni = 2.0;
    double rho = 0.0;

    PrintDebug("[Initial     ] residual : %e, lambda : %e\n",
            current_residual, current_lambda);

    bool stop = false;
    stop = stop || CheckRightTerm(b, criteria);
    if (stop)
        return;

    Timer timer_overall;
    timer_overall.Start();
    for (int iter = 0; !stop; iter++) {
        Timer timer_iter;
        timer_iter.Start();
        int lm_count = 0;
        do {
            Eigen::MatrixXd H_LM = H + current_lambda * H_I;
            Eigen::VectorXd delta(H_LM.cols());

            //Using a sparse solver
            Eigen::SparseMatrix<double> H_LM_sparse = H_LM.sparseView();
            Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> chol;
            chol.compute(H_LM_sparse);

            if (chol.info() == Eigen::Success) {
                delta = chol.solve(b);
                if (chol.info() != Eigen::Success) {
                    PrintInfo("[GlobalOptimizationLM] sparse solver couldn't solve !! switching to dense solver");
                    delta = H_LM.ldlt().solve(b);
                    }
            } else {
                PrintInfo("[GlobalOptimizationLM] Cholesky Decomposition Failed !! switching to dense solver");
                delta = H_LM.ldlt().solve(b);
            }

            stop = stop || CheckRelativeIncrement(delta, x, criteria);
            if (!stop) {
                std::shared_ptr<PoseGraph> pose_graph_new =
                        UpdatePoseGraph(pose_graph, delta);

                Eigen::VectorXd zeta_new;
                zeta_new = ComputeZeta(*pose_graph_new);
                new_residual = ComputeResidual(pose_graph, zeta_new,
                        line_process_weight, option);
                rho = (current_residual - new_residual) /
                        (delta.dot(current_lambda * delta + b) + 1e-3);
                if (rho > 0) {
                    stop = stop || CheckRelativeResidualIncrement(
                            current_residual, new_residual, criteria);
                    if (stop)
                        break;
                    double alpha = 1. - pow((2 * rho - 1), 3);
                    alpha = (std::min)(alpha, criteria.upper_scale_factor_);
                    double scaleFactor = (std::max)
                            (criteria.lower_scale_factor_, alpha);
                    current_lambda *= scaleFactor;
                    ni = 2;
                    current_residual = new_residual;

                    zeta = zeta_new;
                    pose_graph = *pose_graph_new;
                    x = UpdatePoseVector(pose_graph);
                    valid_edges_num = UpdateConfidence(pose_graph, zeta,
                            line_process_weight, option);
                    std::tie(H, b) = ComputeLinearSystem(pose_graph, zeta);

                    stop = stop || CheckRightTerm(b, criteria);
                    if (stop)
                        break;
                } else {
                    current_lambda *= ni;
                    ni *= 2;
                }
            }
            lm_count++;
            stop = stop || CheckMaxIterationLM(lm_count, criteria);
        } while (!((rho > 0) || stop));
        timer_iter.Stop();
        if (!stop) {
            PrintDebug("[Iteration %02d] residual : %e, valid edges : %d, time : %.3f sec.\n",
                    iter, current_residual, valid_edges_num,
                    timer_iter.GetDuration() / 1000.0);
        }
        stop = stop || CheckResidual(current_residual, criteria)
                || CheckMaxIteration(iter, criteria);
    }    // end for
    timer_overall.Stop();
    PrintDebug("[GlobalOptimizationLM] total time : %.3f sec.\n",
            timer_overall.GetDuration() / 1000.0);
}

void GlobalOptimization(
        PoseGraph &pose_graph,
        const GlobalOptimizationMethod &method
        /* = GlobalOptimizationLevenbergMarquardt() */,
        const GlobalOptimizationConvergenceCriteria &criteria
        /* = GlobalOptimizationConvergenceCriteria() */,
        const GlobalOptimizationOption &option
        /* = GlobalOptimizationOption() */)
{
    std::shared_ptr<PoseGraph> pose_graph_pre =
            std::make_shared<PoseGraph>();
    *pose_graph_pre = pose_graph;
    method.OptimizePoseGraph(*pose_graph_pre, criteria, option);
    auto pose_graph_pruned = CreatePoseGraphWithoutInvalidEdges(
            *pose_graph_pre, option);
    method.OptimizePoseGraph(*pose_graph_pruned, criteria, option);
    CompensateReferencePoseGraphNode(*pose_graph_pruned,
            pose_graph, option.reference_node_);
    pose_graph = *pose_graph_pruned;
}

}    // namespace three


#include <boost/python.hpp>
#include "integrand.h"

using namespace Eigen;

void GetBandHamiltonian(const VectorXd &x, ComplexMatrix &mat, const std::vector<ComplexMatrix> &HR,
    const MatrixXi &R, const double &H, const double &delta)
{
  static Complex I(0,1);
  mat.fill(0);
  for (int i = 0; i < R.rows(); ++i) {
    mat += HR[i]*exp(I*(x(0)*R(i,0) + x(1)*R(i,1) + x(2)*R(i,2)));
  }
  for (int i = 0; i < mat.rows(); ++i)
    mat(i,i) -= H;
  for (int i = 0; i < kNCor*kNLayers; ++i)
    mat(i,i) -= delta;
}

GreenIntegrand::GreenIntegrand(const Eigen::VectorXcd &omega, const double &chemical_potential,
    const std::vector<ComplexMatrix> &hamiltonian_r, const Eigen::MatrixXi &r_vectors,
    const Eigen::MatrixXcd &all_self_energy, const double &magnetic_field, const double &double_counting)
    : md_int::GeneralIntegrand<Complex>(), delta_(double_counting), chemical_potential_(chemical_potential),
    ham_r_(hamiltonian_r), r_vectors_(r_vectors), h_field_(magnetic_field), all_omega_(omega),
    all_self_energy_(all_self_energy), self_energy_(ComplexMatrix::Zero()) {
  set_dim_in(kDim);
  set_dim_out(kMSize*kMSize);
}

// set omega and self energy:
// e.g. tilted 5d band case: 4 blocks for 4 cell, each block is a 5x5 matrix
void GreenIntegrand::set_data(const int &r) {
    w_ = all_omega_(r);

    for (int c = 0; c < kNLayers; ++c)
        for (int i = 0; i < kNCor; ++i)
            for (int j = 0; j < kNCor; ++j)
                self_energy_(kNCor*c+i,kNCor*c+j) = all_self_energy_(r, kNCor*kNCor*c+kNCor*i+j);
}

void GreenIntegrand::CalculateOriginalIntegrand(const VectorXd &x, VectorXcd &y) const
{
    y.resize(dim_out());
    ComplexMatrix tmp, negative_green;
    GetBandHamiltonian(x, tmp, ham_r_, r_vectors_, h_field_, delta_);

    for (int n = 0; n < kMSize; ++n) tmp(n, n) -= w_ + chemical_potential_;
    tmp += self_energy_;

    inv::inverse(tmp, negative_green);
    for (int i = 0; i < kMSize; ++i)
        for (int j = 0; j < kMSize; ++j)
            y(kMSize*i+j) = -negative_green(i, j);
}

HamiltonianIntegrand::HamiltonianIntegrand(const int &num_order, const std::vector<ComplexMatrix> &hamiltonian_r,
    const Eigen::MatrixXi &r_vectors, const double &magnetic_field, const double &double_counting)
    : md_int::GeneralIntegrand<Complex>(), num_order_(num_order), ham_r_(hamiltonian_r),
    r_vectors_(r_vectors), h_field_(magnetic_field), delta_(double_counting) {
  set_dim_in(kDim);
  set_dim_out(num_order_*kMSize*kMSize);
}

void HamiltonianIntegrand::CalculateOriginalIntegrand(const VectorXd &x, VectorXcd &y) const
{
    ComplexMatrix ham, ham_pow(ComplexMatrix::Identity());
    y.resize(dim_out());
    GetBandHamiltonian(x, ham, ham_r_, r_vectors_, h_field_, delta_);
    for (int n = 0; n < num_order_; ++n) {
        ham_pow *= ham;
        for (int i = 0; i < kMSize; ++i)
            for (int j = 0; j < kMSize; ++j)
                y(kMSize*kMSize*n+kMSize*i+j) = ham_pow(i, j);
    }
}


// fixed_point.hpp
//
// Copyright (c) 2016 Piotr K. Semenov (piotr.k.semenov at gmail dot com)
// Distributed under the New BSD License. (See accompanying file LICENSE)

/*!
 \file fixed_point.hpp

 \brief Provides the fixed-point numbers of any format and CORDIC-based
 overloading of STD math as well.
*/

#ifndef INC_LIBQ_FIXED_POINT_HPP_
#define INC_LIBQ_FIXED_POINT_HPP_

#include <boost/integer.hpp>
#include <boost/integer/integer_mask.hpp>

#include <cstdint>
#include <cmath>
#include <limits>
#include <exception>
#include <type_traits>

#include "arithmetics_safety.hpp"
#include "type_promotion.hpp"

#ifndef IMPLICIT_COPY_CTR
#define COPY_CTR_EXPLICIT_SPECIFIER explicit
#else
#define COPY_CTR_EXPLICIT_SPECIFIER
#endif

namespace libq {
namespace details {
    double exp2(double _val) {
#if defined(_MSC_VER)
        return std::exp2(_val);
#elif defined(__GNUC__)
        return std::pow(2.0, _val);
#endif
    }
}  // details

/*!
 \brief Gets the reference to the stored integer behind the fixed-point number
 \param[in] _x the fixed-point number
*/
template<typename T, std::size_t n, std::size_t f, int e, class ... Ps>
T& lift(fixed_point<T, n, f, e, Ps...>& _x) {  // NOLINT
    return _x.m_value;
}

/*!
 \brief Gets the value of the stored integer behind the fixed-point number
 \param[in] _x the fixed-point number
*/
template<typename T, std::size_t n, std::size_t f, int e, class ... Ps>
T const lift(fixed_point<T, n, f, e, Ps...> const& _x) {
    return _x.m_value;
}

/*!
 \brief Implements the fixed-point number arithmetics. Note, this extends the
 Q-formats like UQn.f, Qn.f with fixed pre-scaling factor \f$2^e\f$.
 \tparam value_type Built-in integral type to represent a fixed-point number.
 \tparam n Number of integral bits.
 \tparam f Number of fractional bits.
 \remark Note, \f$n\f$ and \f$f\f$ exclude the sign bit. So if storage_type is
 signed then total number of bits is \f$(n + f + 1)\f$.
 \remark Note, the supremum of \f$(n + f)\f$ is
 std::numeric_limits<std::uintmax_t>::digits in case of the unsigned numbers
 and std::numeric_limits<std::intmax_t>::digits in case of the signed numbers.
 \tparam e Exponent of the pre-scaling factor \f$2^e\f$.
 \tparam op Policy class specifying the actions to do if overflow occurred.
 \tparam up Policy class specifying the actions to do if underflow occurred.

 Usage

 Example 1: Flexible switch between the floating-point calculations and
 fixed precision calculations while algorithm staying the same.
 \code{.cpp}
    #include "fixed_point.hpp"
    #include <iostream>
    #include <cstdlib>

    namespace floating_point {
        using value_type = double;
    };  // namespace floating_point
    namespace fixed_precision {
        using value_type = libq::Q<30, 20>;
    };  // namespace fixed_precision
    using value_type = fixed_precision::value_type;

    int main(int, char**) {
        value_type input{ 0.0 };
        std::cin >> input;

        value_type const result = your_algorithm_here(input);
        std::cout << result << std::endl;

        return EXIT_SUCCESS;
    }
 \endcode

 \note Please, see http://en.wikipedia.org/wiki/Q_(number_format) for details.
*/
template<typename value_type,
         std::size_t n,
         std::size_t f,
         int e,
         class op,
         class up>
class fixed_point {
    static_assert(std::is_integral<value_type>::value,
                  "value_type must be of the built-in integral type");

    using this_class = fixed_point<value_type, n, f, e, op, up>;
    using largest_type = typename std::conditional<
                                    std::numeric_limits<value_type>::is_signed,
                                    std::intmax_t, std::uintmax_t>::type;

 public:
    using type = this_class;
    using overflow_policy = op;
    using underflow_policy = up;

    /*!
     \brief Used type for the stored integer.
    */
    using storage_type = value_type;

    enum: int {
        scaling_factor_exponent = e
    };
    enum: std::size_t {
        /*!
         \brief Total number of significant bits.
        */
        number_of_significant_bits = n + f,

        /*!
         \brief Queried number of bits to represent the fractional part of
         fixed-point number.
        */
        bits_for_fractional = f,

        /*!
         \brief Number of bits to represent the integral part.
        */
        bits_for_integral = n,

        /*!
         \brief This checks if this fixed-point number is signed.
        */
        is_signed = std::numeric_limits<storage_type>::is_signed
    };

    /*!
     \brief Gets the scaling factor for this fixed-point number.
    */
    inline static double scaling_factor() {
        static double const factor = details::exp2(
                    -static_cast<double>(this_class::scaling_factor_exponent));

        return factor;
    }

    static_assert(this_class::number_of_significant_bits <=
                    std::numeric_limits<largest_type>::digits,
                  "too big word size is required");
#define EXP2N(N) (std::uintmax_t(1u) << (N))
    enum : std::uintmax_t {
        /*!
         \brief Scale factor for this fixed-point number.
        */
        scale = EXP2N(this_class::bits_for_fractional),

        /*!
         \brief Binary mask to extract the integral bits only from the stored
         integer.
        */
        // This is tricky to process if n + f = max. possible word size.
        integer_bits_mask = (this_class::bits_for_integral > 0u) ?
            2u * ( EXP2N(this_class::bits_for_fractional + this_class::bits_for_integral - 1u) -  // NOLINT
                   ( (this_class::bits_for_fractional > 0u) ? EXP2N(this_class::bits_for_fractional - 1u) : 0u)) : 0u,  // NOLINT

        /*!
         \brief Binary mask to extract the fractional bits from the stored
         integer.
        */
        fractional_bits_mask = (this_class::bits_for_fractional > 0u) ?
            2u * (EXP2N(this_class::bits_for_fractional - 1u) - 1u) + 1u : 0u
    };
#undef EXP2N

    /*!
     \brief The maximum value of stored integer for this fixed-point format.
    */
    static typename this_class::largest_type const largest_stored_integer =
        boost::low_bits_mask_t<this_class::number_of_significant_bits>::sig_bits;  // NOLINT
    /*!
     \brief Gets the maximum available fixed-point number.
    */
    static this_class largest() {
        return
            this_class::wrap<typename this_class::largest_type>(
                                           this_class::largest_stored_integer);
    }

    /*
     \brief The minimum value of stored integer for this fixed-point format.
    */
    static std::intmax_t const least_stored_integer =
        this_class::is_signed * (-static_cast<std::intmax_t>(this_class::largest_stored_integer) - 1);  // NOLINT
    /*!
     \brief Gets the minimum available fixed-point number.
    */
    static this_class least() {
        return
            this_class::wrap(this_class::least_stored_integer);
    }

    /*!
     \brief Gets the dynamic range (dB) for this fixed-point format
    */
    static double dynamic_range_db() {
        double const max_stored_integer =
            static_cast<double>(this_class::largest_stored_integer);

        return
            20.0 * std::log10(max_stored_integer);
    }

    /*!
     \brief Gets the precision of this fixed-point number.
    */
    static double precision() {
        return
            1.0 / this_class::scale;
    }

    /*!
     \brief Signed version of this fixed-point number type.
    */
    using to_signed_type = fixed_point<
                                 typename std::make_signed<storage_type>::type,
                                 this_class::bits_for_integral,
                                 this_class::bits_for_fractional,
                                 this_class::scaling_factor_exponent,
                                 overflow_policy,
                                 underflow_policy>;

    /*!
     \brief Unsigned version of this fixed-point number type.
    */
    using to_unsigned_type = fixed_point<
                               typename std::make_unsigned<storage_type>::type,
                               this_class::bits_for_integral,
                               this_class::bits_for_fractional,
                               this_class::scaling_factor_exponent,
                               overflow_policy,
                               underflow_policy>;

    /*!
     \brief Wraps the input integer _val as a fixed-point number.

     Usage

     Example 1:
     \code{.cpp}
         #include "fixed_point.hpp"
         #include <cstdint>

         int main(int, char**) {
             using Q = libq::Q<30, 20>;

             std::uint8_t input{ 23u };
             Q const fp = Q::wrap(input);

             return EXIT_SUCCESS;
         }
     \endcode
    */
    template<typename T>
    static this_class wrap(T const& _val) {
        static_assert(std::is_integral<T>::value,
                      "input param must be of the built-in integral type");

        if ((_val < 0 && _val < this_class::least_stored_integer) ||
            (_val > 0 && _val > this_class::largest_stored_integer)) {
            overflow_policy::raise_event();
        }

        this_class x;
        x.m_value = storage_type(_val);

        return x;
    }
    static this_class wrap(float const&) = delete;
    static this_class wrap(double const&) = delete;

    fixed_point() = default;
    COPY_CTR_EXPLICIT_SPECIFIER fixed_point(this_class const& _x) = default;  // NOLINT

    /*!
     \brief Normalizes the input fixed-point number to be accepted by current
     format.
    */
    template<typename T1,
             std::size_t n1,
             std::size_t f1,
             int e1,
             typename op1,
             typename up1>
    COPY_CTR_EXPLICIT_SPECIFIER
        fixed_point(fixed_point<T1, n1, f1, e1, op1, up1> const& _x)
        : m_value(
            this_class::normalize(_x,
                std::integral_constant<bool, (int(f1) + e1 - int(this_class::bits_for_fractional) - this_class::scaling_factor_exponent > 0)>())) { // NOLINT
    }

    /*!
     \brief Creates the fixed-point number from any arithmetic object.
    */
    template<typename T>
    COPY_CTR_EXPLICIT_SPECIFIER fixed_point(T const& _value)
        : m_value(
            this_class::calc_stored_integer_from(_value,
                                                 std::integral_constant<bool, std::is_floating_point<T>::value>())) {  // NOLINT
    }

    /*!
     \brief Assigns the fixed-point number being of different format.
    */
    template<typename T1,
            std::size_t n1,
            std::size_t f1,
            int e1,
            typename op1,
            typename up1>
    this_class& operator =(fixed_point<T1, n1, f1, e1, op1, up1> const& _x) {
        using status_type =
            std::integral_constant<bool,
                            (static_cast<int>(f1) + e1 -
                            static_cast<int>(this_class::bits_for_fractional) -
                            this_class::scaling_factor_exponent > 0)>;  // NOLINT
        return
            this->set_value_to(this_class::normalize(_x, status_type()));
    }
    this_class& operator =(this_class const& _x) = default;

    /*!
     \brief Assigns any arithmetic type.
    */
    template<typename T>
    void operator =(T const& _x) {
        static_assert(std::is_arithmetic<T>::value,
                      "T must be of the arithmetic type");
        using status_type = std::integral_constant<bool,
                                             std::is_floating_point<T>::value>;

        this->m_value =
            this_class::calc_stored_integer_from(_x, status_type());
    }

    /*!
     \brief Converts this fixed-point number to the single-precision
     floating-point number.
    */
    operator float() const {
        return static_cast<float>(to_floating_point());
    }

    /*!
     \brief Converts this fixed-point number to the double-precision
     floating-point number.
    */
    operator double() const {
        return to_floating_point();
    }

    /*!
     \brief Gets the stored integer behind this fixed-point number.
    */
    storage_type value() const {
        return this->m_value;
    }

    // Unfortunately, nobody can use BOOST.Operators here because it cannot
    // handle the template operators
#define COMPARISON_OPERATOR(op)\
    template<typename T>\
    bool operator op(T const& _x) const {\
        return this->value() op this_class(_x).value();\
     }

    COMPARISON_OPERATOR(<);  // NOLINT
    COMPARISON_OPERATOR(<=);  // NOLINT
    COMPARISON_OPERATOR(>);  // NOLINT
    COMPARISON_OPERATOR(>=);  // NOLINT
    COMPARISON_OPERATOR(==);  // NOLINT
    COMPARISON_OPERATOR(!=);  // NOLINT
#undef COMPARISON_OPERATOR

    bool operator !() const {
        return this->value() == 0;
    }

    /*!
     \brief Fixed-point approximation of the widely-used constants.
     \note This uses the following naming convention:
     - CONST_2PI is for \f$2 * \pi\f$.
     - CONST_2_PI is for \f$\frac{2}{\pi}\f$.
     - CONST_PI_2 is for \f$\frac{\pi}{2}\f$.
     \note Weirdly, g++ 5.3.0 needs the full specification of type here.
     Otherwise, it will not interpret lines 705-707 as declaration instead of
     definition.
    */
    static fixed_point<value_type, n, f, e, op, up> const
        CONST_E, CONST_LOG2E, CONST_1_LOG2E, CONST_LOG10E, CONST_LOG102,
        CONST_LN2, CONST_LN10, CONST_2PI, CONST_PI, CONST_PI_2, CONST_PI_4,
        CONST_1_PI, CONST_2_PI, CONST_2_SQRTPI, CONST_SQRT2, CONST_SQRT1_2,
        CONST_2SQRT2;

    /*!
     \brief Calculates the sum of the current fixed-point number and some
     numeric object.
     \note If no extra significant bits is available for the promoted type then
     the result type is equal to std::common_type<L, R>::type = L.
    */
    template<typename T>
    typename libq::details::sum_traits<this_class>::promoted_type
        operator +(T const& _x) const {
        using sum_type = typename libq::details::sum_traits<this_class>::promoted_type;  // NOLINT
        using word_type = typename sum_type::storage_type;

        this_class const converted(_x);
        if (details::does_add_overflow(*this, converted)) {
            overflow_policy::raise_event();
        }

        word_type const stored_integer = word_type(this->value()) +
            word_type(converted.value());
        return sum_type::wrap(stored_integer);
    }
    template<typename T>
    inline this_class& operator +=(T const& _x) {
        this_class const result(*this + _x);

        return this->set_value_to(result.value());
    }

    /*!
     \brief Subtracts some numeric object from the current fixed-point number.
     \note If no extra significant bits is available for the promoted type then
     the result type is equal to std::common_type<L, R>::type = L.
    */
    template<typename T>
    typename libq::details::sum_traits<this_class>::promoted_type
        operator -(T const& _x) const {
        using diff_type = typename libq::details::sum_traits<this_class>::promoted_type;  // NOLINT
        using word_type = typename diff_type::storage_type;

        this_class const converted(_x);
        if (details::does_sub_overflow(*this, converted)) {
            overflow_policy::raise_event();
        }

        word_type const stored_integer = word_type(this->value()) -
            word_type(converted.value());
        return diff_type::wrap(stored_integer);
    }
    template<typename T>
    this_class operator -=(T const& _x) {
        this_class const result(*this - _x);

        return this->set_value_to(result.value());
    }

    /*!
     \brief Multiplies the current fixed-point number with some numeric object.
     \note If no extra significant bits is available for the promoted type then
     the result type is equal to std::common_type<L, R>::type = L.
    */
    template<typename T1,
             std::size_t n1,
             std::size_t f1,
             int e1,
             class op1,
             class up1>
    typename libq::details::mult_of<this_class,
                                    libq::fixed_point<T1, n1, f1, e1, op1, up1> >::promoted_type  // NOLINT
        operator *(libq::fixed_point<T1, n1, f1, e1, op1, up1> const& _x)
                                                                        const {
        using operand_type =
            typename libq::fixed_point<T1, n1, f1, e1, op1, up1>;
        using promotion_traits = libq::details::mult_of<this_class,
                                                        operand_type>;
        using result_type = typename promotion_traits::promoted_type;
        using word_type = typename promotion_traits::promoted_storage_type;

        if (details::does_mul_overflow(*this, _x)) {
            overflow_policy::raise_event();
        }

        // do the exact/approximate multiplication of fixed-point numbers
        return result_type::wrap(
            (static_cast<word_type>(this->value()) * static_cast<word_type>(_x.value()))  // NOLINT
                >> (promotion_traits::is_expandable ? 0 : operand_type::bits_for_fractional));  // NOLINT
    }
    template<typename T1, std::size_t n1, std::size_t f1, int e1, class op1, class up1>  // NOLINT
    this_class
        operator *=(libq::fixed_point<T1, n1, f1, e1, op1, up1> const& _x) {
        this_class const result(*this * _x);

        return this->set_value_to(result.value());
    }

    /*!
     \brief Divides the current fixed-point number with some numeric object.
     \note If no extra significant bits is available for the promoted type then
     the result type is equal to std::common_type<L, R>::type = L.
    */
    template<typename T1, std::size_t n1, std::size_t f1, int e1, class... Ps>
    typename libq::details::div_of<this_class, libq::fixed_point<T1, n1, f1, e1, Ps...> >::promoted_type  // NOLINT
        operator /(libq::fixed_point<T1, n1, f1, e1, Ps...> const& _x) const {
        using operand_type = typename libq::fixed_point<T1, n1, f1, e1, Ps...>;
        using promotion_traits =
            libq::details::div_of<this_class, operand_type>;
        using result_type = typename promotion_traits::promoted_type;
        using word_type = typename promotion_traits::promoted_storage_type;

        if (details::does_div_overflow(*this, _x)) {
            overflow_policy::raise_event();
        }

        word_type const shifted = static_cast<word_type>(this->value()) << operand_type::number_of_significant_bits;  // NOLINT
        if (!promotion_traits::is_expandable && _x.value() !=
                (shifted >> operand_type::number_of_significant_bits)) {
            overflow_policy::raise_event();
        }

        return
            result_type::wrap(shifted / static_cast<word_type>(_x.value()));
    }
    template<typename T1, std::size_t n1, std::size_t f1, int e1, class... Ps>
    this_class
        operator /=(libq::fixed_point<T1, n1, f1, e1, Ps...> const& _x) {
        this_class const result(*this / _x);

        return this->set_value_to(result.value());
    }

    /*!
     \brief Gets the negative value of the current fixed-point number.
    */
    this_class operator -() const {
        if (details::does_unary_neg_overflow(*this)) {
            overflow_policy::raise_event();
        }

        return this_class::wrap(-this->value());
    }

 private:
    /*!
     \brief Represents some floating-point number as a fixed-point number.
     \note It uses the rounding-to-nearest logics in case of floating-point
     types.
    */
    template<typename T>
    static storage_type
        calc_stored_integer_from(T const& _x, std::true_type) {
        double const scale = static_cast<double>(
                                          this_class::scaling_factor_exponent);
        double const value = static_cast<double>(_x) / details::exp2(scale);
        if (_x > T(0)) {
            storage_type const converted = static_cast<storage_type>(
                                  std::floor(value * this_class::scale + 0.5));
            if (converted < 0) {
                overflow_policy::raise_event();
            }

            return converted;
        }

        return
            static_cast<storage_type>(
                                   std::ceil(value * this_class::scale - 0.5));
    }

    /*!
     \brief Represents some integral number as a fixed-point number.
    */
    template<typename T>
    static storage_type
        calc_stored_integer_from(T const& _x, std::false_type) {
        double const scale = static_cast<double>(
                                          this_class::scaling_factor_exponent);
        double const value = static_cast<double>(_x) / details::exp2(scale);
        return storage_type(value) << this_class::bits_for_fractional;
    }

    /*!
     \brief Normalizes the input fixed-point number to one of the current
     format in case \f$f + e - f1 - e1 > 0\f$.
    */
    template<typename T1, std::size_t n1, std::size_t f1, int e1, class... Ps>
    static storage_type normalize(fixed_point<T1, n1, f1, e1, Ps...> const& _x,
                                  std::false_type) {
        static std::size_t const shifts =
            (static_cast<int>(this_class::bits_for_fractional) + this_class::scaling_factor_exponent) -  // NOLINT
            (static_cast<int>(e1) + f1);
        storage_type const normalized = storage_type(_x.value()) << shifts;

        if (_x.value() != (normalized >> shifts)) {
            overflow_policy::raise_event();
        }
        return normalized;
    }

    /*!
     \brief Normalizes the input fixed-point number to one of the current
     format in case \f$f + e - f1 - e1 < 0\f$.
    */
    template<typename T1, std::size_t n1, std::size_t f1, int e1, class... Ps>
    static storage_type normalize(fixed_point<T1, n1, f1, e1, Ps...> const& _x,
                                  std::true_type) {
        static std::size_t const shifts =
            (static_cast<int>(e1) + f1) -
            (static_cast<int>(this_class::bits_for_fractional) + this_class::scaling_factor_exponent);  // NOLINT
        storage_type const normalized =
            static_cast<storage_type>(_x.value() >> shifts);

        if (_x.value() && !normalized) {
            underflow_policy::raise_event();
        }
        return normalized;
    }

    /*!
     \brief Converts the fixed-point number to the floating-point number.
    */
    double to_floating_point() const {
        return
            this->scaling_factor() * static_cast<double>(value()) / this_class::scale;  // NOLINT
    }

    storage_type m_value;
    /*!
     \brief This also checks if the stored integer is within the range of
     current fixed-point number.
    */
    this_class& set_value_to(storage_type const _x) {
        if (_x < this_class::least_stored_integer ||
            _x > this_class::largest_stored_integer) {
            overflow_policy::raise_event();
        }

        this->m_value = _x;
        return *this;
    }

    friend storage_type& lift<value_type, n, f, e, overflow_policy, underflow_policy>(this_class&);  // NOLINT
};

/*!
 \brief Short-cut for the signed fixed-point with just 2 template parameters
 n and f.
*/
template<std::size_t n, std::size_t f, int e = 0, class op = libq::ignorance_policy, class up = libq::ignorance_policy>  // NOLINT
using Q = libq::fixed_point<typename boost::int_t<n+1>::least, n-f, f, e, op, up>;  // NOLINT

/*!
 \brief Short-cut for the unsigned fixed-point with just 2 template parameters
 n and f.
*/
template<std::size_t n, std::size_t f, int e = 0, class op = libq::ignorance_policy, class up = libq::ignorance_policy>  // NOLINT
using UQ = libq::fixed_point<typename boost::uint_t<n>::least, n-f, f, e, op, up>;  // NOLINT

#define CONSTANT(name, value)\
    template<class T, std::size_t n, std::size_t f, int e, class op, class up>\
    fixed_point<T, n, f, e, op, up> const fixed_point<T, n, f, e, op, up>::name(value);  // NOLINT

CONSTANT(CONST_E, 2.71828182845904523536)
CONSTANT(CONST_1_LOG2E, 0.6931471805599453)
CONSTANT(CONST_LOG2E, 1.44269504088896340736)
CONSTANT(CONST_LOG10E, 0.434294481903251827651)
CONSTANT(CONST_LOG102, 0.301029995663981195214)
CONSTANT(CONST_LN2, 0.693147180559945309417)
CONSTANT(CONST_LN10, 2.30258509299404568402)
CONSTANT(CONST_2PI, 6.283185307179586)
CONSTANT(CONST_PI, 3.14159265358979323846)
CONSTANT(CONST_PI_2, 1.57079632679489661923)
CONSTANT(CONST_PI_4, 0.785398163397448309616)
CONSTANT(CONST_1_PI, 0.318309886183790671538)
CONSTANT(CONST_2_PI, 0.636619772367581343076)
CONSTANT(CONST_2_SQRTPI, 1.12837916709551257390)
CONSTANT(CONST_SQRT2, 1.41421356237309504880)
CONSTANT(CONST_SQRT1_2, 0.707106781186547524401)
CONSTANT(CONST_2SQRT2, 2.82842712474619009760)

#undef CONSTANT
}  // namespace libq

#include "details/sum_traits.inl"
#include "details/mult_of.inl"
#include "details/div_of.inl"

#include "details/sign.inl"

#include "details/ceil.inl"
#include "details/fabs.inl"
#include "details/floor.inl"
#include "details/round.inl"
#include "details/remainder.inl"
#include "details/fmod.inl"
#include "details/numeric_limits.inl"
#include "details/type_traits.inl"

#include "loop_unroller.hpp"

#include "CORDIC/lut/lut.hpp"

#include "CORDIC/log.inl"
#include "CORDIC/sqrt.inl"

#include "CORDIC/sin.inl"
#include "CORDIC/cos.inl"
#include "CORDIC/tan.inl"

#include "CORDIC/exp.inl"

#include "CORDIC/sinh.inl"
#include "CORDIC/cosh.inl"
#include "CORDIC/tanh.inl"

#include "CORDIC/acos.inl"
#include "CORDIC/asin.inl"
#include "CORDIC/atan.inl"

#include "CORDIC/asinh.inl"
#include "CORDIC/acosh.inl"
#include "CORDIC/atanh.inl"

#endif  // INC_LIBQ_FIXED_POINT_HPP_


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

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>
namespace nt2
{
  namespace tag
  {
   /*!
     @brief Pix2_1 generic tag

     Represents the Pix2_1 constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    // 12.56637061
    BOOST_SIMD_CONSTANT_REGISTER( Pix2_1, double
                                , 1, 0x40c90f00
                                , 0x401921fb54400000LL
                                )
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::Pix2_1, Site> dispatching_Pix2_1(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::Pix2_1, Site>();
   }
   template<class... Args>
   struct impl_Pix2_1;
  }
  /*!
    Constant used in modular computation involving \f$\pi\f$

    @par Semantic:

    For type T0:

    @code
    T0 r = Pix2_1<T0>();
    @endcode

    @return a value of type T0
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Pix2_1, Pix2_1);
}

#endif


// Boost.Geometry

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

// This file was modified by Oracle on 2020.
// Modifications copyright (c) 2020 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_BUFFER_POINT_CIRCLE_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_BUFFER_POINT_CIRCLE_HPP

#include <cstddef>

#include <boost/range/value_type.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

#include <boost/geometry/strategies/buffer.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace buffer
{

/*!
\brief Create a circular buffer around a point, on the Earth
\ingroup strategies
\details This strategy can be used as PointStrategy for the buffer algorithm.
    It creates a circular buffer around a point, on the Earth. It can be applied
    for points and multi_points.

\qbk{
[heading Example]
[buffer_geographic_point_circle]
[buffer_geographic_point_circle_output]
[heading See also]
\* [link geometry.reference.algorithms.buffer.buffer_7_with_strategies buffer (with strategies)]
\* [link geometry.reference.strategies.strategy_buffer_point_circle point_circle]
\* [link geometry.reference.strategies.strategy_buffer_point_square point_square]
}
 */
template
<
    typename FormulaPolicy = strategy::andoyer,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class geographic_point_circle
{
public :
    //! \brief Constructs the strategy
    //! \param count number of points for the created circle (if count
    //! is smaller than 3, count is internally set to 3)
    explicit geographic_point_circle(std::size_t count = 90)
        : m_count((count < 3u) ? 3u : count)
    {}

#ifndef DOXYGEN_SHOULD_SKIP_THIS
    //! Fills output_range with a circle around point using distance_strategy
    template
    <
        typename Point,
        typename OutputRange,
        typename DistanceStrategy
    >
    inline void apply(Point const& point,
                DistanceStrategy const& distance_strategy,
                OutputRange& output_range) const
    {
        typedef typename boost::range_value<OutputRange>::type output_point_type;

        typedef typename select_calculation_type
            <
                Point, output_point_type,
                CalculationType
                //double
            >::type calculation_type;

        calculation_type const buffer_distance = distance_strategy.apply(point, point,
                        strategy::buffer::buffer_side_left);

        typedef typename FormulaPolicy::template direct
            <
                calculation_type, true, false, false, false
            > direct_t;

        calculation_type const two_pi = geometry::math::two_pi<calculation_type>();
        calculation_type const pi = geometry::math::pi<calculation_type>();

        calculation_type const diff = two_pi / calculation_type(m_count);
        // TODO: after calculation of some angles is corrected,
        // we can start at 0.0
        calculation_type angle = 0.001;

        for (std::size_t i = 0; i < m_count; i++, angle += diff)
        {
            if (angle > pi)
            {
                angle -= two_pi;
            }

            typename direct_t::result_type
                dir_r = direct_t::apply(get_as_radian<0>(point), get_as_radian<1>(point),
                                        buffer_distance, angle,
                                        m_spheroid);
            output_point_type p;
            set_from_radian<0>(p, dir_r.lon2);
            set_from_radian<1>(p, dir_r.lat2);
            output_range.push_back(p);
        }

        {
            // Close the range
            const output_point_type p = output_range.front();
            output_range.push_back(p);
        }
    }
#endif // DOXYGEN_SHOULD_SKIP_THIS

private :
    std::size_t m_count;
    Spheroid m_spheroid;
};

}} // namespace strategy::buffer

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_BUFFER_POINT_CIRCLE_HPP


/** \file   pr_kmeans.cpp
    \brief  Implement kmeans method with different initialization scheme
    \author Hui Xue
*/

#include "pr_kmeans.h"
#include "log.h"

#include "hoNDArray_reductions.h"
#include "hoNDArray_elemwise.h"
#include "hoNDArray_math.h"
#include "hoNDArray_linalg.h"

#include <boost/math/special_functions/sign.hpp>

#include <random>

namespace Gadgetron {

template <typename T>
kmeans<T>::kmeans()
{
    max_iter_ = 100;
    replicates_ = 10;
    perform_online_update_ = true;

    verbose_ = false;
    perform_timing_ = false;

    gt_timer_local_.set_timing_in_destruction(false);
    gt_timer_.set_timing_in_destruction(false);
}

template <typename T>
kmeans<T>::~kmeans()
{
}

template <typename T>
void kmeans<T>::get_initial_guess_sample(const ArrayType& X, size_t K, ArrayType& C_for_initial)
{
    try
    {
        if (this->perform_timing_) gt_timer_local_.start("get_initial_guess_sample");

        size_t P = X.get_size(0);
        size_t N = X.get_size(1);

        GADGET_CHECK_THROW(N>K);

        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_real_distribution<> dis(0, 1);

        C_for_initial.create(P, K, this->replicates_);
        Gadgetron::clear(C_for_initial);

        size_t n, k;
        for (n = 0; n < this->replicates_; n++)
        {
            for (k = 0; k < K; k++)
            {
                size_t ind = (size_t)(dis(gen)*N);
                if (ind >= N) ind = N - 1;
                memcpy(&C_for_initial(0, k, n), &X(0, ind), sizeof(T)*P);
            }
        }

        if (this->perform_timing_) gt_timer_local_.stop();
    }
    catch(...)
    {
        GERROR_STREAM("Exceptions happened in kmeans<T>::get_initial_guess_sample(...) ... ");
    }
}

template <typename T>
void kmeans<T>::get_initial_guess_uniform(const ArrayType& X, size_t K, ArrayType& C_for_initial)
{
    try
    {
        if (this->perform_timing_) gt_timer_local_.start("get_initial_guess_uniform");

        size_t P = X.get_size(0);
        size_t N = X.get_size(1);

        GADGET_CHECK_THROW(N>K);

        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_real_distribution<> dis(0, 1);

        C_for_initial.create(P, K, this->replicates_);
        Gadgetron::clear(C_for_initial);

        // find the range of X
        std::vector<T> xmin(P, 0);
        std::vector<T> xmax(P, 0);

        size_t p, n, k;

        for (p = 0; p<P; p++)
        {
            xmin[p] = X(p, 0);
            xmax[p] = xmin[p];
            for (n = 1; n < N; n++)
            {
                T v = X(p, n);
                if (v < xmin[p]) xmin[p] = v;
                if (v > xmax[p]) xmax[p] = v;
            }
        }

        for (n = 0; n < this->replicates_; n++)
        {
            for (k = 0; k < K; k++)
            {
                for (p = 0; p < P; p++)
                {
                    T v = xmin[p] + dis(gen) * (xmax[p] - xmin[p]);
                    C_for_initial(p, k, n) = v;
                }
            }
        }

        if (this->perform_timing_) gt_timer_local_.stop();
    }
    catch (...)
    {
        GERROR_STREAM("Exceptions happened in kmeans<T>::get_initial_guess_uniform(...) ... ");
    }
}

template <typename T>
void kmeans<T>::get_initial_guess_cluster(const ArrayType& X, size_t K, ArrayType& C_for_initial)
{
    try
    {
        if (this->perform_timing_) gt_timer_.start("get_initial_guess_cluster");

        size_t P = X.get_size(0);
        size_t N = X.get_size(1);

        GADGET_CHECK_THROW(N>K);

        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_real_distribution<> dis(0, 1);

        C_for_initial.create(P, K, this->replicates_);
        Gadgetron::clear(C_for_initial);

        // randomly pick 20% of data
        double ratio = 0.2;
        size_t M = (size_t)(ratio*N);
        while(M<K && ratio<=1.0)
        {
            ratio = ratio + 0.1;
            M = (size_t)(ratio*N);
        }

        if(M>=N)
        {
            GWARN_STREAM("Too few samples in the data array X ... ");
            this->get_initial_guess_sample(X, K, C_for_initial);
            return;
        }

        ArrayType X_subset;
        X_subset.create(P, M);

        ArrayType C_for_initial_subset;
        ClusterType IDX;
        ArrayType C;
        T sumD;

        size_t n, m;
        for (n = 0; n < this->replicates_; n++)
        {
            for (m = 0; m < M; m++)
            {
                size_t ind = (size_t)(dis(gen)*N);
                if (ind >= N) ind = N - 1;
                memcpy(&X_subset(0, m), &X(0, ind), sizeof(T)*P);
            }

            this->get_initial_guess_sample(X_subset, K, C_for_initial_subset);

            // call kmeans
            this->run(X_subset, K, C_for_initial_subset, IDX, C, sumD);

            memcpy(&C_for_initial(0, 0, n), C.begin(), sizeof(T)*K*P);
        }

        if (this->perform_timing_) gt_timer_.stop();
    }
    catch (...)
    {
        GERROR_STREAM("Exceptions happened in kmeans<T>::get_initial_guess_cluster(...) ... ");
    }
}

template <typename T>
void kmeans<T>::get_initial_guess_kmeansplusplus(const ArrayType& X, size_t K, ArrayType& C_for_initial)
{
    try
    {
        if (this->perform_timing_) gt_timer_.start("get_initial_guess_kmeansplusplus");

        size_t P = X.get_size(0);
        size_t N = X.get_size(1);

        GADGET_CHECK_THROW(N>K);

        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_real_distribution<> dis(0, 1);

        C_for_initial.create(P, K, this->replicates_);
        Gadgetron::clear(C_for_initial);

        ClusterType IDX;
        IDX.resize(N, 0);

        // find the first center
        ArrayType C;
        C.create(P, K);
        Gadgetron::clear(C);

        VectorType norm_C(K, 0);

        ArrayType D;
        D.create(P, N);

        ArrayType D_norm;
        D_norm.create(N);

        ArrayType cumsum_D_norm;
        cumsum_D_norm.create(N);

        ArrayType CX;

        size_t n, i, t, s;

        for (n = 0; n < this->replicates_; n++)
        {
            size_t ind = (size_t)(dis(gen)*N);
            if (ind >= N) ind = N - 1;
            memcpy(&C(0, 0), &X(0, ind), sizeof(T)*P);

            ArrayType aC;
            aC.create(P, C.begin());
            norm_C[0] = Gadgetron::dot(aC, aC,false);

            for (i = 1; i < K; i++)
            {
                // compute distance to the nearest centroid for all data points
                this->compute_dist(X, IDX, C, D);

                // compute norm of distance vector
                this->compute_norm_dist(D, D_norm);

                // compute accumulated distrance
                cumsum_D_norm(0) = D_norm(0);
                for (t = 1; t < N; t++)
                {
                    cumsum_D_norm(t) = cumsum_D_norm(t - 1) + D_norm(t);
                }

                if (std::abs(cumsum_D_norm(N - 1)) < FLT_EPSILON)
                {
                    GERROR_STREAM("std::abs(cumsum_D_norm(N-1))<FLT_EPSILON ... ");
                    // set centroid from i to K

                    for (s = i; s < K; s++)
                    {
                        size_t ind = (size_t)(dis(gen)*N);
                        if (ind >= N) ind = N - 1;
                        memcpy(&C(0, s), &X(0, ind), sizeof(T)*P);
                    }
                    break;
                }

                // convert to probability
                for (t = 0; t < N; t++)
                {
                    cumsum_D_norm(t) /= cumsum_D_norm(N - 1);
                }

                T v = dis(gen);
                for (t = 0; t < N; t++)
                {
                    if (cumsum_D_norm(t)>=v) break;
                }

                memcpy(&C(0, i), &X(0, t), sizeof(T)*P);

                aC.create(P, &C(0, i));
                norm_C[i] = Gadgetron::dot(aC, aC,false);

                // update the IDX
                ArrayType curr_C;
                curr_C.create(P, i+1, C.begin());

                this->update_IDX(X, curr_C, norm_C, IDX);
            }

            memcpy(&C_for_initial(0, 0, n), C.begin(), sizeof(T)*P*K);
        }

        if (this->perform_timing_) gt_timer_.stop();
    }
    catch (...)
    {
        GERROR_STREAM("Exceptions happened in kmeans<T>::get_initial_guess_kmeansplusplus(...) ... ");
    }
}

template <typename T>
void kmeans<T>::run_replicates(const ArrayType& X, size_t K, const ArrayType& C_for_initial, ClusterType& IDX, ArrayType& C, VectorType& sumD_rep, T& sumD)
{
    try
    {
        Gadgetron::GadgetronTimer timer;
        timer.set_timing_in_destruction(false);

        if (this->perform_timing_) gt_timer_.start("run_replicates");

        size_t P = X.get_size(0);
        size_t N = X.get_size(1);

        GADGET_CHECK_THROW(N>K);
        GADGET_CHECK_THROW(C_for_initial.get_size(0) == P);
        GADGET_CHECK_THROW(C_for_initial.get_size(1) == K);

        size_t R = C_for_initial.get_size(2);

        std::vector<ArrayType> C_rep(R);
        std::vector<ClusterType> IDX_rep(R);

        sumD_rep.resize(R, 0);

        size_t r;
        for (r=0; r<R; r++)
        {
            std::stringstream outs;
            outs << "-----> Kmeans, replicate " << r << " out of " << R;

            if (this->verbose_)
            {
                GDEBUG_STREAM(outs.str());
            }

            ArrayType curr_C_initial;
            curr_C_initial.create(P, K, const_cast<T*>(&C_for_initial(0, 0, r)) );

            if (this->perform_timing_) timer.start(outs.str().c_str());
            this->run(X, K, curr_C_initial, IDX_rep[r], C_rep[r], sumD_rep[r]);
            if (this->perform_timing_) timer.stop();

            if(this->verbose_)
            {
                GDEBUG_STREAM("Kmeans, replicate " << r << " out of " << R << " - " << sumD_rep[r]);
            }
        }

        size_t best_r = 0;
        sumD = sumD_rep[0];
        for (r = 1; r < R; r++)
        {
            if(sumD>sumD_rep[r])
            {
                sumD = sumD_rep[r];
                best_r = r;
            }
        }

        IDX = IDX_rep[best_r];
        C = C_rep[best_r];

        if (this->perform_timing_) gt_timer_.stop();
    }
    catch(...)
    {
        GERROR_STREAM("Exceptions happened in kmeans<T>::run_replicates(...) ... ");
    }
}

template <typename T>
void kmeans<T>::run(const ArrayType& X, size_t K, const ArrayType& C_for_initial, ClusterType& IDX, ArrayType& C, T& sumD)
{
    try
    {
        size_t P = X.get_size(0);
        size_t N = X.get_size(1);

        GADGET_CHECK_THROW(N>K);
        GADGET_CHECK_THROW(C_for_initial.get_size(0) == P);
        GADGET_CHECK_THROW(C_for_initial.get_size(1) == K);

        this->replicates_ = C_for_initial.get_size(2);

        IDX.resize(N, 0);
        C.create(P, K);
        Gadgetron::clear(C);
        sumD  = 0;

        C = C_for_initial;

        VectorType norm_C(K, 0);

        size_t k, p;
        for (k=0; k<K; k++)
        {
            T v = 0;
            for (p=0; p<P; p++)
            {
                v += C(p, k)*C(p, k);
            }

            norm_C[k] = v;
        }

        // first round of clustering
        this->update_IDX(X, C, norm_C, IDX);

        ClusterType prev_IDX;
        ArrayType D, D_norm;

        size_t num_iter = 0;
        T prev_sumD = std::numeric_limits<T>::max();

        std::vector<size_t> cluster_size;

        while (num_iter<=this->max_iter_ &&  this->is_clustering_changed(prev_IDX, IDX))
        {
            prev_IDX = IDX;

            // update the centroid
            this->update_centroid(X, IDX, C, norm_C);
            // update clustering
            this->update_IDX(X, C, norm_C, IDX);

            this->compute_dist(X, IDX, C, D);
            this->compute_norm_dist(D, D_norm);

            // if there are clusters having no member, find a point furthest away from its own cluster centroid
            // replace the empty cluster centroid with this point
            std::vector<size_t> empty_clusters;
            this->has_empty_cluster(C, IDX, cluster_size, empty_clusters);

            if (!empty_clusters.empty())
            {
                if (this->verbose_)
                {
                    GDEBUG_STREAM("Kmeas iteration, found empty cluster : iter - " << num_iter);
                }

                size_t e;
                for (e = 0; e < empty_clusters.size(); e++)
                {
                    // find the most "lonely" point
                    size_t lonely(0);
                    this->find_lonely_point(IDX, D_norm, cluster_size, lonely);

                    // replace this empty centroid with the lonely point, update IDX
                    IDX[lonely] = empty_clusters[e];

                    // updates centroid
                    this->update_centroid(X, IDX, C, norm_C);

                    // update distances
                    this->compute_dist(X, IDX, C, D);
                    this->compute_norm_dist(D, D_norm);
                }
            }

            sumD = 0;
            for (p = 0; p<N; p++)
            {
                sumD += D_norm(p)*D_norm(p);
            }

            if (sumD>prev_sumD)
            {
                if (this->verbose_)
                {
                    GDEBUG_STREAM("Kmeas iteration terminated due to increasted total distance : iter " << num_iter << " - " << sumD << " > " << prev_sumD);
                }

                IDX = prev_IDX;

                break;
            }
            else
            {
                prev_sumD = sumD;
            }

            num_iter++;
        }

        if (this->verbose_)
        {
            GDEBUG_STREAM("Kmeas iteration stopped : iter " << num_iter << " - " << sumD);
        }

        if(this->perform_online_update_)
        {
            this->perform_online_update(X, IDX, C, sumD);
            if (this->verbose_)
            {
                GDEBUG_STREAM("Kmeas online update : " << sumD);
            }
        }
    }
    catch (...)
    {
        GERROR_STREAM("Exceptions happened in kmeans<T>::run(...) ... ");
    }
}

template <typename T>
void kmeans<T>::compute_dist(const ArrayType& X, const ClusterType& IDX, const ArrayType& C, ArrayType& D)
{
    try
    {
        size_t P = X.get_size(0);
        size_t N = X.get_size(1);

        size_t K = C.get_size(1);

        GADGET_CHECK_THROW(N== IDX.size());

        D.create(P, N);
        Gadgetron::clear(D);

        const T* pX = X.begin();
        const T* pC = C.begin();
        T* pD = D.begin();

        long long p, n;

#pragma omp parallel for default(none) private(n, p) shared(N, IDX, K, P, pD, pX, pC)
        for (n=0; n<N; n++)
        {
            size_t nC = IDX[n];

            if (nC >= K)
            {
                // GWARN_STREAM("nC >= K :" << nC << " - " << K << " for " << n);
                continue;
            }

            for (p=0; p<P; p++)
            {
                pD[p + n*P] = pX[p + n*P] - pC[p + nC*P];
            }
        }
    }
    catch (...)
    {
        GERROR_STREAM("Exceptions happened in kmeans<T>::compute_dist(...) ... ");
    }
}

template <typename T>
void kmeans<T>::compute_norm_dist(const ArrayType& D, ArrayType& D_norm)
{
    try
    {
        size_t P = D.get_size(0);
        size_t N = D.get_size(1);

        D_norm.create(N);
        Gadgetron::clear(D_norm);

        const T* pD = D.begin();

        long long n, p;

#pragma omp parallel for default(none) private(n, p) shared(N, P, pD, D_norm)
        for(n=0; n<N; n++)
        {
            T v = 0;
            for (p = 0; p<P; p++)
            {
                v += pD[p + n*P] * pD[p + n*P];
            }

            D_norm(n) = std::sqrt(v);
        }
    }
    catch (...)
    {
        GERROR_STREAM("Exceptions happened in kmeans<T>::compute_norm_dist(...) ... ");
    }
}

template <typename T>
void kmeans<T>::update_IDX(const ArrayType& X, const ArrayType& C, const VectorType& norm_C, ClusterType& IDX)
{
    try
    {
        size_t P = X.get_size(0);
        size_t N = X.get_size(1);

        size_t K = C.get_size(1);

        IDX.resize(N);

        ArrayType CX;
        Gadgetron::gemm(CX, C, true, X, false);

        size_t t, s;

        for (t = 0; t < N; t++)
        {
            for (s = 0; s < K; s++)
            {
                CX(s, t) = 2* CX(s, t) - norm_C[s];
            }

            T maxCX = CX(0, t);
            IDX[t] = 0;
            for (s = 1; s < K; s++)
            {
                if (CX(s, t) > maxCX)
                {
                    maxCX = CX(s, t);
                    IDX[t] = s;
                }
            }
        }
    }
    catch (...)
    {
        GERROR_STREAM("Exceptions happened in kmeans<T>::update_IDX(...) ... ");
    }
}

template <typename T>
void kmeans<T>::update_centroid(const ArrayType& X, const ClusterType& IDX, ArrayType& C, VectorType& norm_C)
{
    try
    {
        size_t P = X.get_size(0);
        size_t N = X.get_size(1);
        const T* pX = X.begin();

        size_t K = C.get_size(1);

        norm_C.resize(K);

        std::vector<size_t> num_in_C(K, 0);

        Gadgetron::clear(C);
        T* pC = C.begin();

        size_t n, p;
        for (n=0; n<N; n++)
        {
            size_t currK = IDX[n];

            if(currK<K)
            {
                for (p=0; p<P; p++)
                {
                    pC[p+currK*P] += pX[p+n*P];
                }

                num_in_C[currK]++;
            }
            else
            {
                GERROR_STREAM("kmeans, currC>=K, in update_centroid : " << n);
            }
        }

        for (n = 0; n < K; n++)
        {
            T v = 0;
            for (p = 0; p<P; p++)
            {
                if (num_in_C[n] > 0)
                {
                    pC[p + n*P] /= num_in_C[n];
                    v += pC[p + n*P] * pC[p + n*P];
                }
            }

            norm_C[n] = v;
        }
    }
    catch (...)
    {
        GERROR_STREAM("Exceptions happened in kmeans<T>::update_centroid(...) ... ");
    }
}

template <typename T>
void kmeans<T>::has_empty_cluster(const ArrayType& C, const ClusterType& IDX, std::vector<size_t>& cluster_size, std::vector<size_t>& empty_clusters)
{
    try
    {
        size_t K = C.get_size(1);
        size_t N = IDX.size();

        cluster_size.resize(K, 0);

        size_t n;
        for (n=0; n<N; n++)
        {
            if (IDX[n] < K)
            {
                cluster_size[IDX[n]]++;
            }
        }

        empty_clusters.clear();
        for (n = 0; n < K; n++)
        {
            if(cluster_size[n]==0)
            {
                empty_clusters.push_back(n);
            }
        }
    }
    catch (...)
    {
        GERROR_STREAM("Exceptions happened in kmeans<T>::has_empty_cluster(...) ... ");
    }
}

template <typename T>
void kmeans<T>::find_lonely_point(const ClusterType& IDX, const ArrayType& D_norm, const std::vector<size_t>& cluster_size, size_t& lonely)
{
    try
    {
        size_t N = IDX.size();

        T maxD (0);
        Gadgetron::maxAbsolute(D_norm, maxD, lonely);

        size_t cluster = IDX[lonely];

        size_t n, c;

        if(cluster_size[cluster]<2)
        {
            // if the picked cluster has only one member ...
            std::vector<size_t> big_clusters;
            for (c=0; c<cluster_size.size(); c++)
            {
                if(cluster_size[c]>2)
                {
                    big_clusters.push_back(c);
                }
            }

            if(big_clusters.empty())
            {
                GADGET_THROW("All clusters have only one member ... ");
            }
            else
            {
                size_t cluster_picked = big_clusters[big_clusters.size() / 2]; // pick one cluster

                // find the furthest point in the picked cluster
                lonely = 0;
                maxD = 0;
                for (n=0; n<N; n++)
                {
                    if(IDX[n]==cluster_picked)
                    {
                        if(D_norm(n)>maxD)
                        {
                            maxD = D_norm(n);
                            lonely = n;
                        }
                    }
                }
            }
        }
    }
    catch (...)
    {
        GERROR_STREAM("Exceptions happened in kmeans<T>::find_lonely_point(...) ... ");
    }
}

template <typename T>
bool kmeans<T>::is_clustering_changed(const ClusterType&prev_IDX, const ClusterType& IDX)
{
    try
    {
        if (prev_IDX.size() != IDX.size()) return true;

        size_t N = IDX.size();

        size_t n;
        for (n=0; n<N; n++)
        {
            if (prev_IDX[n] != IDX[n])
            {
                return true;
            }
        }

        return false;
    }
    catch (...)
    {
        GERROR_STREAM("Exceptions happened in kmeans<T>::is_clustering_changed(...) ... ");
        return false;
    }
}

template <typename T>
void kmeans<T>::perform_online_update(const ArrayType& X, ClusterType& IDX, ArrayType& C, T& sumD)
{
    try
    {
        size_t P = X.get_size(0);
        size_t N = X.get_size(1);
        const T* pX = X.begin();

        size_t K = C.get_size(1);

        ArrayType del_cost; // store the delat change of sum cost if reassign a point
        del_cost.create(N, K);

        // count the number of points in each cluster
        std::vector<size_t> num_pt_clusters(K, 0);

        size_t n, p, k;

        num_pt_clusters.resize(K, 0);
        for (n = 0; n < N; n++)
        {
            num_pt_clusters[IDX[n]]++;
        }

        size_t iter(0);

        size_t lastmoved = 0;
        size_t nummoved = 0;
        ClusterType prevIDX, newIDX(IDX);

        while (iter < this->max_iter_)
        {
            // for every cluster K and every point N
            // compute change of delta sum cost
            for (k = 0; k < K; k++)
            {
                for (n = 0; n < N; n++)
                {
                    T v;
                    if (IDX[n] == k)
                    {
                        if (num_pt_clusters[k] > 1)
                            v = (T)num_pt_clusters[k] / (T)(num_pt_clusters[k] - 1);
                        else
                            v = 1;
                    }
                    else
                        v = (T)num_pt_clusters[k] / (T)(num_pt_clusters[k] + 1);

                    T t(0), d = 0;
                    for (p = 0; p < P; p++)
                    {
                        t = X(p, n) - C(p, k);
                        d += t*t;
                    }

                    del_cost(n, k) = v * d;
                }
            }

            prevIDX = IDX;

            // get the new IDX
            for (n = 0; n < N; n++)
            {
                newIDX[n] = 0;
                T min_del_cost = del_cost(n, 0);
                for (k = 1; k < K; k++)
                {
                    if(del_cost(n, k) < min_del_cost)
                    {
                        newIDX[n] = k;
                        min_del_cost = del_cost(n, k);
                    }
                }
            }

            // marked the moving points
            std::vector<size_t> moved;
            for (n = 0; n < N; n++)
            {
                if(prevIDX[n] != newIDX[n])
                {
                    moved.push_back(n);
                }
            }

            // if no candidates to move, stop
            if(moved.empty())
            {
                iter++;
                break;
            }

            // pick a point to move
            int moved_ind = N+1;
            int tt(0);
            for (size_t ii = 0; ii < moved.size(); ii++)
            {
                tt = (int)moved[ii] - (int)lastmoved - 1;
                if (tt < 0) tt += N;
                if (tt >= N) tt -= N;

                tt += lastmoved;

                if (tt < moved_ind) moved_ind = tt;
            }

            if (tt < 0) tt += N;
            if (tt >= N) tt -= N;
            moved_ind = (size_t)(tt);

            if(moved_ind<=lastmoved)
            {
                iter++;
                if (iter >= this->max_iter_) break;
                nummoved = 0;
            }

            nummoved++;
            lastmoved = moved_ind;

            size_t oidx = IDX[moved_ind];
            size_t nidx = newIDX[moved_ind];

            sumD = sumD + del_cost(moved_ind, nidx) - del_cost(moved_ind, oidx);

            IDX[moved_ind] = nidx;

            num_pt_clusters[oidx]--;
            num_pt_clusters[nidx]++;

            for (p=0; p<P; p++)
            {
                C(p, nidx) = C(p, nidx) + (X(p, moved_ind) - C(p, nidx)) / num_pt_clusters[nidx];
                C(p, oidx) = C(p, oidx) - (X(p, moved_ind) - C(p, oidx)) / num_pt_clusters[oidx];
            }
        }
    }
    catch (...)
    {
        GERROR_STREAM("Exceptions happened in kmeans<T>::perform_online_update(...) ... ");
    }
}

// ------------------------------------------------------------
// Instantiation
// ------------------------------------------------------------

template class EXPORTPR kmeans< float >;
template class EXPORTPR kmeans< double >;

}


//
//  Copyright (c) 2012, Institue of Cancer Research.
//  All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//     * Neither the name of Institue of Cancer Research.
//       nor the names of its contributors may be used to endorse or promote
//       products derived from this software without specific prior written
//       permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// For more information on the Plane of Best Fit please see
// http://pubs.acs.org/doi/abs/10.1021/ci300293f
//
//  If this code has been useful to you, please include the reference
//  in any work which has made use of it:

//  Plane of Best Fit: A Novel Method to Characterize the Three-Dimensionality
//  of Molecules, Nicholas C. Firth, Nathan Brown, and Julian Blagg, Journal of
//  Chemical Information and Modeling 2012 52 (10), 2516-2525

//
//
// Created by Nicholas Firth, November 2011
// Modified by Greg Landrum for inclusion in the RDKit distribution November
// 2012
// Further modified by Greg Landrum for inclusion in the RDKit core September
// 2016
//

#include <GraphMol/RDKitBase.h>
#include <GraphMol/MolTransforms/MolTransforms.h>

#include "PBF.h"
#include <Numerics/Matrix.h>
#include <Numerics/SquareMatrix.h>
#include <Numerics/SymmMatrix.h>
#include <boost/foreach.hpp>

#include <Eigen/Dense>

namespace RDKit {
namespace Descriptors {
namespace {

double distanceFromAPlane(const RDGeom::Point3D &pt,
                          const std::vector<double> &plane, double denom) {
  double numer = 0.0;
  numer =
      std::abs(pt.x * plane[0] + pt.y * plane[1] + pt.z * plane[2] + plane[3]);

  return numer / denom;
}

bool getBestFitPlane(const Conformer &conf,
                     const std::vector<RDGeom::Point3D> &points,
                     std::vector<double> &plane,
                     const std::vector<double> *weights) {
  PRECONDITION((!weights || weights->size() >= points.size()),
               "bad weights vector");
  PRECONDITION(plane.size() >= 4, "bad plane");
  RDGeom::Point3D origin(0, 0, 0);
  double wSum = 0.0;

  for (unsigned int i = 0; i < points.size(); ++i) {
    if (weights) {
      double w = (*weights)[i];
      wSum += w;
      origin += points[i] * w;
    } else {
      wSum += 1;
      origin += points[i];
    }
  }
  origin /= wSum;

  Eigen::Matrix3d evects;
  Eigen::Vector3d evals;
  MolTransforms::computePrincipalAxesAndMomentsFromGyrationMatrix(
      conf, evects, evals, false, weights);
  RDGeom::Point3D normal;
  normal.x = evects(0, 0);
  normal.y = evects(1, 0);
  normal.z = evects(2, 0);

  plane[0] = normal.x;
  plane[1] = normal.y;
  plane[2] = normal.z;
  plane[3] = -1 * normal.dotProduct(origin);
  return true;
}

}  // end of anonymous namespace

double PBF(const ROMol &mol, int confId) {
  PRECONDITION(mol.getNumConformers() >= 1, "molecule has no conformers")
  unsigned int numAtoms = mol.getNumAtoms();
  if (numAtoms < 4) return 0;

  const Conformer &conf = mol.getConformer(confId);
  if (!conf.is3D()) return 0;

  std::vector<RDGeom::Point3D> points;
  points.reserve(numAtoms);
  for (unsigned int i = 0; i < numAtoms; ++i) {
    points.push_back(conf.getAtomPos(i));
  }

  std::vector<double> plane(4);
  if (!getBestFitPlane(conf, points, plane, NULL)) {
    // the eigenvalue calculation failed, return 0
    // FIX: throw an exception here?
    return 0.0;
  }

  double denom = 0.0;
  for (unsigned int i = 0; i < 3; ++i) {
    denom += plane[i] * plane[i];
  }
  denom = sqrt(denom);

  double res = 0.0;
  for (unsigned int i = 0; i < numAtoms; ++i) {
    res += distanceFromAPlane(points[i], plane, denom);
  }
  res /= numAtoms;

  return res;
}

}  // end of Descriptors namespace
}  // end of RDKit namespace