/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     hypothesis.cpp
 * \author   Collin Johnson
 *
 * Implementation of AreaHypothesis.
 */

#include "hssh/local_topological/area_detection/labeling/hypothesis.h"
#include "hssh/local_topological/area_detection/labeling/area_graph.h"
#include "hssh/local_topological/area_detection/labeling/area_proposal.h"
#include "hssh/local_topological/area_detection/labeling/boundary.h"
#include "hssh/local_topological/area_detection/labeling/debug.h"
#include "hssh/local_topological/area_detection/labeling/hypothesis_features.h"
#include "hssh/local_topological/area_detection/labeling/loops_and_trees.h"
#include "hssh/local_topological/area_detection/labeling/path_endpoints.h"
#include "hssh/local_topological/area_detection/labeling/small_scale_star_builder.h"
#include "hssh/local_topological/area_detection/local_topo_isovist_field.h"
#include "hssh/local_topological/area_extent.h"
#include "hssh/local_topological/voronoi_skeleton_grid.h"
#include "math/covariance.h"
#include "math/geometry/convex_hull.h"
#include "math/geometry/shape_fitting.h"
#include "utils/algorithm_ext.h"
#include "utils/func_ptr.h"
#include <algorithm>
#include <array>
#include <boost/core/null_deleter.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/range/algorithm_ext.hpp>
#include <boost/range/as_array.hpp>
#include <boost/tuple/tuple.hpp>
#include <cassert>
#include <iomanip>
#include <iostream>
#include <queue>

// #define DEBUG_EIG_STATS
// #define DEBUG_CONTAINMENT
// #define DEBUG_ENDPOINTS

