// Ceres Solver - A fast non-linear least squares minimizer

// Copyright 2015 Google Inc. All rights reserved.

// http://ceres-solver.org/

//

// 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 Google 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.

//

// Author: sameeragarwal@google.com (Sameer Agarwal)

//

// Various algorithms that operate on undirected graphs.

#ifndef CERES_INTERNAL_GRAPH_ALGORITHMS_H_

#define CERES_INTERNAL_GRAPH_ALGORITHMS_H_

#include <algorithm>

#include <vector>

#include <utility>

#include "ceres/collections_port.h"

#include "ceres/graph.h"

#include "ceres/wall_time.h"

#include "glog/logging.h"

namespace ceres {

namespace internal {

// Compare two vertices of a graph by their degrees, if the degrees

// are equal then order them by their ids.

template <typename Vertex>

class VertexTotalOrdering {

 public:

  explicit VertexTotalOrdering(const Graph<Vertex>& graph)

      : graph_(graph) {}

  bool operator()(const Vertex& lhs, const Vertex& rhs) const {

    if (graph_.Neighbors(lhs).size() == graph_.Neighbors(rhs).size()) {

      return lhs < rhs;

    }

    return graph_.Neighbors(lhs).size() < graph_.Neighbors(rhs).size();

  }

 private:

  const Graph<Vertex>& graph_;

};

template <typename Vertex>

class VertexDegreeLessThan {

 public:

  explicit VertexDegreeLessThan(const Graph<Vertex>& graph)

      : graph_(graph) {}

  bool operator()(const Vertex& lhs, const Vertex& rhs) const {

    return graph_.Neighbors(lhs).size() < graph_.Neighbors(rhs).size();

  }

 private:

