#ifndef TURN_INSTRUCTIONS_HPP
#define TURN_INSTRUCTIONS_HPP

#include <algorithm>
#include <cmath>

#include <boost/assert.hpp>

namespace osrm
{
namespace extractor
{

enum class TurnInstruction : unsigned char
{
    NoTurn = 0,
    GoStraight,
    TurnSlightRight,
    TurnRight,
    TurnSharpRight,
    UTurn,
    TurnSharpLeft,
    TurnLeft,
    TurnSlightLeft,
    ReachViaLocation,
    HeadOn,
    EnterRoundAbout,
    LeaveRoundAbout,
    StayOnRoundAbout,
    StartAtEndOfStreet,
    ReachedYourDestination,
    NameChanges,
    EnterAgainstAllowedDirection,
    LeaveAgainstAllowedDirection,
    InverseAccessRestrictionFlag = 127,
    AccessRestrictionFlag = 128,
    AccessRestrictionPenalty = 129
};

// shiftable turns to left and right
const constexpr bool shiftable_left[] = {false, false, true, true, true, false, false, true, true};
const constexpr bool shiftable_right[] = {false, false, true, true, false, false, true, true, true};

inline TurnInstruction shiftTurnToLeft(TurnInstruction turn)
{
    BOOST_ASSERT_MSG(static_cast<int>(turn) < 9,
                     "Shift turn only supports basic turn instructions");
    if (turn > TurnInstruction::TurnSlightLeft)
        return turn;
    else
        return shiftable_left[static_cast<int>(turn)]
                   ? (static_cast<TurnInstruction>(static_cast<int>(turn) - 1))
                   : turn;
}

inline TurnInstruction shiftTurnToRight(TurnInstruction turn)
{
    BOOST_ASSERT_MSG(static_cast<int>(turn) < 9,
                     "Shift turn only supports basic turn instructions");
    if (turn > TurnInstruction::TurnSlightLeft)
        return turn;
    else
        return shiftable_right[static_cast<int>(turn)]
                   ? (static_cast<TurnInstruction>(static_cast<int>(turn) + 1))
                   : turn;
}

inline double angularDeviation(const double angle, const double from)
{
    const double deviation = std::abs(angle - from);
    return std::min(360 - deviation, deviation);
}

inline double getAngularPenalty(const double angle, TurnInstruction instruction)
{
    BOOST_ASSERT_MSG(static_cast<int>(instruction) < 9,
                     "Angular penalty only supports basic turn instructions");
    const double center[] = {180, 180, 135, 90, 45,
                             0,   315, 270, 225}; // centers of turns from getTurnDirection
    return angularDeviation(center[static_cast<int>(instruction)], angle);
}

inline double getTurnConfidence(const double angle, TurnInstruction instruction)
{

    // special handling of U-Turns and Roundabout
    if (instruction >= TurnInstruction::HeadOn || instruction == TurnInstruction::UTurn ||
        instruction == TurnInstruction::NoTurn || instruction == TurnInstruction::EnterRoundAbout ||
        instruction == TurnInstruction::StayOnRoundAbout || instruction == TurnInstruction::LeaveRoundAbout )
        return 1.0;

    BOOST_ASSERT_MSG(static_cast<int>(instruction) < 9,
                     "Turn confidence only supports basic turn instructions");
    const double deviations[] = {10, 10, 35, 50, 45, 0, 45, 50, 35};
    const double difference = getAngularPenalty(angle, instruction);
    const double max_deviation = deviations[static_cast<int>(instruction)];
    return 1.0 - (difference / max_deviation) * (difference / max_deviation);
}

// Translates between angles and their human-friendly directional representation
inline TurnInstruction getTurnDirection(const double angle)
{
    // An angle of zero is a u-turn
    // 180 goes perfectly straight
    // 0-180 are right turns
    // 180-360 are left turns
    if (angle > 0 && angle < 60)
        return TurnInstruction::TurnSharpRight;
    if (angle >= 60 && angle < 140)
        return TurnInstruction::TurnRight;
    if (angle >= 140 && angle < 170)
        return TurnInstruction::TurnSlightRight;
    if (angle >= 170 && angle <= 190)
        return TurnInstruction::GoStraight;
    if (angle > 190 && angle <= 220)
        return TurnInstruction::TurnSlightLeft;
    if (angle > 220 && angle <= 300)
        return TurnInstruction::TurnLeft;
    if (angle > 300 && angle < 360)
        return TurnInstruction::TurnSharpLeft;
    return TurnInstruction::UTurn;
}

// Decides if a turn is needed to be done for the current instruction
inline bool isTurnNecessary(const TurnInstruction turn_instruction)
{
    if (TurnInstruction::NoTurn == turn_instruction ||
        TurnInstruction::StayOnRoundAbout == turn_instruction)
    {
        return false;
    }
    return true;
}

inline bool resolve(TurnInstruction &to_resolve, const TurnInstruction neighbor, bool resolve_right)
{
    const auto shifted_turn =
        resolve_right ? shiftTurnToRight(to_resolve) : shiftTurnToLeft(to_resolve);
    if (shifted_turn == neighbor || shifted_turn == to_resolve)
        return false;

    to_resolve = shifted_turn;
    return true;
}

inline bool resolveTransitive(TurnInstruction &first,
                              TurnInstruction &second,
                              const TurnInstruction third,
                              bool resolve_right)
{
    if (resolve(second, third, resolve_right))
    {
        first = resolve_right ? shiftTurnToRight(first) : shiftTurnToLeft(first);
        return true;
    }
    return false;
}

inline bool isSlightTurn(const TurnInstruction turn)
{
    return turn == TurnInstruction::GoStraight || turn == TurnInstruction::TurnSlightRight ||
           turn == TurnInstruction::TurnSlightLeft || turn == TurnInstruction::NoTurn;
}

inline bool isSharpTurn(const TurnInstruction turn)
{
    return turn == TurnInstruction::TurnSharpLeft || turn == TurnInstruction::TurnSharpRight;
}

inline bool isStraight(const TurnInstruction turn)
{
    return turn == TurnInstruction::GoStraight || turn == TurnInstruction::NoTurn;
}

inline bool isConflict(const TurnInstruction first, const TurnInstruction second)
{
    return first == second || (isStraight(first) && isStraight(second));
}
}
}

#endif /* TURN_INSTRUCTIONS_HPP */


/**
 * Copyright (c) 2014-present, Facebook, Inc.
 *
 * This source code is licensed under the MIT license found in the
 * LICENSE file in the root directory of this source tree.
 */

#include "Decision.h"

#include <chrono>
#include <string>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <fbzmq/service/logging/LogSample.h>
#include <fbzmq/service/stats/ThreadData.h>
#include <fbzmq/zmq/Zmq.h>
#include <folly/Format.h>
#include <folly/MapUtil.h>
#include <folly/Memory.h>
#include <folly/Optional.h>
#include <folly/String.h>
#include <gflags/gflags.h>

#include <openr/common/AddressUtil.h>
#include <openr/common/Util.h>

using namespace std;

using apache::thrift::FRAGILE;

using Weight = uint64_t;
using Graph = boost::adjacency_list<
    boost::listS,
    boost::vecS,
    boost::directedS,
    boost::no_property,
    boost::property<boost::edge_weight_t, Weight>>;
using VertexDescriptor = boost::graph_traits<Graph>::vertex_descriptor;
using EdgeDescriptor = boost::graph_traits<Graph>::edge_descriptor;
using EdgeProperty = boost::property<boost::edge_weight_t, Weight>;
using IndexMap = boost::property_map<Graph, boost::vertex_index_t>::type;
using WeightMap = boost::property_map<Graph, boost::edge_weight_t>;
using PredecessorMap = boost::iterator_property_map<
    VertexDescriptor*,
    IndexMap,
    VertexDescriptor,
    VertexDescriptor&>;
using DistanceMap =
    boost::iterator_property_map<Weight*, IndexMap, Weight, Weight&>;

namespace openr {

namespace {

// Default HWM is 1k. We set it to 0 to buffer all received messages.
const int kStoreSubReceiveHwm{0};

/**
 * Create a route with single path.
 */
folly::Optional<thrift::Route>
createRoute(
    const thrift::IpPrefix& prefix,
    const thrift::Adjacency& adjacency,
    Weight metric,
    bool enableV4) {
  auto addr = toIPAddress(prefix.prefixAddress);
  if (addr.isV4() and !enableV4) {
    LOG(ERROR) << "Received v4 prefix while v4 is not enabled.";
    return nullptr;
  }

  thrift::Path path(
      FRAGILE,
      addr.isV4() ? adjacency.nextHopV4 : adjacency.nextHopV6,
      adjacency.ifName,
      metric);
  return thrift::Route(FRAGILE, prefix, {std::move(path)});
}

/**
 * Create multi-path route, prefix with set of loop-free next-hops. If any
 * error occurs then the returned value will be empty and error will be logged.
 */
folly::Optional<thrift::Route>
createRouteMulti(
    const thrift::IpPrefix& prefix,
    const vector<pair<thrift::Adjacency, Weight>>& adjacencies,
    bool enableV4) {
  auto addr = toIPAddress(prefix.prefixAddress);
  if (addr.isV4() and !enableV4) {
    LOG(ERROR) << "Received v4 prefix while v4 is not enabled.";
    return nullptr;
  }

  if (adjacencies.empty()) {
    return nullptr;
  }

  vector<thrift::Path> paths;
  for (auto const& kv : adjacencies) {
    auto const& adjacency = kv.first;
    paths.emplace_back(
        FRAGILE,
        addr.isV4() ? adjacency.nextHopV4 : adjacency.nextHopV6,
        adjacency.ifName,
        kv.second /* metric */);
  }

  return thrift::Route(FRAGILE, prefix, std::move(paths));
}

/**
 * Given a thrift::IpPrefix returns a node name for it.
 */
inline std::string
getPrefixVertexName(const thrift::IpPrefix& ipPrefix) {
  return "pfxnd-" + toString(ipPrefix);
}

} // anonymous namespace

/**
 * Private implementation of the SpfSolver
 */
class SpfSolver::SpfSolverImpl {
 public:
  explicit SpfSolverImpl(bool enableV4) : enableV4_(enableV4) {}

  ~SpfSolverImpl() = default;