namespace vulcan
{
namespace hssh
{

std::vector<AreaHypothesis*> extract_hypotheses(const std::vector<const AreaHypothesisBoundary*>& boundaries);

AreaHypothesis::AreaHypothesis(int id,
                               AreaGraph* graph,
                               const std::set<AreaNode*>& nodes,
                               const std::set<AreaEdge*>& edges,
                               const VoronoiSkeletonGrid* grid,
                               const VoronoiIsovistField* isovistField,
                               const utils::VisibilityGraph* visGraph,
                               const utils::VisibilityGraph* skeletonGraph,
                               const SmallScaleStarBuilder* starBuilder)
: areaGraph_(graph)
, nodes_(nodes.begin(), nodes.end())
, edges_(edges.begin(), edges.end())
, grid_(grid)
, isovistField_(isovistField)
, visGraph_(visGraph)
, skelGraph_(skeletonGraph)
, starBuilder_(starBuilder)
, boundariesDirty_(true)
, type_(HypothesisType::kArea)
, frontier_(false)
, boundary_(false)
, id_(id)
, length_(0.0)
, width_(0.0)
{
    assert(!nodes_.empty());
    assert(!edges_.empty());
    assert(std::find(nodes_.begin(), nodes_.end(), nullptr) == nodes_.end());
    //     assert(std::find(edges_.begin(), edges_.end(), nullptr) == edges_.end());

    assert(edges_.find(nullptr) == edges_.end());

    initialize();

    // Must find path endpoints after initialize is called because that is where the axis direction is established.
    findPathEndpoints();

    for (auto cell : cells_) {
        if (visGraph->isVertex(cell)) {
            visibilityVertices_.push_back(cell);
        }
    }

    for (auto node : nodes_) {
        graphVertices_.push_back(node->getCell());
    }

    // Sanity check the nodes for the hypothesis and their loop conditions. If no boundaries are on a loop, but
    // there's a loop node, then turn it to a non-loop because it is local to the hypothesis, which means that it
    // comes from a small loop in the skeleton, not a larger loop through the map
    bool haveBoundaryLoop = false;
    for (auto node : nodes) {
        if ((node->getType() & AreaNode::kGateway) && node->isLoop()) {
            haveBoundaryLoop = true;
            break;
        }
    }

    // Go through all the nodes and turn off any loops if no boundaries are loops
    if (!haveBoundaryLoop) {
        for (auto node : nodes) {
            if ((~node->getType() & AreaNode::kGateway) && node->isLoop() && !haveBoundaryLoop) {
                node->setLoop(false);
            }
        }
    }
}

AreaHypothesis::AreaHypothesis(const std::vector<const AreaHypothesisBoundary*>& boundaries,
                               HypothesisType classification,
                               bool needExtent)
: AreaHypothesis(extract_hypotheses(boundaries), needExtent)
{
    assert(!boundaries.empty());
    type_ = classification;
}

AreaHypothesis::AreaHypothesis(const std::vector<AreaHypothesis*>& areas, bool needExtent)
: endNodes_{{nullptr, nullptr}}
, boundariesDirty_(true)
, frontier_(false)
, boundary_(false)
, length_(0.0f)
{
    assert(!areas.empty());

    id_ = areas.front()->getId();
    areaGraph_ = areas.front()->areaGraph_;

    std::set<AreaNode*> mergedNodes;
    //     std::set<AreaEdge*>       mergedEdges;
    std::set<const AreaHypothesisBoundary*> mergedBoundaries;

    std::size_t numSkeletonCells = 0;

    for (auto area : areas) {
        grid_ = area->grid_;
        isovistField_ = area->isovistField_;
        visGraph_ = area->visGraph_;
        skelGraph_ = area->skelGraph_;
        starBuilder_ = area->starBuilder_;

        mergedNodes.insert(area->nodes_.begin(), area->nodes_.end());
        edges_.insert(area->edges_.begin(), area->edges_.end());
        numSkeletonCells += area->cells_.size();
        mergedBoundaries.insert(area->boundaries_.begin(), area->boundaries_.end());

        boost::push_back(visibilityVertices_, boost::as_array(area->visibilityVertices_));
        boost::push_back(graphVertices_, boost::as_array(area->graphVertices_));

        boundary_ |= area->boundary_;
    }

    std::sort(visibilityVertices_.begin(), visibilityVertices_.end());
    utils::erase_unique(visibilityVertices_);

    std::sort(graphVertices_.begin(), graphVertices_.end());
    utils::erase_unique(graphVertices_);

    // One type of invalid area will have a boundary appear multiple times because the gateways doesn't actually enclose
    // the space, and thus a single hypothesis contains the edges on both sides of the boundary. This boundary will be
    // removed, but this case needs to be handled properly.
    auto unequalBoundaryCondition = [&areas](const AreaHypothesisBoundary* toCopy) -> bool {
        std::array<bool, 2> similar = {{false, false}};
        for (auto a : areas) {
            if (!similar[0] && toCopy->getHypotheses()[0]->isSimilar(*a)) {
                similar[0] = true;
            } else if (!similar[1] && toCopy->getHypotheses()[1]->isSimilar(*a)) {
                similar[1] = true;
            }
        }
        return !similar[0] || !similar[1];
    };

    boundaries_.reserve(mergedBoundaries.size());
    std::copy_if(mergedBoundaries.begin(),
                 mergedBoundaries.end(),
                 back_inserter(boundaries_),
                 unequalBoundaryCondition);

    nodes_.reserve(mergedNodes.size());
    nodes_.insert(nodes_.end(), mergedNodes.begin(), mergedNodes.end());

    if (boundaries_.empty()) {
        // If there are no boundaries, then there must be at least two termination points for the area
        assert(getPoints(AreaNode::kDeadEnd | AreaNode::kFrontier | AreaNode::kJunction).size() >= 2);
    }

    // For every boundary, copy over its max path dist -- yes this might run through the area, but it should be a
    // close enough approximation. The path most likely won't since that path would have to be internal to get
    // through the merged area though
    for (auto& area : areas) {
        for (auto& node : area->maxPathDist_) {
            bool isBoundary = utils::contains_if(boundaries_, [node](const auto& bnd) {
                return bnd->getNode() == node.first;
            });

            if (isBoundary) {
                maxPathDist_[node.first] = node.second;
            }
        }
    }

    assert(std::find(nodes_.begin(), nodes_.end(), nullptr) == nodes_.end());

    for (auto& e : edges_) {
        length_ += e->getLength();
        width_ += e->getAverageWidth() * e->getLength();
    }

    if (length_ > 0.0) {
        width_ /= length_;
    }

    // Copy over all skeleton cells and merge the extents into one

    if (needExtent) {
        bool haveExtents = true;

        std::vector<const AreaExtent*> extents;
        extents.reserve(areas.size());
        cells_.reserve(numSkeletonCells);
        for (auto& hyp : areas) {
            cells_.insert(cells_.end(), hyp->cells_.begin(), hyp->cells_.end());
            extents.push_back(hyp->extent_.get());
            haveExtents &= hyp->extent_ != nullptr;
        }

        if (haveExtents) {
            extent_.reset(new AreaExtent(extents));
            calculateAxis();
            polygonBoundary_ = extent_->polygonBoundary(math::ReferenceFrame::GLOBAL);
            rectangleBoundary_ = extent_->rectangleBoundary(math::ReferenceFrame::GLOBAL);
            center_ = extent_->center().toPoint();
        } else {
            initialize();
        }
    } else {
        // Create the polygon boundary via convex hull of the individual boundaries
        std::vector<Point<double>> hullPoints;
        for (auto& hyp : areas) {
            hullPoints.insert(hullPoints.end(), hyp->polygonBoundary_.begin(), hyp->polygonBoundary_.end());
        }
        polygonBoundary_ = math::convex_hull<double>(hullPoints.begin(), hullPoints.end());
        rectangleBoundary_ =
          math::axis_aligned_bounding_rectangle<double>(polygonBoundary_.begin(), polygonBoundary_.end());

        // Compute weighted mean of center
        center_ = Point<double>(0, 0);
        double totalArea = 0.0;
        for (auto& hyp : areas) {
            center_.x += hyp->center().x * hyp->extent_->area();
            center_.y += hyp->center().y * hyp->extent_->area();
            totalArea += hyp->extent_->area();
        }
        center_.x /= totalArea;
        center_.y /= totalArea;

        double sumCos = 0.0;
        double sumSin = 0.0;
        double sumEcc = 0.0;

        for (auto& hyp : areas) {
            sumCos += std::cos(hyp->axisDirection_) * hyp->cells_.size();
            sumSin += std::sin(hyp->axisDirection_) * hyp->cells_.size();
            sumEcc += hyp->axisRatio_ * hyp->cells_.size();
        }

        axisDirection_ = std::atan2(sumSin, sumCos);
        axisRatio_ = sumEcc / numSkeletonCells;
    }

    mergeEndpoints(areas);
}

AreaHypothesis::~AreaHypothesis(void)
{
    // For unique_ptr
}

Point<float> AreaHypothesis::center(void) const
{
    return center_;
}

math::Rectangle<float> AreaHypothesis::rectangleBoundary(void) const
{
    //     assert(extent_);
    //     return extent_->rectangleBoundary(math::ReferenceFrame::GLOBAL);
    return rectangleBoundary_;
}

Endpoints AreaHypothesis::endpoints(void) const
{
    return std::make_pair(endNodes_[0]->getPosition(), endNodes_[1]->getPosition());
}

bool AreaHypothesis::isSimilar(const AreaHypothesis& rhs) const
{
    // Always similar to self
    if (&rhs == this) {
        return true;
    }

    for (auto& edge : edges_) {
        if (rhs.edges_.find(edge) != rhs.edges_.end()) {
            return true;
        }
    }

    return false;
}

HypothesisContainment AreaHypothesis::amountContained(const AreaHypothesis& rhs) const
{
    //  amountContained determines how much this hypothesis contains the input hypothesis. The containment is determined
    //  by comparing the boundaries of rhs with gateway nodes in this hypothesis. If rhs.boundaries is a subset of the
    //  gateway nodes in this hypothesis, the containment is complete. If only some of rhs.boundaries are gateway nodes,
    //  the containment is partial. If none of rhs.boundaries are gateway nodes, there is no containment.

    std::size_t numContained = 0;

    // Keep the most recent match around for disambiguating the one-boundary match areas
    const AreaNode* lhsMatch = nullptr;
    const AreaNode* rhsMatch = nullptr;

    // Search through the boundaries in rhs
    for (auto& b : rhs.boundaries_) {
        auto boundaryGateway = b->getGateway();

        // See if any of the internal or boundary gateway nodes are similar to the boundary's gateway
        for (auto& n : nodes_) {
            if ((n->getType() & AreaNode::kGateway) && boundaryGateway.isSimilarTo(n->getGateway())) {
                lhsMatch = n;
                rhsMatch = b->getNode();
                ++numContained;
                break;
            }
        }
    }

#ifdef DEBUG_CONTAINMENT
    std::cout << "DEBUG:AreaHypothesis:Contained:" << numContained << " Total:" << rhs.numBoundaries() << '\n';
#endif

    if (numContained == 0) {
        return HypothesisContainment::none;
    } else if (numContained > 1) {
        if (numContained < rhs.numBoundaries()) {
            return HypothesisContainment::partial;
        } else   // numContained == rhs.numBoundaries()
        {
            return HypothesisContainment::complete;
        }
    } else   // numContained == 1
    {
        assert(lhsMatch);
        assert(rhsMatch);
        return singleBoundaryContainment(lhsMatch, rhs, rhsMatch);
    }
}

bool AreaHypothesis::addBoundary(const AreaHypothesisBoundary* boundary)
{
    if (std::find(nodes_.begin(), nodes_.end(), boundary->getNode()) == nodes_.end()) {
        std::cerr << "WARNING:AreaHypothesis::addBoundary: Attempted to add invalid boundary.\n";
        return false;
    }

    // Trying to add a duplicate boundary means the graph is being constructed incorrectly and should fail
    assert(std::find(boundaries_.begin(), boundaries_.end(), boundary) == boundaries_.end());
    boundaries_.push_back(boundary);
    boundariesDirty_ = true;

    return true;
}

bool AreaHypothesis::isValid(void) const
{
    // Need at least two cells to calculate valid isovist statistics
    if (cells_.size() < 5) {
        if (extent_) {
            std::cout << "Invalid area. " << rectangleBoundary() << " Not enough cells: " << cells_.size() << '\n';
        }
        return false;
    }

    if (edges_.size() > 1) {
        return true;
    }

    // The type of an edge is the OR of the endpoint types. If both endpoints are gateways, then the edge type will
    // be exactly GATEWAY_NODE, and thus the edge is valid.
    AreaEdge* edge = *edges_.begin();
    if ((edge->getEndpoint(0)->getType() & AreaNode::kGateway)
        && (edge->getEndpoint(1)->getType() & AreaNode::kGateway)) {
        return true;
    }

    assert(extent_);
    if (extent_->area() <= 0.5) {
        std::cout << "Invalid area. " << rectangleBoundary() << " Not big enough: " << extent_->area() << '\n';
    }
    return (extent_->area() > 0.5);

    // The estimated length is the measured length + the distance from the end of the edge to the wall
    // which is just half the width because the width comes from the voronoi skeleton
    //     return (nodes_.size() > 1) && (length_ > 1.0);
}

SmallScaleStar AreaHypothesis::calculateStar(HypothesisType typeMask) const
{
    return calculateStar(typeMask, &AreaHypothesis::getGatewaysFullType);
}

std::vector<std::pair<const Gateway*, const Gateway*>> AreaHypothesis::pairwiseUnalignedGateways(void) const
{
    std::vector<std::pair<const Gateway*, const Gateway*>> unaligned;

    for (auto bIt = beginBoundary(); bIt < endBoundary(); ++bIt) {
        const Gateway& firstGateway = (*bIt)->getGateway();

        for (auto nextIt = bIt + 1; nextIt < endBoundary(); ++nextIt) {
            const Gateway& secondGateway = (*nextIt)->getGateway();

            if (!starBuilder_->areGatewaysAligned(firstGateway, secondGateway, center())) {
                unaligned.push_back(std::make_pair(&firstGateway, &secondGateway));
            }
        }
    }

    return unaligned;
}

std::vector<std::pair<const Gateway*, const Gateway*>> AreaHypothesis::pairwiseAlignedGateways(void) const
{
    std::vector<std::pair<const Gateway*, const Gateway*>> aligned;

    for (auto bIt = beginBoundary(); bIt < endBoundary(); ++bIt) {
        const Gateway& firstGateway = (*bIt)->getGateway();

        for (auto nextIt = bIt + 1; nextIt < endBoundary(); ++nextIt) {
            const Gateway& secondGateway = (*nextIt)->getGateway();

            if (starBuilder_->areGatewaysAligned(firstGateway, secondGateway, center())) {
                aligned.push_back(std::make_pair(&firstGateway, &secondGateway));
            }
        }
    }

    return aligned;
}

int AreaHypothesis::numGatewaysAlignedToAxis(HypothesisType typeMask) const
{
    auto gateways = getGatewaysFullType(typeMask);
    return starBuilder_->numGatewaysAlignedToAxis(gateways, axisDirection_);
}

int AreaHypothesis::numNonGatewayEnds(void) const
{
    const auto kEndType = AreaNode::kFrontier | AreaNode::kDeadEnd;

    int numNonGateway = 0;

    for (auto& node : endNodes_) {
        if (node && (node->getType() & kEndType)) {
            ++numNonGateway;
        }
    }

    return numNonGateway;
}

bool AreaHypothesis::isEndGateway(int32_t gatewayId) const
{
    for (auto& node : endNodes_) {
        if (node && (node->getType() & AreaNode::kGateway) && (node->getGateway().id() == gatewayId)) {
            return true;
        }
    }

    return false;
}

int AreaHypothesis::numEndpointsWithType(HypothesisType type) const
{
    int count = 0;

    for (auto& node : endNodes_) {
        for (auto& boundary : boundaries_) {
            if (boundary->getNode() == node) {
                auto otherGraph = boundary->getOtherHypothesis(*this);
                assert(otherGraph);
                if (is_hypothesis_type(otherGraph->getGatewayType(*(boundary->getNode())), type)) {
                    ++count;
                }
                break;
            }
        }
    }

    return count;
}

std::size_t AreaHypothesis::countGatewaysWithMask(HypothesisType typeMask) const
{
    std::size_t count = 0;

    for (auto& boundary : boundaries_) {
        auto otherGraph = boundary->getOtherHypothesis(*this);
        assert(otherGraph);
        if (is_hypothesis_type(otherGraph->getGatewayType(*(boundary->getNode())), typeMask)) {
            ++count;
        }
    }

    return count;
}

std::vector<AreaHypothesis*> AreaHypothesis::findAdjacentHypotheses(void) const
{
    std::vector<AreaHypothesis*> adjacent;

    for (auto& boundary : boundaries_) {
        auto otherGraph = boundary->getOtherHypothesis(*this);
        if (otherGraph) {
            adjacent.push_back(otherGraph);
        }
    }

    return adjacent;
}

AreaHypothesis* AreaHypothesis::adjacentArea(const Gateway& gateway)
{
    for (auto& boundary : boundaries_) {
        if (boundary->getGateway() == gateway) {
            return boundary->getOtherHypothesis(*this);
        }
    }

    return nullptr;
}

bool AreaHypothesis::hasMergeableBoundary(void) const
{
    for (auto boundary : boundaries_) {
        if (boundary->shouldMerge(BoundaryMergeCondition::kSameType)) {
            return true;
        }
    }

    return false;
}

bool AreaHypothesis::isFrontier(void) const
{
    return extent_->frontierRatio() > 0.5;
}

bool AreaHypothesis::isBoundary(void) const
{
    return boundary_;
}

const HypothesisFeatures& AreaHypothesis::features(void) const
{
    calculateFeaturesIfNeeded();
    return *features_;
}

HypothesisType AreaHypothesis::getType(void) const
{
    return type_;
}

HypothesisType AreaHypothesis::getGatewayType(const AreaNode& gateway) const
{
    auto currentType = getType();

    if (currentType != HypothesisType::kPath) {
        return currentType;
    }

    return isEndpoint(gateway) ? HypothesisType::kPathEndpoint : HypothesisType::kPathDest;
}

AreaProposal AreaHypothesis::toAreaProposal(void) const
{
    AreaType proposalClassification;

    HypothesisType proposalType = getType();

    if (proposalType == HypothesisType::kArea) {
        proposalClassification = AreaType::area;
        std::cerr << "WARNING::AreaSegmenter: Invalid final classification of hypothesis. Creating a generic area.\n";
    } else if (is_hypothesis_type(proposalType, HypothesisType::kPath)) {
        proposalClassification = AreaType::path_segment;
    } else if (is_hypothesis_type(proposalType, HypothesisType::kDecision)) {
        proposalClassification = AreaType::decision_point;
    } else if (is_hypothesis_type(proposalType, HypothesisType::kDest)) {
        proposalClassification = AreaType::destination;
    } else {
        assert(is_hypothesis_type(proposalType, HypothesisType::kArea) && "Hypothesis type didn't match area!");
        proposalClassification = AreaType::area;
    }

    // A path is created with different data than a place
    if (proposalClassification == AreaType::path_segment) {
        AreaProposal::path_endpoint_t endpoints[2];
        for (int n = 0; n < 2; ++n) {
            assert(endNodes_[n]);

            // A gateway node will be on the boundary, see if this node is a boundary node or not
            auto bIt = std::find_if(boundaries_.begin(), boundaries_.end(), [n, this](const AreaHypothesisBoundary* b) {
                return b->getNode() == endNodes_[n];
            });

            endpoints[n].point = endNodes_[n]->getPosition();

            if (bIt != boundaries_.end()) {
                endpoints[n].type = AreaType::area;
                endpoints[n].isGatewayEndpoint = true;
                endpoints[n].gateway = (*bIt)->getGateway();
            } else {
                endpoints[n].isGatewayEndpoint = false;
                if (endNodes_[n]->getType() & AreaNode::kFrontier) {
                    endpoints[n].type = AreaType::frontier;
                } else if (endNodes_[n]->getType() & AreaNode::kDeadEnd) {
                    endpoints[n].type = AreaType::dead_end;
                } else   // don't know what type it is!
                {
                    std::cerr << "WARNING: AreaHypothesis: Unknown endpoint node type: " << endNodes_[n]->getType()
                              << ". Assigning as a dead end to be safe.\n";
                    endpoints[n].type = AreaType::dead_end;
                }
            }
        }

        return AreaProposal(id_,
                            endpoints[0],
                            endpoints[1],
                            frontier_,
                            boundary_,
                            *extent_,
                            getGatewaysBasicType(HypothesisType::kPath),
                            getGatewaysBasicType(HypothesisType::kDest),
                            getGatewaysBasicType(HypothesisType::kDecision),
                            getPoints(AreaNode::kFrontier),
                            getPoints(AreaNode::kDeadEnd | AreaNode::kJunction));
    } else {
        return AreaProposal(id_,
                            proposalClassification,
                            frontier_,
                            boundary_,
                            *extent_,
                            getGatewaysBasicType(HypothesisType::kPath),
                            getGatewaysBasicType(HypothesisType::kDest),
                            getGatewaysBasicType(HypothesisType::kDecision),
                            getPoints(AreaNode::kFrontier),
                            getPoints(AreaNode::kDeadEnd | AreaNode::kJunction));
    }
}

DebugHypothesis AreaHypothesis::toDebug(const HypothesisTypeDistribution& distribution) const
{
    HypothesisType current = getType();

    calculateFeaturesIfNeeded();
    assert(extent_);
    assert(features_);

    if (current == HypothesisType::kPath) {
        assert(endNodes_[0] && endNodes_[1]);
        DebugHypothesis::Endpoints debugEndpoints = {{endNodes_[0]->getPosition(), endNodes_[1]->getPosition()}};
        return DebugHypothesis(debugEndpoints,
                               *extent_,
                               distribution,
                               axisRatio_,
                               axisDirection_,
                               extent_->frontierRatio(),
                               std::vector<double>(features_->begin(), features_->end()));
    } else {
        return DebugHypothesis(current,
                               *extent_,
                               distribution,
                               axisRatio_,
                               axisDirection_,
                               extent_->frontierRatio(),
                               std::vector<double>(features_->begin(), features_->end()));
    }
}

void AreaHypothesis::initialize(void)
{
    extractSkeletonCells();
    calculateAxis();

    std::vector<Gateway> gateways;

    for (auto& n : nodes_) {
        if (boundaries_.empty() && (n->getType() & AreaNode::kGateway)) {
            gateways.push_back(n->getGateway());
        }
    }

    for (auto& b : boundaries_) {
        gateways.push_back(b->getGateway());
    }

    extent_.reset(new AreaExtent(gateways, cells_, *grid_));
    polygonBoundary_ = extent_->polygonBoundary(math::ReferenceFrame::GLOBAL);
    rectangleBoundary_ = extent_->rectangleBoundary(math::ReferenceFrame::GLOBAL);
    center_ = extent_->center().toPoint();

    // Normalize the initial type distribution
    distribution_.normalize();

    // Setup the distances for paths leaving through each of the gateway nodes
    LoopGraph loopGraph;
    construct_loop_graph(*areaGraph_, loopGraph);
    NotExcludedEdge<LoopGraph> edgeFilter(&nodes_, &loopGraph);
    auto filtered = boost::make_filtered_graph(loopGraph, edgeFilter);

    std::vector<int> components(areaGraph_->sizeNodes(), -1);
    boost::connected_components(filtered, &components[0]);

    // Find the vertex index for the boundary so the correct component can be found
    for (auto node : nodes_) {
        if (node->getType() & AreaNode::kGateway) {
            int vertex = 0;
            for (std::size_t n = 0; n < areaGraph_->sizeNodes(); ++n) {
                if (loopGraph[n].node == node) {
                    vertex = n;
                    break;
                }
            }

            double maxDist = 0.0;
            for (std::size_t n = 0; n < areaGraph_->sizeNodes(); ++n) {
                if (components[n] == components[vertex]) {
                    maxDist = std::max(areaGraph_->distanceBetweenNodes(node, loopGraph[n].node), maxDist);
                }
            }

            maxPathDist_[node] = maxDist;
        }
    }
}

SmallScaleStar AreaHypothesis::calculateStar(HypothesisType mask, GatewayFunc func) const
{
    auto gateways = CALL_MEMBER_FN(*this, func)(mask);
    return starBuilder_->buildStar(gateways, center(), rectangleBoundary());
}

std::vector<Gateway> AreaHypothesis::getGatewaysBasicType(HypothesisType type) const
{
    std::vector<Gateway> gateways;

    for (auto& boundary : boundaries_) {
        auto otherGraph = boundary->getOtherHypothesis(*this);
        if (otherGraph && (otherGraph->getType() == type)) {
            gateways.push_back(boundary->getGateway());
        }
        assert(otherGraph);
    }

    return gateways;
}

std::vector<Gateway> AreaHypothesis::getGatewaysFullType(HypothesisType mask) const
{
    std::vector<Gateway> gateways;

    for (auto& boundary : boundaries_) {
        auto otherGraph = boundary->getOtherHypothesis(*this);
        if (otherGraph && (is_hypothesis_type(otherGraph->getGatewayType(*(boundary->getNode())), mask))) {
            gateways.push_back(boundary->getGateway());
        }
        assert(otherGraph);
    }

    return gateways;
}

std::vector<Point<double>> AreaHypothesis::getPoints(char mask) const
{
    std::vector<Point<double>> points;

    for (auto& node : nodes_) {
        if (node->getType() & mask) {
            points.push_back(node->getPosition());
        }
    }

    return points;
}

std::vector<const AreaNode*> AreaHypothesis::getNodes(char mask) const
{
    std::vector<const AreaNode*> nodes;

    for (auto& node : nodes_) {
        if (node->getType() & mask) {
            nodes.push_back(node);
        }
    }

    return nodes;
}

void AreaHypothesis::extractSkeletonCells(void)
{
    length_ = 0.0;

    for (auto& edge : edges_) {
        assert(edge);

        for (auto cellIt = edge->beginCell(), cellEnd = edge->endCell(); cellIt != cellEnd; ++cellIt) {
            cells_.push_back(cellIt->cell);
        }

        boundary_ |= edge->isBorderEdge();
        length_ += edge->getLength();
        width_ += edge->getAverageWidth() * edge->getLength();
    }

    if (length_ > 0.0) {
        width_ /= length_;
    }
}

void AreaHypothesis::calculateAxis(void)
{
    assert(!cells_.empty());

    double sumCos = 0.0;
    double sumSin = 0.0;
    double sumCosPlusPi = 0.0;
    double sumSinPlusPi = 0.0;
    double sumEccentricity = 0.0;

    for (auto& c : cells_) {
        // Try range [-pi/2,pi/2] and [0,pi] to see which gives a better average
        // Ignore any cells that haven't ended up in the isovist field
        if (isovistField_->contains(c)) {
            double orientation = wrap_to_pi_2(isovistField_->at(c).scalar(utils::Isovist::kMaxThroughDistOrientation));
            double flippedOrientation = (orientation < 0) ? orientation + M_PI : orientation;

            sumCos += std::cos(orientation);
            sumSin += std::sin(orientation);
            sumCosPlusPi += std::cos(flippedOrientation);
            sumSinPlusPi += std::sin(flippedOrientation);
            sumEccentricity += isovistField_->at(c).scalar(utils::Isovist::kShapeEccentricity);
        }
    }

    double dir = wrap_to_pi_2(std::atan2(sumSin, sumCos));
    double plusPiDir = wrap_to_pi_2(std::atan2(sumSinPlusPi, sumCosPlusPi));

    double totalDiffDir = 0.0;
    double totalDiffPlusPi = 0.0;

    for (auto& c : cells_) {
        if (isovistField_->contains(c)) {
            double orientation = wrap_to_pi_2(isovistField_->at(c).scalar(utils::Isovist::kMaxThroughDistOrientation));
            totalDiffDir += angle_diff_abs_pi_2(orientation, dir);
            totalDiffPlusPi += angle_diff_abs_pi_2(orientation, plusPiDir);
        }
    }

    axisDirection_ = (totalDiffDir < totalDiffPlusPi) ? dir : plusPiDir;
    axisRatio_ = sumEccentricity / cells_.size();
}

void AreaHypothesis::calculateFeaturesIfNeeded(void) const
{
    if (!features_) {
        //         auto subgraph = visGraph_->createSubgraph(beginVisVertices(), endVisVertices());
        features_.reset(new HypothesisFeatures(*this, *extent_, *grid_, *isovistField_, *visGraph_, *skelGraph_));
    }
}

void AreaHypothesis::findPathEndpoints(void)
{
    std::vector<const AreaNode*> validBoundaryNodes;
    std::vector<DirectedPoint> possibleEndpoints;

    for (auto bnd : boundaryNodes()) {
        // If there isn't space for the robot to fit on the other side, then skip this boundary
        if (maxPathDist_.empty() || (maxPathDist_[bnd] > 3.0)) {
            validBoundaryNodes.push_back(bnd);
        }
    }

    // Use all nodes if there aren't enough valid boundaries found
    if (validBoundaryNodes.size() < 2) {
        validBoundaryNodes = boundaryNodes();
    }

    std::size_t numLoopNodes = 0;

    for (auto& bnd : validBoundaryNodes) {
        if (bnd->getType() & AreaNode::kGateway) {
            const auto& gwy = bnd->getGateway();

            DirectedPoint endpoint;
            endpoint.point = gwy.center();
            endpoint.node = bnd;
            endpoint.weight =
              gwy.probability();   // 1.0 - other->getTypeDistribution().typeAppropriateness(HypothesisType::kDest);
            endpoint.width = gwy.length();
            endpoint.isGateway = true;

            double leftDiff = angle_diff_abs_pi_2(gwy.leftDirection(), axisDirection_);
            double rightDiff = angle_diff_abs_pi_2(gwy.rightDirection(), axisDirection_);
            endpoint.direction = leftDiff < rightDiff ? gwy.leftDirection() : gwy.rightDirection();

            if (bnd->isLoop()) {
                ++numLoopNodes;
            }

            possibleEndpoints.push_back(endpoint);
        }
    }

    // Find the axis line for the path as the line intersecting the convex hull originating at the center of the
    // area and extending along the axis direction
    // The intersect is between line segment, so make the segment larger than any possible map to ensure an intersection
    // with the boundary occurs.
    Line<double> centerLine(
      Point<double>(center().x + 10000.0 * std::cos(axisDirection_), center().y + 10000.0 * std::sin(axisDirection_)),
      Point<double>(center().x - 10000.0 * std::cos(axisDirection_), center().y - 10000.0 * std::sin(axisDirection_)));
    //
    //     // If the center line intersects a gateway, then its an endpoint. If it intersects more than two gateways,
    //     consider
    //     // the weird polygon approach.
    //     for(auto& node : gatewayNodes)
    //     {
    //         if(node->isLoop() || line_segments_intersect(node->getGateway().boundary(), centerLine))
    //         {
    //             const auto& gwy = node->getGateway();
    //
    //             if((angle_diff_abs_pi_2(gwy.leftDirection(), axisDirection_) < M_PI / 3.0)
    //                 || (angle_diff_abs_pi_2(gwy.rightDirection(), axisDirection_) < M_PI / 3.0))
    //             {
    //                 filteredGateways.push_back(node);
    //             }
    //         }
    //     }
    //
    //     // obvious ends occur if at least two gateways are aligned to the axis of the area from the start
    //     bool hasObviousEndGateways = filteredGateways.size() > 1;
    //
    //     // If there aren't end viable candidates based on loops, then consider the amount of graph that
    //     // exists on the other side of a gateway
    //     if(!hasObviousEndGateways && !maxPathDist_.empty())
    //     {
    //         filteredGateways.clear();
    //         for(auto& bnd : maxPathDist_)
    //         {
    //             // Can the wheelchair fit back there?
    //             if(bnd.second > 3.0)
    //             {
    //                 filteredGateways.push_back(bnd.first);
    //             }
    //         }
    //
    //         hasObviousEndGateways = filteredGateways.size() > 1;
    //     }
    //
    //     if(!hasObviousEndGateways)
    //     {
    //         filteredGateways = gatewayNodes;
    //         hasObviousEndGateways = filteredGateways.size() > 1;
    //     }
    //
    //     for(auto& bnd : filteredGateways)
    //     {
    //         // For each gateway, select the direction that is most well-aligned with the axis of the area to get the
    //         best
    //         // possible alignment for the endpoints
    //         const auto& gwy = bnd->getGateway();
    //         double leftDiff = angle_diff_abs_pi_2(gwy.leftDirection(), axisDirection_);
    //         double rightDiff = angle_diff_abs_pi_2(gwy.rightDirection(), axisDirection_);
    //         double direction =  leftDiff < rightDiff ? gwy.leftDirection() : gwy.rightDirection();
    //
    //         possibleEndpoints.push_back({gwy.center(), bnd, direction, true});
    //     }

    if ((possibleEndpoints.size() < 2) || (numLoopNodes < 2)) {
        auto frontiers = getNodes(AreaNode::kDeadEnd);

        // If there aren't enough possible endpoints, then consider the junctions as well. By default they shouldn't be
        // included because they usually align very well with the axis and result in the path being too short.
        if (frontiers.size() + possibleEndpoints.size() < 2) {
            boost::push_back(frontiers, boost::as_array(getNodes(AreaNode::kJunction)));
        }

        // For each frontier point, need to find the direction. Use the orientation of the isovist at the cell.
        for (auto node : frontiers) {
            cell_t cell = utils::global_point_to_grid_cell_round(node->getPosition(), *grid_);
            if (isovistField_->contains(cell)) {
                double direction = isovistField_->at(cell).scalar(utils::Isovist::kWeightedOrientation);
                possibleEndpoints.push_back(
                  {node->getPosition(), node, direction, 0.1, grid_->getMetricDistance(cell.x, cell.y), false});
            } else {
                std::cerr << "ERROR: No isovist found for skeleton cell at " << cell << '\n';
            }
        }

        // If there aren't enough points for two endpoints, then this area can't have them
        if (possibleEndpoints.size() < 2) {
            std::cerr
              << "ERROR: AreaHypothesis: Created area without enough endpoints. Must be at least two gateways + "
              << "frontiers/dead ends for a valid area."
              << " Areas:" << possibleEndpoints.size() << " Frontiers:" << frontiers.size()
              << " Nodes:" << nodes_.size() << '\n';
            for (auto n : nodes_) {
                std::cout << "Node:" << n->getPosition() << ':' << static_cast<int>(n->getType()) << '\n';
            }

            assert(possibleEndpoints.size() >= 2);
        }
    }

    PointPair endpoints;

    if (possibleEndpoints.size() != 2) {
        // NOTE: To ensure the correct inter-node, a new AreaGraph containing only the nodes in this area
        // would need to be computed. It's REALLY slow, so approximate with the original AreaGraph. In most cases,
        // these graphs will give the same inter-node distances, so the approximation is find for the 40% speedup.
        std::vector<Point<double>> boundaryIntersections;
        polygonBoundary_.intersections(centerLine, boundaryIntersections);
        if (boundaryIntersections.size() != 2) {
            if (boundaryIntersections.size() < 2) {
                std::cerr
                  << "ERROR: AreaHypothesis: Axis line starting from center doesn't intersect the convex hull! Axis:"
                  << centerLine << " Boundary:" << polygonBoundary_ << '\n';
            }

            boundaryIntersections.resize(2);
            boundaryIntersections[0] = centerLine.a;
            boundaryIntersections[1] = centerLine.b;
        }

        endpoints = find_path_endpoints(possibleEndpoints,
                                        Line<double>(boundaryIntersections[0], boundaryIntersections[1]),
                                        axisDirection_,
                                        *areaGraph_);
    } else {
        endpoints.first = possibleEndpoints[0];
        endpoints.second = possibleEndpoints[1];
    }

    assignEndNodes(std::make_pair(endpoints.first.point, endpoints.second.point));

#ifdef DEBUG_ENDPOINTS
    std::cout << "DEBUG: AreaHypothesis: Selecting endpoint amongst gateways: ";
    for (auto& g : gateways) {
        std::cout << g.id() << ' ';
    }
    std::cout << '\n';

    std::cout << "DEBUG: AreaHypothesis: Endpoints: ";
    for (auto& node : endNodes_) {
        if (node->getType() & AreaNode::kGateway) {
            std::cout << "G:" << node->getGateway().id() << ':' << node->getGateway().boundary() << ' ';
        } else {
            std::cout << "F:" << node->getPosition() << ' ';
        }
    }
    std::cout << '\n';
#endif
}

void AreaHypothesis::mergeEndpoints(const std::vector<AreaHypothesis*>& areas)
{
    // Collect all the endpoints.
    std::vector<Point<double>> endpoints;
    std::vector<int8_t> hasMatch;
    std::vector<int> ids;
    for (auto& area : areas) {
        hasMatch.resize(area->endNodes_.size());
        std::fill(hasMatch.begin(), hasMatch.end(), 0);
        ids.clear();

        for (auto& node : area->endNodes_) {
            ids.push_back((node->getType() & AreaNode::kGateway) ? node->getGateway().id() : -1);
        }

        // We care about any endpoint that doesn't match another endpoint in one of the merged areas. Any matching
        // end gateways should be the merge point for the boundary.
        for (auto& other : areas) {
            if (other != area) {
                for (std::size_t n = 0; n < ids.size(); ++n) {
                    hasMatch[n] |= other->isEndGateway(ids[n]);
                }
            }
        }

        // There there weren't matches, then this endpoint is a new possible endpoint for the merged area, as it
        // won't be internal to the area like any matched gateway, which are now internal to the merged area
        for (std::size_t n = 0; n < ids.size(); ++n) {
            if (!hasMatch[n]) {
                endpoints.push_back(area->endNodes_[n]->getPosition());
            }
        }
    }

    // Remove any endpoints where the count is more than one, as they are now internal to the area
    // Can't use erase_unique because unique still leaves one of the original values. Need to remove them all.
    auto pEnd = endpoints.end();
    for (auto pIt = endpoints.begin(); pIt < pEnd; ++pIt) {
        if (std::count(pIt, pEnd, *pIt) > 1) {
            pEnd = std::remove(pIt, pEnd, *pIt);
            pIt = endpoints.begin();
        }
    }
    endpoints.erase(pEnd, endpoints.end());

    // If only two endpoints remain, then a merge can be performed
    if (endpoints.size() == 2) {
        assignEndNodes(Endpoints(endpoints[0], endpoints[1]));
    }
    // Otherwise, need to do a full search for new endpoints
    else {
        findPathEndpoints();
    }
}

void AreaHypothesis::assignEndNodes(Endpoints endpoints)
{
    endNodes_.clear();

    // Once the actual position of the endpoints have been found, go through the nodes in the hypothesis
    // to determine which endpoint they correspond to
    for (auto& node : nodes_) {
        if (node->getPosition() == endpoints.first) {
            addEndNode(node);
        }

        if (node->getPosition() == endpoints.second) {
            addEndNode(node);
        }
    }

    if (endNodes_.size() < 2) {
        std::cerr << "ERROR: AreaHypothesis: Have a null end node. Endpoints:" << endpoints.first << "->"
                  << endpoints.second << " Nodes:\n";
        for (auto& node : nodes_) {
            std::cerr << node->getPosition() << " Type:" << static_cast<int>(node->getType()) << '\n';

            if (node->getType() & AreaNode::kGateway) {
                std::cerr << "Gateway:" << node->getGateway().center() << '\n';
            }
        }
        assert(endNodes_.size() >= 2);
    }

    // Now check if either of the assigned end nodes leads to the same area as the established end node
    // If this node is associated with a boundary, then see if any other boundaries lead to the same area.
    // If so, those gateways are also end nodes, as the endpoint condition is area-based not gateway-based.
    for (int n = 0; n < 2; ++n) {
        const AreaNode* node = endNodes_[n];
        auto bndIt = std::find_if(boundaries_.begin(), boundaries_.end(), [node](auto& bnd) {
            return bnd->getNode() == node;
        });

        if (bndIt != boundaries_.end()) {
            auto otherHyp = (*bndIt)->getOtherHypothesis(*this);

            for (auto& boundary : boundaries_) {
                if ((boundary != *bndIt) && (boundary->getOtherHypothesis(*this) == otherHyp)) {
                    endNodes_.push_back(boundary->getNode());
                }
            }
        }
    }
}

void AreaHypothesis::addEndNode(AreaNode* node)
{
    // This node will definitely be an end node
    endNodes_.push_back(node);
}

bool AreaHypothesis::isNonEndpointGateway(const AreaNode* node) const
{
    // If the node isn't on the boundary, then obviously, it isn't a gateway
    // NOTE: Can't just check if it is a gateway node because gateway nodes won't be on the boundary of merged areas!
    //       They'll be marked gateway because they are associated with a potential gateway, but only the boundaries of
    //       an area represent the final gateways
    auto boundaryNodeComp = [node](const AreaHypothesisBoundary* boundary) {
        return boundary->getNode() == node;
    };

    if (std::find_if(boundaries_.begin(), boundaries_.end(), boundaryNodeComp) == boundaries_.end()) {
        return false;
    }

    return !utils::contains(endNodes_, node);
}

bool AreaHypothesis::isEndpoint(const AreaNode& node) const
{
    return utils::contains_if(endNodes_, [&node](auto& endNode) {
        return node.getPosition() == endNode->getPosition();
    });
}

std::vector<const AreaNode*> AreaHypothesis::boundaryNodes(void) const
{
    std::vector<const AreaNode*> bndNodes;

    // If boundaries exist, just use those nodes
    if (!boundaries_.empty()) {
        for (auto& bnd : boundaries_) {
            bndNodes.push_back(bnd->getNode());
        }
    }
    // Otherwise internal gateway nodes must be valid.
    else {
        for (auto& node : nodes_) {
            if (node->getType() & AreaNode::kGateway) {
                bndNodes.push_back(node);
            }
        }
    }

    return bndNodes;
}

HypothesisContainment AreaHypothesis::singleBoundaryContainment(const AreaNode* lhsNode,
                                                                const AreaHypothesis& rhs,
                                                                const AreaNode* rhsNode) const
{
    auto otherLhsNode = findConnectedNode(lhsNode);
    auto otherRhsNode = rhs.findConnectedNode(rhsNode);

    auto lhsGateway = lhsNode->getGateway();

    if (lhsGateway.isPointToLeft(otherLhsNode->getPosition())
        == lhsGateway.isPointToLeft(otherRhsNode->getPosition())) {
        return HypothesisContainment::complete;
    } else {
        return HypothesisContainment::none;
    }
}

const AreaNode* AreaHypothesis::findConnectedNode(const AreaNode* node) const
{
    auto edgeIt = std::find_if(edges_.begin(), edges_.end(), [node](const AreaEdge* e) {
        return e->nodeIndex(node) >= 0;
    });
    assert(edgeIt != edges_.end());
    int otherIndex = ((*edgeIt)->nodeIndex(node) + 1) % 2;
    return (*edgeIt)->getEndpoint(otherIndex).get();
}

std::vector<AreaHypothesis*> extract_hypotheses(const std::vector<const AreaHypothesisBoundary*>& boundaries)
{
    std::set<AreaHypothesis*> areasToMerge;

    for (auto boundary : boundaries) {
        areasToMerge.insert(boundary->beginHypotheses(), boundary->endHypotheses());
    }

    return std::vector<AreaHypothesis*>(areasToMerge.begin(), areasToMerge.end());
}

}   // namespace hssh
}   // namespace vulcan