  const Graph<Vertex>& graph_;

};

// Order the vertices of a graph using its (approximately) largest

// independent set, where an independent set of a graph is a set of

// vertices that have no edges connecting them. The maximum

// independent set problem is NP-Hard, but there are effective

// approximation algorithms available. The implementation here uses a

// breadth first search that explores the vertices in order of

// increasing degree. The same idea is used by Saad & Li in "MIQR: A

// multilevel incomplete QR preconditioner for large sparse

// least-squares problems", SIMAX, 2007.

//

// Given a undirected graph G(V,E), the algorithm is a greedy BFS

// search where the vertices are explored in increasing order of their

// degree. The output vector ordering contains elements of S in

// increasing order of their degree, followed by elements of V - S in

// increasing order of degree. The return value of the function is the

// cardinality of S.

template <typename Vertex>

int IndependentSetOrdering(const Graph<Vertex>& graph,

                           std::vector<Vertex>* ordering) {

  const HashSet<Vertex>& vertices = graph.vertices();

  const int num_vertices = vertices.size();

  CHECK_NOTNULL(ordering);

  ordering->clear();

  ordering->reserve(num_vertices);

  // Colors for labeling the graph during the BFS.

  const char kWhite = 0;

  const char kGrey = 1;

  const char kBlack = 2;

  // Mark all vertices white.

  HashMap<Vertex, char> vertex_color;

  std::vector<Vertex> vertex_queue;

  for (typename HashSet<Vertex>::const_iterator it = vertices.begin();

       it != vertices.end();

       ++it) {

    vertex_color[*it] = kWhite;

    vertex_queue.push_back(*it);

  }

  std::sort(vertex_queue.begin(), vertex_queue.end(),

            VertexTotalOrdering<Vertex>(graph));

  // Iterate over vertex_queue. Pick the first white vertex, add it

  // to the independent set. Mark it black and its neighbors grey.

  for (int i = 0; i < vertex_queue.size(); ++i) {

    const Vertex& vertex = vertex_queue[i];

    if (vertex_color[vertex] != kWhite) {

      continue;

    }

    ordering->push_back(vertex);

    vertex_color[vertex] = kBlack;

    const HashSet<Vertex>& neighbors = graph.Neighbors(vertex);

    for (typename HashSet<Vertex>::const_iterator it = neighbors.begin();

         it != neighbors.end();

         ++it) {

      vertex_color[*it] = kGrey;

    }

  }

  int independent_set_size = ordering->size();

  // Iterate over the vertices and add all the grey vertices to the

  // ordering. At this stage there should only be black or grey

  // vertices in the graph.

  for (typename std::vector<Vertex>::const_iterator it = vertex_queue.begin();

       it != vertex_queue.end();

       ++it) {

    const Vertex vertex = *it;

    DCHECK(vertex_color[vertex] != kWhite);

    if (vertex_color[vertex] != kBlack) {

      ordering->push_back(vertex);

    }

  }

  CHECK_EQ(ordering->size(), num_vertices);

  return independent_set_size;

}

// Same as above with one important difference. The ordering parameter

// is an input/output parameter which carries an initial ordering of

// the vertices of the graph. The greedy independent set algorithm

// starts by sorting the vertices in increasing order of their

// degree. The input ordering is used to stabilize this sort, i.e., if

// two vertices have the same degree then they are ordered in the same

// order in which they occur in "ordering".

//

// This is useful in eliminating non-determinism from the Schur

// ordering algorithm over all.

template <typename Vertex>

int StableIndependentSetOrdering(const Graph<Vertex>& graph,

                                 std::vector<Vertex>* ordering) {

  CHECK_NOTNULL(ordering);

  const HashSet<Vertex>& vertices = graph.vertices();

  const int num_vertices = vertices.size();

  CHECK_EQ(vertices.size(), ordering->size());

  // Colors for labeling the graph during the BFS.

  const char kWhite = 0;

  const char kGrey = 1;

  const char kBlack = 2;

  std::vector<Vertex> vertex_queue(*ordering);

  std::stable_sort(vertex_queue.begin(), vertex_queue.end(),

                  VertexDegreeLessThan<Vertex>(graph));

  // Mark all vertices white.

  HashMap<Vertex, char> vertex_color;

  for (typename HashSet<Vertex>::const_iterator it = vertices.begin();

       it != vertices.end();

       ++it) {

    vertex_color[*it] = kWhite;

  }

  ordering->clear();

  ordering->reserve(num_vertices);

  // Iterate over vertex_queue. Pick the first white vertex, add it

  // to the independent set. Mark it black and its neighbors grey.

  for (int i = 0; i < vertex_queue.size(); ++i) {

    const Vertex& vertex = vertex_queue[i];

    if (vertex_color[vertex] != kWhite) {

      continue;

    }

    ordering->push_back(vertex);

    vertex_color[vertex] = kBlack;

    const HashSet<Vertex>& neighbors = graph.Neighbors(vertex);

    for (typename HashSet<Vertex>::const_iterator it = neighbors.begin();

         it != neighbors.end();

         ++it) {

      vertex_color[*it] = kGrey;

    }

  }

  int independent_set_size = ordering->size();

  // Iterate over the vertices and add all the grey vertices to the

  // ordering. At this stage there should only be black or grey

  // vertices in the graph.

  for (typename std::vector<Vertex>::const_iterator it = vertex_queue.begin();

       it != vertex_queue.end();

       ++it) {

    const Vertex vertex = *it;

    DCHECK(vertex_color[vertex] != kWhite);

    if (vertex_color[vertex] != kBlack) {

      ordering->push_back(vertex);

    }

  }

  CHECK_EQ(ordering->size(), num_vertices);

  return independent_set_size;

}

// Find the connected component for a vertex implemented using the

// find and update operation for disjoint-set. Recursively traverse

// the disjoint set structure till you reach a vertex whose connected

// component has the same id as the vertex itself. Along the way

// update the connected components of all the vertices. This updating

// is what gives this data structure its efficiency.

template <typename Vertex>

Vertex FindConnectedComponent(const Vertex& vertex,

                              HashMap<Vertex, Vertex>* union_find) {

  typename HashMap<Vertex, Vertex>::iterator it = union_find->find(vertex);

  DCHECK(it != union_find->end());

  if (it->second != vertex) {

    it->second = FindConnectedComponent(it->second, union_find);

  }

  return it->second;

}

// Compute a degree two constrained Maximum Spanning Tree/forest of

// the input graph. Caller owns the result.

//

// Finding degree 2 spanning tree of a graph is not always

// possible. For example a star graph, i.e. a graph with n-nodes

// where one node is connected to the other n-1 nodes does not have

// a any spanning trees of degree less than n-1.Even if such a tree

// exists, finding such a tree is NP-Hard.

// We get around both of these problems by using a greedy, degree

// constrained variant of Kruskal's algorithm. We start with a graph

// G_T with the same vertex set V as the input graph G(V,E) but an

// empty edge set. We then iterate over the edges of G in decreasing

// order of weight, adding them to G_T if doing so does not create a

// cycle in G_T} and the degree of all the vertices in G_T remains

// bounded by two. This O(|E|) algorithm results in a degree-2

// spanning forest, or a collection of linear paths that span the

// graph G.

template <typename Vertex>

WeightedGraph<Vertex>*

Degree2MaximumSpanningForest(const WeightedGraph<Vertex>& graph) {

  // Array of edges sorted in decreasing order of their weights.

  std::vector<std::pair<double, std::pair<Vertex, Vertex> > > weighted_edges;

  WeightedGraph<Vertex>* forest = new WeightedGraph<Vertex>();

  // Disjoint-set to keep track of the connected components in the

  // maximum spanning tree.

  HashMap<Vertex, Vertex> disjoint_set;

  // Sort of the edges in the graph in decreasing order of their

  // weight. Also add the vertices of the graph to the Maximum

  // Spanning Tree graph and set each vertex to be its own connected

  // component in the disjoint_set structure.

  const HashSet<Vertex>& vertices = graph.vertices();

  for (typename HashSet<Vertex>::const_iterator it = vertices.begin();

       it != vertices.end();

       ++it) {

    const Vertex vertex1 = *it;

    forest->AddVertex(vertex1, graph.VertexWeight(vertex1));

    disjoint_set[vertex1] = vertex1;

    const HashSet<Vertex>& neighbors = graph.Neighbors(vertex1);

    for (typename HashSet<Vertex>::const_iterator it2 = neighbors.begin();

         it2 != neighbors.end();

         ++it2) {

      const Vertex vertex2 = *it2;

      if (vertex1 >= vertex2) {

        continue;

      }

      const double weight = graph.EdgeWeight(vertex1, vertex2);

      weighted_edges.push_back(

          std::make_pair(weight, std::make_pair(vertex1, vertex2)));

    }

  }

  // The elements of this vector, are pairs

  // edge>. Sorting it using the reverse iterators gives us the edges

  // in decreasing order of edges.

  std::sort(weighted_edges.rbegin(), weighted_edges.rend());

  // Greedily add edges to the spanning tree/forest as long as they do

  // not violate the degree/cycle constraint.

  for (int i =0; i < weighted_edges.size(); ++i) {

    const std::pair<Vertex, Vertex>& edge = weighted_edges[i].second;

    const Vertex vertex1 = edge.first;

    const Vertex vertex2 = edge.second;

    // Check if either of the vertices are of degree 2 already, in

    // which case adding this edge will violate the degree 2

    // constraint.

    if ((forest->Neighbors(vertex1).size() == 2) ||

        (forest->Neighbors(vertex2).size() == 2)) {

      continue;

    }

    // Find the id of the connected component to which the two

    // vertices belong to. If the id is the same, it means that the

    // two of them are already connected to each other via some other

    // vertex, and adding this edge will create a cycle.

    Vertex root1 = FindConnectedComponent(vertex1, &disjoint_set);

    Vertex root2 = FindConnectedComponent(vertex2, &disjoint_set);

    if (root1 == root2) {

      continue;

    }

    // This edge can be added, add an edge in either direction with

    // the same weight as the original graph.

    const double edge_weight = graph.EdgeWeight(vertex1, vertex2);

    forest->AddEdge(vertex1, vertex2, edge_weight);

    forest->AddEdge(vertex2, vertex1, edge_weight);

    // Connected the two connected components by updating the

    // disjoint_set structure. Always connect the connected component

    // with the greater index with the connected component with the

    // smaller index. This should ensure shallower trees, for quicker

    // lookup.

    if (root2 < root1) {

      std::swap(root1, root2);

    }

    disjoint_set[root2] = root1;

  }

  return forest;

}

}  // namespace internal

}  // namespace ceres

#endif  // CERES_INTERNAL_GRAPH_ALGORITHMS_H_