  bool updateAdjacencyDatabase(thrift::AdjacencyDatabase const& adjacencyDb);
  bool deleteAdjacencyDatabase(const std::string& nodeName);
  std::unordered_map<std::string /* nodeName */, thrift::AdjacencyDatabase>
  getAdjacencyDatabases();
  bool updatePrefixDatabase(thrift::PrefixDatabase const& prefixDb);
  bool deletePrefixDatabase(const std::string& nodeName);
  std::unordered_map<std::string /* nodeName */, thrift::PrefixDatabase>
  getPrefixDatabases();

  thrift::RouteDatabase buildShortestPaths(const std::string& myNodeName);
  thrift::RouteDatabase buildMultiPaths(const std::string& myNodeName);

  /**
   * Graph is prepared as followed
   * 1. Add all node vertices (adjacencyDbs_.keys())
   * 2. Use adjacencyDbs_ to add edges between nodes. bidirectional check is
   *    enforced here.
   * 3. Add a vertex for each prefix
   * 4. Add a directed edge from a node-vertex to prefix-vertex if node has
   *    advertised the prefix.
   *
   * Bidirectional Check: An edge between node will be considered only when
   * both nodes reports each other. Otherwise it is ignored.
   *
   * Prepare data structures that we use for SPF run and it can be re-used
   * across multiple SPF computations.
   */
  void prepareGraph();

  std::unordered_map<std::string, int64_t> getCounters();

  fbzmq::ThreadData&
  getThreadData() noexcept {
    return tData_;
  }

 private:
  // no copy
  SpfSolverImpl(SpfSolverImpl const&) = delete;
  SpfSolverImpl& operator=(SpfSolverImpl const&) = delete;

  // run SPF and produce map from node name to next-hop and metric to reach it
  unordered_map<
      string /* other node name */,
      pair<string /* next-hop node name */, Weight /* metric */>>
  runSpf(const std::string& myNodeName);

  // adjacencies per advertising router
  std::unordered_map<std::string /* nodeName */, thrift::AdjacencyDatabase>
      adjacencyDbs_;

  // prefix to set of node names who advertised the prefix
  std::unordered_map<thrift::IpPrefix, std::unordered_set<std::string>>
      prefixToNodeNames_;

  // the graph we operate on for SPF
  Graph graph_;

  // helpers for mapping router names to descriptros and vice versa
  unordered_map<string /* router name */, VertexDescriptor> nameToDescriptor_;
  unordered_map<VertexDescriptor, string /* router name */> descriptorToName_;

  // compare two adjacencies
  struct AdjComp {
    bool
    operator()(
        const thrift::Adjacency& lhs, const thrift::Adjacency& rhs) const {
      return lhs.metric < rhs.metric;
    }
  };

  // Adjacencies indexed by originating router and destination router
  // Note: use multiset, not set, since there are duplicates, i.e., adjacencies
  // with same metric
  unordered_map<
      pair<string /* thisRouterName */, string /* otherRouterName */>,
      multiset<
          thrift::Adjacency,
          AdjComp> /* adjacencies ordered by non-descreasing metric */>
      adjacencyIndex_;

  // track some stats
  fbzmq::ThreadData tData_;