#ifndef MPLLIBS_METAPARSE_V1_IMPL_ITERATE_IMPL_UNCHECKED_HPP
#define MPLLIBS_METAPARSE_V1_IMPL_ITERATE_IMPL_UNCHECKED_HPP

// Copyright Abel Sinkovics (abel@sinkovics.hu)  2013.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <mpllibs/metaparse/v1/impl/fwd/iterate_impl.hpp>
#include <mpllibs/metaparse/v1/get_result.hpp>
#include <mpllibs/metaparse/v1/get_remaining.hpp>
#include <mpllibs/metaparse/v1/get_position.hpp>

#include <boost/mpl/apply.hpp>
#include <boost/mpl/apply_wrap.hpp>
#include <boost/mpl/push_back.hpp>

namespace mpllibs
{
  namespace metaparse
  {
    namespace v1
    {
      namespace impl
      {
        template <int N, class P, class Accum, class S, class Pos>
        struct iterate_impl_unchecked :
          boost::mpl::apply_wrap2<
            iterate_impl<
              N - 1,
              P,
              typename boost::mpl::push_back<
                Accum,
                typename get_result<boost::mpl::apply<P, S, Pos> >::type
              >::type
            >,
            typename get_remaining<boost::mpl::apply<P, S, Pos> >::type,
            typename get_position<boost::mpl::apply<P, S, Pos> >::type
          >
        {};
      }
    }
  }
}

