/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2012, Willow Garage, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id$
 *
 */

#ifndef FAAT_PCL_RECOGNITION_GRAPH_GEOMETRIC_CONSISTENCY_IMPL_H_
#define FAAT_PCL_RECOGNITION_GRAPH_GEOMETRIC_CONSISTENCY_IMPL_H_

#include "v4r/common/graph_geometric_consistency.h"
#include <pcl/registration/correspondence_types.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/common/io.h>
#include <pcl/common/time.h>
#include <boost/unordered_map.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/biconnected_components.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <exception>

struct V4R_EXPORTS ExtendedClique
{
    std::vector<size_t> * correspondences_;
    float avg_descriptor_distance_;
    float avg_pair_3D_distance_;
    float normalized_clique_size_;
    float avg_pair_3D_distance_unnormalized_;
    float far_away_correspondences_weight_;
};

bool less_clique_vectors (const std::vector<size_t> * a, const std::vector<size_t> * b);
bool best_clique_vectors (const std::pair<float, std::vector<size_t> *> a, const std::pair<float, std::vector<size_t> *> b);
bool best_extended_cliques (const ExtendedClique & a, const ExtendedClique & b);
bool gcGraphCorrespSorter (pcl::Correspondence i, pcl::Correspondence j);

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool
gcGraphCorrespSorter (pcl::Correspondence i, pcl::Correspondence j)
{
    return (i.distance < j.distance);
}

struct V4R_EXPORTS ViewD
{
    size_t idx_;
    size_t degree_;

    bool
    operator< (const ViewD & j) const
    {
        if (degree_ == j.degree_)
            return (idx_ < j.idx_);

        return degree_ > j.degree_;
    }
};

struct V4R_EXPORTS vertexDegreeSorter
{
    bool
    operator() (const ViewD & i, const ViewD & j) const
    {
        return i < j;
    }
};

template<typename Graph>
class V4R_EXPORTS save_cliques
{

public:
    /*save_cliques (std::size_t& max, std::size_t& maximum_clique, std::size_t& n_cliques, std::vector<std::vector<void *> *> & cliquess) :
     min_size (max), maximum (maximum_clique), n_cliques (n_cliques), cliques (cliquess)
     {
     }*/

    save_cliques (std::size_t& max, std::size_t& maximum_clique, std::size_t& n_cliques, std::vector<std::vector<size_t> *> & cliquess) :
        min_size (max), maximum (maximum_clique), n_cliques (n_cliques), cliques (cliquess)
    {
    }

    template<typename Clique, typename Graph2>
    inline void
    clique (const Clique& c, Graph2& g)
    {

        if (c.size () >= min_size)
        {
            BOOST_USING_STD_MAX();
            maximum = std::max BOOST_PREVENT_MACRO_SUBSTITUTION (maximum, c.size());

            //save clique...
            typename Clique::const_iterator i, end = c.end ();
            //std::vector<void *> * cc = new std::vector<void *> (c.size ());
            std::vector<size_t> * cc = new std::vector<size_t> (c.size ());
            cliques.push_back (cc);
            size_t p;
            for (i = c.begin (); i != end; ++i, ++p)
            {
                //cc->at (p) = static_cast<void *> (*i);
                cc->at (p) = (*i);
            }

            n_cliques++;
        }
        else
        {
            return;
        }

        // Simply assert that each vertex in the clique is connected
        // to all others in the clique.
        /*typename Clique::const_iterator i, j, end = c.end();
         for(i = c.begin(); i != end; ++i) {
         for(j = c.begin(); j != end; ++j) {
         if(i != j) {
         BOOST_ASSERT(edge(*i, *j, g).second);
         }
         }
         }*/
    }

    std::size_t& min_size;
    std::size_t& maximum;
    std::size_t& n_cliques;
    //std::vector<std::vector<void *> *> & cliques;
    std::vector<std::vector<size_t> *> & cliques;
};

class FAATPCL_CliquesException: public std::exception
{
    virtual const char* what() const throw()
    {
        return "My exception happened";
    }
} myex;

template<typename Graph>
class V4R_EXPORTS Tomita
{
    typedef std::set<typename boost::graph_traits<Graph>::vertex_descriptor> SetType;
    typedef std::vector<typename boost::graph_traits<Graph>::vertex_descriptor> VectorType;
    std::vector<VectorType *> cliques_found_;
    size_t min_clique_size_;
    typedef boost::unordered_map<typename boost::graph_traits<Graph>::vertex_descriptor, size_t> MapType;
    MapType used_ntimes_in_cliques_;
    std::vector<SetType> nnbrs;
    float max_time_allowed_;
    pcl::StopWatch time_elapsed_;
    bool max_time_reached_;

    void
    addClique (VectorType & clique)
    {
        if (clique.size () >= min_clique_size_)
        {
            VectorType * vt = new VectorType (clique);
            cliques_found_.push_back (vt);
            for (size_t i = 0; i < clique.size (); i++)
            {
                used_ntimes_in_cliques_[clique[i]]++;
            }

        }
    }

    void
    printSet (SetType & s)
    {
        typename SetType::iterator vertexIt, vertexEnd;
        SetType tmp;

        vertexIt = s.begin ();
        vertexEnd = s.end ();

        for (; vertexIt != vertexEnd; ++vertexIt)
        {
            std::cout << *vertexIt + 1 << " ";
        }

        std::cout << std::endl;
    }
    //_extend(nnbrs,cand,done,clique_so_far,cliques);
    void
    extend (SetType & cand, SetType & done, VectorType & clique_so_far)
    {
        SetType small_cand, pivot_nbrs;
        int maxconn = -1;
        size_t num_cand = cand.size ();

        //iterate over done and compute maximum intersection between candidates and the adjacents of done (nnbrs)
        typename SetType::iterator vertexIt, vertexEnd;
        SetType tmp;

        vertexIt = done.begin ();
        vertexEnd = done.end ();

        for (; vertexIt != vertexEnd; ++vertexIt)
        {
            std::set_intersection (cand.begin (), cand.end (), nnbrs[*vertexIt].begin (), nnbrs[*vertexIt].end (), std::inserter (tmp, tmp.begin ()));

            if (static_cast<int> (tmp.size ()) > maxconn)
            {
                maxconn = static_cast<int> (tmp.size ());
                pivot_nbrs = tmp;
                if (maxconn == (int)num_cand)
                {
                    //All possible cliques already found
                    return;
                }
            }

            tmp.clear ();
        }

        //same for candidates
        vertexIt = cand.begin ();
        vertexEnd = cand.end ();

        for (; vertexIt != vertexEnd; ++vertexIt)
        {
            std::set_intersection (cand.begin (), cand.end (), nnbrs[*vertexIt].begin (), nnbrs[*vertexIt].end (), std::inserter (tmp, tmp.begin ()));

            if (static_cast<int> (tmp.size ()) > maxconn)
            {
                maxconn = static_cast<int> (tmp.size ());
                pivot_nbrs = tmp;
            }
            tmp.clear ();
        }

        std::set_difference (cand.begin (), cand.end (), pivot_nbrs.begin (), pivot_nbrs.end (), std::inserter (small_cand, small_cand.begin ()));
        vertexIt = small_cand.begin ();
        vertexEnd = small_cand.end ();

        for (; vertexIt != vertexEnd; ++vertexIt)
        {
            cand.erase (*vertexIt);
            clique_so_far.push_back (*vertexIt);
            SetType new_cand, new_done;
            std::set_intersection (cand.begin (), cand.end (), nnbrs[*vertexIt].begin (), nnbrs[*vertexIt].end (),
                    std::inserter (new_cand, new_cand.begin ()));

            std::set_intersection (done.begin (), done.end (), nnbrs[*vertexIt].begin (), nnbrs[*vertexIt].end (),
                    std::inserter (new_done, new_done.begin ()));

            if (new_done.size () == 0 && new_cand.size () == 0)
                addClique (clique_so_far);
            else if (new_done.size () == 0 && (new_cand.size () == 1))
            {
                if ((clique_so_far.size () + 1) >= min_clique_size_)
                {
                    VectorType tt = clique_so_far;
                    tt.push_back (*(new_cand.begin ()));
                    addClique (tt);
                }
            }
            else
            {
                float t_elapsed = static_cast<float>(time_elapsed_.getTime());
                if(t_elapsed > max_time_allowed_)
                {
                    max_time_reached_ = true;
                    return;
                }

                extend (new_cand, new_done, clique_so_far);
            }

            clique_so_far.erase (clique_so_far.begin () + (clique_so_far.size () - 1));
            done.insert (*vertexIt);
        }
    }

public:

    Tomita (size_t mins = 3)
    {
        min_clique_size_ = mins;
        max_time_allowed_ = std::numeric_limits<float>::infinity();
        max_time_reached_ = false;
    }

    bool
    getMaxTimeReached()
    {
        return max_time_reached_;
    }

    void
    setMaxTimeAllowed(float t)
    {
        max_time_allowed_ = t;
    }

    void
    find_cliques (Graph & G, size_t num_v)
    {
        SetType cand, done;
        VectorType clique_so_far;
        nnbrs.clear ();
        used_ntimes_in_cliques_.clear ();
        cliques_found_.clear ();
        time_elapsed_.reset();
        max_time_reached_ = false;

        typename boost::graph_traits<Graph>::vertex_iterator vertexIt, vertexEnd;
        boost::tie (vertexIt, vertexEnd) = vertices (G);
        nnbrs.resize (num_v);

        size_t i = 0;
        for (; vertexIt != vertexEnd; ++vertexIt, ++i)
        {
            typename boost::graph_traits<Graph>::adjacency_iterator vi, vi_end;
            size_t k = 0;
            for (boost::tie (vi, vi_end) = boost::adjacent_vertices (*vertexIt, G); vi != vi_end; ++vi, ++k)
            {
                nnbrs[i].insert (*vi);
                cand.insert (*vi);
            }

            used_ntimes_in_cliques_[*vertexIt] = 0;
        }

        extend (cand, done, clique_so_far);
    }

    size_t
    getNumCliquesFound ()
    {
        return cliques_found_.size ();
    }

    void
    getCliques (std::vector<VectorType *> & cliques)
    {
        cliques = cliques_found_;
    }
};

bool
less_clique_vectors (const std::vector<size_t> * a, const std::vector<size_t> * b)
{
    return a->size () < b->size ();
}

bool
best_clique_vectors (const std::pair<float, std::vector<size_t> *> a,
                     const std::pair<float, std::vector<size_t> *> b)
{
    if(a.second->size() == b.second->size())
    {
        return a.first > b.first;
    }

    return a.second->size () > b.second->size ();
}