  // is v4 enabled. If yes then Decision will forward v4 prefixes with v4
  // nexthops to Fib module for programming. Else it will just drop them.
  const bool enableV4_{false};
};

bool
SpfSolver::SpfSolverImpl::updateAdjacencyDatabase(
    thrift::AdjacencyDatabase const& adjacencyDb) {
  auto const& nodeName = adjacencyDb.thisNodeName;
  VLOG(2) << "Updating adjacency database for node " << nodeName;
  tData_.addStatValue("decision.adj_db_update", 1, fbzmq::COUNT);
  VLOG(3) << "  New adjacencies for node " << nodeName;
  for (auto const& adj : adjacencyDb.adjacencies) {
    VLOG(3) << "  nbr: " << adj.otherNodeName << ", ifName: " << adj.ifName
            << ", metric: " << adj.metric
            << ", overloaded: " << adj.isOverloaded << ", rtt: " << adj.rtt;
  }

  // Insert if not exists
  auto res = adjacencyDbs_.emplace(nodeName, adjacencyDb);

  // NOTE: We need to clear SPF cache on any change in Adjacency DB
  // Insertion took place
  if (res.second) {
    return true;
  }

  // There already exists a key. See if value has changed or not
  auto& it = res.first;
  if (adjacencyDb != it->second) {
    it->second = adjacencyDb;
    return true;
  }

  // There is no change
  return false;
}

bool
SpfSolver::SpfSolverImpl::deleteAdjacencyDatabase(const std::string& nodeName) {
  if (adjacencyDbs_.erase(nodeName) > 0) {
    LOG(INFO) << nodeName << "'s adjacency db is deleted";
    return true;
  } else {
    LOG(WARNING) << "Trying to delete adjacency db for nonexisting node "
                 << nodeName;
    return false;
  }
}

std::unordered_map<std::string /* nodeName */, thrift::AdjacencyDatabase>
SpfSolver::SpfSolverImpl::getAdjacencyDatabases() {
  return adjacencyDbs_;
}

bool
SpfSolver::SpfSolverImpl::updatePrefixDatabase(
    thrift::PrefixDatabase const& prefixDb) {
  auto const& nodeName = prefixDb.thisNodeName;
  VLOG(2) << "Updating prefix database for node " << nodeName;
  tData_.addStatValue("decision.prefix_db_update", 1, fbzmq::COUNT);

  // Add new ones
  bool updated = false;
  std::unordered_set<thrift::IpPrefix> prefixes;
  for (auto const& prefixEntry : prefixDb.prefixEntries) {
    prefixes.insert(prefixEntry.prefix);
    auto& nodes = prefixToNodeNames_[prefixEntry.prefix];
    updated |= nodes.insert(nodeName).second;
  }

  // Remove old ones
  for (auto it = prefixToNodeNames_.begin(); it != prefixToNodeNames_.end();) {
    const auto& prefix = it->first;
    auto& nodes = it->second;
    if (prefixes.count(prefix) or !nodes.count(nodeName)) {
      // a node advertised this prefix before, but not anymore
      ++it;
      continue;
    }

    updated = true;
    nodes.erase(nodeName);
    if (nodes.empty()) {
      it = prefixToNodeNames_.erase(it);
    } else {
      ++it;
    }
  }

  return updated;
}

bool
SpfSolver::SpfSolverImpl::deletePrefixDatabase(const std::string& nodeName) {
  bool updated = false;

  // Renounce all prefixes it has advertised
  for (auto it = prefixToNodeNames_.begin(); it != prefixToNodeNames_.end();) {
    const auto& prefix = it->first;
    auto& nodes = it->second;

    if (nodes.erase(nodeName) == 0) {
      ++it;
      continue;
    }

    LOG(INFO) << nodeName << " no longer advertises " << toString(prefix);
    updated = true;
    if (nodes.empty()) {
      it = prefixToNodeNames_.erase(it);
    } else {
      ++it;
    }
  }

  if (not updated) {
    LOG(WARNING) << "Trying to delete prefix db for nonexisting node "
                 << nodeName;
  }
  return updated;
}

std::unordered_map<std::string /* nodeName */, thrift::PrefixDatabase>
SpfSolver::SpfSolverImpl::getPrefixDatabases() {
  // prefixes per advertising router
  std::unordered_map<std::string /* nodeName */, thrift::PrefixDatabase>
      prefixDbs;

  // construct prefix dbs from prefixToNodeNames_
  for (const auto& prefixNodeNames : prefixToNodeNames_) {
    const auto& prefix = prefixNodeNames.first;
    const auto& nodeNames = prefixNodeNames.second;

    for (const auto& nodeName : nodeNames) {
      prefixDbs[nodeName].thisNodeName = nodeName;
      prefixDbs[nodeName].prefixEntries.emplace_back(
          apache::thrift::FRAGILE, prefix, thrift::PrefixType::LOOPBACK, "");
    }
  }

  return prefixDbs;
}

void
SpfSolver::SpfSolverImpl::prepareGraph() {
  VLOG(2) << "SpfSolver::SpfSolverImpl::prepareGraph() called.";

  graph_.clear();
  nameToDescriptor_.clear();
  descriptorToName_.clear();
  adjacencyIndex_.clear();

  // Create a set of all unidirectional adjacencies, which is from one node to
  // another one connected by at least one interface
  std::unordered_set<
      std::pair<std::string /* myNodeName */, std::string /* otherNodeName */>>
      presentAdjacencies;
  for (auto const& kv : adjacencyDbs_) {
    auto const& thisNodeName = kv.first;

    for (auto const& adjacency : kv.second.adjacencies) {
      // Skip adjacency from SPF graph if it is overloaded to avoid transit
      // traffic through it.
      if (adjacency.isOverloaded) {
        continue;
      }

      presentAdjacencies.insert({thisNodeName, adjacency.otherNodeName});
    }
  }

  // Build adjacencyIndex_ with bidirectional check enforced
  for (auto const& kv : adjacencyDbs_) {
    auto const& thisNodeName = kv.first;

    // walk all my ajdacencies
    for (auto const& myAdjacency : kv.second.adjacencies) {
      auto const& otherNodeName = myAdjacency.otherNodeName;

      // Skip if overloaded
      if (myAdjacency.isOverloaded) {
        continue;
      }

      // Check for reverse adjacency
      bool isBidir = presentAdjacencies.count({otherNodeName, thisNodeName});
      if (!isBidir) {
        VLOG(2) << "Failed to find matching adjacency for `" << thisNodeName
                << "' in `" << otherNodeName << "' database";
        continue;
      }

      adjacencyIndex_[{thisNodeName, otherNodeName}].insert(myAdjacency);
    } // for adjacencies
  } // for adjacencyDbs_

  // 1. Add all node vertices
  for (auto const& kv : adjacencyDbs_) {
    auto const& nodeName = kv.first;
    auto const& descriptor = boost::add_vertex(graph_);
    nameToDescriptor_[nodeName] = descriptor;
    descriptorToName_[descriptor] = nodeName;
  }

  // 2. Add edges between node vertices
  for (const auto& kv : adjacencyIndex_) {
    const auto& thisNodeName = kv.first.first;
    const auto& otherNodeName = kv.first.second;
    const auto& adjacencies = kv.second;

    // this will not throw by virtue of building the name to descriptor map.
    // bidirectional check is enforced
    auto const& srcVertex = nameToDescriptor_.at(thisNodeName);
    auto const& dstVertex = nameToDescriptor_.at(otherNodeName);

    // Is thisNode overloaded. If node is overloaded then we bump up metric
    // values of it's outgoing edges by `kOverloadNodeMetric`. We use this
    // information later on to deduce if a shortest path is going over
    // overloaded node or not.
    // NOTE: link's metric can never be greater than kOverloadNodeMetric
    uint64_t metric = adjacencies.begin()->metric;
    if (adjacencyDbs_.at(thisNodeName).isOverloaded) {
      metric += Constants::kOverloadNodeMetric;
    }

    // for two nodes with multiple adjacencies in between, use the min metric
    // as their edge metric
    boost::add_edge(srcVertex, dstVertex, EdgeProperty(metric), graph_);
  } // for adjacencyIndex_

  // 3./4. Add all prefix vertices and directed edges from node->prefix vertices
  for (const auto& kv : prefixToNodeNames_) {
    std::string prefixVertexName = getPrefixVertexName(kv.first);

    // Add the prefix vertex
    auto const& prefixVertex = boost::add_vertex(graph_);
    nameToDescriptor_[prefixVertexName] = prefixVertex;
    descriptorToName_[prefixVertex] = prefixVertexName;

    for (const auto& nodeName : kv.second) {
      // Skip the node if we haven't seen it's adjacency DB
      if (!adjacencyDbs_.count(nodeName)) {
        continue;
      }

      // Add edge from node->prefix with zero cost
      auto const& nodeVertex = nameToDescriptor_.at(nodeName);
      boost::add_edge(nodeVertex, prefixVertex, 0 /* cost */, graph_);
    }
  }
} // prepareGraph

/**
 * Compute shortest-path routes from perspective of myNodeName; NOTE: you
 * need to call prepareGraph() to initialize the graph structure before
 * calling this method.
 */
unordered_map<
    string /* otherVertexName (prefix/node) */,
    pair<string /* nextHopVertexName (node) */, Weight>>
SpfSolver::SpfSolverImpl::runSpf(const std::string& myNodeName) {
  tData_.addStatValue("decision.spf_runs", 1, fbzmq::COUNT);

  // map of other nodes, next-hops and distances to reach to them
  unordered_map<
      string /* otherVertexName */,
      pair<string /* nextHopVertexName */, Weight>>
      result;

  // Find our node-vertex-descriptor before proceeding
  auto it = nameToDescriptor_.find(myNodeName);
  if (it == nameToDescriptor_.end()) {
    LOG(ERROR) << "Could not find descriptor for router `" << myNodeName << "`";
    return result;
  }
  auto const& myVertex = it->second;

  // Refer here for example of BGL Dijkstra use:
  // http://programmingexamples.net/wiki/Boost/BGL/DijkstraComputePath

  // holds predecessors for vertices in shortest-path tree
  // i.e. indexed by vertex and gives you the vertex that
  // precedes this one on the path toward the SPT root
  vector<VertexDescriptor> pred(boost::num_vertices(graph_));

  // distances from each vertex to the root of SPT
  vector<Weight> dist(boost::num_vertices(graph_));

  // build SPT from this vertex
  IndexMap indexMap = boost::get(boost::vertex_index, graph_);
  PredecessorMap predMap(&pred[0], indexMap);
  DistanceMap distMap(&dist[0], indexMap);
  boost::dijkstra_shortest_paths(
      graph_, myVertex, boost::distance_map(distMap).predecessor_map(predMap));

  // walk all the nodes in the topology and compute paths to their prefixes
  for (auto const& kv : nameToDescriptor_) {
    auto const& otherVertexName = kv.first;
    auto const& otherVertex = kv.second;

    // skip self
    if (otherVertexName == myNodeName) {
      continue;
    }

    // indirectly connected, trace the path
    auto prevVertex = otherVertex;
    auto currVertex = pred[otherVertex];

    if (currVertex == prevVertex) {
      // unreachable from the root of the SPT
      continue;
    }

    while (currVertex != myVertex) {
      prevVertex = currVertex;
      currVertex = pred[currVertex];
    }

    // at this point prevVertex -> to our nextHop
    auto nextHopVertexName = descriptorToName_[prevVertex];
    result[otherVertexName] = {nextHopVertexName, dist[otherVertex]};
  } // for nameToDescriptor_

  // Print SPF table for debugging
  VLOG(4) << "SPF table for " << myNodeName;
  for (auto const& kv : result) {
    auto const& otherVertexName = kv.first;
    auto const& nextHopVertexName = kv.second.first;
    auto const& weight = kv.second.second;
    VLOG(4) << folly::sformat(
        "  {} via {}, cost {}", otherVertexName, nextHopVertexName, weight);
  }

  return result;
}

thrift::RouteDatabase
SpfSolver::SpfSolverImpl::buildShortestPaths(const std::string& myNodeName) {
  VLOG(4) << "SpfSolver::buildShortestPaths for " << myNodeName;
  tData_.addStatValue("decision.paths_build_requests", 1, fbzmq::COUNT);

  thrift::RouteDatabase routeDb;
  routeDb.thisNodeName = myNodeName;
  if (adjacencyDbs_.count(myNodeName) == 0) {
    LOG(ERROR) << "Could not find adjacency Db for myself: `" << myNodeName
               << "`, skipping spf run";
    return routeDb;
  }

  const auto startTime = std::chrono::steady_clock::now();

  prepareGraph();
  auto spfMap = runSpf(myNodeName);
  const bool myselfOverloaded = adjacencyDbs_.at(myNodeName).isOverloaded;

  for (auto const& kv : prefixToNodeNames_) {
    auto const& prefix = kv.first;
    auto const& nodeNames = kv.second;
    auto const prefixVertexName = getPrefixVertexName(prefix);

    // Skip a prefix if we own it
    if (nodeNames.count(myNodeName)) {
      continue;
    }

    // Find minimum nexthop to reach that prefix
    auto prefixIt = spfMap.find(prefixVertexName);
    if (prefixIt == spfMap.end()) {
      VLOG(2) << "Skipping unreachable prefix " << toString(prefix);
      continue;
    }

    auto const& nextHopNodeName = prefixIt->second.first;
    auto metric = prefixIt->second.second;

    // Subtract overload metric value if myself is overloaded
    if (myselfOverloaded) {
      CHECK_LT(Constants::kOverloadNodeMetric, metric);
      metric -= Constants::kOverloadNodeMetric;
    }

    // Skip route if it is going through a node which is overloaded
    if (metric > Constants::kOverloadNodeMetric) {
      VLOG(2) << "Skipping shortest route to prefix " << toString(prefix)
              << " because it is via an overloaded node.";
      continue;
    }

    // Add a route for this prefix via the nextHopNodeName
    auto const& nhAdjs = adjacencyIndex_.at({myNodeName, nextHopNodeName});
    auto route = createRoute(prefix, *nhAdjs.begin(), metric, enableV4_);
    if (route) {
      routeDb.routes.emplace_back(std::move(*route));
    }
  }

  auto deltaTime = std::chrono::duration_cast<std::chrono::milliseconds>(
      std::chrono::steady_clock::now() - startTime);
  LOG(INFO) << "Decision::buildShortestPaths took " << deltaTime.count()
            << "ms.";
  tData_.addStatValue(
      "decision.spf.shortestpath_ms", deltaTime.count(), fbzmq::AVG);

  return routeDb;
} // buildShortestPaths

thrift::RouteDatabase
SpfSolver::SpfSolverImpl::buildMultiPaths(const std::string& myNodeName) {
  VLOG(4) << "SpfSolver::buildMultiPaths for " << myNodeName;
  tData_.addStatValue("decision.paths_build_requests", 1, fbzmq::COUNT);

  thrift::RouteDatabase routeDb;
  routeDb.thisNodeName = myNodeName;
  if (adjacencyDbs_.count(myNodeName) == 0) {
    LOG(ERROR) << "Could not find adjacency Db for myself: " << myNodeName
               << ", skipping multi-spf run";
    return routeDb;
  }

  const auto startTime = std::chrono::steady_clock::now();

  // This accumulates all direct next-hops toward a given destination prefix.
  // we add those as we find more loop-free alternate neighbors
  std::unordered_map<
      std::string /* prefixVertexName */,
      std::map<
          std::string /* nextHopNodeName */,
          Weight /* metric from nextHopNodeName to prefixVertexName */>>
      allNextHopsForPrefix;

  prepareGraph();
  auto mySpfPaths = runSpf(myNodeName);

  // avoid duplicate iterations over a neighbor which can happen due to multiple
  // adjacencies to it
  std::unordered_set<std::string /* adjacent node name */> visitedAdjNodes;

  // Go over all neighbors and find shortest distance of prefixes from them
  for (auto const& adj : adjacencyDbs_.at(myNodeName).adjacencies) {
    auto const& adjNodeName = adj.otherNodeName;

    // Skip if already visited before
    if (not visitedAdjNodes.insert(adjNodeName).second) {
      VLOG(4) << "Adjacent neighbor " << adjNodeName << " has been considered.";
      continue;
    }

    // Skip if we don't have reverse edge from adjNodeName->myNodeName
    if (!adjacencyIndex_.count({adjNodeName, myNodeName})) {
      VLOG(2) << "Neighbor " << adjNodeName << " doesn't have reverse edge "
              << "to myself " << myNodeName << ". Skipping spf run for it.";
      continue;
    }

    // Run SPF for the node
    VLOG(4) << "Running SPF for neighbor: " << adjNodeName;
    auto nbrSpfPaths = runSpf(adjNodeName);

    // Walk through all prefixes and find shortest distance of prefix from
    // myNodeName to the prefix via adjNodeName
    for (auto const& kv : prefixToNodeNames_) {
      auto const& prefix = kv.first;
      auto const prefixVertexName = getPrefixVertexName(prefix);

      // Skip if prefix is unreachable from neighbor
      auto prefixIt = nbrSpfPaths.find(prefixVertexName);
      if (prefixIt == nbrSpfPaths.end()) {
        continue; // Prefix is unreachable from neighbor
      }

      // Get distances from adjNodeName
      auto adjPrefixDist = prefixIt->second.second;
      auto adjMyNodeDist = nbrSpfPaths.at(myNodeName).second;

      // Get optimal distance of prefixVertexName from us. Prefix must be
      // reachable from us because it is reachable from our neighbor
      auto myPrefixDist = mySpfPaths.at(prefixVertexName).second;

      // A neighbor (N) can provide loop free alternate to destination (D) from
      // source (S) if and only if
      //   Distance_opt(N, D) < Distance_opt(N, S) + Distance_opt(S, D)
      // For more info read: https://tools.ietf.org/html/rfc5286
      // NOTE: We are using negated condition here to skip the neighbor
      if (adjPrefixDist >= adjMyNodeDist + myPrefixDist) {
        VLOG(4) << "Skipping non LFA nexthop " << adjNodeName << " to "
                << "prefix " << prefixVertexName;
        continue;
      }

      VLOG(4) << "Adding LFA nexthop " << adjNodeName << " for prefix "
              << prefixVertexName;
      allNextHopsForPrefix[prefixVertexName][adjNodeName] = adjPrefixDist;
    } // for prefixToNodeNames_
  } // for adjacencyDbs_.at(myNodeName).adjacencies

  // Build RouteDb for all prefixes including LFAs
  for (auto const& kv : prefixToNodeNames_) {
    auto const& prefix = kv.first;
    auto const& nodeNames = kv.second;
    auto const prefixVertexName = getPrefixVertexName(prefix);

    // Skip a prefix if we own it
    if (nodeNames.count(myNodeName)) {
      continue;
    }

    auto prefixIt = allNextHopsForPrefix.find(prefixVertexName);
    if (prefixIt == allNextHopsForPrefix.end()) {
      VLOG(4) << "Skipping route to unreachable prefix " << prefixVertexName;
      continue;
    }

    std::vector<std::pair<thrift::Adjacency, Weight>> adjacencies;
    for (auto const& nhMetric : prefixIt->second) {
      auto const& nhNodeName = nhMetric.first;
      auto metric = nhMetric.second;

      // Skip route if it is going through a node which is overloaded
      // NOTE: the metric here is a distance to prefix-node via our neighbor
      if (metric > Constants::kOverloadNodeMetric) {
        VLOG(2) << "Skipping shortest route to prefix " << toString(prefix)
                << " because it is via an overloaded node.";
        continue;
      }

      // Add all adjacencies to nextHopNodeName as potential nexthops. FIB will
      // only program ones with min metric and others will act as LFAs
      for (const auto& adj : adjacencyIndex_.at({myNodeName, nhNodeName})) {
        adjacencies.push_back({adj, metric + adj.metric});
      }
    }

    auto route = createRouteMulti(prefix, adjacencies, enableV4_);
    if (route) {
      routeDb.routes.emplace_back(std::move(*route));
    }
  } // for prefixToNodeNames_ (build RouteDb)

  auto deltaTime = std::chrono::duration_cast<std::chrono::milliseconds>(
      std::chrono::steady_clock::now() - startTime);
  LOG(INFO) << "Decision::buildMultiPaths took " << deltaTime.count() << "ms.";
  tData_.addStatValue(
      "decision.spf.multipath_ms", deltaTime.count(), fbzmq::AVG);

  return routeDb;
} // buildMultiPaths

std::unordered_map<std::string, int64_t>
SpfSolver::SpfSolverImpl::getCounters() {
  return tData_.getCounters();
}

//
// Public SpfSolver
//

SpfSolver::SpfSolver(bool enableV4)
    : impl_(new SpfSolver::SpfSolverImpl(enableV4)) {}

SpfSolver::~SpfSolver() {}

// update adjacencies for the given router; everything is replaced
bool
SpfSolver::updateAdjacencyDatabase(
    thrift::AdjacencyDatabase const& adjacencyDb) {
  return impl_->updateAdjacencyDatabase(adjacencyDb);
}

bool
SpfSolver::deleteAdjacencyDatabase(const std::string& nodeName) {
  return impl_->deleteAdjacencyDatabase(nodeName);
}

std::unordered_map<std::string /* nodeName */, thrift::AdjacencyDatabase>
SpfSolver::getAdjacencyDatabases() {
  return impl_->getAdjacencyDatabases();
}

// update prefixes for a given router
bool
SpfSolver::updatePrefixDatabase(thrift::PrefixDatabase const& prefixDb) {
  return impl_->updatePrefixDatabase(prefixDb);
}

bool
SpfSolver::deletePrefixDatabase(const std::string& nodeName) {
  return impl_->deletePrefixDatabase(nodeName);
}

std::unordered_map<std::string /* nodeName */, thrift::PrefixDatabase>
SpfSolver::getPrefixDatabases() {
  return impl_->getPrefixDatabases();
}

thrift::RouteDatabase
SpfSolver::buildShortestPaths(const std::string& myNodeName) {
  return impl_->buildShortestPaths(myNodeName);
}

thrift::RouteDatabase
SpfSolver::buildMultiPaths(const std::string& myNodeName) {
  return impl_->buildMultiPaths(myNodeName);
}

std::unordered_map<std::string, int64_t>
SpfSolver::getCounters() {
  return impl_->getCounters();
}

//
// Decision class implementation
//

Decision::Decision(
    std::string myNodeName,
    bool enableV4,
    const AdjacencyDbMarker& adjacencyDbMarker,
    const PrefixDbMarker& prefixDbMarker,
    std::chrono::milliseconds debounceMinDur,
    std::chrono::milliseconds debounceMaxDur,
    const KvStoreLocalCmdUrl& storeCmdUrl,
    const KvStoreLocalPubUrl& storePubUrl,
    const DecisionCmdUrl& decisionCmdUrl,
    const DecisionPubUrl& decisionPubUrl,
    const MonitorSubmitUrl& monitorSubmitUrl,
    fbzmq::Context& zmqContext)
    : expBackoff_(debounceMinDur, debounceMaxDur),
      myNodeName_(myNodeName),
      adjacencyDbMarker_(adjacencyDbMarker),
      prefixDbMarker_(prefixDbMarker),
      storeCmdUrl_(storeCmdUrl),
      storePubUrl_(storePubUrl),
      decisionCmdUrl_(decisionCmdUrl),
      decisionPubUrl_(decisionPubUrl),
      storeSub_(zmqContext),
      decisionRep_(zmqContext),
      decisionPub_(zmqContext) {
  processPendingUpdatesTimer_ = fbzmq::ZmqTimeout::make(
      this, [this]() noexcept { processPendingUpdates(); });
  spfSolver_ = std::make_unique<SpfSolver>(enableV4);

  zmqMonitorClient_ =
      std::make_unique<fbzmq::ZmqMonitorClient>(zmqContext, monitorSubmitUrl);

  // Initialize ZMQ sockets
  scheduleTimeout(
      std::chrono::seconds(0), [this, &zmqContext]() { prepare(zmqContext); });
}

void
Decision::prepare(fbzmq::Context& zmqContext) noexcept {
  VLOG(2) << "Decision: Binding cmdUrl '" << decisionCmdUrl_ << "'";
  const auto repBind = decisionRep_.bind(fbzmq::SocketUrl{decisionCmdUrl_});
  if (repBind.hasError()) {
    LOG(FATAL) << "Error binding to URL '" << decisionCmdUrl_ << "' "
               << repBind.error();
  }

  VLOG(2) << "Decision: Binding pubUrl '" << decisionPubUrl_ << "'";
  const auto pubBind = decisionPub_.bind(fbzmq::SocketUrl{decisionPubUrl_});
  if (pubBind.hasError()) {
    LOG(FATAL) << "Error binding to URL '" << decisionPubUrl_ << "' "
               << pubBind.error();
  }

  VLOG(2) << "Decision: Connecting to store '" << storePubUrl_ << "'";
  const auto optRet =
      storeSub_.setSockOpt(ZMQ_RCVHWM, &kStoreSubReceiveHwm, sizeof(int));
  if (optRet.hasError()) {
    LOG(FATAL) << "Error setting ZMQ_RCVHWM to " << kStoreSubReceiveHwm << " "
               << optRet.error();
  }
  const auto subConnect = storeSub_.connect(fbzmq::SocketUrl{storePubUrl_});
  if (subConnect.hasError()) {
    LOG(FATAL) << "Error binding to URL '" << storePubUrl_ << "' "
               << subConnect.error();
  }
  const auto subRet = storeSub_.setSockOpt(ZMQ_SUBSCRIBE, "", 0);
  if (subRet.hasError()) {
    LOG(FATAL) << "Error setting ZMQ_SUBSCRIBE to "
               << ""
               << " " << subRet.error();
  }

  VLOG(2) << "Decision thread attaching socket/event callbacks...";

  // Schedule periodic timer for submission to monitor
  const bool isPeriodic = true;
  monitorTimer_ =
      fbzmq::ZmqTimeout::make(this, [this]() noexcept { submitCounters(); });
  monitorTimer_->scheduleTimeout(Constants::kMonitorSubmitInterval, isPeriodic);

  // Attach callback for processing publications on storeSub_ socket
  addSocket(
      fbzmq::RawZmqSocketPtr{*storeSub_}, ZMQ_POLLIN, [this](int) noexcept {
        VLOG(3) << "Decision: publication received...";

        auto maybeThriftPub = storeSub_.recvThriftObj<thrift::Publication>(
            serializer_, Constants::kReadTimeout);
        if (maybeThriftPub.hasError()) {
          LOG(ERROR) << "Error processing KvStore publication: "
                     << maybeThriftPub.error();
          return;
        }

        // Apply publication and compute routes with exponential
        // backoff timer if needed
        if (processPublication(maybeThriftPub.value())) {
          if (!expBackoff_.atMaxBackoff()) {
            expBackoff_.reportError();
            processPendingUpdatesTimer_->scheduleTimeout(
                expBackoff_.getTimeRemainingUntilRetry());
          } else {
            CHECK(processPendingUpdatesTimer_->isScheduled());
          }
        }
      });

  // Attach callback for processing requests on REP socket
  addSocket(
      fbzmq::RawZmqSocketPtr{*decisionRep_}, ZMQ_POLLIN, [this](int) noexcept {
        VLOG(3) << "Decision: request received...";
        processRequest();
      });

  initialSync(zmqContext);
}

void
Decision::processRequest() {
  auto maybeThriftReq = decisionRep_.recvThriftObj<thrift::DecisionRequest>(
      serializer_, Constants::kReadTimeout);
  if (maybeThriftReq.hasError()) {
    LOG(ERROR) << "Decision: Error processing request on REP socket: "
               << maybeThriftReq.error();
    return;
  }

  auto thriftReq = maybeThriftReq.value();
  thrift::DecisionReply reply;
  switch (thriftReq.cmd) {
  case thrift::DecisionCommand::ROUTE_DB_GET: {
    auto nodeName = thriftReq.nodeName;
    if (nodeName.empty()) {
      VLOG(1) << "Decision: Routes requested with no specific node name. "
              << "Returning " << myNodeName_ << " routes.";
      nodeName = myNodeName_;
    }

    reply.routeDb = spfSolver_->buildMultiPaths(nodeName);
    break;
  }

  case thrift::DecisionCommand::ADJ_DB_GET: {
    reply.adjDbs = spfSolver_->getAdjacencyDatabases();
    break;
  }

  case thrift::DecisionCommand::PREFIX_DB_GET: {
    reply.prefixDbs = spfSolver_->getPrefixDatabases();
    break;
  }

  default: {
    LOG(ERROR) << "Unexpected command received: "
               << folly::get_default(
                      thrift::_DecisionCommand_VALUES_TO_NAMES,
                      thriftReq.cmd,
                      "UNKNOWN");
    return;
  }
  }

  auto sendRc = decisionRep_.sendThriftObj(reply, serializer_);
  if (sendRc.hasError()) {
    LOG(ERROR) << "Error sending response: " << sendRc.error();
  }
}

std::unordered_map<std::string, int64_t>
Decision::getCounters() {
  return spfSolver_->getCounters();
}

bool
Decision::processPublication(thrift::Publication const& thriftPub) {
  bool graphChanged{false};

  // LSDB addition/update
  // deserialize contents of every LSDB key

  // Nothing to process if no adj/prefix db changes
  if (thriftPub.keyVals.empty() and thriftPub.expiredKeys.empty()) {
    return false;
  }

  for (const auto& kv : thriftPub.keyVals) {
    const auto& key = kv.first;
    const auto& rawVal = kv.second;
    std::string prefix, nodeName;
    folly::split(Constants::kPrefixNameSeparator, key, prefix, nodeName);

    if (not rawVal.value.hasValue()) {
      // skip TTL update
      DCHECK(rawVal.ttlVersion > 0);
      continue;
    }

    try {
      if (key.find(adjacencyDbMarker_) == 0) {
        auto adjacencyDb =
            fbzmq::util::readThriftObjStr<thrift::AdjacencyDatabase>(
                rawVal.value.value(), serializer_);
        CHECK_EQ(nodeName, adjacencyDb.thisNodeName);
        if (spfSolver_->updateAdjacencyDatabase(adjacencyDb)) {
          graphChanged = true;
          pendingUpdates_.addUpdate(myNodeName_, adjacencyDb.perfEvents);
        }
        continue;
      }

      if (key.find(prefixDbMarker_) == 0) {
        auto prefixDb = fbzmq::util::readThriftObjStr<thrift::PrefixDatabase>(
            rawVal.value.value(), serializer_);
        CHECK_EQ(nodeName, prefixDb.thisNodeName);
        if (spfSolver_->updatePrefixDatabase(prefixDb)) {
          graphChanged = true;
          pendingUpdates_.addUpdate(myNodeName_, prefixDb.perfEvents);
        }
        continue;
      }
    } catch (std::exception const& e) {
      LOG(ERROR) << "Failed to deserialize info for key " << key
                 << ". Exception: " << folly::exceptionStr(e);
    }
  }

  // LSDB deletion
  for (const auto& key : thriftPub.expiredKeys) {
    std::string prefix, nodeName;
    folly::split(Constants::kPrefixNameSeparator, key, prefix, nodeName);

    if (key.find(adjacencyDbMarker_) == 0) {
      if (spfSolver_->deleteAdjacencyDatabase(nodeName)) {
        graphChanged = true;
        pendingUpdates_.addUpdate(myNodeName_, folly::none);
      }
      continue;
    }

    if (key.find(prefixDbMarker_) == 0) {
      if (spfSolver_->deletePrefixDatabase(nodeName)) {
        graphChanged = true;
        pendingUpdates_.addUpdate(myNodeName_, folly::none);
      }
      continue;
    }
  }

  return graphChanged;
}

// perform full dump of all LSDBs and run initial routing computations
void
Decision::initialSync(fbzmq::Context& zmqContext) {
  thrift::Request thriftReq;

  fbzmq::Socket<ZMQ_REQ, fbzmq::ZMQ_CLIENT> storeReq(zmqContext);

  // we'll be using this to get the full dump from the KvStore
  const auto reqConnect = storeReq.connect(fbzmq::SocketUrl{storeCmdUrl_});
  if (reqConnect.hasError()) {
    LOG(FATAL) << "Error connecting to URL '" << storeCmdUrl_ << "' "
               << reqConnect.error();
  }

  thriftReq.cmd = thrift::Command::KEY_DUMP;
  storeReq.sendThriftObj(thriftReq, serializer_);

  VLOG(2) << "Decision process requesting initial state...";

  // receive the full dump of the database
  auto maybeThriftPub = storeReq.recvThriftObj<thrift::Publication>(
      serializer_, Constants::kReadTimeout);
  if (maybeThriftPub.hasError()) {
    LOG(ERROR) << "Error processing KvStore publication: "
               << maybeThriftPub.error();
    return;
  }

  // Process publication and immediately apply updates
  if (processPublication(maybeThriftPub.value())) {
    processPendingUpdates();
  }
}

// periodically submit counters to Counters thread
void
Decision::submitCounters() {
  VLOG(2) << "Submitting counters...";

  // Prepare for submitting counters
  fbzmq::CounterMap submittingCounters =
      prepareSubmitCounters(spfSolver_->getCounters());

  zmqMonitorClient_->setCounters(submittingCounters);
}

void
Decision::logRouteEvent(const std::string& event, const int numOfRoutes) {
  fbzmq::LogSample sample{};

  sample.addString("event", event);
  sample.addString("entity", "Decision");
  sample.addString("node_name", myNodeName_);
  sample.addInt("num_of_routes", numOfRoutes);

  zmqMonitorClient_->addEventLog(fbzmq::thrift::EventLog(
      apache::thrift::FRAGILE,
      Constants::kEventLogCategory,
      {sample.toJson()}));
}

void
Decision::logDebounceEvent(
    const int numUpdates, const std::chrono::milliseconds debounceTime) {
  fbzmq::LogSample sample{};

  sample.addString("event", "DECISION_DEBOUNCE");
  sample.addString("entity", "Decision");
  sample.addString("node_name", myNodeName_);
  sample.addInt("updates", numUpdates);
  sample.addInt("duration_ms", debounceTime.count());

  zmqMonitorClient_->addEventLog(fbzmq::thrift::EventLog(
      apache::thrift::FRAGILE,
      Constants::kEventLogCategory,
      {sample.toJson()}));
}

void
Decision::processPendingUpdates() {
  VLOG(1) << "Decision: processing " << pendingUpdates_.getCount()
          << " accumulated updates.";

  if (!pendingUpdates_.getCount()) {
    LOG(ERROR) << "Decision route computation triggered without any pending "
               << "updates.";
    return;
  }

  // Retrieve perf events, add debounce perf event, log information to
  // ZmqMonitor, ad and clear pending updates
  auto maybePerfEvents = pendingUpdates_.getPerfEvents();
  if (maybePerfEvents) {
    addPerfEvent(*maybePerfEvents, myNodeName_, "DECISION_DEBOUNCE");
    auto const& events = maybePerfEvents->events;
    auto const& eventsCnt = events.size();
    CHECK_LE(2, eventsCnt);
    auto duration = events[eventsCnt - 1].unixTs - events[eventsCnt - 2].unixTs;
    logDebounceEvent(
        pendingUpdates_.getCount(), std::chrono::milliseconds(duration));
  }
  pendingUpdates_.clear();

  // run SPF once for all updates received
  LOG(INFO) << "Decision: computing new paths.";
  auto routeDb = spfSolver_->buildMultiPaths(myNodeName_);
  logRouteEvent("ROUTE_CALC", routeDb.routes.size());
  if (maybePerfEvents) {
    addPerfEvent(*maybePerfEvents, myNodeName_, "DECISION_SPF");
  }
  routeDb.perfEvents = maybePerfEvents;

  // publish the new route state
  auto sendRc = decisionPub_.sendThriftObj(routeDb, serializer_);
  if (sendRc.hasError()) {
    LOG(ERROR) << "Error publishing new routing table: " << sendRc.error();
  }

  // update decision debounce flag
  expBackoff_.reportSuccess();
}

} // namespace openr