#endif


/*
interaction.cpp

Copyright (c) 2014, 2015, 2016 Terumasa Tadano

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

#include <iostream>
#include <iomanip>
#include <vector>
#include <algorithm>
#include <set>
#include <boost/lexical_cast.hpp>
#include "interaction.h"
#include "memory.h"
#include "system.h"
#include "error.h"
#include "symmetry.h"
#include "combination.h"
#include "constants.h"
#include "listcomparison.h"
#include "files.h"
#include "timer.h"
#include "fcs.h"
#include <cmath>

using namespace ALM_NS;

Interaction::Interaction(ALM *alm) : Pointers(alm) {
    nsize[0] = nsize[1] = nsize[2] = 1;
}

Interaction::~Interaction()
{
    memory->deallocate(x_image);
    memory->deallocate(exist_image);
    memory->deallocate(str_order);
    memory->deallocate(nbody_include);
    memory->deallocate(pairs);
    memory->deallocate(mindist_pairs);
    memory->deallocate(interaction_pair);
    memory->deallocate(mindist_cluster);
    memory->deallocate(distall);
}

void Interaction::init()
{
    int i, j, k;
    int nat = system->nat;
    int nkd = system->nkd;

    std::cout << " INTERACTION" << std::endl;
    std::cout << " ===========" << std::endl << std::endl;

    memory->allocate(str_order, maxorder);
    set_ordername();

    std::cout << "  +++ Cutoff Radii Matrix in Bohr Unit (NKD x NKD matrix) +++" << std::endl;

    for (i = 0; i < maxorder; ++i) {
        std::cout << "  " <<  std::setw(9) << str_order[i] << std::endl;
        for (j = 0; j < nkd; ++j) {
            for (k = 0; k < nkd; ++k) {
                if (rcs[i][j][k] < 0.0) {
                    std::cout << std::setw(9) << "None";
                } else {
                    std::cout << std::setw(9) << rcs[i][j][k];
                }
            }
            std::cout << std::endl;
        }
        std::cout << std::endl;
    }

    nneib = (2 * nsize[0] + 1) * (2 * nsize[1] + 1) * (2 * nsize[2] + 1);
    memory->allocate(x_image, nneib, nat, 3);
    memory->allocate(exist_image, nneib);
    memory->allocate(distall, nat, nat);
    memory->allocate(mindist_pairs, nat, nat);
    memory->allocate(interaction_pair, maxorder, symmetry->natmin);
    memory->allocate(mindist_cluster, maxorder, symmetry->natmin);
    memory->allocate(pairs, maxorder);

    generate_coordinate_of_periodic_images(nat, system->xcoord, is_periodic, x_image, exist_image);
    get_pairs_of_minimum_distance(nat, x_image, exist_image, distall, mindist_pairs);
    print_neighborlist(mindist_pairs);
    search_interactions(interaction_pair, pairs);
    //    calc_mindist_clusters2(interaction_pair, mindist_pairs, distall, exist_image, mindist_cluster);
    calc_mindist_clusters(interaction_pair, mindist_pairs, distall, exist_image, mindist_cluster);
    generate_pairs(pairs, mindist_cluster);

    /*
    for (int order = 0; order < maxorder; ++order) {
    for (i = 0; i < symmetry->natmin; ++i) {
    for (std::set<MinimumDistanceCluster>::const_iterator it = mindist_cluster[order][i].begin();
    it != mindist_cluster[order][i].end(); ++it) {
    std::cout << " order = " << std::setw(5) << order;
    std::cout << " iat = " << std::setw(5) << i << " : ";
    for (j = 0; j < (*it).atom.size(); ++j) {
    std::cout << std::setw(5) << (*it).atom[j];
    }
    std::cout << std::endl;
    for (j = 0; j < (*it).cell.size(); ++j) {
    for (k = 0; k < (*it).cell[j].size(); ++k) {
    std::cout << std::setw(5) << (*it).cell[j][k];
    }
    std::cout << std::endl;
    }
    std::cout << std::endl;
    }
    }
    }

    for (int order = 0; order < maxorder; ++order) {
    std::cout << "order = " << order << " pairs_size = " << pairs[order].size() << std::endl;
    for (std::set<IntList>::const_iterator it = pairs[order].begin(); it != pairs[order].end(); ++it) {
    for (i = 0; i < (*it).iarray.size(); ++i) {
    std::cout << std::setw(5) << (*it).iarray[i];
    }
    std::cout << std::endl;
    }
    }
    */

    timer->print_elapsed();
    std::cout << " --------------------------------------------------------------" << std::endl;
    std::cout << std::endl;
}