bool
best_extended_cliques (const ExtendedClique & a,
                       const ExtendedClique & b)
{
    /*float a_value = static_cast<float>(a.correspondences_->size()) * 0.5f + a.avg_descriptor_distance_ * 0.25f + a.avg_pair_3D_distance_ * 0.25f;
    float b_value = static_cast<float>(b.correspondences_->size()) * 0.5f + b.avg_descriptor_distance_ * 0.25f + b.avg_pair_3D_distance_ * 0.25f;*/

    /*float a_value = a.avg_pair_3D_distance_ * 0.5f + a.avg_descriptor_distance_ * 0.5f;
    float b_value = b.avg_pair_3D_distance_ * 0.5f + b.avg_descriptor_distance_ * 0.5f;*/

    /*float a_value = a.avg_pair_3D_distance_ * 0.5f;
    float b_value = b.avg_pair_3D_distance_ * 0.5f;*/

    /*float a_value = static_cast<float>(a.normalized_clique_size_) * 0.5f + a.avg_pair_3D_distance_ * 0.5f;
    float b_value = static_cast<float>(b.normalized_clique_size_) * 0.5f + b.avg_pair_3D_distance_ * 0.5f;*/

    /*float a_value = static_cast<float>(a.normalized_clique_size_) * 0.25f + a.avg_descriptor_distance_ * 0.25f + a.avg_pair_3D_distance_ * 0.25f + a.far_away_correspondences_weight_ * 0.25f;
    float b_value = static_cast<float>(b.normalized_clique_size_) * 0.25f + b.avg_descriptor_distance_ * 0.25f + b.avg_pair_3D_distance_ * 0.25f + b.far_away_correspondences_weight_ * 0.25f;*/

    float a_value = static_cast<float>(a.normalized_clique_size_) * 0.25f + a.avg_descriptor_distance_ * 0.25f; //+ a.far_away_correspondences_weight_ * 0.1f;
    float b_value = static_cast<float>(b.normalized_clique_size_) * 0.25f + b.avg_descriptor_distance_ * 0.25f; //+ b.far_away_correspondences_weight_ * 0.1f;

    return a_value > b_value;
}

template<typename PointModelT, typename PointSceneT>
void
v4r::GraphGeometricConsistencyGrouping<PointModelT, PointSceneT>::cleanGraph2(GraphGGCG & g, size_t gc_thres)
{
    typename boost::graph_traits<GraphGGCG>::vertex_iterator vertexIt, vertexEnd;
    std::vector<typename boost::graph_traits<GraphGGCG>::vertex_descriptor> to_be_removed;

    do
    {
        to_be_removed.clear();
        boost::tie (vertexIt, vertexEnd) = vertices (g);
        for (; vertexIt != vertexEnd; ++vertexIt)
        {
            size_t deg = boost::out_degree (*vertexIt, g);

            if ((deg > 0) && (deg < (gc_thres - 1)))
                to_be_removed.push_back (*vertexIt);
        }

        for (size_t i = 0; i < to_be_removed.size (); i++)
            clear_vertex (to_be_removed[i], g);

    } while(to_be_removed.size() > 0);
}