//chapter 9

#include <iostream>

#include <boost/type_traits/is_reference.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/is_pointer.hpp>
#include <boost/type_traits/is_float.hpp>
#include <boost/type_traits/alignment_of.hpp>
#include <boost/type_traits/remove_reference.hpp>
#include <boost/type_traits/remove_pointer.hpp>
#include <boost/type_traits/add_pointer.hpp>
#include <boost/type_traits/is_function.hpp>
#include <boost/type_traits/is_const.hpp>
#include <boost/static_assert.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/apply.hpp>

#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/not_equal_to.hpp>
#include <boost/mpl/greater.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/or.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/multiplies.hpp>

#include <boost/mpl/at.hpp>
#include <boost/mpl/next.hpp>
#include <boost/mpl/prior.hpp>
#include <boost/mpl/begin.hpp>
#include <boost/mpl/end.hpp>
#include <boost/mpl/advance.hpp>
#include <boost/mpl/push_front.hpp>
#include <boost/mpl/pop_front.hpp>
#include <boost/mpl/insert.hpp>
#include <boost/mpl/size.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/mpl/max_element.hpp>
#include <boost/mpl/min_element.hpp>
#include <boost/mpl/sizeof.hpp>
#include <boost/mpl/less.hpp>
#include <boost/mpl/copy.hpp>
#include <boost/mpl/print.hpp>
#include <boost/mpl/for_each.hpp>