void Interaction::generate_pairs(std::set<IntList> *pair_out, std::set<MinimumDistanceCluster> **mindist_cluster)
{
    int i, j;
    int iat;
    int order;
    int natmin = symmetry->natmin;
    int nat = system->nat;

    int *pair_tmp;

    for (order = 0; order < maxorder; ++order) {

        pair_out[order].clear();

        if (order + 2 > nbody_include[order]) {
            std::cout << "  For " << std::setw(8) << interaction->str_order[order] << ", ";
            std::cout << "interactions related to more than" << std::setw(2) << nbody_include[order];
            std::cout << " atoms will be neglected." << std::endl;
        }

        memory->allocate(pair_tmp, order + 2);

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

            iat = symmetry->map_p2s[i][0];

            for (std::set<MinimumDistanceCluster>::const_iterator it = mindist_cluster[order][i].begin();
                it != mindist_cluster[order][i].end(); ++it) {

                    pair_tmp[0] = iat;
                    for (j = 0; j < order + 1; ++j) {
                        pair_tmp[j+1] = (*it).atom[j];
                    }

                    insort(order+2, pair_tmp);

                    // Ignore many-body case
                    if (nbody(order + 2, pair_tmp) > nbody_include[order]) continue;
                    pair_out[order].insert(IntList(order + 2, pair_tmp));
            }
        }
        memory->deallocate(pair_tmp);
    }
}

void Interaction::generate_coordinate_of_periodic_images(const unsigned int nat, double **xf_in,
                                                         const int periodic_flag[3], double ***xc_out,
                                                         int *is_allowed)
{
    //
    // Generate Cartesian coordinates of atoms in the neighboring 27 supercells
    //

    unsigned int i, j;
    int ia, ja, ka;
    int icell;

    icell = 0;
    for (i = 0; i < nat; ++i) {
        for (j = 0; j < 3; ++j) {
            xc_out[0][i][j] = xf_in[i][j];
        }
    }
    // Convert to Cartesian coordinate
    system->frac2cart(xc_out[0]);

    for (ia = -1; ia <= 1; ++ia) {
        for (ja = -1; ja <= 1; ++ja) {
            for (ka = -1; ka <= 1; ++ka) {

                if (ia == 0 && ja == 0 && ka == 0) continue;

                ++icell;
                for (i = 0; i < nat; ++i) {
                    xc_out[icell][i][0] = xf_in[i][0] + static_cast<double>(ia);
                    xc_out[icell][i][1] = xf_in[i][1] + static_cast<double>(ja);
                    xc_out[icell][i][2] = xf_in[i][2] + static_cast<double>(ka);
                }
                // Convert to Cartesian coordinate
                system->frac2cart(xc_out[icell]);
            }
        }
    }

    icell = 0;
    is_allowed[0] = 1;

    for (ia = -1; ia <= 1; ++ia) {
        for (ja = -1; ja <= 1; ++ja) {
            for (ka = -1; ka <= 1; ++ka) {

                ++icell;

                // When periodic flag is zero along an axis,
                // periodic images along that axis cannot be considered.
                if (((std::abs(ia) == 1) && (periodic_flag[0] == 0)) ||
                    ((std::abs(ja) == 1) && (periodic_flag[1] == 0)) ||
                    ((std::abs(ka) == 1) && (periodic_flag[2] == 0)) ) {

                        is_allowed[icell] = 0;

                } else {

                    is_allowed[icell] = 1;
                }
            }
        }
    }

}

double Interaction::distance(double *x1, double *x2)
{
    double dist;
    dist = std::pow(x1[0] - x2[0], 2) + std::pow(x1[1] - x2[1], 2) + std::pow(x1[2] - x2[2], 2);
    dist = std::sqrt(dist);

    return dist;
}

