/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2012-, Open Perception, 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.
 *
 */

#ifndef PCL_SEGMENTATION_IMPL_RANDOM_WALKER_HPP
#define PCL_SEGMENTATION_IMPL_RANDOM_WALKER_HPP

#include <boost/bimap.hpp>

#include <Eigen/Sparse>

namespace pcl
{

  namespace segmentation
  {

    namespace detail
    {

      /** \brief Multilabel graph segmentation using random walks.
        *
        * This is an implementation of the algorithm described in "Random Walks
        * for Image Segmentation" by Leo Grady.
        *
        * See the documentation of the randomWalker() function for details.
        *
        * \author Sergey Alexandrov
        * \ingroup segmentation
        */
      template <class Graph, class EdgeWeightMap, class VertexColorMap>
      class RandomWalker
      {

        public:

          using Color = typename boost::property_traits<VertexColorMap>::value_type;
          using Weight = typename boost::property_traits<EdgeWeightMap>::value_type;
          using GraphTraits = boost::graph_traits<Graph>;
          using EdgeDescriptor = typename GraphTraits::edge_descriptor;
          using VertexDescriptor = typename GraphTraits::vertex_descriptor;
          using EdgeIterator = typename GraphTraits::edge_iterator;
          using OutEdgeIterator = typename GraphTraits::out_edge_iterator;
          using VertexIterator = typename GraphTraits::vertex_iterator;
          using VertexIndexMap = typename boost::property_map<Graph, boost::vertex_index_t>::type;
          using VertexDegreeMap = boost::iterator_property_map<typename std::vector<Weight>::iterator, VertexIndexMap>;
          using SparseMatrix = Eigen::SparseMatrix<Weight>;
          using Matrix = Eigen::Matrix<Weight, Eigen::Dynamic, Eigen::Dynamic>;
          using Vector = Eigen::Matrix<Weight, Eigen::Dynamic, 1>;

          RandomWalker (Graph& g, EdgeWeightMap weights, VertexColorMap colors)
          : g_ (g)
          , weight_map_ (weights)
          , color_map_ (colors)
          , index_map_ (boost::get (boost::vertex_index, g_))
          , degree_storage_ (boost::num_vertices (g_), 0)
          , degree_map_ (boost::make_iterator_property_map (degree_storage_.begin (), index_map_))
          {
          }

          bool
          segment ()
          {
            computeVertexDegrees ();
            buildLinearSystem ();
            return solveLinearSystem ();
          }

          void
          computeVertexDegrees ()
          {
            using namespace boost;
            EdgeIterator ei, e_end;
            for (tie (ei, e_end) = edges (g_); ei != e_end; ++ei)
            {
              Weight w = weight_map_[*ei];
              degree_map_[source (*ei, g_)] += w;
              degree_map_[target (*ei, g_)] += w;
            }
          }