using namespace boost::mpl::placeholders;
namespace mpl = boost::mpl;
using namespace boost::lambda;

#include <iterator>
#include <utility>
#include <list>
#include <map>
#include <vector>
#include <string>
#include <assert.h>
#include <algorithm>

struct none {};
template<class T>
struct wrapAssert
{
	BOOST_STATIC_ASSERT(0);
};
template<class T>
struct wrap {};

struct print_type
{
	template<class T>
	void operator() (wrap<T>)
	{
		std::cout << typeid(T).name() << std::endl;
	};
};

typedef mpl::vector<int, double, int&, char*> v;

namespace crtp
{
	template<class T>
	struct signed_number
	{
		friend T abs(T x)
		{
			return x < 0? -x : x;
		}
	};
}

class Float : crtp::signed_number<Float>
{
public:
	Float(float x) : value(x) {}
	Float operator-() const { return Float(-value); }
	bool operator<(float x) const { return value < x; }
	float value;
};

void main()
{
	mpl::for_each<v, wrap<mpl::_1> >(print_type());

	const Float pi = -3.1415f;
	const Float pi2 = abs(pi);
//	crtp::signed_number<Float>::abs(Float(10));

}


//cmake -DCPLEX_DIR=/Applications/CPLEX_Studio1210/cplex -DCMAKE_PREFIX_PATH=/Users/vikramansathiyanarayanan/Downloads/libtorch ..
//4.87283e+10 -> node 99
//data/open_\(50\,\ 50\)/10_1_uniform.yaml
#include <iostream>
#include <iomanip>
#include <cstdio>
#include <cstdlib>
#include <cuda.h>
#include <cuda_runtime_api.h>
#include <hybrid_BC/main.cuh>
#include <hybrid_BC/mapf.hpp>
#include <boost/program_options.hpp>