void Interaction::get_pairs_of_minimum_distance(int nat, double ***xc_in, int *exist, std::vector<DistInfo> **distall,
                                                std::vector<DistInfo> **mindist_pairs)
{
    int icell = 0;
    int i, j, k;
    double dist_tmp;
    double vec[3];

    for (i = 0; i < nat; ++i) {
        for (j = 0; j < nat; ++j) {

            distall[i][j].clear();

            for (icell = 0; icell < nneib; ++icell) {

                if (exist[icell]) {

                    dist_tmp = distance(xc_in[0][i], xc_in[icell][j]);

                    for (k = 0; k < 3; ++k) vec[k] = xc_in[icell][j][k] - xc_in[0][i][k];

                    distall[i][j].push_back(DistInfo(icell, dist_tmp, vec));
                }
            }
            std::sort(distall[i][j].begin(), distall[i][j].end());
        }
    }

    // Construct pairs of minimum distance.

    double dist_min;
    for (i = 0; i < nat; ++i) {
        for (j = 0; j < nat; ++j) {
            mindist_pairs[i][j].clear();

            dist_min = distall[i][j][0].dist;
            for (std::vector<DistInfo>::const_iterator it = distall[i][j].begin(); it != distall[i][j].end(); ++it) {
                if (std::abs((*it).dist - dist_min) < eps8) {
                    mindist_pairs[i][j].push_back(DistInfo(*it));
                }
            }
        }

        /*
        for (j = 0; j < nat; ++j) {
        std::cout << "Mindist : " << std::setw(5) << i + 1 << " <--> " << std::setw(5) << j + 1 << std::endl;
        for (k = 0; k < mindist_pairs[i][j].size(); ++k) {
        std::cout << " cell_j = " << std::setw(5) << mindist_pairs[i][j][k].cell;
        std::cout << " dist = " << std::setw(5) << mindist_pairs[i][j][k].dist;
        std::cout << std::endl;
        }
        std::cout << std::endl;
        }
        */
    }
}

void Interaction::print_neighborlist(std::vector<DistInfo> **mindist)
{
    int i, j, k;
    int iat;
    int nat = system->nat;
    int icount;

    double dist_tmp;
    std::vector<DistList> *neighborlist;

    memory->allocate(neighborlist, symmetry->natmin);

    for (i = 0; i < symmetry->natmin; ++i) {
        neighborlist[i].clear();

        iat = symmetry->map_p2s[i][0];

        for (j = 0; j < nat; ++j) {
            neighborlist[i].push_back(DistList(j, mindist[iat][j][0].dist));
        }
        std::sort(neighborlist[i].begin(), neighborlist[i].end());
    }

    std::cout << std::endl;
    std::cout << "  List of neighboring atoms below." << std::endl;
    std::cout << "  Format [N th-nearest shell, distance in Bohr (Number of atoms on the shell)]" << std::endl << std::endl;

    int nthnearest;
    std::vector<int> atomlist;

    for (i = 0; i < symmetry->natmin; ++i) {

        nthnearest = 0;
        atomlist.clear();

        iat = symmetry->map_p2s[i][0];
        std::cout << std::setw(5) << iat + 1 << " (" << std::setw(3) << system->kdname[system->kd[iat] - 1] << "): ";

        dist_tmp = 0.0;

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

            if (neighborlist[i][j].dist < eps8) continue; // distance is zero

            if (std::abs(neighborlist[i][j].dist - dist_tmp) > eps6) {

                if (atomlist.size() > 0) {
                    nthnearest += 1;

                    if (nthnearest > 1) std::cout << std::setw(13) << " ";

                    std::cout << std::setw(3) << nthnearest << std::setw(10) << dist_tmp
                        << " (" << std::setw(3) << atomlist.size() << ") -";

                    icount = 0;
                    for (k = 0; k < atomlist.size(); ++k) {

                        if (icount % 4 == 0 && icount > 0) {
                            std::cout << std::endl;
                            std::cout << std::setw(34) << " ";
                        }
                        ++icount;

                        std::cout << std::setw(4) << atomlist[k] + 1;
                        std::cout <<  "(" << std::setw(3) << system->kdname[system->kd[atomlist[k]] - 1] << ")";

                    }
                    std::cout << std::endl;
                }

                dist_tmp = neighborlist[i][j].dist;
                atomlist.clear();
                atomlist.push_back(neighborlist[i][j].atom);
            } else {
                atomlist.push_back(neighborlist[i][j].atom);
            }
        }
        if (atomlist.size() > 0) {
            nthnearest += 1;

            if (nthnearest > 1) std::cout << std::setw(13) << " ";

            std::cout << std::setw(3) << nthnearest << std::setw(10) << dist_tmp
                << " (" << std::setw(3) << atomlist.size() << ") -";

            icount = 0;
            for (k = 0; k < atomlist.size(); ++k) {

                if (icount % 4 == 0 && icount > 0) {
                    std::cout << std::endl;
                    std::cout << std::setw(34) << " ";
                }
                ++icount;

                std::cout << std::setw(4) << atomlist[k] + 1;
                std::cout <<  "(" << std::setw(3) << system->kdname[system->kd[atomlist[k]] - 1] << ")";

            }
            std::cout << std::endl;
        }
        std::cout << std::endl;
    }
    atomlist.clear();
    memory->deallocate(neighborlist);
}

void Interaction::search_interactions(std::vector<int> **interaction_list_out, std::set<IntList> *pair_out)
{
    int i;
    int order;
    int natmin = symmetry->natmin;
    int iat, jat;
    int nat = system->nat;
    int ikd, jkd;

    double cutoff_tmp;
    std::set<IntList> *interacting_atom_pairs;
    std::vector<int> intlist;

    memory->allocate(interacting_atom_pairs, maxorder);

    for (order = 0; order < maxorder; ++order) {
        for (i = 0; i < natmin; ++i) {
            interaction_list_out[order][i].clear();

            iat = symmetry->map_p2s[i][0];
            ikd = system->kd[iat] - 1;

            for (jat = 0; jat < nat; ++jat) {

                jkd = system->kd[jat] - 1;

                cutoff_tmp = rcs[order][ikd][jkd];

                if (cutoff_tmp < 0.0) {

                    interaction_list_out[order][i].push_back(jat);

                } else {

                    if (mindist_pairs[iat][jat][0].dist <= cutoff_tmp) {
                        interaction_list_out[order][i].push_back(jat);
                    }
                }
            }
        }
    }

    intlist.clear();
    std::cout << std::endl;
    std::cout << "  List of interacting atom pairs considered for each order:" << std::endl;

    for (order = 0; order < maxorder; ++order) {

        interacting_atom_pairs[order].clear();

        std::cout << std::endl << "   ***" << str_order[order] << "***" << std::endl;

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

            if (interaction_list_out[order][i].size() == 0) {
                std::cout << "   No interacting atoms! Skipped." << std::endl;
                continue; // no interaction
            }

            iat = symmetry->map_p2s[i][0];

            intlist.clear();
            for (std::vector<int>::const_iterator it = interaction_list_out[order][i].begin();
                it != interaction_list_out[order][i].end(); ++it) {
                    intlist.push_back((*it));
            }
            std::sort(intlist.begin(), intlist.end()); // Necessarily to sort here

            // write atoms inside the cutoff radius
            int id = 0;
            std::cout << "    Atom " << std::setw(5) << iat + 1
                << "(" << std::setw(3) << system->kdname[system->kd[iat]-1] << ")" << " interacts with atoms ... " << std::endl;

            for (int id = 0; id < intlist.size(); ++id) {
                if (id%6 == 0) {
                    if (id == 0) {
                        std::cout << "   ";
                    } else {
                        std::cout << std::endl;
                        std::cout << "   ";
                    }
                }
                std::cout << std::setw(5) << intlist[id] + 1 << "("
                    << std::setw(3) << system->kdname[system->kd[intlist[id]]-1] << ")";
            }

            std::cout << std::endl << std::endl;
            std::cout << "    Number of total interaction pairs = "
                << interaction_list_out[order][i].size() << std::endl << std::endl;

            int *intarr;
            memory->allocate(intarr, order + 2);

            if (intlist.size() > 0) {
                if (order == 0) {
                    for (unsigned int ielem = 0; ielem < intlist.size(); ++ielem) {
                        intarr[0] = iat;
                        intarr[1] = intlist[ielem];
                        insort(2, intarr);

                        interacting_atom_pairs[0].insert(IntList(2, intarr));
                    }
                } else if (order > 0) {
                    CombinationWithRepetition<int> g(intlist.begin(), intlist.end(), order + 1);
                    do {
                        std::vector<int> data = g.now();
                        intarr[0] = iat;
                        intarr[1] = data[0];
                        for (unsigned int isize = 1; isize < data.size() ; ++isize) {
                            intarr[isize + 1] = data[isize];
                        }
                        if (!is_incutoff(order+2, intarr, order)) continue;

                        insort(order+2, intarr);
                        interacting_atom_pairs[order].insert(IntList(order + 2, intarr));

                    } while(g.next());
                }
            }
            intlist.clear();
            memory->deallocate(intarr);
        }
    }

    std::cout << std::endl;

    /*
    int *pair_tmp;

    for (order = 0; order < maxorder; ++order) {

    pair_out[order].clear();

    if (order + 2 > nbody_include[order]) {
    std::cout << "  For " << std::setw(8) << interaction->str_order[order] << ", ";
    std::cout << "interactions related to more than" << std::setw(2) << nbody_include[order];
    std::cout << " atoms will be neglected." << std::endl;
    }

    memory->allocate(pair_tmp, order + 2);

    for (std::set<IntList>::const_iterator it = interacting_atom_pairs[order].begin();
    it != interacting_atom_pairs[order].end(); ++it) {
    for (j = 0; j < order + 2; ++j) {
    pair_tmp[j] = (*it).iarray[j];
    }

    // Ignore many-body case
    if (nbody(order + 2, pair_tmp) > nbody_include[order]) continue;

    pair_out[order].insert(IntList(order + 2, pair_tmp));
    }
    memory->deallocate(pair_tmp);
    }
    */

    memory->deallocate(interacting_atom_pairs);
}

bool Interaction::is_incutoff(const int n, int *atomnumlist, const int order)
{
    int i, j;
    int iat, jat;
    int ikd, jkd;
    int ncheck = n - 1;
    double cutoff_tmp;
    std::vector<DistInfo>::const_iterator it, it2;

    iat = atomnumlist[0];
    ikd = system->kd[iat] - 1;

    for (i = 0; i < n; ++i) {
        iat = atomnumlist[i];
        ikd = system->kd[iat] - 1;

        for (j = i + 1; j < n; ++j) {
            jat = atomnumlist[j];
            jkd = system->kd[jat] - 1;

            cutoff_tmp = rcs[order][ikd][jkd];

            if (cutoff_tmp >= 0.0 &&
                (mindist_pairs[iat][jat][0].dist > cutoff_tmp)) return false;

        }
    }
    return true;

    /*
    for (i = 0; i < ncheck; ++i) {

    jat = atomnumlist[i + 1];
    jkd = system->kd[jat] - 1;

    if (rcs[order][ikd][jkd] >= 0.0 &&
    (mindist_pairs[iat][jat][0].dist > rcs[order][ikd][jkd])) return false;

    for (j = i + 1; j < ncheck; ++j) {

    kat = atomnumlist[j + 1];
    kkd = system->kd[kat] - 1;

    if (rcs[order][ikd][kkd] >= 0.0 &&
    (mindist_pairs[iat][kat][0].dist > rcs[order][ikd][kkd])) return false;

    cutoff_tmp = rcs[order][jkd][kkd];

    if (cutoff_tmp >= 0.0) {

    in_cutoff_tmp = false;

    for (it = mindist_pairs[iat][jat].begin(); it != mindist_pairs[iat][jat].end(); ++it) {
    for (it2 = mindist_pairs[iat][kat].begin(); it2 != mindist_pairs[iat][kat].end(); ++it2) {
    dist_tmp = distance(x_image[(*it).cell][jat], x_image[(*it2).cell][kat]);

    if (n == 4) {
    if (iat == 0 && jat == 4 && kat == 20) {
    std::cout << "DEBUG :" << std::setw(5) << (*it).cell;
    std::cout << std::setw(5) << (*it2).cell << std::setw(15) << dist_tmp << std::endl;
    }
    }

    if (dist_tmp <= cutoff_tmp) {
    in_cutoff_tmp = true;
    }
    }
    }
    if (!in_cutoff_tmp) return false;
    }
    }
    }
    return true;
    */

}