          void
          buildLinearSystem ()
          {
            using namespace boost;

            using T = Eigen::Triplet<float>;
            using Triplets = std::vector<T>;
            Triplets L_triplets;
            Triplets B_triplets;

            VertexIterator vi, v_end;
            for (tie (vi, v_end) = vertices (g_); vi != v_end; ++vi)
            {
              // If this is a labeled vertex add it to the seeds list and register its color
              if (color_map_[*vi])
              {
                seeds_.push_back (*vi);
                colors_.insert (color_map_[*vi]);
              }
              // Skip seeds and vertices with zero connectivity
              if (color_map_[*vi] || std::fabs (degree_map_[*vi]) < std::numeric_limits<Weight>::epsilon ())
                continue;
              // Create a row in L matrix for the vertex
              std::size_t current_row = insertInBimap (L_vertex_bimap, *vi);
              // Add diagonal degree entry for the vertex
              L_triplets.push_back (T (current_row, current_row, degree_map_[*vi]));
              // Iterate over incident vertices and add entries on corresponding columns of L or B
              OutEdgeIterator ei, e_end;
              for (tie (ei, e_end) = out_edges (*vi, g_); ei != e_end; ++ei)
              {
                Weight w = weight_map_[*ei];
                VertexDescriptor tgt = target (*ei, g_);
                Color color = color_map_[tgt];
                if (color)
                {
                  // This is a seed and will go to B matrix
                  std::size_t column;
                  if (B_color_bimap.right.count (color) == 0)
                  {
                    // This is the first time we encountered this color, create a new column in B
                    column = insertInBimap (B_color_bimap, color);
                  }
                  else
                  {
                    column = B_color_bimap.right.at (color);
                  }
                  B_triplets.push_back (T (current_row, column, w));
                }
                else
                {
                  // This is a non-seed and will go to L matrix,
                  // but only if a row for this vertex already exists
                  if (L_vertex_bimap.right.count (tgt) && L_vertex_bimap.right.at (tgt) != current_row)
                  {
                    L_triplets.push_back (T (current_row, L_vertex_bimap.right.at (tgt), -w));
                  }
                }
              }
            }

            std::size_t num_equations = L_vertex_bimap.size ();
            std::size_t num_colors = B_color_bimap.size ();
            L.resize (num_equations, num_equations);
            B.resize (num_equations, num_colors);
            if (!L_triplets.empty ())
              L.setFromTriplets(L_triplets.begin(), L_triplets.end());
            if (!B_triplets.empty ())
              B.setFromTriplets(B_triplets.begin(), B_triplets.end());
          }

          bool solveLinearSystem()
          {
            X.resize (L.rows (), B.cols ());

            // Nothing to solve
            if (L.rows () == 0 || B.cols () == 0)
              return true;

            Eigen::SimplicialCholesky<SparseMatrix, Eigen::Lower> cg;
            cg.compute (L);
            bool succeeded = true;
            for (Eigen::Index i = 0; i < B.cols (); ++i)
            {
              Vector b = B.col (i);
              X.col (i) = cg.solve (b);
              if (cg.info () != Eigen::Success)
                succeeded = false;
            }

            assignColors ();
            return succeeded;
          }

          void
          assignColors ()
          {
            using namespace boost;
            if (X.cols ())
              for (Eigen::Index i = 0; i < X.rows (); ++i)
              {
                std::size_t max_column;
                X.row (i).maxCoeff (&max_column);
                VertexDescriptor vertex = L_vertex_bimap.left.at (i);
                Color color = B_color_bimap.left.at (max_column);
                color_map_[vertex] = color;
              }
          }

          void
          getPotentials (Matrix& potentials, std::map<Color, std::size_t>& color_to_column_map)
          {
            using namespace boost;
            potentials = Matrix::Zero (num_vertices (g_), colors_.size ());
            // Copy over rows from X
            for (Eigen::Index i = 0; i < X.rows (); ++i)
              potentials.row (L_vertex_bimap.left.at (i)).head (X.cols ()) = X.row (i);
            // In rows that correspond to seeds put ones in proper columns
            for (std::size_t i = 0; i < seeds_.size (); ++i)
            {
              VertexDescriptor v = seeds_[i];
              insertInBimap (B_color_bimap, color_map_[v]);
              potentials (seeds_[i], B_color_bimap.right.at (color_map_[seeds_[i]])) = 1;
            }
            // Fill in a map that associates colors with columns in potentials matrix
            color_to_column_map.clear ();
            for (Eigen::Index i = 0; i < potentials.cols (); ++i)
              color_to_column_map[B_color_bimap.left.at (i)] = i;
          }

          template <typename T> static inline std::size_t
          insertInBimap (boost::bimap<std::size_t, T>& bimap, T value)
          {
            if (bimap.right.count (value) != 0)
            {
              return bimap.right.at (value);
            }
            std::size_t s = bimap.size ();
            bimap.insert (typename boost::bimap<std::size_t, T>::value_type (s, value));
            return s;
          }