#include <yaml-cpp/yaml.h>

int main(int argc, char* argv[]) {

  namespace po = boost::program_options;
  // Declare the supported options.
  po::options_description desc("Allowed options");

  std::string inputFile;
  int k; //Independence detection

  desc.add_options()("help", "produce help message")(
      "input,i", po::value<std::string>(&inputFile)->required(),
      "Input file (YAML)")(
      "k", po::value<int>(&k)->default_value(0),
      "Approximation value");

  try {
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help") != 0u) {
      std::cout << desc << "\n";
      return 0;
    }
  } catch (po::error& e) {
    std::cerr << e.what() << std::endl << std::endl;
    std::cerr << desc << std::endl;
    return 1;
  }

  YAML::Node agents_config = YAML::LoadFile(inputFile);

  std::vector<std::pair<int, int> > obstacles;
  std::vector<std::pair<int, int> > goals;
  std::vector<std::pair<int, int> > starts;

  std::string mapFile = agents_config["map_path"].as<std::string>();

  YAML::Node map_config = YAML::LoadFile(mapFile);

  const auto& dim = map_config["dimensions"];
  int dimx = dim[0].as<int>();
  int dimy = dim[1].as<int>();

  for (const auto& node : map_config["obstacles"]) {
    obstacles.emplace_back(std::make_pair(node[0].as<int>(), node[1].as<int>()));
  }

  for (const auto& node : agents_config["agents"]) {
    const auto& start = node["start"];
    const auto& goal = node["goal"];
    starts.emplace_back(std::make_pair(start[0].as<int>(), start[1].as<int>()));
    // std::cout << "s: " << startStates.back() << std::endl;
    goals.emplace_back(std::make_pair(goal[0].as<int>(), goal[1].as<int>()));
  }

  mapf_adapters::mapf mapf(dimx, dimy, obstacles, goals);
  std::vector<std::pair<int, int> > edges = mapf.get_graph().get_edges();

  bool approx = false;
  if(k) approx = true;

  std::map<int, double> bc_scores = compute_bc(edges, k, approx, 0);

  std::cout<<bc_scores.size()<<std::endl;
  for(int i=0; i<mapf.get_x()*mapf.get_y(); i++){
    std::cout<<i<<" "<<bc_scores[i]<<std::endl;
  }

  std::cout<<std::endl;
  return 0;
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE COMPUTE

#include <boost/test/unit_test.hpp>

#include "api/umo.hpp"

#include <cmath>
#include <stdexcept>
#include <vector>

#define STRINGIFY(x) #x
#define TOSTRING(x) STRINGIFY(x)

using namespace umo;

BOOST_AUTO_TEST_CASE(SatifyBoolDec) {
    Model model;
    model.boolVar();
    model.boolVar();
    model.boolVar();
    BOOST_CHECK(model.getStatus() == Status::Valid);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyVarConstrainedPos) {
    Model model;
    BoolExpression x1 = model.boolVar();
    constraint(x1);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x1.getValue());
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyVarConstrainedNeg) {
    Model model;
    BoolExpression x1 = model.boolVar();
    constraint(!x1);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(!x1.getValue());
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedAnd1) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    constraint(x1 && x2);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x1.getValue() && x2.getValue());
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedAnd2) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    BoolExpression x3 = model.boolVar();
    BoolExpression x4 = model.boolVar();
    constraint(x1 && x2 && x3 && x4);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x1.getValue() && x2.getValue() && x3.getValue() && x4.getValue());
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedAnd3) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    BoolExpression x3 = model.boolVar();
    BoolExpression x4 = model.boolVar();
    BoolExpression x5 = model.boolVar();
    constraint(x1 && x2 && x3 && x4 && x5);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x1.getValue() && x2.getValue() && x3.getValue() && x4.getValue() && x5.getValue());
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedAnd4) {
    Model model;
    bool x1 = true;
    BoolExpression x2 = model.boolVar();
    bool x3 = true;
    BoolExpression x4 = model.boolVar();
    BoolExpression x5 = model.boolVar();
    constraint(x1 && x2 && x3 && x4 && x5);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x1 && x2.getValue() && x3 && x4.getValue() && x5.getValue());
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedOr1) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    constraint(x1 || x2);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x1.getValue() || x2.getValue());
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedOr2) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    BoolExpression x3 = model.boolVar();
    BoolExpression x4 = model.boolVar();
    constraint(x1 || x2 || x3 || x4);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x1.getValue() || x2.getValue() || x3.getValue() || x4.getValue());
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedOr3) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    BoolExpression x3 = model.boolVar();
    BoolExpression x4 = model.boolVar();
    BoolExpression x5 = model.boolVar();
    constraint(x1 || x2 || x3 || x4 || x5);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x1.getValue() || x2.getValue() || x3.getValue() || x4.getValue() || x5.getValue());
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedOr4) {
    Model model;
    bool x1 = false;
    BoolExpression x2 = model.boolVar();
    bool x3 = false;
    BoolExpression x4 = model.boolVar();
    BoolExpression x5 = model.boolVar();
    constraint(x1 || x2 || x3 || x4 || x5);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x1 || x2.getValue() || x3 || x4.getValue() || x5.getValue());
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedXor1) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    constraint(x1 ^ x2);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x1.getValue() ^ x2.getValue());
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedXor2) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    constraint(!(x1 ^ x2));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(!(x1.getValue() ^ x2.getValue()));
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedXor3) {
    Model model;
    BoolExpression x1 = model.boolVar();
    constraint(model.logical_xor({x1}));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x1.getValue());
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedXor4) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    BoolExpression x3 = model.boolVar();
    BoolExpression x4 = model.boolVar();
    constraint(model.logical_xor({x1, x2, x3, x4}));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x1.getValue() ^ x2.getValue() ^ x3.getValue() ^ x4.getValue());
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedXor5) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    BoolExpression x3 = model.boolVar();
    BoolExpression x4 = model.boolVar();
    constraint(x1 ^ x2 ^ x3 ^ x4);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x1.getValue() ^ x2.getValue() ^ x3.getValue() ^ x4.getValue());
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedXor6) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    BoolExpression x3 = model.boolVar();
    BoolExpression x4 = model.boolVar();
    BoolExpression x5 = model.boolVar();
    constraint(x1 ^ x2 ^ x3 ^ x4 ^ x5);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x1.getValue() ^ x2.getValue() ^ x3.getValue() ^ x4.getValue() ^ x5.getValue());
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedXor7) {
    Model model;
    bool x1 = true;
    BoolExpression x2 = model.boolVar();
    bool x3 = false;
    BoolExpression x4 = model.boolVar();
    BoolExpression x5 = model.boolVar();
    constraint(x1 ^ x2 ^ x3 ^ x4 ^ x5);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x1 ^ x2.getValue() ^ x3 ^ x4.getValue() ^ x5.getValue());
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedXor8) {
    Model model;
    bool x1 = false;
    BoolExpression x2 = model.boolVar();
    bool x3 = true;
    BoolExpression x4 = model.boolVar();
    BoolExpression x5 = model.boolVar();
    constraint(x1 ^ x2 ^ x3 ^ x4 ^ x5);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x1 ^ x2.getValue() ^ x3 ^ x4.getValue() ^ x5.getValue());
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedAndNot) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    constraint(!x1 && !x2);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(!x1.getValue() && !x2.getValue());
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedOrNot) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    constraint(!x1 || !x2);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(!x1.getValue() || !x2.getValue());
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedNand1) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    constraint(!(x1 && x2));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(!(x1.getValue() && x2.getValue()));
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedNand2) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    BoolExpression x3 = model.boolVar();
    BoolExpression x4 = model.boolVar();
    BoolExpression x5 = model.boolVar();
    constraint(!(x1 && x2 && x3 && x4 && x5));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(!(x1.getValue() && x2.getValue() && x3.getValue() && x4.getValue() && x5.getValue()));
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedNor1) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    constraint(!(x1 || x2));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(!(x1.getValue() || x2.getValue()));
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyConstrainedNor2) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    BoolExpression x3 = model.boolVar();
    BoolExpression x4 = model.boolVar();
    BoolExpression x5 = model.boolVar();
    constraint(!(x1 || x2 || x3 || x4 || x5));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(!(x1.getValue() || x2.getValue() || x3.getValue() || x4.getValue() || x5.getValue()));
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyAnd) {
    Model model;
    model.boolVar() && model.boolVar();
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
}