bool Interaction::is_incutoff2(const int n, int *atomnumlist, const int order)
{
    int i, j;
    int iat, jat, kat;
    int ikd, jkd, kkd;
    int ncheck = n - 1;
    //    int order = n - 2;
    bool in_cutoff_tmp;
    double cutoff_tmp;
    double dist_tmp;
    std::vector<DistInfo>::const_iterator it, it2;

    iat = atomnumlist[0];
    ikd = system->kd[iat] - 1;

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

        jat = atomnumlist[i + 1];
        jkd = system->kd[jat] - 1;

        if (rcs[order][ikd][jkd] >= 0.0 &&
            (mindist_pairs[iat][jat][0].dist > rcs[order][ikd][jkd])) return false;

        for (j = i + 1; j < ncheck; ++j) {

            kat = atomnumlist[j + 1];
            kkd = system->kd[kat] - 1;

            if (rcs[order][ikd][kkd] >= 0.0 &&
                (mindist_pairs[iat][kat][0].dist > rcs[order][ikd][kkd])) return false;

            cutoff_tmp = rcs[order][jkd][kkd];

            if (cutoff_tmp >= 0.0) {

                in_cutoff_tmp = false;

                for (it = mindist_pairs[iat][jat].begin(); it != mindist_pairs[iat][jat].end(); ++it) {
                    for (it2 = mindist_pairs[iat][kat].begin(); it2 != mindist_pairs[iat][kat].end(); ++it2) {
                        dist_tmp = distance(x_image[(*it).cell][jat], x_image[(*it2).cell][kat]);

                        if (dist_tmp <= cutoff_tmp) {
                            in_cutoff_tmp = true;
                        }
                    }
                }
                if (!in_cutoff_tmp) return false;
            }
        }
    }

    return true;
}

void Interaction::set_ordername()
{
    std::string strnum;

    str_order[0] = "HARMONIC";

    for (int i = 1;  i < maxorder; ++i) {
        strnum = boost::lexical_cast<std::string>(i+2);
        str_order[i] = "ANHARM" + strnum;
    }
}

int Interaction::nbody(const int n, const int *arr)
{
    std::vector<int> v;
    v.clear();
    int ret;

    for (unsigned int i = 0; i < n; ++i) {
        v.push_back(arr[i]);
    }
    std::sort(v.begin(), v.end());
    v.erase(std::unique(v.begin(), v.end()), v.end());

    ret = v.size();
    v.clear();

    return ret;
}

void Interaction::calc_mindist_clusters(std::vector<int> **interaction_pair_in, std::vector<DistInfo> **mindist_pair_in,
                                        std::vector<DistInfo> **distance_image, int *exist,
                                        std::set<MinimumDistanceCluster> **mindist_cluster_out)
{
    std::vector<MinDistList> distance_list;

    int natmin = symmetry->natmin;
    int i, j, k;
    int iat, jat;
    int order;
    int ii;
    int ikd, jkd;
    //    int *intpair_tmp;
    std::vector<int> intlist;

    double dist_tmp, rc_tmp;
    double distmax;

    bool isok;

    std::vector<int> cell_vector;
    std::vector<double> dist_vector;
    std::vector<std::vector<int> > pairs_icell, comb_cell, comb_cell_min;
    std::vector<std::vector<int> > comb_cell_atom_center;
    std::vector<int> accum_tmp;
    std::vector<int> atom_tmp, cell_tmp;
    std::vector<int> intpair_uniq, cellpair;
    std::vector<int> group_atom;

    int icount;

    //    memory->allocate(intpair_tmp, maxorder + 2);

    for (order = 0; order < maxorder; ++order) {
        for (i = 0; i < symmetry->natmin; ++i) {

            mindist_cluster_out[order][i].clear();

            iat = symmetry->map_p2s[i][0];
            ikd = system->kd[iat] - 1;

            // List of 2-body interaction pairs
            intlist.clear();
            for (std::vector<int>::const_iterator it  = interaction_pair_in[order][i].begin();
                it != interaction_pair_in[order][i].end(); ++it) {
                    intlist.push_back((*it));
            }
            std::sort(intlist.begin(), intlist.end()); // Need to sort here

            if (intlist.size() > 0) {
                if (order == 0){
                    for (unsigned int ielem = 0; ielem < intlist.size(); ++ielem) {

                        comb_cell_min.clear();
                        atom_tmp.clear();

                        jat = intlist[ielem];
                        atom_tmp.push_back(jat);

                        for (j = 0; j < mindist_pair_in[iat][jat].size(); ++j) {
                            cell_tmp.clear();
                            cell_tmp.push_back(mindist_pair_in[iat][jat][j].cell);
                            comb_cell_min.push_back(cell_tmp);
                        }
                        distmax = mindist_pair_in[iat][jat][0].dist;
                        mindist_cluster_out[order][i].insert(MinimumDistanceCluster(atom_tmp, comb_cell_min, distmax));
                    }
                } else if (order > 0) {
                    CombinationWithRepetition<int> g(intlist.begin(), intlist.end(), order + 1);
                    do {
                        std::vector<int> data = g.now();

                        // Uniq the list of atoms in data like as follows:
                        // cubic   term : (i, i) --> (i) x 2
                        // quartic term : (i, i, j) --> (i, j) x (2, 1)
                        intpair_uniq.clear();
                        group_atom.clear();
                        icount = 1;

                        for (j = 0; j < order; ++j) {
                            if (data[j] == data[j+1]) {
                                ++icount;
                            } else {
                                group_atom.push_back(icount);
                                intpair_uniq.push_back(data[j]);
                                icount = 1;
                            }
                        }
                        group_atom.push_back(icount);
                        intpair_uniq.push_back(data[order]);

                        pairs_icell.clear();
                        for (j = 0; j < intpair_uniq.size(); ++j) {
                            jat = intpair_uniq[j];
                            jkd = system->kd[jat] - 1;

                            rc_tmp = rcs[order][ikd][jkd];
                            cell_vector.clear();

                            // Loop over the cell images of atom 'jat' and add to the list
                            // as a candidate for the minimum distance cluster
                            for (std::vector<DistInfo>::const_iterator it = distance_image[iat][jat].begin();
                                it != distance_image[iat][jat].end(); ++it) {
                                    if (exist[(*it).cell]) {
                                        if (rc_tmp < 0.0 || (*it).dist <= rc_tmp) {
                                            cell_vector.push_back((*it).cell);
                                        }
                                    }
                            }
                            pairs_icell.push_back(cell_vector);
                        }

                        accum_tmp.clear();
                        comb_cell.clear();
                        cell_combination(pairs_icell, 0, accum_tmp, comb_cell);

                        distance_list.clear();
                        for (j = 0; j < comb_cell.size(); ++j) {

                            cellpair.clear();

                            for (k = 0; k < group_atom.size(); ++k) {
                                for (ii = 0; ii < group_atom[k]; ++ii) {
                                    cellpair.push_back(comb_cell[j][k]);
                                }
                            }

                            dist_vector.clear();

                            for (k = 0; k < cellpair.size(); ++k) {
                                dist_tmp = distance(x_image[cellpair[k]][data[k]], x_image[0][iat]);
                                //   if (dist_tmp > eps15) dist_vector.push_back(dist_tmp);
                                dist_vector.push_back(dist_tmp);
                            }

                            // Flag to check if the distance is smaller than the cutoff radius
                            isok = true;

                            for (k = 0; k < cellpair.size(); ++k) {
                                for (ii = k + 1; ii < cellpair.size(); ++ii) {
                                    dist_tmp = distance(x_image[cellpair[k]][data[k]], x_image[cellpair[ii]][data[ii]]);
                                    rc_tmp = rcs[order][system->kd[data[k]]-1][system->kd[data[ii]]-1];
                                    if (rc_tmp >= 0.0 && dist_tmp > rc_tmp) {
                                        isok = false;
                                    }
                                    //                                    if (dist_tmp > eps15) dist_vector.push_back(dist_tmp);
                                    dist_vector.push_back(dist_tmp);
                                }
                            }
                            if (isok) {
                                // This combination is a candidate of the minimum distance cluster
                                distance_list.push_back(MinDistList(cellpair, dist_vector));
                            }
                        } // close loop over the mirror image combination

                        if (distance_list.size() > 0) {
                            // If the distance_list is not empty, there is a set of mirror images
                            // that satisfies the condition of the interaction.

                            pairs_icell.clear();
                            for (j = 0; j < intpair_uniq.size(); ++j) {
                                jat = intpair_uniq[j];
                                cell_vector.clear();

                                for (ii = 0; ii < mindist_pair_in[iat][jat].size(); ++ii) {
                                    cell_vector.push_back(mindist_pair_in[iat][jat][ii].cell);
                                }
                                pairs_icell.push_back(cell_vector);
                            }

                            accum_tmp.clear();
                            comb_cell.clear();
                            comb_cell_atom_center.clear();
                            cell_combination(pairs_icell, 0, accum_tmp, comb_cell);

                            for (j = 0; j < comb_cell.size(); ++j) {
                                cellpair.clear();
                                for (k = 0; k < group_atom.size(); ++k) {
                                    for (ii = 0; ii < group_atom[k]; ++ii) {
                                        cellpair.push_back(comb_cell[j][k]);
                                    }
                                }
                                comb_cell_atom_center.push_back(cellpair);
                            }

                            std::sort(distance_list.begin(), distance_list.end(), MinDistList::compare_max_distance);
                            distmax = *std::max_element(distance_list[0].dist.begin(), distance_list[0].dist.end());
                            mindist_cluster_out[order][i].insert(MinimumDistanceCluster(data, comb_cell_atom_center,distmax));
                            /*
                            std::sort(distance_list.begin(), distance_list.end(), MinDistList::compare_sum_distance);
                            comb_cell_min.clear();

                            sum_dist_min = 0.0;
                            for (j = 0; j < distance_list[0].dist.size(); ++j) {
                            sum_dist_min += distance_list[0].dist[j];
                            }

                            for (j = 0; j < distance_list.size(); ++j) {
                            sum_dist = 0.0;

                            for (k = 0; k < distance_list[j].dist.size(); ++k) {
                            sum_dist += distance_list[j].dist[k];
                            }

                            // In the following, only pairs having minimum sum of distances
                            // are stored. However, we found that this treatment didn't
                            // return a reliable value of phonon linewidth.
                            if (std::abs(sum_dist - sum_dist_min) < eps6) {
                            comb_cell_min.push_back(distance_list[j].cell);
                            } else {
                            break;
                            }
                            // Therefore, we consider all duplicate pairs
                            //comb_cell_min.push_back(distance_list[j].cell);
                            }
                            //                            mindist_cluster_out[order][i].insert(MinimumDistanceCluster(data, comb_cell_min));
                            */

                        }

                    } while(g.next());
                }
            }
            intlist.clear();
        }
    }

    //    memory->deallocate(intpair_tmp);
}