template<typename PointModelT, typename PointSceneT>
void
v4r::GraphGeometricConsistencyGrouping<PointModelT, PointSceneT>::cleanGraph(GraphGGCG & g, size_t gc_thres)
{
    cleanGraph2(g, gc_thres);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointModelT, typename PointSceneT>
void
v4r::GraphGeometricConsistencyGrouping<PointModelT, PointSceneT>::clusterCorrespondences (std::vector<pcl::Correspondences> &model_instances)
{
  try
  {
    model_instances.clear ();
    found_transformations_.clear ();

    //for the old gc...
    std::sort (model_scene_corrs_.begin (), model_scene_corrs_.end (), gcGraphCorrespSorter);

    if (model_scene_corrs_.empty())
        throw std::runtime_error("[pcl::GeometricConsistencyGrouping::clusterCorrespondences()] Error! Correspondences not set, please set them before calling again this function.\n");

    //temp copy of scene cloud with the type cast to ModelT in order to use Ransac
    PointCloudPtr temp_scene_cloud_ptr (new PointCloud ());
    pcl::copyPointCloud<PointSceneT, PointModelT> (*scene_, *temp_scene_cloud_ptr);

    GraphGGCG correspondence_graph (model_scene_corrs_.size ());
    float min_dist_for_cluster = param_.gc_size_ * param_.dist_for_cluster_factor_;

    for (size_t k = 0; k < model_scene_corrs_.size (); ++k)
    {
        int scene_index_k = model_scene_corrs_[k].index_match;
        int model_index_k = model_scene_corrs_[k].index_query;
        const Eigen::Vector3f& scene_point_k = scene_->at (scene_index_k).getVector3fMap ();
        const Eigen::Vector3f& model_point_k = input_->at (model_index_k).getVector3fMap ();
        const Eigen::Vector3f& scene_normal_k = scene_normals_->at (scene_index_k).getNormalVector3fMap ();
        const Eigen::Vector3f& model_normal_k = input_normals_->at (model_index_k).getNormalVector3fMap ();

        for (size_t j = (k + 1); j < model_scene_corrs_.size (); ++j)
        {
            int scene_index_j = model_scene_corrs_[j].index_match;
            int model_index_j = model_scene_corrs_[j].index_query;

            //same scene or model point constraint
            if(scene_index_j == scene_index_k || model_index_j == model_index_k)
                continue;

            const Eigen::Vector3f& scene_point_j = scene_->at (scene_index_j).getVector3fMap ();
            const Eigen::Vector3f& model_point_j = input_->at (model_index_j).getVector3fMap ();

            const Eigen::Vector3f& scene_normal_j = scene_normals_->at (scene_index_j).getNormalVector3fMap ();
            const Eigen::Vector3f& model_normal_j = input_normals_->at (model_index_j).getNormalVector3fMap ();

            Eigen::Vector3f dist_trg = model_point_k - model_point_j;
            Eigen::Vector3f dist_ref = scene_point_k - scene_point_j;

            //minimum distance constraint
            if ((dist_trg.norm () < min_dist_for_cluster) || (dist_ref.norm () < min_dist_for_cluster))
                continue;

            double distance = fabs (dist_trg.norm () - dist_ref.norm());
            double dot_distance = 0;
            if (pcl_isnan(scene_normal_k.dot (scene_normal_j)) || pcl_isnan(model_normal_k.dot (model_normal_j)))
                dot_distance = 0.f;
            else
            {
                float dot_model = model_normal_k.dot (model_normal_j);
                dot_distance = std::abs (scene_normal_k.dot (scene_normal_j) - dot_model);
            }

            //Model normals should be consistently oriented! otherwise reject!
            float dot_distance_model = model_normal_k.dot (model_normal_j);
            if(dot_distance_model < -0.1f)
                continue;

            //gc constraint and dot_product constraint!
            if ((distance < param_.gc_size_) && (dot_distance <= param_.thres_dot_distance_))
                boost::add_edge (k, j, correspondence_graph);
        }
    }

    typename boost::property_map < GraphGGCG, edge_component_t>::type components = get(edge_component, correspondence_graph);
    size_t n_cc = biconnected_components(correspondence_graph, components);

    if(n_cc < 1)
        return;

    std::vector<size_t> model_instances_kept_indices;

    std::vector< std::set<size_t> > unique_vertices_per_cc (n_cc);
    std::vector<size_t> cc_sizes (n_cc, 0);

    typename boost::graph_traits<GraphGGCG>::edge_iterator edgeIt, edgeEnd;
    boost::tie (edgeIt, edgeEnd) = edges (correspondence_graph);
    for (; edgeIt != edgeEnd; ++edgeIt)
    {
        int c = components[*edgeIt];
        unique_vertices_per_cc[c].insert(boost::source(*edgeIt, correspondence_graph));
        unique_vertices_per_cc[c].insert(boost::target(*edgeIt, correspondence_graph));
    }

    for(size_t i=0; i < unique_vertices_per_cc.size(); i++)
        cc_sizes[i] = unique_vertices_per_cc[i].size();

    pcl::registration::CorrespondenceRejectorSampleConsensus<PointModelT> corr_rejector;
    corr_rejector.setMaximumIterations (10000);
    corr_rejector.setInlierThreshold (param_.ransac_threshold_);
    corr_rejector.setInputSource (input_);
    corr_rejector.setInputTarget (temp_scene_cloud_ptr);
    corr_rejector.setSaveInliers(true);

    //Go through the connected components and decide whether to use CliqueGC or usualGC or ignore (cc_sizes[i] < gc_threshold_)
    //Decision based on the number of vertices in the connected component and graph arbocity...

    //std::cout << "Number of connected components over threshold..." << over_gc << std::endl;
    //    std::cout << "Number of connected components..." << n_cc << std::endl;
    size_t analyzed_ccs = 0;
    std::vector<bool> cliques_computation_possible_;
    cliques_computation_possible_.resize(n_cc, param_.use_graph_);
    for (size_t c = 0; c < n_cc; c++)
    {
        //ignore if not enough vertices...
        size_t num_v_in_cc = cc_sizes[c];
        if (num_v_in_cc < param_.gc_threshold_)
            continue;

        analyzed_ccs++;

        GraphGGCG connected_graph(correspondence_graph);

        //iterate over edges and remove those not belonging to this biconnected component
        boost::tie (edgeIt, edgeEnd) = edges (connected_graph);
        for (; edgeIt != edgeEnd; ++edgeIt)
        {
            if (components[*edgeIt] != c)
                boost::remove_edge(*edgeIt, connected_graph);
        }

        //std::cout << "Num edges connnected component:" << boost::num_edges(connected_graph) << std::endl;
        //visualizeGraph(connected_graph, "connected component");

        float arboricity = num_edges (connected_graph) / static_cast<float>(num_v_in_cc - 1);
        //std::cout << "arboricity:" << arboricity << " num_v:" << num_v_in_cc << " edges:" << num_edges (connected_graph) << std::endl;
        //std::vector<std::pair<int, int> > edges_used;
        std::set<size_t> correspondences_used;

        std::vector< std::vector<size_t> > correspondence_to_instance;
        if(param_.prune_by_CC_)
            correspondence_to_instance.resize(model_scene_corrs_.size());

        if (cliques_computation_possible_[c] && arboricity < 25 /*&& (num_v_in_cc < 400) && (num_edges (connected_graph) < 8000) && arboricity < 10*/)
        {
            //std::cout << "Using cliques" << std::endl;
            //std::cout << "N edges: " << num_edges (connected_graph) << " vertices:" << num_v_in_cc << " arboricity:" << arboricity <<  std::endl;

            std::vector<std::vector<size_t> *> cliques;
            {
                //pcl::ScopeTime t ("tomita cliques...");
                Tomita<GraphGGCG> tom (param_.gc_threshold_);
                tom.setMaxTimeAllowed(param_.max_time_allowed_cliques_comptutation_);
                tom.find_cliques (connected_graph, model_scene_corrs_.size ());
                if(tom.getMaxTimeReached())
                {
                    std::cout << "Max time ( " << std::setprecision(2) << param_.max_time_allowed_cliques_comptutation_ << " ms) reached during clique computation" << std::endl;
                    cliques_computation_possible_[c] = false;
                    c--;
                    analyzed_ccs--;

                    //free memory for cliques
                    tom.getCliques (cliques);
                    for (size_t p = 0; p < cliques.size (); p++)
                        delete cliques[p];

                    continue;
                }
                //std::cout << "Number of cliques found by tomita..." << tom.getNumCliquesFound () << std::endl;
                tom.getCliques (cliques);
            }

            std::vector< ExtendedClique > extended_cliques;
            std::vector<std::pair<float, std::vector<size_t> * > > cliques_with_average_weight;
            for(size_t k = 0; k < cliques.size(); k++)
            {
                float avg_dist = 0.f;
                float max_dist_ = 0.03f; //3 centimeters
                float far_away_average_weight_ = 0.f;

                for(size_t jj=0; jj < cliques[k]->size(); jj++)
                    avg_dist += model_scene_corrs_[ cliques[k]->at(jj) ].distance;

                avg_dist /= static_cast<float>(cliques[k]->size());
                cliques_with_average_weight.push_back(std::make_pair(avg_dist, cliques[k]));

                float avg_3D_dist = 0.f;

                for(size_t jj=0; jj < cliques[k]->size(); jj++)
                {

                    int scene_index_j = model_scene_corrs_[ cliques[k]->at(jj) ].index_match;
                    int model_index_j = model_scene_corrs_[ cliques[k]->at(jj) ].index_query;
                    const Eigen::Vector3f& scene_point_j = scene_->at (scene_index_j).getVector3fMap ();
                    const Eigen::Vector3f& model_point_j = input_->at (model_index_j).getVector3fMap ();

                    for(size_t kk=(jj+1); kk < cliques[k]->size(); kk++)
                    {
                        //for each pair, average 3D distance

                        int scene_index_k = model_scene_corrs_[ cliques[k]->at(kk) ].index_match;
                        int model_index_k = model_scene_corrs_[ cliques[k]->at(kk) ].index_query;

                        const Eigen::Vector3f& scene_point_k = scene_->at (scene_index_k).getVector3fMap ();
                        const Eigen::Vector3f& model_point_k = input_->at (model_index_k).getVector3fMap ();

                        Eigen::Vector3f dist_trg = model_point_k - model_point_j;
                        Eigen::Vector3f dist_ref = scene_point_k - scene_point_j;

                        float distance_ref_norm = dist_ref.norm();
                        float distance = fabs (dist_trg.norm () - dist_ref.norm());
                        avg_3D_dist += distance;

                        far_away_average_weight_ += std::min((distance_ref_norm / max_dist_), 1.f);
                    }
                }

                avg_3D_dist /= (static_cast<float>(cliques[k]->size()) * static_cast<float>(cliques[k]->size() - 1)) / 2.f;
                far_away_average_weight_ /= (static_cast<float>(cliques[k]->size()) * static_cast<float>(cliques[k]->size() - 1)) / 2.f;

                ExtendedClique ec;
                ec.correspondences_ = cliques[k];
                ec.avg_pair_3D_distance_ = avg_3D_dist;
                ec.avg_descriptor_distance_ = avg_dist;
                ec.avg_pair_3D_distance_unnormalized_ = avg_3D_dist;
                ec.far_away_correspondences_weight_ = far_away_average_weight_;
                extended_cliques.push_back(ec);
            }

            float max_avg_3D_dist = 0;
            float max_avg_descriptor_dist = 0;
            size_t max_clique_size = 0;

            for(size_t k = 0; k < cliques.size(); k++)
            {
                if(extended_cliques[k].avg_pair_3D_distance_ > max_avg_3D_dist)
                    max_avg_3D_dist = extended_cliques[k].avg_pair_3D_distance_;

                if(extended_cliques[k].correspondences_->size() > max_clique_size)
                    max_clique_size = extended_cliques[k].correspondences_->size();

                if(extended_cliques[k].avg_descriptor_distance_ > max_avg_descriptor_dist)
                    max_avg_descriptor_dist = extended_cliques[k].avg_descriptor_distance_;
            }

            for(size_t k = 0; k < cliques.size(); k++)
            {
                extended_cliques[k].avg_pair_3D_distance_ = 1.f - (extended_cliques[k].avg_pair_3D_distance_ / max_avg_3D_dist);
                extended_cliques[k].avg_descriptor_distance_ = 1.f - (extended_cliques[k].avg_descriptor_distance_ / max_avg_descriptor_dist);
                extended_cliques[k].normalized_clique_size_ = static_cast<float>(extended_cliques[k].correspondences_->size()) / static_cast<float>(max_clique_size);
            }

            //process cliques to remove similar ones...
            //sort (cliques.begin (), cliques.end (), less_clique_vectors); //cliques are sorted in increasing order (smaller cliques first)

            /*sort (cliques_with_average_weight.begin (), cliques_with_average_weight.end (), best_clique_vectors);
        for(size_t k = 0; k < cliques.size(); k++)
        {
            cliques[k] = cliques_with_average_weight[k].second;
        }*/

            sort (extended_cliques.begin (), extended_cliques.end (), best_extended_cliques);

            std::vector<std::vector<size_t> *>::iterator it;
            std::vector<size_t> taken_corresps (model_scene_corrs_.size (), 0);
            int max_taken = param_.max_taken_correspondence_;

            if(!param_.cliques_big_to_small_)
                std::reverse (cliques.begin (), cliques.end ());

            for (it = cliques.begin (); it != cliques.end (); it++)
            {
                //std::cout << "clique size:" << (*it)->size () << std::endl;
                //create a new clique based on how many time the correspondences in *it clique were used
                std::vector<size_t> * new_clique = new std::vector<size_t>;
                new_clique->reserve ((*it)->size ());
                size_t used = 0;
                for (size_t i = 0; i < (*it)->size (); i++)
                {
                    if (taken_corresps[(**it)[i]] < max_taken)
                    {
                        new_clique->push_back ((**it)[i]); //(**it)
                        used++;
                    }
                }

                if (used >= param_.gc_threshold_)
                {
                    new_clique->resize (used);

                    //do ransac with these correspondences...
                    pcl::Correspondences temp_corrs, filtered_corrs;
                    temp_corrs.reserve (used);
                    for (size_t j = 0; j < new_clique->size (); j++)
                    {
                        assert(new_clique->at (j) < model_scene_corrs_.size());
                        temp_corrs.push_back (model_scene_corrs_[ new_clique->at (j) ]);
                    }

                    corr_rejector.getRemainingCorrespondences (temp_corrs, filtered_corrs);

                    std::vector<int> inlier_indices;
                    corr_rejector.getInliersIndices (inlier_indices);

                    //check if corr_rejector.getBestTransformation () was not found already
                    bool found = poseExists (corr_rejector.getBestTransformation ());

                    if ((filtered_corrs.size () >= param_.gc_threshold_) && !found && (inlier_indices.size() != 0))
                    {
                        Eigen::Matrix4f trans = corr_rejector.getBestTransformation ();

                        //check if the normals are ok after applying the transformation
                        bool all_wrong = param_.check_normals_orientation_;

                        if( param_.check_normals_orientation_ )
                        {
                            for(size_t j=0; j < filtered_corrs.size(); j++)
                            {
                                //transform normal
                                const Eigen::Vector3f& model_normal = input_normals_->at (filtered_corrs[j].index_query).getNormalVector3fMap ();
                                const Eigen::Vector3f& scene_normal = scene_normals_->at (filtered_corrs[j].index_match).getNormalVector3fMap ();
                                if(!pcl_isfinite(model_normal[0]) || !pcl_isfinite(scene_normal[0]) ||
                                        !pcl_isfinite(model_normal[1]) || !pcl_isfinite(scene_normal[1]) ||
                                        !pcl_isfinite(model_normal[2]) || !pcl_isfinite(scene_normal[2]))
                                {
                                    continue;
                                }

                                Eigen::Vector3f nt;
                                nt[0] = static_cast<float> (trans (0, 0) * model_normal[0] + trans (0, 1) * model_normal[1] + trans (0, 2) * model_normal[2]);
                                nt[1] = static_cast<float> (trans (1, 0) * model_normal[0] + trans (1, 1) * model_normal[1] + trans (1, 2) * model_normal[2]);
                                nt[2] = static_cast<float> (trans (2, 0) * model_normal[0] + trans (2, 1) * model_normal[1] + trans (2, 2) * model_normal[2]);
                                if(nt.dot(scene_normal) >= (1.f - param_.thres_dot_distance_))
                                    all_wrong = false;

                            }
                        }

                        if(!all_wrong)
                        {
                            //PCL_INFO("Normals are consistent %d!!\n", static_cast<int>(all_nans));
                            found_transformations_.push_back (trans);
                            model_instances.push_back (filtered_corrs);

                            //mark all inliers
                            for (size_t j = 0; j < inlier_indices.size (); j++)
                            {
                                /*std::cout << "1" << inlier_indices[j] << std::endl;
                                std::cout << "2" << new_clique->size() << std::endl;
                                std::cout << "3" << new_clique->at (inlier_indices[j]) << std::endl;
                                std::cout << "4" << taken_corresps.size() << std::endl;*/

                                taken_corresps[new_clique->at (inlier_indices[j])]++;

                                if( param_.prune_by_CC_ )
                                    correspondence_to_instance[new_clique->at (inlier_indices[j])].push_back(model_instances.size() - 1);
                            }

                            if( param_.prune_by_CC_ )
                            {
                                for (size_t j = 0; j < inlier_indices.size (); j++)
                                    correspondences_used.insert(new_clique->at (inlier_indices[j]));
                            }
                        }
                        delete new_clique;
                    }
                    else
                        delete new_clique;
                }
                else
                    delete new_clique;
            }

            for (size_t p = 0; p < cliques.size (); p++)
                delete cliques[p];
        }
        else
        {
            //use iterative gc for simple cases with lots of correspondences...
            std::cout << "Correspondence grouping is too hard to solve it using cliques..." << std::endl;
//            std::cout << "N edges: " << num_edges (connected_graph) << " vertices:" << num_v_in_cc << " arboricity:" << arboricity <<  std::endl;

            std::vector<size_t> consensus_set;
            consensus_set.resize(model_scene_corrs_.size ());
            std::vector<bool> taken_corresps (model_scene_corrs_.size (), false);

            GraphGGCG connected_graph(correspondence_graph);
            //iterate over edges and remove those not belonging to this biconnected component
            boost::tie (edgeIt, edgeEnd) = edges (connected_graph);
            for (; edgeIt != edgeEnd; ++edgeIt)
            {
                if (components[*edgeIt] != c)
                {
                    boost::remove_edge(*edgeIt, connected_graph);
                }
            }

            typename boost::graph_traits<GraphGGCG>::vertex_iterator vertexIt, vertexEnd;
            boost::tie (vertexIt, vertexEnd) = vertices (connected_graph);
            for (; vertexIt != vertexEnd; ++vertexIt)
            {
                if ( boost::out_degree(*vertexIt, connected_graph) < (param_.gc_threshold_ - 1))
                    taken_corresps[*vertexIt] = true;
            }

            for (size_t i = 0; i < model_scene_corrs_.size (); ++i)
            {
                if (taken_corresps[i])
                    continue;

                int consensus_size = 0;
                consensus_set[consensus_size++] = i;

                for (size_t j = 0; j < model_scene_corrs_.size (); ++j)
                {
                    if (j != i && !taken_corresps[j])
                    {
                        //Let's check if j fits into the current consensus set
                        bool is_a_good_candidate = true;

                        for (int k = 0; k < consensus_size; ++k)
                        {
                            //check if edge (j, consensus_set[k] exists in the graph, if it does not, is_a_good_candidate = false!...
                            if (!(boost::edge (j, consensus_set[k], connected_graph).second))
                            {
                                is_a_good_candidate = false;
                                break;
                            }
                        }

                        if (is_a_good_candidate)
                            consensus_set[consensus_size++] = j;
                    }
                }

                if (consensus_size >= param_.gc_threshold_)
                {
                    pcl::Correspondences temp_corrs, filtered_corrs;
                    temp_corrs.reserve (consensus_size);

                    for (size_t j = 0; j < consensus_size; j++)
                        temp_corrs.push_back (model_scene_corrs_[ consensus_set[j] ]);

                    if ( param_.ransac_threshold_ > 0)
                    {
                        //pcl::ScopeTime tt("ransac filtering");
                        //ransac filtering
                        corr_rejector.getRemainingCorrespondences (temp_corrs, filtered_corrs);
                        //check if corr_rejector.getBestTransformation () was not found already
                        bool found = poseExists (corr_rejector.getBestTransformation ());

                        std::vector<int> inlier_indices;
                        corr_rejector.getInliersIndices (inlier_indices);

                        //save transformations for recognize
                        if ((filtered_corrs.size () >= param_.gc_threshold_) && !found && (inlier_indices.size() != 0))
                        {
                            Eigen::Matrix4f trans = corr_rejector.getBestTransformation ();

                            //check if the normals are ok after applying the transformation
                            bool all_wrong = param_.check_normals_orientation_;

                            if( param_.check_normals_orientation_ )
                            {
                                for(size_t j=0; j < filtered_corrs.size(); j++)
                                {
                                    //transform normal
                                    const Eigen::Vector3f& model_normal = input_normals_->at (filtered_corrs[j].index_query).getNormalVector3fMap ();
                                    const Eigen::Vector3f& scene_normal = scene_normals_->at (filtered_corrs[j].index_match).getNormalVector3fMap ();
                                    if(!pcl_isfinite(model_normal[0]) || !pcl_isfinite(scene_normal[0]) ||
                                            !pcl_isfinite(model_normal[1]) || !pcl_isfinite(scene_normal[1]) ||
                                            !pcl_isfinite(model_normal[2]) || !pcl_isfinite(scene_normal[2]))
                                    {
                                        continue;
                                    }

                                    Eigen::Vector3f nt;
                                    nt[0] = static_cast<float> (trans (0, 0) * model_normal[0] + trans (0, 1) * model_normal[1] + trans (0, 2) * model_normal[2]);
                                    nt[1] = static_cast<float> (trans (1, 0) * model_normal[0] + trans (1, 1) * model_normal[1] + trans (1, 2) * model_normal[2]);
                                    nt[2] = static_cast<float> (trans (2, 0) * model_normal[0] + trans (2, 1) * model_normal[1] + trans (2, 2) * model_normal[2]);
                                    if(nt.dot(scene_normal) >= (1.f - param_.thres_dot_distance_))
                                        all_wrong = false;

                                }
                            }

                            if(all_wrong)
                            {
                                //PCL_ERROR("Normals are not consistent %d %d!!\n", static_cast<int>(all_wrong), static_cast<int>(all_nans));
                                for (size_t j = 0; j < consensus_size; j++)
                                    taken_corresps[consensus_set[j]] = false;
                            }
                            else
                            {
                                //PCL_INFO("Normals are consistent, pushing filtered_corrs %d!!\n", static_cast<int>(filtered_corrs.size()));
                                found_transformations_.push_back (trans);
                                model_instances.push_back (filtered_corrs);

                                //mark all inliers
                                for (size_t j = 0; j < inlier_indices.size (); j++)
                                {
                                    taken_corresps[consensus_set[inlier_indices[j]]] = true;

                                    if(param_.prune_by_CC_)
                                        correspondence_to_instance[consensus_set[inlier_indices[j]]].push_back(model_instances.size() - 1);
                                }

                                if(param_.prune_by_CC_)
                                {
                                    for (size_t j = 0; j < inlier_indices.size (); j++)
                                        correspondences_used.insert(consensus_set[inlier_indices[j]]);
                                }
                            }
                        }
                        else
                        {
                            //Free the correspondences so they can be used in another set...
                            //PCL_ERROR("Freeing %d correspondences from invalid set...\n", consensus_set.size ());
                            for (size_t j = 0; j < consensus_size; j++)
                                taken_corresps[consensus_set[j]] = false;
                        }
                    }
                }
            }
        }

        if( param_.prune_by_CC_ )
        {
            //pcl::ScopeTime t("final post-processing...");
            GraphGGCG connected_graph_used_edges(connected_graph);
            typename boost::graph_traits<GraphGGCG>::vertex_iterator vertexIt, vertexEnd;
            std::vector<typename boost::graph_traits<GraphGGCG>::vertex_descriptor> to_be_removed;
            boost::tie (vertexIt, vertexEnd) = vertices (connected_graph_used_edges);
            for (; vertexIt != vertexEnd; ++vertexIt)
            {
                std::set<size_t>::const_iterator it;
                it = correspondences_used.find(*vertexIt);
                if (it == correspondences_used.end())
                    to_be_removed.push_back (*vertexIt);
            }

            for (size_t i = 0; i < to_be_removed.size (); i++)
                clear_vertex (to_be_removed[i], connected_graph_used_edges);

            boost::vector_property_map<size_t> components (boost::num_vertices (connected_graph_used_edges));
            size_t n_cc = boost::connected_components (connected_graph_used_edges, &components[0]);

            std::vector<size_t> cc_sizes  (n_cc, 0);
            for (size_t i = 0; i < model_scene_corrs_.size (); i++)
                cc_sizes[components[i]]++;

            size_t ncc_overthres = 0;
            for (size_t i = 0; i < n_cc; i++)
            {
                if(cc_sizes[i] >= param_.gc_threshold_)
                    ncc_overthres++;
            }

            //std::cout << "Number of connected components over threshold: " << ncc_overthres << std::endl;

            //somehow now i need to do a Nonmax supression of the model_instances that are in the same CC
            //gather instances that were generated with correspondences found in a specific CC
            //correspondence_to_instance maps correspondences (vertices) to instance, we can use that i guess

            for (size_t internal_c = 0; internal_c < n_cc; internal_c++)
            {
                //ignore if not enough vertices...
                size_t num_v_in_cc_tmp = cc_sizes[internal_c];
                if (num_v_in_cc_tmp < param_.gc_threshold_)
                    continue;

                std::set<size_t> instances_for_this_cc;
                {
                    boost::tie (vertexIt, vertexEnd) = vertices (connected_graph_used_edges);

                    for (; vertexIt != vertexEnd; ++vertexIt)
                    {
                        if (components[*vertexIt] == internal_c)
                        {
                            for(size_t k=0; k < correspondence_to_instance[*vertexIt].size(); k++)
                            {
                                instances_for_this_cc.insert(correspondence_to_instance[*vertexIt][k]);
                            }
                        }
                    }
                }

                //std::cout << "instances in this cc:" << instances_for_this_cc.size() << std::endl;
                std::set<size_t>::const_iterator it;
                size_t max_size = 0;
                for(it = instances_for_this_cc.begin(); it != instances_for_this_cc.end(); it++)
                {
                    //std::cout << *it << " " << model_instances[*it].size() << std::endl;
                    if(max_size <= model_instances[*it].size())
                    {
                        max_size = model_instances[*it].size();
                        //max_idx = *it;
                    }
                }

                //std::cout << std::endl;

                float thres = 0.5f;
                for(it = instances_for_this_cc.begin(); it != instances_for_this_cc.end(); it++)
                {
                    if( model_instances[*it].size() > (max_size * thres))
                        model_instances_kept_indices.push_back(*it);
                }
            }

            /*if(visualize_graph_ && correspondences_used.size() > 0)
            visualizeGraph(connected_graph_used_edges, "used edges");*/
        }
    }

    if(param_.prune_by_CC_)
    {
        for(size_t i=0; i < model_instances_kept_indices.size(); i++)
        {
            model_instances[i] = model_instances[model_instances_kept_indices[i]];
            found_transformations_[i] = found_transformations_[model_instances_kept_indices[i]];
        }

        model_instances.resize(model_instances_kept_indices.size());
        found_transformations_.resize(model_instances_kept_indices.size());
    }

    //visualizeCorrespondences(*model_scene_corrs_);

  }
  // HACK: Michael Zillich: added this try/catch, as somewhere above somtimes a range
  // check exception is thrown. This will have to be handled properly eventually
  catch(std::exception e)
  {
      std::cerr << "v4r::GraphGeometricConsistencyGroupingi: caught exception: "
        << e.what() << "\n";
  }
  // HACK END
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointModelT, typename PointSceneT>
bool
v4r::GraphGeometricConsistencyGrouping<PointModelT, PointSceneT>::recognize (
        std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<
        Eigen::Matrix4f> > &transformations)
{
    std::vector<pcl::Correspondences> model_instances;
    return (this->recognize (transformations, model_instances));
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointModelT, typename PointSceneT>
bool
v4r::GraphGeometricConsistencyGrouping<PointModelT, PointSceneT>::recognize (
        std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<
        Eigen::Matrix4f> > &transformations,
        std::vector<pcl::Correspondences> &clustered_corrs)
{
    transformations.clear ();
    if (!this->initCompute ())
    {
        PCL_ERROR("[v4r::GraphGeometricConsistencyGrouping::recognize()] Error! Model cloud or Scene cloud not set, please set them before calling again this function.\n");
        return (false);
    }

    clusterCorrespondences (clustered_corrs);

    transformations = found_transformations_;

    this->deinitCompute ();
    return (true);
}

#endif // FAAT_PCL_RECOGNITION_SI_GEOMETRIC_CONSISTENCY_IMPL_H_


/*!
@file
Internal header to break cyclic dependencies.

@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_HANA_DETAIL_MAYBE_FWD_HPP
#define BOOST_HANA_DETAIL_MAYBE_FWD_HPP

#include <boost/hana/bool.hpp>
#include <boost/hana/logical/logical.hpp>

#include <boost/hana/comparable/comparable.hpp>
#include <boost/hana/core/datatype.hpp>
#include <boost/hana/monad/monad.hpp>

namespace boost { namespace hana {
    //! @ingroup group-datatypes
    //! Represents an optional value.
    //!
    //! A `Maybe` either contains a value (represented as `just(x)`), or it
    //! is empty (represented as `nothing`).
    //!
    //! ## Instance of
    //! `Comparable`, `Functor`, `Applicative`, `Monad`, `Foldable`
    //! and `Searchable`
    struct Maybe { };

    namespace maybe_detail {
        template <bool is_valid, typename T,
            typename = operators<Comparable, Monad>>
        struct maybe {
            using hana_datatype = Maybe;

            template <typename Default, typename F>
            constexpr auto maybe_impl(Default d, F) const
            { return d; }
        };

        template <typename T>
        struct maybe<true, T> {
            using hana_datatype = Maybe;
            T val;

            template <typename Default, typename F>
            constexpr auto maybe_impl(Default, F f) const
            { return f(val); }
        };

        template <typename T>
        using just = maybe<true, T>;
        using nothing = maybe<false, void>;
    }

#ifdef BOOST_HANA_DOXYGEN_INVOKED
    //! Create an optional value containing `x`.
    //! @relates Maybe
    //!
    //! ### Example
    //! @snippet example/maybe/just.cpp main
    constexpr auto just = [](auto x) {
        return unspecified;
    };

    //! An empty optional value.
    //! @relates Maybe
    //!
    //! ### Example
    //! @snippet example/maybe/nothing.cpp main
    constexpr unspecified nothing{};
#else
    BOOST_HANA_CONSTEXPR_LAMBDA auto just = [](auto x) {
        return maybe_detail::just<decltype(x)>{x};
    };

    constexpr maybe_detail::nothing nothing{};
#endif

    //! Create a `Maybe` with the result of a function, but only if a
    //! predicate is satisfied.
    //! @relates Maybe
    //!
    //! Specifically, returns `just(f(x))` if `predicate(x)` is a true-valued
    //! `Logical`, and `nothing` otherwise.
    //!
    //!
    //! @param predicate
    //! A function called as `predicate(x)` and returning a true-valued
    //! `Logical` if `just(f(x))` should be the resulting value, and a
    //! false-valued one if `nothing` should be the resulting value.
    //! In the current version of the library, the result of `predicate`
    //! has to be a [compile-time](@ref Logical_terminology) `Logical`.
    //!
    //! @param f
    //! A function called as `f(x)` if the `predicate` returns a true-valued
    //! `Logical`, and not called at all otherwise. If the `predicate` returns
    //! a false-valued `Logical`, the `f(x)` expression is not even required
    //! to be well-formed.
    //!
    //! @param x
    //! The value to either transform and put in a `just`, or discard.
    //!
    //!
    //! ### Example
    //! @snippet example/maybe/only_when.cpp main
    BOOST_HANA_CONSTEXPR_LAMBDA auto only_when = [](auto predicate, auto f, auto x) {
        return eval_if(predicate(x),
            [=](auto _) { return just(_(f)(x)); },
            [](auto _) { return nothing; }
        );
    };

    //! Apply a function to the contents of a `Maybe`, with a fallback
    //! result.
    //! @relates Maybe
    //!
    //! Specifically, `maybe` takes a default value, a function and an
    //! optional value. If the optional value is `nothing`, the default
    //! value is returned. Otherwise, the function is applied to the
    //! content of the `just`.
    //!
    //!
    //! @param default_
    //! A default value returned if `m` is `nothing`.
    //!
    //! @param f
    //! A function called as `f(x)` if and only if `m` is an optional value
    //! of the form `just(x)`. In that case, the result returend by `maybe`
    //! is the result of `f`.
    //!
    //! @param m
    //! An optional value.
    //!
    //!
    //! ### Example
    //! @snippet example/maybe/maybe.cpp main
#ifdef BOOST_HANA_DOXYGEN_INVOKED
    constexpr auto maybe = [](auto default_, auto f, auto m) {
        unspecified;
    };
#else
    BOOST_HANA_CONSTEXPR_LAMBDA auto maybe = [](auto default_, auto f, auto m) {
        return m.maybe_impl(default_, f);
    };
#endif

    //! Return whether a `Maybe` contains a value.
    //! @relates Maybe
    //!
    //! Specifically, returns a [compile-time](@ref Logical_terminology)
    //! true-valued `Logical` if `m` is of the form `just(x)` for some `x`,
    //! and a false-valued one otherwise.
    //!
    //! ### Example
    //! @snippet example/maybe/is_just.cpp main
    BOOST_HANA_CONSTEXPR_LAMBDA auto is_just = [](auto m) {
        return maybe(false_, [](auto) { return true_; }, m);
    };

    //! Return whether a `Maybe` is empty.
    //! @relates Maybe
    //!
    //! Specifically, returns a [compile-time](@ref Logical_terminology)
    //! true-valued `Logical` if `m` is of the form `nothing`, and a
    //! false-valued one otherwise.
    //!
    //! ### Example
    //! @snippet example/maybe/is_nothing.cpp main
    BOOST_HANA_CONSTEXPR_LAMBDA auto is_nothing = [](auto m) {
        return maybe(true_, [](auto) { return false_; }, m);
    };

    //! Return the contents of a `Maybe`, with a fallback result.
    //! @relates Maybe
    //!
    //! Specifically, returns `x` if `m` of the form `just(x)`, and `default_`
    //! if `m` is of the form `nothing`.
    //!
    //!
    //! @param default_
    //! The default value to return if `m` is `nothing`.
    //!
    //! @param m
    //! The optional value to try to retrieve the value from.
    //!
    //!
    //! ### Example
    //! @snippet example/maybe/from_maybe.cpp main
    BOOST_HANA_CONSTEXPR_LAMBDA auto from_maybe = [](auto default_, auto m) {
        return maybe(default_, [](auto x) { return x; }, m);
    };

    //! Extract the content of a `Maybe` or fail at compile-time.
    //! @relates Maybe
    //!
    //! Specifically, returns `x` if the optional value is of the form
    //! `just(x)`, and triggers a static assertion otherwise.
    //!
    //! ### Example
    //! @snippet example/maybe/from_just.cpp main
    BOOST_HANA_CONSTEXPR_LAMBDA auto from_just = [](auto m) {
        auto err = [](auto ...dum) {
            constexpr bool always_false = sizeof...(dum) != 0;
            static_assert(always_false,
            "trying to extract the value inside a boost::hana::nothing "
            "with boost::hana::from_just");
        };
        return maybe(err, [](auto x) { return [=] { return x; }; }, m)();
    };
}} // end namespace boost::hana

#endif // !BOOST_HANA_DETAIL_MAYBE_FWD_HPP


#include "MeshCuboidSymmetryGroup.h"
#include "MeshCuboidParameters.h"
#include "Utilities.h"

#include <bitset>
#include <Eigen/Eigenvalues>

MeshCuboidSymmetryGroupInfo::MeshCuboidSymmetryGroupInfo()
	: symmetry_type_(ReflectionSymmetryType)
	, aligned_global_axis_index_(0)
{

}

MeshCuboidSymmetryGroupInfo::MeshCuboidSymmetryGroupInfo(MeshCuboidSymmetryGroupType _symmetry_type,
	unsigned int _aligned_global_axis_index)
	: symmetry_type_(_symmetry_type)
	, aligned_global_axis_index_(_aligned_global_axis_index)
{

}

MeshCuboidSymmetryGroupInfo::MeshCuboidSymmetryGroupInfo(const MeshCuboidSymmetryGroupInfo& _other)
{
	symmetry_type_ = _other.symmetry_type_;
	aligned_global_axis_index_ = _other.aligned_global_axis_index_;
	single_label_indices_ = _other.single_label_indices_;
	pair_label_indices_ = _other.pair_label_indices_;
}

MeshCuboidSymmetryGroup::MeshCuboidSymmetryGroup()
	: num_symmetry_orders_(2)
{

}

MeshCuboidSymmetryGroup::MeshCuboidSymmetryGroup(const MeshCuboidSymmetryGroupInfo &_info)
	: info_(_info)
	, num_symmetry_orders_(2)
{

}

MeshCuboidSymmetryGroup::MeshCuboidSymmetryGroup(const MeshCuboidSymmetryGroup &_other)
	:info_(_other.info_)
	, num_symmetry_orders_(_other.num_symmetry_orders_)
{

}

MeshCuboidSymmetryGroup::~MeshCuboidSymmetryGroup()
{

}

unsigned int MeshCuboidSymmetryGroup::num_symmetry_orders() const
{
	assert(num_symmetry_orders_ >= 2);
	return num_symmetry_orders_;
}

Real MeshCuboidSymmetryGroup::get_rotation_angle() const
{
	return M_PI / 180.0 * (360.0 / num_symmetry_orders_);
}

unsigned int MeshCuboidSymmetryGroup::get_aligned_global_axis_index() const
{
	assert(info_.aligned_global_axis_index_ < 3);
	return info_.aligned_global_axis_index_;
}

void MeshCuboidSymmetryGroup::get_single_cuboid_indices(const std::vector<MeshCuboid *>& _cuboids,
	std::vector<unsigned int> &_single_cuboid_indices) const
{
	const unsigned int num_cuboids = _cuboids.size();
	_single_cuboid_indices.clear();

	unsigned int num_single_labels = info_.single_label_indices_.size();
	_single_cuboid_indices.reserve(num_single_labels);

	for (unsigned int i = 0; i < num_single_labels; ++i)
	{
		Label label_index = info_.single_label_indices_[i];
		int cuboid_index = 0;
		for (; cuboid_index < num_cuboids; ++cuboid_index)
		{
			if (_cuboids[cuboid_index]->get_label_index() == label_index) break;
		}

		if (cuboid_index < num_cuboids)
			_single_cuboid_indices.push_back(cuboid_index);
	}
}

void MeshCuboidSymmetryGroup::get_pair_cuboid_indices(const std::vector<MeshCuboid *>& _cuboids,
	std::vector< std::pair<unsigned int, unsigned int> > &_pair_cuboid_indices) const
{
	const unsigned int num_cuboids = _cuboids.size();
	_pair_cuboid_indices.clear();

	unsigned int num_pair_labels = info_.pair_label_indices_.size();
	_pair_cuboid_indices.reserve(num_pair_labels);

	for (unsigned int i = 0; i < num_pair_labels; ++i)
	{
		Label label_index_1 = info_.pair_label_indices_[i].first;
		Label label_index_2 = info_.pair_label_indices_[i].second;

		unsigned int cuboid_index_1 = 0;
		for (; cuboid_index_1 < num_cuboids; ++cuboid_index_1)
			if (_cuboids[cuboid_index_1]->get_label_index() == label_index_1) break;

		unsigned int cuboid_index_2 = 0;
		for (; cuboid_index_2 < num_cuboids; ++cuboid_index_2)
			if (_cuboids[cuboid_index_2]->get_label_index() == label_index_2) break;

		if (cuboid_index_1 < num_cuboids && cuboid_index_2 < num_cuboids)
			_pair_cuboid_indices.push_back(std::make_pair(cuboid_index_1, cuboid_index_2));
	}
}

void MeshCuboidSymmetryGroup::get_symmetric_sample_point_pairs(
	const std::vector<MeshCuboid *> &_cuboids,
	const std::vector<ANNpointArray> &_cuboid_ann_points,
	const std::vector<ANNkd_tree *> &_cuboid_ann_kd_tree,
	const Real _squared_neighbor_distance,
	std::list<WeightedPointPair> &_sample_point_pairs) const
{
	const unsigned int num_cuboids = _cuboids.size();
	assert(_cuboid_ann_points.size() == num_cuboids);
	assert(_cuboid_ann_kd_tree.size() == num_cuboids);

	_sample_point_pairs.clear();

	std::vector<unsigned int> single_cuboid_indices;
	get_single_cuboid_indices(_cuboids, single_cuboid_indices);

	for (std::vector<unsigned int>::const_iterator it = single_cuboid_indices.begin();
		it != single_cuboid_indices.end(); ++it)
	{
		const unsigned int cuboid_index = (*it);

		get_symmetric_sample_point_pairs(
			_cuboids[cuboid_index],
			_cuboid_ann_points[cuboid_index],
			_cuboid_ann_kd_tree[cuboid_index],
			_squared_neighbor_distance, _sample_point_pairs);
	}

	// NOTE:
	// Get inter-cuboid symmetric points only for reflection symmetry.
	if (get_symmetry_type() == ReflectionSymmetryType)
	{
		std::vector< std::pair<unsigned int, unsigned int> > pair_cuboid_indices;
		get_pair_cuboid_indices(_cuboids, pair_cuboid_indices);

		for (std::vector< std::pair<unsigned int, unsigned int> >::const_iterator it = pair_cuboid_indices.begin();
			it != pair_cuboid_indices.end(); ++it)
		{
			const unsigned int cuboid_index_1 = (*it).first;
			const unsigned int cuboid_index_2 = (*it).second;

			// 1 -> 2.
			get_symmetric_sample_point_pairs(
				_cuboids[cuboid_index_1],
				_cuboid_ann_points[cuboid_index_2],
				_cuboid_ann_kd_tree[cuboid_index_2],
				_squared_neighbor_distance, _sample_point_pairs);

			// 2 -> 1.
			get_symmetric_sample_point_pairs(
				_cuboids[cuboid_index_2],
				_cuboid_ann_points[cuboid_index_1],
				_cuboid_ann_kd_tree[cuboid_index_1],
				_squared_neighbor_distance, _sample_point_pairs);
		}
	}
}

void MeshCuboidSymmetryGroup::get_symmetric_sample_point_pairs(
	const MeshCuboid *_cuboid_1,
	const ANNpointArray &_cuboid_ann_points_2,
	ANNkd_tree *_cuboid_ann_kd_tree_2,
	const Real _squared_neighbor_distance,
	std::list<WeightedPointPair> &_sample_point_pairs) const
{
	if (!_cuboid_1) return;
	std::vector<MyMesh::Point> cuboid_1_sample_points;
	_cuboid_1->get_sample_points(cuboid_1_sample_points);

	get_symmetric_sample_point_pairs(
		cuboid_1_sample_points,
		_cuboid_ann_points_2,
		_cuboid_ann_kd_tree_2,
		_squared_neighbor_distance,
		_sample_point_pairs);
}

void MeshCuboidSymmetryGroup::get_symmetric_sample_point_pairs(
	const std::vector<MyMesh::Point> &_cuboid_1_sample_points,
	const ANNpointArray &_cuboid_ann_points_2,
	ANNkd_tree *_cuboid_ann_kd_tree_2,
	const Real _squared_neighbor_distance,
	std::list<WeightedPointPair> &_sample_point_pairs) const
{
	if (_cuboid_1_sample_points.size() == 0 || !_cuboid_ann_points_2 || !_cuboid_ann_kd_tree_2)
		return;

	const int num_points_1 = _cuboid_1_sample_points.size();
	const int num_points_2 = _cuboid_ann_kd_tree_2->nPoints();

	//
	ANNpoint q = annAllocPt(3);
	ANNidxArray nn_idx = new ANNidx[1];
	ANNdistArray dd = new ANNdist[1];
	//

	for (int point_index_1 = 0; point_index_1 < num_points_1; ++point_index_1)
	{
		MyMesh::Point point_1 = _cuboid_1_sample_points[point_index_1];

		for (int symmetry_order = 1; symmetry_order < num_symmetry_orders_; ++symmetry_order)
		{
			MyMesh::Point symmetric_point_1 = get_symmetric_point(point_1, symmetry_order);

			for (unsigned int i = 0; i < 3; ++i)
				q[i] = symmetric_point_1[i];

			int num_searched_neighbors = _cuboid_ann_kd_tree_2->annkFRSearch(
				q, _squared_neighbor_distance, 1, nn_idx, dd);
			if (num_searched_neighbors > 0)
			{
				int point_index_2 = (int)nn_idx[0];
				assert(point_index_2 < num_points_2);

				MyMesh::Point point_2;
				for (unsigned int i = 0; i < 3; ++i)
					point_2[i] = _cuboid_ann_points_2[point_index_2][i];

				//
				// Debug.
				Real test_distance = (symmetric_point_1 - point_2).norm();
				CHECK_NUMERICAL_ERROR(__FUNCTION__, test_distance, std::sqrt(dd[0]));
				double distance = (std::sqrt(_squared_neighbor_distance) - std::sqrt(dd[0]));
				assert(distance >= 0);
				//

				Real weight = distance * distance;
				Real angle = symmetry_order * get_rotation_angle();
				WeightedPointPair point_pair(point_1, point_2, weight, angle);
				_sample_point_pairs.push_back(point_pair);
			}
		}
	}

	//
	annDeallocPt(q);
	delete[] nn_idx;
	delete[] dd;
	//
}

MeshCuboidReflectionSymmetryGroup* MeshCuboidReflectionSymmetryGroup::constructor(
	const MeshCuboidSymmetryGroupInfo &_info,
	const std::vector<MeshCuboid *>& _cuboids)
{
	MeshCuboidReflectionSymmetryGroup *group = new MeshCuboidReflectionSymmetryGroup(_info);
	bool ret = group->compute_symmetry_axis(_cuboids);
	if (!ret)
	{
		delete group;
		group = NULL;
	}
	return group;
}

MeshCuboidReflectionSymmetryGroup::MeshCuboidReflectionSymmetryGroup(
	const MyMesh::Normal _n, const double _t)
	: MeshCuboidSymmetryGroup()
	, n_(_n)
	, t_(_t)
{

}

MeshCuboidReflectionSymmetryGroup::MeshCuboidReflectionSymmetryGroup(
	const MeshCuboidSymmetryGroupInfo &_info)
	: MeshCuboidSymmetryGroup(_info)
	, n_(MyMesh::Normal(1, 0, 0))
	, t_(0)
{
	assert(_info.symmetry_type_ == ReflectionSymmetryType);
}

MeshCuboidReflectionSymmetryGroup::MeshCuboidReflectionSymmetryGroup(
	const MeshCuboidReflectionSymmetryGroup &_other)
	: MeshCuboidSymmetryGroup(_other)
	, n_(_other.n_)
	, t_(_other.t_)
{
	n_.normalize();
	assert(info_.symmetry_type_ == ReflectionSymmetryType);
}

MeshCuboidReflectionSymmetryGroup::~MeshCuboidReflectionSymmetryGroup()
{

}

unsigned int MeshCuboidReflectionSymmetryGroup::num_axis_parameters()
{
	return (3 + 1);
}

MeshCuboidSymmetryGroupType MeshCuboidReflectionSymmetryGroup::get_symmetry_type() const
{
	assert(info_.symmetry_type_ == ReflectionSymmetryType);
	return info_.symmetry_type_;
}

unsigned int MeshCuboidReflectionSymmetryGroup::num_symmetry_orders() const
{
	// NOTE:
	// Reflection symmetry is order of 2.
	assert(num_symmetry_orders_ == 2);
	return num_symmetry_orders_;
}

void MeshCuboidReflectionSymmetryGroup::set_num_symmetry_order(unsigned int _symmetry_order)
{
	// NOTE:
	// Reflection symmetry is order of 2.
	assert(false);
}

void MeshCuboidReflectionSymmetryGroup::get_reflection_plane(MyMesh::Normal &_n, double &_t) const
{
	_n = n_;
	_t = t_;
}

void MeshCuboidReflectionSymmetryGroup::set_reflection_plane(const MyMesh::Normal &_n, const double &_t)
{
	n_ = _n;
	t_ = _t;
	n_.normalize();
}

bool MeshCuboidReflectionSymmetryGroup::compute_symmetry_axis(const std::vector<MeshCuboid *>& _cuboids)
{
	std::vector< std::pair<unsigned int, unsigned int> > pair_cuboid_indices;
	get_pair_cuboid_indices(_cuboids, pair_cuboid_indices);

	if (pair_cuboid_indices.empty())
	{
		std::vector<unsigned int> single_cuboid_indices;
		get_single_cuboid_indices(_cuboids, single_cuboid_indices);
		if (single_cuboid_indices.empty())
			return false;

		const unsigned int num_cuboids = _cuboids.size();

		std::list<MyMesh::Normal> aligned_normals;
		std::list<MyMesh::Point> aligned_centers;

		for (std::vector<unsigned int>::const_iterator it = single_cuboid_indices.begin();
			it != single_cuboid_indices.end(); ++it)
		{
			int cuboid_index = (*it);
			assert(cuboid_index < num_cuboids);
			const MeshCuboid *cuboid = _cuboids[cuboid_index];

			aligned_normals.push_back(cuboid->get_bbox_axis(get_aligned_global_axis_index()));
			aligned_centers.push_back(cuboid->get_bbox_center());
		}

		assert(!aligned_normals.empty());
		assert(!aligned_centers.empty());

		Eigen::Matrix3d A = Eigen::Matrix3d::Zero();

		for (std::list<MyMesh::Normal>::const_iterator it = aligned_normals.begin();
			it != aligned_normals.end(); ++it)
		{
			Eigen::Vector3d n_vec;
			for (int i = 0; i < 3; ++i) n_vec[i] = (*it)[i];
			A += (n_vec * n_vec.transpose());
		}

		Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es(A);
		Eigen::Vector3d n = es.eigenvectors().col(3 - 1);
		for (unsigned int i = 0; i < 3; ++i) n_[i] = n[i];
		n_.normalize();

		t_ = 0;
		for (std::list<MyMesh::Point>::const_iterator it = aligned_centers.begin();
			it != aligned_centers.end(); ++it)
			t_ += dot(n_, (*it));
		t_ /= aligned_centers.size();
	}
	else
	{
		const unsigned int num_cuboids = _cuboids.size();

		std::list< std::pair < MyMesh::Point, MyMesh::Point > > point_pairs;
		for (std::vector < std::pair <unsigned int, unsigned int> >::const_iterator it = pair_cuboid_indices.begin();
			it != pair_cuboid_indices.end(); ++it)
		{
			int cuboid_index_1 = (*it).first;
			int cuboid_index_2 = (*it).second;
			assert(cuboid_index_1 < num_cuboids);
			assert(cuboid_index_2 < num_cuboids);

			const MeshCuboid *cuboid_1 = _cuboids[cuboid_index_1];
			const MeshCuboid *cuboid_2 = _cuboids[cuboid_index_2];

			add_symmety_cuboid_corner_points(cuboid_1, cuboid_2, _cuboids,
				info_.aligned_global_axis_index_, point_pairs);
		}

		assert(!point_pairs.empty());
		compute_reflection_plane(point_pairs, n_, t_);
	}

	return true;
}

void MeshCuboidReflectionSymmetryGroup::add_symmety_cuboid_corner_points(
	const MeshCuboid *cuboid_1, const MeshCuboid *cuboid_2,
	const std::vector<MeshCuboid *>& _cuboids,
	const unsigned int _reflection_axis_index,
	std::list< std::pair < MyMesh::Point, MyMesh::Point > > &_point_pairs)
{
	assert(cuboid_1);
	assert(cuboid_2);

	const unsigned int dimension = 3;
	assert(_reflection_axis_index < dimension);

	// NOTE:
	// Implemented only for 3 dimension.
	assert(dimension == 3);

	for (unsigned int corner_index_1 = 0; corner_index_1 < MeshCuboid::k_num_corners; ++corner_index_1)
	{
		std::bitset<3> bits(corner_index_1);
		bits[_reflection_axis_index].flip();
		unsigned int corner_index_2 = bits.to_ulong();

		MyMesh::Point corner_point_1 = cuboid_1->get_bbox_corner(corner_index_1);
		MyMesh::Point corner_point_2 = cuboid_2->get_bbox_corner(corner_index_2);

		_point_pairs.push_back(std::make_pair(corner_point_1, corner_point_2));
	}
}

void MeshCuboidReflectionSymmetryGroup::compute_reflection_plane(
	const std::list< std::pair < MyMesh::Point, MyMesh::Point > > &_point_pairs,
	MyMesh::Normal &_n, double &_t)
{
	// Eq (1):
	// min {(I - nn^T)(x - y)}^2. Let d = (x - y).
	// Since (I - nn^T)^2 = (I - nn^T),
	// => min d^T(I - nn^T)d = d^Td - d^Tnn^Td = d^Td - n^Tdd^Tn.
	// => max n^Tdd^Tn.

	// Eq (2):
	// min {n^T(x + y) - 2t}^2.
	// If n is estimated, t = 0.5*n^T(x + y).

	assert(!_point_pairs.empty());

	Eigen::Matrix3d A = Eigen::Matrix3d::Zero();
	Eigen::Vector3d b = Eigen::Vector3d::Zero();

	for (std::list< std::pair < MyMesh::Point, MyMesh::Point > >::const_iterator it = _point_pairs.begin();
		it != _point_pairs.end(); ++it)
	{
		Eigen::Vector3d sum_p, diff_p;
		for (int i = 0; i < 3; ++i)
		{
			sum_p[i] = (*it).first[i] + (*it).second[i];
			diff_p[i] = (*it).first[i] - (*it).second[i];
		}

		A += (diff_p * diff_p.transpose());
		b += (0.5 * sum_p);
	}

	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es(A);
	Eigen::Vector3d n = es.eigenvectors().col(3 - 1);
	for (unsigned int i = 0; i < 3; ++i) _n[i] = n[i];

	_n.normalize();
	_t = (n.transpose() * b);
	_t /= _point_pairs.size();
}

void MeshCuboidReflectionSymmetryGroup::get_reflection_plane_corners(
	const MyMesh::Point &_point, const Real _size,
	std::array<MyMesh::Point, 4>& _corners) const
{
	// Find the closest point on the plane to the given '_point'.
	// p_proj = (p - a) - (n'(p-a))n + a  ('a' is a point on the plane, n'a = t)
	// p_proj = p - (n'p)n + tn

	MyMesh::Point center = _point - dot(n_, _point) * n_ + t_ * n_;

	// Find random two axes of plane.
	MyMesh::Normal axes[2];

	axes[0] = MyMesh::Normal(0.0);
	for (unsigned int i = 0; i < 3; ++i)
	{
		MyMesh::Normal temp(0.0);
		temp[i] = 1.0;
		axes[0] = cross(n_, temp);
		if (axes[0].norm() > 0.1)
		{
			axes[0].normalize();
			break;
		}
	}
	CHECK_NUMERICAL_ERROR(__FUNCTION__, axes[0].norm(), 1.0);
	axes[1] = cross(n_, axes[0]);
	axes[1].normalized();

	_corners[0] = center - 0.5 * _size * axes[0] - 0.5 * _size * axes[1];
	_corners[1] = center + 0.5 * _size * axes[0] - 0.5 * _size * axes[1];
	_corners[2] = center + 0.5 * _size * axes[0] + 0.5 * _size * axes[1];
	_corners[3] = center - 0.5 * _size * axes[0] + 0.5 * _size * axes[1];
}

MyMesh::Point MeshCuboidReflectionSymmetryGroup::get_symmetric_point(
	const MyMesh::Point& _point, unsigned int _symmetry_order) const
{
	// NOTE:
	// Symmetry order of 1 means reflection.
	assert(_symmetry_order == 1);
	return get_symmetric_point(_point);
}

MyMesh::Normal MeshCuboidReflectionSymmetryGroup::get_symmetric_normal(
	const MyMesh::Normal& _normal, unsigned int _symmetry_order) const
{
	// NOTE:
	// Symmetry order of 1 means reflection.
	assert(_symmetry_order == 1);
	return get_symmetric_normal(_normal);
}

MyMesh::Point MeshCuboidReflectionSymmetryGroup::get_symmetric_point(const MyMesh::Point& _point) const
{
	// Assume that 'n_' is normalized.
	MyMesh::Normal plane_to_point = n_ * (dot(n_, _point) - t_);
	MyMesh::Point symmetric_point = _point - (plane_to_point * 2);
	CHECK_NUMERICAL_ERROR(__FUNCTION__, t_ - dot(n_, 0.5 * (_point + symmetric_point)));
	return symmetric_point;
}

MyMesh::Normal MeshCuboidReflectionSymmetryGroup::get_symmetric_normal(const MyMesh::Normal& _normal) const
{
	// Assume that 'n_' and 'normal_' are normalized.
	MyMesh::Normal n_direction_component = n_ * (dot(n_, _normal));
	MyMesh::Normal symmetric_normal = _normal - (n_direction_component * 2);
	CHECK_NUMERICAL_ERROR(__FUNCTION__, dot(n_, 0.5 * (_normal + symmetric_normal)));
	return symmetric_normal;
}

MeshCuboidRotationSymmetryGroup* MeshCuboidRotationSymmetryGroup::constructor(
	const MeshCuboidSymmetryGroupInfo &_info,
	const std::vector<MeshCuboid *>& _cuboids)
{
	MeshCuboidRotationSymmetryGroup *group = new MeshCuboidRotationSymmetryGroup(_info);
	bool ret = (group->compute_symmetry_axis(_cuboids) && group->compute_rotation_angle(_cuboids));
	if (!ret)
	{
		delete group;
		group = NULL;
	}
	return group;
}

MeshCuboidRotationSymmetryGroup::MeshCuboidRotationSymmetryGroup(
	const MeshCuboidSymmetryGroupInfo &_info)
	: MeshCuboidSymmetryGroup(_info)
	, n_(MyMesh::Normal(1, 0, 0))
	, t_(MyMesh::Point(0, 0, 0))
{
	assert(_info.symmetry_type_ == RotationSymmetryType);
}

MeshCuboidRotationSymmetryGroup::MeshCuboidRotationSymmetryGroup(
	const MeshCuboidRotationSymmetryGroup &_other)
	: MeshCuboidSymmetryGroup(_other)
	, n_(_other.n_)
	, t_(_other.t_)
{
	n_.normalize();
	assert(info_.symmetry_type_ == RotationSymmetryType);
}

MeshCuboidRotationSymmetryGroup::~MeshCuboidRotationSymmetryGroup()
{

}

unsigned int MeshCuboidRotationSymmetryGroup::num_axis_parameters()
{
	return (3 + 3);
}

MeshCuboidSymmetryGroupType MeshCuboidRotationSymmetryGroup::get_symmetry_type() const
{
	assert(info_.symmetry_type_ == RotationSymmetryType);
	return info_.symmetry_type_;
}

bool MeshCuboidRotationSymmetryGroup::compute_symmetry_axis(const std::vector<MeshCuboid *>& _cuboids)
{
	std::vector<unsigned int> single_cuboid_indices;
	get_single_cuboid_indices(_cuboids, single_cuboid_indices);
	if (single_cuboid_indices.empty())
		return false;

	const unsigned int num_cuboids = _cuboids.size();

	std::list<MyMesh::Normal> aligned_normals;
	std::list<MyMesh::Point> aligned_centers;

	for (std::vector<unsigned int>::const_iterator it = single_cuboid_indices.begin();
		it != single_cuboid_indices.end(); ++it)
	{
		int cuboid_index = (*it);
		assert(cuboid_index < num_cuboids);
		const MeshCuboid *cuboid = _cuboids[cuboid_index];

		aligned_normals.push_back(cuboid->get_bbox_axis(get_aligned_global_axis_index()));
		aligned_centers.push_back(cuboid->get_bbox_center());
	}

	assert(!aligned_normals.empty());
	assert(!aligned_centers.empty());

	Eigen::Matrix3d A = Eigen::Matrix3d::Zero();

	for (std::list<MyMesh::Normal>::const_iterator it = aligned_normals.begin();
		it != aligned_normals.end(); ++it)
	{
		Eigen::Vector3d n_vec;
		for (int i = 0; i < 3; ++i) n_vec[i] = (*it)[i];
		A += (n_vec * n_vec.transpose());
	}

	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es(A);
	Eigen::Vector3d n = es.eigenvectors().col(3 - 1);
	for (unsigned int i = 0; i < 3; ++i) n_[i] = n[i];
	n_.normalize();

	t_ = MyMesh::Point(0.0);
	for (std::list<MyMesh::Point>::const_iterator it = aligned_centers.begin();
		it != aligned_centers.end(); ++it)
		t_ += (*it);
	t_ /= aligned_centers.size();

	return true;
}

MyMesh::Point MeshCuboidRotationSymmetryGroup::get_symmetric_point(
	const MyMesh::Point& _point, unsigned int _symmetry_order) const
{
	assert(_symmetry_order > 0);
	assert(_symmetry_order < num_symmetry_orders_);

	Eigen::Vector3d axis_vec, point_vec;
	for (unsigned int i = 0; i < 3; ++i)
	{
		axis_vec[i] = n_[i];
		point_vec[i] = _point[i] - t_[i];
	}

	double angle = get_rotation_angle() * _symmetry_order;

	Eigen::AngleAxisd axis_rotation(angle, axis_vec);
	Eigen::Vector3d rot_point_vec = axis_rotation.toRotationMatrix() * point_vec;

	MyMesh::Point rot_point;
	for (unsigned int i = 0; i < 3; ++i)
		rot_point[i] = rot_point_vec[i] + t_[i];

	return rot_point;
}

MyMesh::Normal MeshCuboidRotationSymmetryGroup::get_symmetric_normal(
	const MyMesh::Normal& _normal, unsigned int _symmetry_order) const
{
	assert(_symmetry_order > 0);
	assert(_symmetry_order < num_symmetry_orders_);

	Eigen::Vector3d axis_vec, normal_vec;
	for (unsigned int i = 0; i < 3; ++i)
	{
		axis_vec[i] = n_[i];
		normal_vec[i] = _normal[i];
	}

	double angle = get_rotation_angle() * (_symmetry_order + 1);

	Eigen::AngleAxisd axis_rotation(angle, axis_vec);
	Eigen::Vector3d rot_normal_vec = axis_rotation.toRotationMatrix() * normal_vec;

	MyMesh::Point ret;
	for (unsigned int i = 0; i < 3; ++i)
	{
		ret[i] = rot_normal_vec[i];
	}

	return ret;
}

void MeshCuboidRotationSymmetryGroup::get_rotation_axis(MyMesh::Normal &_n, MyMesh::Point &_t) const
{
	_n = n_;
	_t = t_;
}

void MeshCuboidRotationSymmetryGroup::set_rotation_axis(const MyMesh::Normal &_n, const MyMesh::Point &_t)
{
	n_ = _n;
	t_ = _t;
	n_.normalize();
}

void MeshCuboidRotationSymmetryGroup::get_rotation_axis_corners(
	const MyMesh::Point &_point, const Real _size, std::array<MyMesh::Point, 2>& _corners) const
{
	// Find the closest point on the axis to the given '_point'.
	// min || n*x + t - p ||.
	// x = -(n^T (t - p)).

	double x = -dot(n_, t_ - _point);
	MyMesh::Point center = n_ * x + t_;

	_corners[0] = center - n_ * 0.5 * _size;
	_corners[1] = center + n_ * 0.5 * _size;
}

void MeshCuboidRotationSymmetryGroup::add_symmety_cuboid_corner_points(
	const MeshCuboid *cuboid_1, const MeshCuboid *cuboid_2,
	const std::vector<MeshCuboid *>& _cuboids,
	const unsigned int _reflection_axis_index,
	std::list< std::pair < MyMesh::Point, MyMesh::Point > > &_point_pairs)
{

}

void MeshCuboidRotationSymmetryGroup::compute_rotation_plane(
	const std::list< std::pair < MyMesh::Point, MyMesh::Point > > &_point_pairs,
	MyMesh::Normal &_n, double &_t)
{

}

bool MeshCuboidRotationSymmetryGroup::compute_rotation_angle(
	const std::vector<MeshCuboid *> &_cuboids)
{
	const Real squared_neighbor_distance = FLAGS_param_sparse_neighbor_distance
		* FLAGS_param_sparse_neighbor_distance;
	const unsigned int num_cuboids = _cuboids.size();

	//
	std::vector<ANNpointArray> cuboid_ann_points(num_cuboids);
	std::vector<ANNkd_tree *> cuboid_ann_kd_tree(num_cuboids);

	for (unsigned int cuboid_index = 0; cuboid_index < num_cuboids; ++cuboid_index)
	{
		cuboid_ann_kd_tree[cuboid_index] = NULL;
		cuboid_ann_points[cuboid_index] = NULL;

		MeshCuboid *cuboid = _cuboids[cuboid_index];
		unsigned int num_cuboid_sample_points = cuboid->num_sample_points();
		if (num_cuboid_sample_points == 0)
			continue;

		Eigen::MatrixXd cuboid_sample_points(3, num_cuboid_sample_points);

		for (unsigned int point_index = 0; point_index < num_cuboid_sample_points; ++point_index)
		{
			for (unsigned int i = 0; i < 3; ++i)
				cuboid_sample_points.col(point_index)(i) =
				cuboid->get_sample_point(point_index)->point_[i];
		}

		cuboid_ann_kd_tree[cuboid_index] = ICP::create_kd_tree(cuboid_sample_points,
			cuboid_ann_points[cuboid_index]);
		assert(cuboid_ann_points[cuboid_index]);
		assert(cuboid_ann_kd_tree[cuboid_index]);

		assert(cuboid_ann_points.size() == num_cuboids);
		assert(cuboid_ann_kd_tree.size() == num_cuboids);
	}
	//

	std::vector<unsigned int> single_cuboid_indices;
	get_single_cuboid_indices(_cuboids, single_cuboid_indices);
	if (single_cuboid_indices.empty())
		return false;

	// FIXME.
	const unsigned int min_num_symmetry_orders = 3;
	const unsigned int max_num_symmetry_orders = 6;
	const unsigned int default_num_symmetry_orders = 5;

	Real max_num_point_pairs = 0.0;
	unsigned int best_num_symmetry_orders = default_num_symmetry_orders;

	for (unsigned int num_symmetry_orders = min_num_symmetry_orders; num_symmetry_orders <= max_num_symmetry_orders;
		++num_symmetry_orders)
	{
		assert(num_symmetry_orders > 2);

		// Temporary set symmetry order.
		num_symmetry_orders_ = num_symmetry_orders;
		std::list<WeightedPointPair> sample_point_pairs;
		get_symmetric_sample_point_pairs(_cuboids, cuboid_ann_points, cuboid_ann_kd_tree,
			squared_neighbor_distance, sample_point_pairs);

		Real num_point_pairs = sample_point_pairs.size();

		// Divide by the number of rotations.
		num_point_pairs /= (num_symmetry_orders - 1);

		std::cout << "[" << num_symmetry_orders << "]: " << num_point_pairs << std::endl;

		if (num_point_pairs > max_num_point_pairs)
		{
			max_num_point_pairs = num_point_pairs;
			best_num_symmetry_orders = num_symmetry_orders;
		}
	}

	//
	for (unsigned int cuboid_index = 0; cuboid_index < num_cuboids; ++cuboid_index)
	{
		if (cuboid_ann_points[cuboid_index]) annDeallocPts(cuboid_ann_points[cuboid_index]);
		if (cuboid_ann_kd_tree[cuboid_index]) delete cuboid_ann_kd_tree[cuboid_index];
	}
	cuboid_ann_points.clear();
	cuboid_ann_kd_tree.clear();
	//

	num_symmetry_orders_ = best_num_symmetry_orders;
	std::cout << "num_symmetry_orders = " << num_symmetry_orders_ << std::endl;

	return true;
}


// Copyright (C) 2004-2006 The Trustees of Indiana University.

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_PARALLEL_DIJKSTRA_HPP
#define BOOST_GRAPH_PARALLEL_DIJKSTRA_HPP

#ifndef BOOST_GRAPH_USE_MPI
#error "Parallel BGL files should not be included unless <boost/graph/use_mpi.hpp> has been included"
#endif

#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/overloading.hpp>
#include <boost/graph/distributed/concepts.hpp>
#include <boost/graph/parallel/properties.hpp>
#include <boost/graph/distributed/crauser_et_al_shortest_paths.hpp>
#include <boost/graph/distributed/eager_dijkstra_shortest_paths.hpp>

namespace boost {

  namespace graph { namespace detail {

    template<typename Lookahead>
    struct parallel_dijkstra_impl2
    {
      template<typename DistributedGraph, typename DijkstraVisitor,
               typename PredecessorMap, typename DistanceMap,
               typename WeightMap, typename IndexMap, typename ColorMap,
               typename Compare, typename Combine, typename DistInf,
               typename DistZero>
      static void
      run(const DistributedGraph& g,
          typename graph_traits<DistributedGraph>::vertex_descriptor s,
          PredecessorMap predecessor, DistanceMap distance,
          typename property_traits<DistanceMap>::value_type lookahead,
          WeightMap weight, IndexMap index_map, ColorMap color_map,
          Compare compare, Combine combine, DistInf inf, DistZero zero,
          DijkstraVisitor vis)
      {
        eager_dijkstra_shortest_paths(g, s, predecessor, distance, lookahead,
                                      weight, index_map, color_map, compare,
                                      combine, inf, zero, vis);
      }
    };

    template<>
    struct parallel_dijkstra_impl2< ::boost::param_not_found >
    {
      template<typename DistributedGraph, typename DijkstraVisitor,
               typename PredecessorMap, typename DistanceMap,
               typename WeightMap, typename IndexMap, typename ColorMap,
               typename Compare, typename Combine, typename DistInf,
               typename DistZero>
      static void
      run(const DistributedGraph& g,
          typename graph_traits<DistributedGraph>::vertex_descriptor s,
          PredecessorMap predecessor, DistanceMap distance,
          ::boost::param_not_found,
          WeightMap weight, IndexMap index_map, ColorMap color_map,
          Compare compare, Combine combine, DistInf inf, DistZero zero,
          DijkstraVisitor vis)
      {
        crauser_et_al_shortest_paths(g, s, predecessor, distance, weight,
                                     index_map, color_map, compare, combine,
                                     inf, zero, vis);
      }
    };

    template<typename ColorMap>
    struct parallel_dijkstra_impl
    {
      template<typename DistributedGraph, typename DijkstraVisitor,
               typename PredecessorMap, typename DistanceMap,
               typename Lookahead, typename WeightMap, typename IndexMap,
               typename Compare, typename Combine,
               typename DistInf, typename DistZero>
      static void
      run(const DistributedGraph& g,
          typename graph_traits<DistributedGraph>::vertex_descriptor s,
          PredecessorMap predecessor, DistanceMap distance,
          Lookahead lookahead,
          WeightMap weight, IndexMap index_map, ColorMap color_map,
          Compare compare, Combine combine, DistInf inf, DistZero zero,
          DijkstraVisitor vis)
      {
        graph::detail::parallel_dijkstra_impl2<Lookahead>
          ::run(g, s, predecessor, distance, lookahead, weight, index_map,
                color_map, compare, combine, inf, zero, vis);
      }
    };

    template<>
    struct parallel_dijkstra_impl< ::boost::param_not_found >
    {
    private:
      template<typename DistributedGraph, typename DijkstraVisitor,
               typename PredecessorMap, typename DistanceMap,
               typename Lookahead, typename WeightMap, typename IndexMap,
               typename ColorMap, typename Compare, typename Combine,
               typename DistInf, typename DistZero>
      static void
      run_impl(const DistributedGraph& g,
               typename graph_traits<DistributedGraph>::vertex_descriptor s,
               PredecessorMap predecessor, DistanceMap distance,
               Lookahead lookahead, WeightMap weight, IndexMap index_map,
               ColorMap color_map, Compare compare, Combine combine,
               DistInf inf, DistZero zero, DijkstraVisitor vis)
      {
        BGL_FORALL_VERTICES_T(u, g, DistributedGraph)
          BGL_FORALL_OUTEDGES_T(u, e, g, DistributedGraph)
            local_put(color_map, target(e, g), white_color);

        graph::detail::parallel_dijkstra_impl2<Lookahead>
          ::run(g, s, predecessor, distance, lookahead, weight, index_map,
                color_map, compare, combine, inf, zero, vis);
      }

    public:
      template<typename DistributedGraph, typename DijkstraVisitor,
               typename PredecessorMap, typename DistanceMap,
               typename Lookahead, typename WeightMap, typename IndexMap,
               typename Compare, typename Combine,
               typename DistInf, typename DistZero>
      static void
      run(const DistributedGraph& g,
          typename graph_traits<DistributedGraph>::vertex_descriptor s,
          PredecessorMap predecessor, DistanceMap distance,
          Lookahead lookahead, WeightMap weight, IndexMap index_map,
          ::boost::param_not_found,
          Compare compare, Combine combine, DistInf inf, DistZero zero,
          DijkstraVisitor vis)
      {
        typedef typename graph_traits<DistributedGraph>::vertices_size_type
          vertices_size_type;

        vertices_size_type n = num_vertices(g);
        std::vector<default_color_type> colors(n, white_color);

        run_impl(g, s, predecessor, distance, lookahead, weight, index_map,
                 make_iterator_property_map(colors.begin(), index_map),
                 compare, combine, inf, zero, vis);
      }
    };
  } } // end namespace graph::detail

  /** Dijkstra's single-source shortest paths algorithm for distributed
   * graphs.
   *
   * Also implements the heuristics of:
   *
   *   Andreas Crauser, Kurt Mehlhorn, Ulrich Meyer, and Peter
   *   Sanders. A Parallelization of Dijkstra's Shortest Path
   *   Algorithm. In Lubos Brim, Jozef Gruska, and Jiri Zlatuska,
   *   editors, Mathematical Foundations of Computer Science (MFCS),
   *   volume 1450 of Lecture Notes in Computer Science, pages
   *   722--731, 1998. Springer.
   */
  template<typename DistributedGraph, typename DijkstraVisitor,
           typename PredecessorMap, typename DistanceMap,
           typename WeightMap, typename IndexMap, typename Compare,
           typename Combine, typename DistInf, typename DistZero,
           typename T, typename Tag, typename Base>
  inline
  void
  dijkstra_shortest_paths
    (const DistributedGraph& g,
     typename graph_traits<DistributedGraph>::vertex_descriptor s,
     PredecessorMap predecessor, DistanceMap distance, WeightMap weight,
     IndexMap index_map,
     Compare compare, Combine combine, DistInf inf, DistZero zero,
     DijkstraVisitor vis,
     const bgl_named_params<T, Tag, Base>& params
     BOOST_GRAPH_ENABLE_IF_MODELS_PARM(DistributedGraph,distributed_graph_tag))
  {
    typedef typename graph_traits<DistributedGraph>::vertices_size_type
      vertices_size_type;

    // Build a distributed property map for vertex colors, if we need it
    bool use_default_color_map
      = is_default_param(get_param(params, vertex_color));
    vertices_size_type n = use_default_color_map? num_vertices(g) : 1;
    std::vector<default_color_type> color(n, white_color);
    typedef iterator_property_map<std::vector<default_color_type>::iterator,
                                  IndexMap> DefColorMap;
    DefColorMap color_map(color.begin(), index_map);

    typedef typename get_param_type< vertex_color_t, bgl_named_params<T, Tag, Base> >::type color_map_type;

    graph::detail::parallel_dijkstra_impl<color_map_type>
      ::run(g, s, predecessor, distance,
            get_param(params, lookahead_t()),
            weight, index_map,
            get_param(params, vertex_color),
            compare, combine, inf, zero, vis);
  }
} // end namespace boost

#endif // BOOST_GRAPH_PARALLEL_DIJKSTRA_HPP