BOOST_AUTO_TEST_CASE(SatifyAndNot) {
    Model model;
    !model.boolVar() && !model.boolVar();
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyOr) {
    Model model;
    model.boolVar() || model.boolVar();
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyOrNot) {
    Model model;
    !model.boolVar() || !model.boolVar();
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyAndOr) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    BoolExpression x3 = model.boolVar();
    BoolExpression x4 = model.boolVar();
    constraint((x1 || x2) && (x3 || x4));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK((x1.getValue() || x2.getValue()) && (x3.getValue() || x4.getValue()));
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyOrAnd) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    BoolExpression x3 = model.boolVar();
    BoolExpression x4 = model.boolVar();
    constraint((x1 && x2) || (x3 && x4));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK((x1.getValue() && x2.getValue()) || (x3.getValue() && x4.getValue()));
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}

BOOST_AUTO_TEST_CASE(SatifyBigOr) {
    int nVars = 1000;
    Model model;
    std::vector<BoolExpression> vec;
    for (int i = 0; i < nVars; ++i) {
        vec.push_back(model.boolVar());
    }
    constraint(umo::logical_or(vec));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(model.getStatus() == Status::Optimal);
    int cnt = 0;
    for (BoolExpression x : vec) {
        cnt += x.getValue();
    }
    BOOST_CHECK(cnt >= 1);
}

BOOST_AUTO_TEST_CASE(SatifyBigAnd) {
    int nVars = 1000;
    Model model;
    std::vector<BoolExpression> vec;
    for (int i = 0; i < nVars; ++i) {
        vec.push_back(model.boolVar());
    }
    constraint(umo::logical_and(vec));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(model.getStatus() == Status::Optimal);
    int cnt = 0;
    for (BoolExpression x : vec) {
        cnt += x.getValue();
    }
    BOOST_CHECK_EQUAL(cnt, nVars);
}

BOOST_AUTO_TEST_CASE(SatifyBigNor) {
    int nVars = 1000;
    Model model;
    std::vector<BoolExpression> vec;
    for (int i = 0; i < nVars; ++i) {
        vec.push_back(model.boolVar());
    }
    constraint(!umo::logical_or(vec));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(model.getStatus() == Status::Optimal);
    int cnt = 0;
    for (BoolExpression x : vec) {
        cnt += x.getValue();
    }
    BOOST_CHECK_EQUAL(cnt, 0);
}

BOOST_AUTO_TEST_CASE(SatifyBigNand) {
    int nVars = 1000;
    Model model;
    std::vector<BoolExpression> vec;
    for (int i = 0; i < nVars; ++i) {
        vec.push_back(model.boolVar());
    }
    constraint(!umo::logical_and(vec));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(model.getStatus() == Status::Optimal);
    int cnt = 0;
    for (BoolExpression x : vec) {
        cnt += x.getValue();
    }
    BOOST_CHECK(cnt < nVars);
}

BOOST_AUTO_TEST_CASE(SatifyUnsat) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    constraint(x1 && x2);
    constraint(!(x1 && x2));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Infeasible);
}

BOOST_AUTO_TEST_CASE(SatifyWithTimeLimit) {
    Model model;
    BoolExpression x1 = model.boolVar();
    constraint(x1);
    model.setTimeLimit(2.0);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x1.getValue());
    BOOST_CHECK(model.getStatus() == Status::Optimal);
}


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

/// @version 0.1
/// @author  Xinyan DAI
/// @contact xinyan.dai@outlook.com
//////////////////////////////////////////////////////////////////////////////

#pragma once

#include <utility>
#include <vector>
#include <random>
#include <unordered_map>
#include <boost/progress.hpp>

#include "../index.hpp"

namespace ss {

    using std::vector;

    template<class DataType, class IndexType>
    class ForestIndex : public Index<DataType> {
    protected:
        vector<IndexType* > _indices;
    public:
        explicit ForestIndex(const parameter &para) :
                Index<DataType>(para), _indices(para.forest_size) {

            for (int i = 0; i < this->_para.forest_size; ++i) {
                _indices[i] = new IndexType(para);
            }
        }

        void Train(const Matrix<DataType> &data) override {
            for (int i = 0; i < this->_para.forest_size; ++i) {
                _indices[i]->Train(data);
            }
        }

        void Add(const Matrix<DataType> &data) override {
            for (int i = 0; i < this->_para.forest_size; ++i) {
                _indices[i]->Add(data);
            }
        }

        void Search(const DataType *query, const std::function<void (int)>& prober) override {
            for (int i = 0; i < this->_para.forest_size; ++i) {
                _indices[i]->Search(query, prober);
            }
        }
    };

} // namespace ss


#include <iostream>
#include <fstream>
#include <string>
#include <sstream>
#include <limits>
#include <stdio.h>
#include <opencv2/opencv.hpp>

#include <v4r/io/eigen.h>
#include <v4r/io/filesystem.h>
#include <v4r/registration/noise_model_based_cloud_integration.h>
#include <v4r/common/noise_models.h>
#include <v4r/common/normals.h>
#include <v4r/common/pcl_opencv.h>

#include <pcl/common/transforms.h>
#include <pcl/common/time.h>
#include <pcl/filters/passthrough.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>
#include <boost/format.hpp>
#include <boost/program_options.hpp>
#include <glog/logging.h>

namespace po = boost::program_options;
namespace bf = boost::filesystem;
using namespace v4r;

int main(int argc, const char * argv[]) {
    typedef pcl::PointXYZRGB PointT;
    std::string test_dir, out_dir, view_prefix = "cloud_", obj_indices_prefix = "object_indices_", pose_prefix = "pose_";
    float chop_z = std::numeric_limits<float>::max();
    bool visualize=false, use_object_mask = false, use_pose_file = false, debug = false;

    google::InitGoogleLogging(argv[0]);

    NMBasedCloudIntegrationParameter nm_int_param;
    nm_int_param.min_points_per_voxel_ = 1;
    nm_int_param.octree_resolution_ = 0.002f;

    NguyenNoiseModelParameter nm_param;

    int normal_method = 2;

    po::options_description desc("Noise model based cloud integration\n======================================\n**Allowed options");
    desc.add_options()
            ("help,h", "produce help message")
            ("input_dir,i", po::value<std::string>(&test_dir)->required(), "directory containing point clouds")
            ("out_dir,o", po::value<std::string>(&out_dir), "output directory where the registered cloud will be stored. If not set, nothing will be written to distk")
            ("view_prefix", po::value<std::string>(&view_prefix)->default_value(view_prefix), "view filename prefix for each point cloud (used when using object mask)")
            ("obj_indices_prefix", po::value<std::string>(&obj_indices_prefix)->default_value(obj_indices_prefix), "filename prefix for each object mask file(used when using object mask)")
            ("pose_prefix", po::value<std::string>(&pose_prefix)->default_value(pose_prefix), "filename prefix for each camera pose (used when using use_pose_file)")
            ("resolution,r", po::value<float>(&nm_int_param.octree_resolution_)->default_value(nm_int_param.octree_resolution_), "")
            ("min_points_per_voxel", po::value<size_t>(&nm_int_param.min_points_per_voxel_)->default_value(nm_int_param.min_points_per_voxel_), "")
//            ("threshold_explained", po::value<float>(&nm_int_param.threshold_explained_)->default_value(nm_int_param.threshold_explained_), "")
            ("use_depth_edges", po::value<bool>(&nm_param.use_depth_edges_)->default_value(nm_param.use_depth_edges_), "")
            ("focal_length,f", po::value<float>(&nm_param.focal_length_)->default_value(nm_param.focal_length_), "")
            ("normal_method,n", po::value<int>(&normal_method)->default_value(normal_method), "method used for normal computation")
            ("chop_z,z", po::value<float>(&chop_z)->default_value(chop_z), "cut of distance in m ")
            ("visualize,v", po::bool_switch(&visualize), "turn visualization on")
            ("use_object_mask,m", po::bool_switch(&use_object_mask), "reads mask file and only extracts those indices (only if file exists)")
            ("use_pose_file,p", po::bool_switch(&use_pose_file), "reads pose from seperate pose file instead of extracting it directly from .pcd file (only if file exists)")
            ("debug,d", po::bool_switch(&debug), "saves debug information (e.g. point properties) if output dir is set")
      ;
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help"))
    {
        std::cout << desc << std::endl;
        return false;
    }

    try
    { po::notify(vm); }
    catch(std::exception& e)
    {
        std::cerr << "Error: " << e.what() << std::endl << std::endl << desc << std::endl;
        return false;
    }

    std::vector< std::string> folder_names  = io::getFoldersInDirectory( test_dir );

    if( folder_names.empty() )
        folder_names.push_back("");

    int vp1, vp2;
    boost::shared_ptr<pcl::visualization::PCLVisualizer> vis;

    for (const std::string &sub_folder : folder_names)
    {
        const std::string test_seq = test_dir + "/" + sub_folder;
        std::vector< std::string > views = io::getFilesInDirectory(test_seq, ".*.pcd", false);

        pcl::PointCloud<PointT>::Ptr big_cloud_unfiltered (new pcl::PointCloud<PointT>);
        std::vector< pcl::PointCloud<PointT>::ConstPtr > clouds (views.size());
        std::vector< pcl::PointCloud<pcl::Normal>::ConstPtr > normals (views.size());
        std::vector<std::vector<std::vector<float> > > pt_properties (views.size());
        std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > camera_transforms (views.size());
        std::vector<std::vector<int> > obj_indices (views.size());

        for (size_t v_id=0; v_id<views.size(); v_id++)
        {
            std::stringstream txt;
            txt << "processing view " << v_id;
            pcl::ScopeTime t(txt.str().c_str());
            pcl::PointCloud<PointT>::Ptr cloud( new pcl::PointCloud<PointT>);
            pcl::PointCloud<pcl::Normal>::Ptr normal ( new pcl::PointCloud<pcl::Normal>);

            pcl::io::loadPCDFile(test_seq + "/" + views[ v_id ], *cloud);

            std::string obj_fn (views[v_id]);
            boost::replace_first (obj_fn, view_prefix, obj_indices_prefix);
            boost::replace_last (obj_fn, ".pcd", ".txt");

            if(use_object_mask)
            {
                bf::path object_indices_fn = test_seq;
                object_indices_fn /= obj_fn;
                if( io::existsFile( object_indices_fn.string() ) )
                {
                    ifstream f((test_seq+"/"+obj_fn).c_str());
                    int idx;
                    while (f >> idx)
                        obj_indices[v_id].push_back(idx);
                    f.close();
                }
                else
                    std::cerr << "Parameter use_object_mask is set but object indices file " <<
                                 object_indices_fn.string() << " not found! " << std::endl;
            }

            std::string pose_fn (views[v_id]);
            boost::replace_first (pose_fn, view_prefix, pose_prefix);
            boost::replace_last (pose_fn, ".pcd", ".txt");

            if(io::existsFile(test_seq + "/" + pose_fn) && use_pose_file)
                camera_transforms[v_id] = io::readMatrixFromFile(test_seq + "/" + pose_fn);
            else
                camera_transforms[v_id] = RotTrans2Mat4f(cloud->sensor_orientation_, cloud->sensor_origin_);

            // reset view point otherwise pcl visualization is potentially messed up
            cloud->sensor_orientation_ = Eigen::Quaternionf::Identity();
            cloud->sensor_origin_ = Eigen::Vector4f::Zero();

            pcl::PassThrough<PointT> pass;
            pass.setInputCloud (cloud);
            pass.setFilterFieldName ("z");
            pass.setFilterLimits (0.f, chop_z);
            pass.setKeepOrganized(true);
            pass.filter (*cloud);

            {
                pcl::ScopeTime tt("Computing normals");
                computeNormals<PointT>( cloud, normal, normal_method);
            }

            {
                pcl::ScopeTime tt("Computing noise model parameter for cloud");
                NguyenNoiseModel<PointT> nm (nm_param);
                nm.setInputCloud(cloud);
                nm.setInputNormals(normal);
                nm.compute();
                pt_properties[v_id] = nm.getPointProperties();
            }

            pcl::PointCloud<PointT> object_cloud, object_aligned;
            if( use_object_mask )
                pcl::copyPointCloud(*cloud, obj_indices[v_id], object_cloud);
            else
                pcl::copyPointCloud(*cloud, object_cloud);

            pcl::transformPointCloud( object_cloud, object_aligned, camera_transforms[v_id]);
            *big_cloud_unfiltered += object_aligned;

            clouds[v_id] = cloud;
            normals[v_id] = normal;
        }

        pcl::PointCloud<PointT>::Ptr octree_cloud(new pcl::PointCloud<PointT>);
        NMBasedCloudIntegration<PointT> nmIntegration (nm_int_param);
        nmIntegration.setInputClouds(clouds);
        nmIntegration.setPointProperties(pt_properties);
        nmIntegration.setTransformations(camera_transforms);
        nmIntegration.setInputNormals(normals);
        nmIntegration.setIndices(obj_indices);
        nmIntegration.compute(octree_cloud);
        std::vector< pcl::PointCloud<PointT>::Ptr > clouds_used;
        nmIntegration.getInputCloudsUsed(clouds_used);

        std::cout << "Size cloud unfiltered: " << big_cloud_unfiltered->points.size() << ", filtered: " << octree_cloud->points.size() << std::endl;

        if(vm.count("out_dir"))
        {
            const std::string out_path = out_dir + "/" + sub_folder;
            io::createDirIfNotExist(out_path);
            pcl::io::savePCDFileBinary(out_path + "/registered_cloud_filtered.pcd", *octree_cloud);
            pcl::io::savePCDFileBinary(out_path + "/registered_cloud_unfiltered.pcd", *big_cloud_unfiltered);

            for(size_t v_id=0; v_id<clouds_used.size(); v_id++)
            {
                if(debug)
                {
                    std::stringstream fn; fn << out_path << "/filtered_input_" << setfill('0') << setw(5) << v_id << ".pcd";
                    pcl::io::savePCDFileBinary(fn.str(), *clouds_used[v_id]);

                    fn.str(""); fn << out_path << "/distance_to_edge_px_" << setfill('0') << setw(5) << v_id << ".txt";
                    std::ofstream f (fn.str());
                    for (size_t pt_id = 0; pt_id < clouds_used[v_id]->points.size (); pt_id++) {
                        f << pt_properties[v_id][pt_id][2] << std::endl;

                        PointT &pt_tmp = clouds_used[v_id]->points[pt_id];
                        if( !pcl::isFinite( pt_tmp ) )  // set background color for nan points
                            pt_tmp.r = pt_tmp.g = pt_tmp.b = 255.f;
                    }
                    f.close();

                    fn.str(""); fn << out_path << "/filter_input_image" << setfill('0') << setw(5) << v_id << ".png";
                    PCLOpenCVConverter<PointT> pcl_opencv_converter;
                    pcl_opencv_converter.setInputCloud( clouds_used[v_id] );
                    cv::imwrite(fn.str(), pcl_opencv_converter.getRGBImage() );
                }
            }
        }

        if(visualize)
        {
            if(!vis)
            {
                vis.reset( new pcl::visualization::PCLVisualizer("registered cloud") );
                vis->createViewPort(0,0,0.5,1,vp1);
                    vis->createViewPort(0.5,0,1,1,vp2);
            }
            vis->removeAllPointClouds(vp1);
            vis->removeAllPointClouds(vp2);
            vis->addPointCloud(big_cloud_unfiltered, "unfiltered_cloud", vp1);
            vis->addPointCloud(octree_cloud, "filtered_cloud", vp2);
            vis->spin();
        }
    }
}