          Graph& g_;
          EdgeWeightMap weight_map_;
          VertexColorMap color_map_;
          VertexIndexMap index_map_;

          std::vector<VertexDescriptor> seeds_;
          std::set<Color> colors_;

          std::vector<Weight> degree_storage_;
          VertexDegreeMap degree_map_;
          SparseMatrix L;
          SparseMatrix B;
          Matrix X;

          // Map vertex identifiers to the rows/columns of L and vice versa
          boost::bimap<std::size_t, VertexDescriptor> L_vertex_bimap;
          // Map colors to the columns of B and vice versa
          boost::bimap<std::size_t, Color> B_color_bimap;

      };

    }

    template <class Graph> bool
    randomWalker (Graph& graph)
    {
      return randomWalker (graph,
                           boost::get (boost::edge_weight, graph),
                           boost::get (boost::vertex_color, graph));
    }

    template <class Graph, class EdgeWeightMap, class VertexColorMap> bool
    randomWalker (Graph& graph,
                  EdgeWeightMap weights,
                  VertexColorMap colors)
    {
      using namespace boost;

      using EdgeDescriptor = typename graph_traits<Graph>::edge_descriptor;
      using VertexDescriptor = typename graph_traits<Graph>::vertex_descriptor;

      BOOST_CONCEPT_ASSERT ((VertexListGraphConcept<Graph>));                                 // to have vertices(), num_vertices()
      BOOST_CONCEPT_ASSERT ((EdgeListGraphConcept<Graph>));                                   // to have edges()
      BOOST_CONCEPT_ASSERT ((IncidenceGraphConcept<Graph>));                                  // to have source(), target() and out_edges()
      BOOST_CONCEPT_ASSERT ((ReadablePropertyMapConcept<EdgeWeightMap, EdgeDescriptor>));     // read weight-values from edges
      BOOST_CONCEPT_ASSERT ((ReadWritePropertyMapConcept<VertexColorMap, VertexDescriptor>)); // read and write color-values from vertices

      ::pcl::segmentation::detail::RandomWalker
      <
        Graph,
        EdgeWeightMap,
        VertexColorMap
      >
      rw (graph, weights, colors);
      return rw.segment ();
    }

    template <class Graph, class EdgeWeightMap, class VertexColorMap> bool
    randomWalker (Graph& graph,
                  EdgeWeightMap weights,
                  VertexColorMap colors,
                  Eigen::Matrix<typename boost::property_traits<EdgeWeightMap>::value_type, Eigen::Dynamic, Eigen::Dynamic>& potentials,
                  std::map<typename boost::property_traits<VertexColorMap>::value_type, std::size_t>& colors_to_columns_map)
    {
      using namespace boost;

      using EdgeDescriptor = typename graph_traits<Graph>::edge_descriptor;
      using VertexDescriptor = typename graph_traits<Graph>::vertex_descriptor;

      BOOST_CONCEPT_ASSERT ((VertexListGraphConcept<Graph>));                                 // to have vertices(), num_vertices()
      BOOST_CONCEPT_ASSERT ((EdgeListGraphConcept<Graph>));                                   // to have edges()
      BOOST_CONCEPT_ASSERT ((IncidenceGraphConcept<Graph>));                                  // to have source(), target() and out_edges()
      BOOST_CONCEPT_ASSERT ((ReadablePropertyMapConcept<EdgeWeightMap, EdgeDescriptor>));     // read weight-values from edges
      BOOST_CONCEPT_ASSERT ((ReadWritePropertyMapConcept<VertexColorMap, VertexDescriptor>)); // read and write color-values from vertices

      ::pcl::segmentation::detail::RandomWalker
      <
        Graph,
        EdgeWeightMap,
        VertexColorMap
      >
      rw (graph, weights, colors);
      bool result = rw.segment ();
      rw.getPotentials (potentials, colors_to_columns_map);
      return result;
    }

  }

}

#endif /* PCL_SEGMENTATION_IMPL_RANDOM_WALKER_HPP */