void Interaction::calc_mindist_clusters2(std::vector<int> **interaction_pair_in, std::vector<DistInfo> **mindist_pair_in,
                                         std::vector<DistInfo> **distance_image, int *exist,
                                         std::set<MinimumDistanceCluster> **mindist_cluster_out)
{
    std::vector<MinDistList> distance_list;

    int natmin = symmetry->natmin;
    int i, j, k;
    int iat, jat;
    int ikd, jkd;
    int order;
    int ii;
    std::vector<int> intlist;

    double rc_tmp;
    double dist_tmp;
    double sum_dist_min, sum_dist;
    double dist_max;

    std::vector<int> cell_vector;
    std::vector<double> dist_vector;
    std::vector<std::vector<int> > pairs_icell, comb_cell, comb_cell_min;
    std::vector<int> accum_tmp;
    std::vector<int> atom_tmp, cell_tmp;
    std::vector<int> intpair_uniq, cellpair;
    std::vector<int> group_atom;

    int icount;
    bool isok;

    for (order = 0; order < maxorder; ++order) {
        for (i = 0; i < symmetry->natmin; ++i) {

            mindist_cluster_out[order][i].clear();
            iat = symmetry->map_p2s[i][0];
            ikd = system->kd[iat] - 1;

            intlist.clear();
            for (std::vector<int>::const_iterator it  = interaction_pair_in[order][i].begin();
                it != interaction_pair_in[order][i].end(); ++it) {
                    intlist.push_back((*it));
            }
            std::sort(intlist.begin(), intlist.end()); // Need to sort here

            if (intlist.size() > 0) {

                if (order == 0){
                    // For harmonic case, the minimum distance cluster is equivalen to the
                    // minimum distance pairs.
                    for (unsigned int ielem = 0; ielem < intlist.size(); ++ielem) {

                        comb_cell_min.clear();
                        atom_tmp.clear();

                        jat = intlist[ielem];
                        atom_tmp.push_back(jat);

                        for (j = 0; j < mindist_pair_in[iat][jat].size(); ++j) {
                            cell_tmp.clear();
                            cell_tmp.push_back(mindist_pair_in[iat][jat][j].cell);
                            comb_cell_min.push_back(cell_tmp);
                        }
                        dist_max = mindist_pair_in[iat][jat][0].dist;
                        mindist_cluster_out[order][i].insert(MinimumDistanceCluster(atom_tmp, comb_cell_min));
                    }

                } else if (order > 0) {

                    // For anharmonic cases, the minimum distance cluster is the set of
                    // cell images having the smallest value for the sum of distance.

                    // Generate all possible combination of atoms from intlist
                    CombinationWithRepetition<int> g(intlist.begin(), intlist.end(), order + 1);
                    do {
                        std::vector<int> data = g.now();

                        // Uniq the list of atoms in data like as follows:
                        // cubic   term : (i, i) --> (i) x 2
                        // quartic term : (i, i, j) --> (i, j) x (2, 1)
                        intpair_uniq.clear();
                        group_atom.clear();
                        icount = 1;

                        for (j = 0; j < order; ++j) {
                            if (data[j] == data[j+1]) {
                                ++icount;
                            } else {
                                group_atom.push_back(icount);
                                intpair_uniq.push_back(data[j]);
                                icount = 1;
                            }
                        }
                        group_atom.push_back(icount);
                        intpair_uniq.push_back(data[order]);

                        pairs_icell.clear();
                        for (j = 0; j < intpair_uniq.size(); ++j) {
                            jat = intpair_uniq[j];
                            jkd = system->kd[jat] - 1;

                            rc_tmp = rcs[order][ikd][jkd];
                            cell_vector.clear();

                            // Loop over the cell images of atom 'jat' and add to the list
                            // as a candidate for the minimum distance cluster
                            for (std::vector<DistInfo>::const_iterator it = distance_image[iat][jat].begin();
                                it != distance_image[iat][jat].end(); ++it) {
                                    if (exist[(*it).cell]) {
                                        if (rc_tmp < 0.0 || (*it).dist <= rc_tmp) {
                                            cell_vector.push_back((*it).cell);
                                            // std::cout << " iat = " << std::setw(5) << iat;
                                            // std::cout << " jat = " << std::setw(5) << jat;
                                            // std::cout << " cell = " << (*it).cell;
                                            // std::cout << " dist = " << (*it).dist;
                                            // std::cout << " rc = " << rc_tmp << std::endl;
                                        }
                                    }
                            }
                            pairs_icell.push_back(cell_vector);
                        }

                        // Generate all possible combinations of mirror images
                        accum_tmp.clear();
                        comb_cell.clear();
                        cell_combination(pairs_icell, 0, accum_tmp, comb_cell);

                        distance_list.clear();

                        // Loop over the combination of the mirror images and
                        // generate the list of atomic distances of all pairs for each combination.
                        // Add to list only if the distance is smaller than the cutoff radius.

                        for (j = 0; j < comb_cell.size(); ++j) {

                            cellpair.clear();

                            for (k = 0; k < group_atom.size(); ++k) {
                                for (ii = 0; ii < group_atom[k]; ++ii) {
                                    cellpair.push_back(comb_cell[j][k]);
                                }
                            }

                            dist_vector.clear();

                            for (k = 0; k < cellpair.size(); ++k) {
                                dist_tmp = distance(x_image[cellpair[k]][data[k]], x_image[0][iat]);
                                if (dist_tmp > eps15) dist_vector.push_back(dist_tmp);
                            }

                            // Flag to check if the distance is smaller than the cutoff radius
                            isok = true;

                            for (k = 0; k < cellpair.size(); ++k) {
                                for (ii = k + 1; ii < cellpair.size(); ++ii) {
                                    dist_tmp = distance(x_image[cellpair[k]][data[k]], x_image[cellpair[ii]][data[ii]]);
                                    rc_tmp = rcs[order][system->kd[data[k]]-1][system->kd[data[ii]]-1];
                                    if (rc_tmp >= 0.0 && dist_tmp > rc_tmp) {
                                        isok = false;
                                        //                                            break;
                                    }
                                    if (dist_tmp > eps15) dist_vector.push_back(dist_tmp);
                                }
                            }
                            if (isok) {
                                // This combination a candidate for the minimum distance cluster
                                distance_list.push_back(MinDistList(cellpair, dist_vector));
                            }
                        }

                        std::sort(distance_list.begin(), distance_list.end(), MinDistList::compare_sum_distance);

                        comb_cell_min.clear();

                        sum_dist_min = 0.0;
                        dist_max = 0.0;
                        for (j = 0; j < distance_list[0].dist.size(); ++j) {
                            sum_dist_min += distance_list[0].dist[j];
                            dist_max = std::max<double>(dist_max, distance_list[0].dist[j]);
                        }

                        for (j = 0; j < distance_list.size(); ++j) {
                            sum_dist = 0.0;

                            for (k = 0; k < distance_list[j].dist.size(); ++k) {
                                sum_dist += distance_list[j].dist[k];
                            }

                            // In the following, only pairs having minimum sum of distances
                            // are stored.
                            if (std::abs(sum_dist - sum_dist_min) < eps6) {
                                comb_cell_min.push_back(distance_list[j].cell);
                            } else {
                                break;
                            }
                        }

                        if (comb_cell_min.size() > 0) {
                            mindist_cluster_out[order][i].insert(MinimumDistanceCluster(data, comb_cell_min));
                        }

                    } while(g.next());
                }
            }
            intlist.clear();
        }
    }
}

void Interaction::cell_combination(std::vector<std::vector<int> > array, int i,
                                   std::vector<int> accum, std::vector<std::vector<int> > &comb)
{
    if (i == array.size())  {
        comb.push_back(accum);
    } else  {
        std::vector<int> row = array[i];
        for (int j = 0; j < row.size(); ++j) {
            std::vector<int> tmp(accum);
            tmp.push_back(row[j]);
            cell_combination(array,i+1,tmp, comb);
        }
    }
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_BITWISE_FUNCTIONS_SIMD_SSE_SSE2_SHLI_HPP_INCLUDED
#define BOOST_SIMD_BITWISE_FUNCTIONS_SIMD_SSE_SSE2_SHLI_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT
#include <boost/simd/bitwise/functions/shli.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/bitwise_or.hpp>
#include <boost/simd/include/constants/int_splat.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/sdk/meta/make_dependent.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::shli_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , ((simd_<type8_<A0>,boost::simd::tag::sse_>))
                                      (scalar_< integer_<A1> >)
                                    )
  {
    typedef A0 result_type;
    typedef typename meta::make_dependent<int64_t,A0>::type int_t;

    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      typedef simd::native<int_t,boost::simd::tag::sse_> gen_type;
      result_type const
      Mask1 = bitwise_cast<result_type>( boost::simd::integral_constant< gen_type
                                                      , 0x00ff00ff00ff00ffll
                                                      >()
                                    );
      result_type const
      Mask2 = bitwise_cast<result_type>( boost::simd::integral_constant < gen_type
                                                        , 0xff00ff00ff00ff00ll
                                                        >()
                                    );
      result_type tmp  = b_and(a0, Mask1);
      result_type tmp1 = _mm_slli_epi16(tmp, int(a1));
      tmp1 = b_and(tmp1, Mask1);
      tmp = b_and(a0, Mask2);
      result_type tmp3 = _mm_slli_epi16(tmp, int(a1));
      return tmp1 | b_and(tmp3, Mask2);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::shli_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , ((simd_<type32_<A0>,boost::simd::tag::sse_>))
                                      (scalar_< integer_<A1> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      typedef typename dispatch::meta::as_integer<A0,signed>::type sint;
      sint const that = _mm_slli_epi32(bitwise_cast<sint>(a0), int(a1));
      return bitwise_cast<A0>(that);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::shli_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , ((simd_<type64_<A0>,boost::simd::tag::sse_>))
                                      (scalar_< integer_<A1> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      typedef typename dispatch::meta::as_integer<A0,signed>::type sint;
      sint const that = _mm_slli_epi64(bitwise_cast<sint>(a0), int(a1));
      return bitwise_cast<A0>(that);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::shli_
                                    , boost::simd::tag::sse2_
                                    , (A0)(A1)
                                    , ((simd_<type16_<A0>,boost::simd::tag::sse_>))
                                      (scalar_< integer_<A1> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      return _mm_slli_epi16(a0, int(a1));
    }
  };
} } }

#endif
#endif