/* -*-c++-*--------------------------------------------------------------------
 * 2019 Bernd Pfrommer bernd.pfrommer@gmail.com
 */

#include "tagslam/factor/coordinate.h"
#include "tagslam/graph.h"
#include "tagslam/body.h"
#include "tagslam/logging.h"
#include "tagslam/xml.h"
#include <geometry_msgs/Point.h>
#include <XmlRpcException.h>
#include <string>
#include <sstream>
#include <boost/range/irange.hpp>

namespace tagslam {
  namespace factor {
    using boost::irange;

    Coordinate::Coordinate(double len, double noise, const Point3d &direction,
                           const int corn, const TagConstPtr &tag,
                           const string &name) :
      Factor(name, ros::Time(0)),
      length_(len), noise_(noise),
      direction_(direction), corner_(corn),
      tag_(tag) {
    }

    VertexDesc Coordinate::addToGraph(const VertexPtr &vp, Graph *g) const {
      const ros::Time t0 = ros::Time(0);
      const VertexDesc vtp = g->findTagPose(getTag()->getId());
      checkIfValid(vtp, "no tag pose found");
      const VertexDesc vbp = g->findBodyPose(t0,
                                             getTag()->getBody()->getName());
      checkIfValid(vbp, "no body pose found");
      const VertexDesc fv = g->insertFactor(vp);
      g->addEdge(fv, vbp, 0);
      g->addEdge(fv, vtp, 1);
      return (fv);
    }

    void Coordinate::addToOptimizer(Graph *g) const {
      const VertexDesc v = g->find(this);
      checkIfValid(v, "factor not found");
      const std::vector<ValueKey> optKeys = g->getOptKeysForFactor(v, 2);
      const FactorKey fk = g->getOptimizer()->addCoordinateMeasurement(
        getLength(), getNoise(),
        getDirection(), getCorner(),
        optKeys[0] /* T_w_b */, optKeys[1] /* T_b_o */);
      g->markAsOptimized(v, fk);
    }

    double Coordinate::coordinate(
      const Transform &T_w_b, const Transform &T_b_o) const {
      const auto X = T_w_b * T_b_o * getCorner();
      return (direction_.dot(X));
    }

    const Eigen::Vector3d Coordinate::getCorner() const {
      return (tag_->getObjectCorner(corner_));
    }

    CoordinateFactorPtr
    Coordinate::parse(const string &name, XmlRpc::XmlRpcValue meas,
                    TagFactory *tagFactory) {
      int tag(-1), c(-1);
      double len(-1e10), noise(-1);
      Eigen::Vector3d dir(0.0, 0.0, 0.0);
      try {
        tag   = xml::parse<int>(meas, "tag");
        c     = xml::parse<int>(meas, "corner");
        len   = xml::parse<double>(meas, "length");
        noise = xml::parse<double>(meas, "noise");
        dir   = make_point(
          xml::parse_container<std::vector<double>>(meas, "direction"));
      } catch (const XmlRpc::XmlRpcException &e) {
        BOMB_OUT("error parsing measurement: " << name);
      }
      if (std::abs(dir.norm() - 1.0) > 1e-5) {
        BOMB_OUT("measurement " + name + " has non-unit direction");
      }
      CoordinateFactorPtr fp;
      if (tag >= 0 && c >= 0 && len > -1e10 && noise > 0) {
        TagConstPtr tagPtr = tagFactory->findTag(tag);
        if (!tagPtr) {
          ROS_WARN_STREAM("ignoring unknown measured tag: " << name);
          return CoordinateFactorPtr();
        }
        fp.reset(new factor::Coordinate(len, noise, dir, c, tagPtr,
                                        name));
      } else {
        BOMB_OUT("coordinate measurement incomplete: " << name);
      }
      return (fp);
    }

    CoordinateFactorPtrVec
    Coordinate::parse(XmlRpc::XmlRpcValue meas, TagFactory *tagFactory) {
      CoordinateFactorPtrVec fv;
      if (meas.getType() != XmlRpc::XmlRpcValue::TypeArray) {
        BOMB_OUT("invalid node type for coordinate measurements!");
      }
      for (const auto i: irange(0, meas.size())) {
        if (meas[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) continue;
        for (XmlRpc::XmlRpcValue::iterator it = meas[i].begin();
             it != meas[i].end(); ++it) {
          if (it->second.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
            continue;
          }
          CoordinateFactorPtr d = parse(it->first, it->second, tagFactory);
          if (d) {
            fv.push_back(d);
          }
        }
      }
      return (fv);
    }

    string Coordinate::getLabel() const {
      std::stringstream ss;
      ss << name_;
      return (ss.str());
    }
    // static function!
    double Coordinate::getOptimized(const VertexDesc &v, const Graph &g) {
      if (!g.isOptimized(v)) {
        return (-1.0); // not optimized yet!
      }
      const auto p = std::dynamic_pointer_cast<const factor::Coordinate>(g[v]);
      if (!p) {
        BOMB_OUT("vertex is not coord: " << *g[v]);
      }
      const std::vector<ValueKey> optKeys = g.getOptKeysForFactor(v, 2);
      const auto opt = g.getOptimizer();
      const double l = p->coordinate(opt->getPose(optKeys[0]),
                                     opt->getPose(optKeys[1]));
      return (l);
    }
  } // namespace factor
}  // namespace tagslam