/**
 * 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 <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <gtest/gtest.h>

#include <folly/Random.h>
#include <folly/gen/Base.h>
#include <folly/io/async/EventBase.h>
#include <openr/dual/Dual.h>

#include <vector>

using namespace openr;
using namespace folly;

// sync time-ms to wait for all DUAL nodes to converge
const std::chrono::milliseconds syncms{500};

// I/O response delay to mimic real senario
// e.g one link-up-event might result in different ack time on each end.
const uint32_t kIoDelayBaseMs{5};
const uint32_t kIoDelayVarMs{5};

struct Vertex {
  std::string name;
  bool up = true;
};

using Weight = int64_t;
struct Edge {
  std::string name1;
  std::string name2;
  Weight weight;
  bool up = true;
};

using Graph = boost::adjacency_list<
    boost::setS,
    boost::vecS,
    boost::undirectedS,
    Vertex,
    boost::property<boost::edge_weight_t, Weight>>;
using VertexDescriptor = boost::graph_traits<Graph>::vertex_descriptor;
using EdgeProperty = boost::property<boost::edge_weight_t, Weight>;
using IndexMap = boost::property_map<Graph, boost::vertex_index_t>::type;
using PredecessorMap = boost::iterator_property_map<
    VertexDescriptor*,
    IndexMap,
    VertexDescriptor,
    VertexDescriptor&>;
using DistanceMap =
    boost::iterator_property_map<Weight*, IndexMap, Weight, Weight&>;

using StatusStrings =
    std::pair<std::string, std::unordered_map<std::string, std::string>>;

// handy funtion to return a random delay in ms
uint32_t
randomDelayMs() {
  return kIoDelayBaseMs + folly::Random::rand32() % kIoDelayVarMs;
}

/**
 * check if a given graph is a spanning tree or not
 * - check all nodes are connected
 * - #edges == #nodes - 1
 */
bool
isSpt(const Graph& g) {
  auto numVertices = boost::num_vertices(g);
  if (numVertices < 1) {
    LOG(ERROR) << "numVertices: " << numVertices;
    return false;
  }
  auto numEdges = boost::num_edges(g);
  if (numEdges != numVertices - 1) {
    LOG(ERROR) << "numVertices: " << numVertices << ", numEdges: " << numEdges;
    return false;
  }

  std::vector<int> components(numVertices);
  auto numComponents = boost::connected_components(g, components.data());
  if (numComponents != 1) {
    LOG(ERROR) << "not a connected graph, num of components: " << numComponents;
    return false;
  }
  return true;
}

// validate route-info on a given dual node
// this ensures
// - node is in passive state
// - if distance is inf, then nexthop has to be none, and vice versa
bool
validateRouteInfo(const Dual::RouteInfo& info) {
  if (info.sm.state != DualState::PASSIVE) {
    LOG(ERROR) << "not in PASSIVE state";
    return false;
  }
  if (not info.nexthop.has_value()) {
    if (info.distance != std::numeric_limits<int64_t>::max()) {
      LOG(ERROR) << "nexthop: none but distance != intf";
      return false;
    }
  }
  if (info.distance == std::numeric_limits<int64_t>::max()) {
    if (info.nexthop.has_value()) {
      LOG(ERROR) << "distance: inf but nexthop: " << *info.nexthop;
      return false;
    }
  }
  return true;
}

// Test basic state machine logic to ensure state is transitioned properly
TEST(Dual, StateMachine) {
  DualStateMachine sm;
  EXPECT_EQ(sm.state, DualState::PASSIVE);

  /**
   * passive state
   */
  // meet fc -> no state change
  sm.state = DualState::PASSIVE;
  sm.processEvent(DualEvent::QUERY_FROM_SUCCESSOR);
  EXPECT_EQ(sm.state, DualState::PASSIVE);

  // not meet fc
  sm.state = DualState::PASSIVE;
  sm.processEvent(DualEvent::QUERY_FROM_SUCCESSOR, false);
  EXPECT_EQ(sm.state, DualState::ACTIVE3);

  sm.state = DualState::PASSIVE;
  sm.processEvent(DualEvent::OTHERS, false);
  EXPECT_EQ(sm.state, DualState::ACTIVE1);

  sm.state = DualState::PASSIVE;
  sm.processEvent(DualEvent::INCREASE_D, false);
  EXPECT_EQ(sm.state, DualState::ACTIVE1);

  /**
   * active0 state
   */
  sm.state = DualState::ACTIVE0;
  sm.processEvent(DualEvent::OTHERS);
  EXPECT_EQ(sm.state, DualState::ACTIVE0);

  sm.state = DualState::ACTIVE0;
  sm.processEvent(DualEvent::LAST_REPLY, true);
  EXPECT_EQ(sm.state, DualState::PASSIVE);

  sm.state = DualState::ACTIVE0;
  sm.processEvent(DualEvent::LAST_REPLY, false);
  EXPECT_EQ(sm.state, DualState::ACTIVE2);

  sm.state = DualState::ACTIVE0;
  sm.processEvent(DualEvent::QUERY_FROM_SUCCESSOR);
  EXPECT_EQ(sm.state, DualState::ACTIVE0);

  /**
   * active1 state
   */
  sm.state = DualState::ACTIVE1;
  sm.processEvent(DualEvent::INCREASE_D);
  EXPECT_EQ(sm.state, DualState::ACTIVE0);

  sm.state = DualState::ACTIVE1;
  sm.processEvent(DualEvent::LAST_REPLY);
  EXPECT_EQ(sm.state, DualState::PASSIVE);

  sm.state = DualState::ACTIVE1;
  sm.processEvent(DualEvent::QUERY_FROM_SUCCESSOR);
  EXPECT_EQ(sm.state, DualState::ACTIVE2);

  sm.state = DualState::ACTIVE1;
  sm.processEvent(DualEvent::OTHERS);
  EXPECT_EQ(sm.state, DualState::ACTIVE1);

  /**
   * active2 state
   */
  sm.state = DualState::ACTIVE2;
  sm.processEvent(DualEvent::OTHERS);
  EXPECT_EQ(sm.state, DualState::ACTIVE2);

  sm.state = DualState::ACTIVE2;
  sm.processEvent(DualEvent::LAST_REPLY, true);
  EXPECT_EQ(sm.state, DualState::PASSIVE);

  sm.state = DualState::ACTIVE2;
  sm.processEvent(DualEvent::LAST_REPLY, false);
  EXPECT_EQ(sm.state, DualState::ACTIVE3);

  sm.state = DualState::ACTIVE2;
  sm.processEvent(DualEvent::QUERY_FROM_SUCCESSOR);
  EXPECT_EQ(sm.state, DualState::ACTIVE2);

  /**
   * active3 state
   */
  sm.state = DualState::ACTIVE3;
  sm.processEvent(DualEvent::LAST_REPLY);
  EXPECT_EQ(sm.state, DualState::PASSIVE);

  sm.state = DualState::ACTIVE3;
  sm.processEvent(DualEvent::INCREASE_D);
  EXPECT_EQ(sm.state, DualState::ACTIVE2);

  sm.state = DualState::ACTIVE3;
  sm.processEvent(DualEvent::OTHERS);
  EXPECT_EQ(sm.state, DualState::ACTIVE3);

  sm.state = DualState::ACTIVE3;
  sm.processEvent(DualEvent::QUERY_FROM_SUCCESSOR);
  EXPECT_EQ(sm.state, DualState::ACTIVE3);
}

// Dual Implementation Test Node
class DualTestNode final : public DualNode {
 public:
  DualTestNode(
      const std::string& nodeId,
      bool isRoot,
      std::shared_ptr<folly::EventBase> evb,
      std::map<std::string, std::shared_ptr<DualTestNode>>& nodes)
      : DualNode(nodeId, isRoot), evb_(std::move(evb)), nodes_(nodes) {}

  bool
  sendDualMessages(
      const std::string& neighbor,
      const thrift::DualMessages& msgs) noexcept override {
    auto& otherNode = nodes_.at(neighbor);
    CHECK(neighborUp(neighbor))
        << nodeId << ": sending dual msgs to down neighbor " << neighbor;
    CHECK(msgs.messages_ref()->size()) << " send empty messages";

    evb_->runInEventBaseThread(
        [&, otherNode, msgs]() { otherNode->processDualMessages(msgs); });

    return true;
  }

  void
  processNexthopChange(
      const std::string& rootId,
      const std::optional<std::string>& oldNh,
      const std::optional<std::string>& newNh) noexcept override {
    std::string oldNhStr = oldNh.has_value() ? *oldNh : "none";
    std::string newNhStr = newNh.has_value() ? *newNh : "none";
    VLOG(1) << "node: " << nodeId << " at root: " << rootId << " nexthop"
            << " change " << oldNhStr << " -> " << newNhStr;
    return;
  }

  // event base loop
  std::shared_ptr<folly::EventBase> evb_;
  // reference to map<node-id: DualTestNode*>
  std::map<std::string, std::shared_ptr<DualTestNode>>& nodes_;
};

// Dual test fixture
class DualBaseFixture : public ::testing::Test {
 protected:
  void
  SetUp() override {
    evb = std::make_shared<folly::EventBase>();
    thread = std::thread([&]() {
      VLOG(1) << "evb starting";
      evb->loopForever();
      VLOG(1) << "evb stopped";
    });
  }

  void
  TearDown() override {
    evb->terminateLoopSoon();
    thread.join();
    VLOG(1) << "evb thread stopped";
  }

  void
  addNode(const std::string& nodeId, bool isRoot) {
    auto node = std::make_shared<DualTestNode>(nodeId, isRoot, evb, nodes);
    nodes.emplace(nodeId, node);
    vertices.emplace_back(Vertex{nodeId, true});
    if (isRoot) {
      VLOG(1) << "add root " << nodeId;
      rootIds.emplace_back(nodeId);
    }
  }

  void
  addLink(const std::string& node1, const std::string& node2, int64_t cost) {
    for (const auto& edge : edges) {
      if ((edge.name1 == node1 and edge.name2 == node2) or
          (edge.name1 == node2 and edge.name2 == node1)) {
        return; // edge exist already
      }
    }
    edges.emplace_back(Edge{node1, node2, cost, false});
    peerUp(node1, node2, cost);
  }

  // trigger a link up event on both ends with random delay
  void
  peerUp(const std::string& node1, const std::string& node2, int64_t cost) {
    for (auto& edge : edges) {
      if ((edge.name1 == node1 and edge.name2 == node2) or
          (edge.name1 == node2 and edge.name2 == node1)) {
        if (edge.up) {
          // link already up
          return;
        }
        edge.up = true;
        break;
      }
    }
    evb->runInEventBaseThread([&, node1, node2, cost]() {
      evb->runAfterDelay(
          [&, node1, node2, cost]() { nodes.at(node1)->peerUp(node2, cost); },
          randomDelayMs());
      evb->runAfterDelay(
          [&, node1, node2, cost]() { nodes.at(node2)->peerUp(node1, cost); },
          randomDelayMs());
    });
  }

  // trigger a link down event on both ends with random delay
  void
  peerDown(const std::string& node1, const std::string& node2) {
    for (auto& edge : edges) {
      if ((edge.name1 == node1 and edge.name2 == node2) or
          (edge.name1 == node2 and edge.name2 == node1)) {
        if (not edge.up) {
          // link already down
          return;
        }
        edge.up = false;
        break;
      }
    }
    evb->runInEventBaseThread([&, node1, node2]() {
      evb->runAfterDelay(
          [&, node1, node2]() { nodes.at(node1)->peerDown(node2); },
          randomDelayMs());
      evb->runAfterDelay(
          [&, node1, node2]() { nodes.at(node2)->peerDown(node1); },
          randomDelayMs());
    });
  }

  // flap a link between node1 and node2
  void
  linkFlap(const std::string& node1, const std::string& node2, int64_t cost) {
    for (auto& edge : edges) {
      if ((edge.name1 == node1 and edge.name2 == node2) or
          (edge.name1 == node2 and edge.name2 == node1)) {
        if (not edge.up) {
          // link already down
          return;
        }
        break;
      }
    }
    evb->runInEventBaseThread([&, node1, node2, cost]() {
      // bring link down on both ends
      evb->runAfterDelay(
          [&, node1, node2]() { nodes.at(node1)->peerDown(node2); }, 0);
      evb->runAfterDelay(
          [&, node1, node2]() { nodes.at(node2)->peerDown(node1); }, 0);

      // bring them back up
      // Make sure link-up event is scheduled AFTER link-down event
      evb->runAfterDelay(
          [&, node1, node2, cost]() { nodes.at(node1)->peerUp(node2, cost); },
          randomDelayMs());
      evb->runAfterDelay(
          [&, node1, node2, cost]() { nodes.at(node2)->peerUp(node1, cost); },
          randomDelayMs());
    });
  }

  // trigger a node down event, all associated links will be brought down
  void
  nodeDown(const std::string& node) {
    for (auto& vertex : vertices) {
      if (vertex.name == node) {
        vertex.up = false;
        break;
      }
    }
    // bring corresponding edges down
    for (auto& edge : edges) {
      if (edge.name1 != node and edge.name2 != node) {
        continue; // link not affected
      }
      if (not edge.up) {
        continue;
      }
      peerDown(edge.name1, edge.name2);
    }
  }

  // trigger a node up event, all associated links will be brought up
  void
  nodeUp(const std::string& node) {
    for (auto& vertex : vertices) {
      if (vertex.name == node) {
        vertex.up = true;
        break;
      }
    }
    for (auto& edge : edges) {
      if (edge.name1 != node and edge.name2 != node) {
        continue; // link not affected
      }
      if (edge.up) {
        continue;
      }
      peerUp(edge.name1, edge.name2, edge.weight);
    }
  }

  // flap a node
  void
  nodeFlap(const std::string& node) {
    for (auto& edge : edges) {
      if (edge.name1 != node and edge.name2 != node) {
        continue; // link not affected
      }
      linkFlap(edge.name1, edge.name2, edge.weight);
    }
  }

  // print all nodes status according to given root
  static void
  printStatus(
      const std::unordered_map<std::string, StatusStrings>& status,
      std::optional<std::string> rootId = std::nullopt) {
    for (const auto& kv : status) {
      const auto& node = kv.first;
      // node level status
      const auto& nodeStatus = kv.second.first;
      // root level status
      const auto& rootsStatus = kv.second.second;
      if (rootId.has_value()) {
        // print specific root status
        if (rootsStatus.count(*rootId) == 0) {
          LOG(ERROR) << "node: " << node << " has no root " << *rootId
                     << "info";
          continue;
        }
        LOG(ERROR)
            << folly::sformat("{}\n{}", nodeStatus, rootsStatus.at(*rootId));
      } else {
        // print all roots status
        std::vector<std::string> strs;
        for (const auto& rootStr : rootsStatus) {
          strs.emplace_back(rootStr.second);
        }
        LOG(ERROR)
            << folly::sformat("{}\n{}", nodeStatus, folly::join("\n", strs));
      }
    }
  }

  // validate no-root case (no node declares itself as root)
  // if this case, we expect everynode reports a empty route-info map
  bool
  validateNoRoot(const std::unordered_map<
                 std::string,
                 std::unordered_map<std::string, Dual::RouteInfo>>& results) {
    for (const auto& kv : results) {
      const auto& node = kv.first;
      const auto& infos = kv.second;
      if (not infos.empty()) {
        LOG(ERROR) << node
                   << " has non-empty route-info map, size: " << infos.size();
        return false;
      }
    }
    return true;
  }

  // validate the correctness of Dual Algorithm for a given rootId
  // - ensure all nodes are in PASSIVE state
  // - ensure formed flooding topology is a SPT
  // - ensure flooding topology matches actual physical topology (spf)
  bool
  validateOnRoot(
      const std::string& rootId,
      const std::unordered_map<
          std::string,
          std::unordered_map<std::string, Dual::RouteInfo>>& results) {
    // construct flooding topology from results
    Graph floodTopo;
    std::unordered_map<std::string, VertexDescriptor> floodDescriptors;
    for (const auto& kv : results) {
      const auto& node = kv.first;
      if (kv.second.count(rootId) == 0) {
        LOG(ERROR) << node << " has no route-info for root " << rootId;
        return false;
      }
      const auto& info = kv.second.at(rootId);
      const auto& nexthop = info.nexthop;
      const auto& distance = info.distance;

      if (not validateRouteInfo(info)) {
        LOG(ERROR) << node << " route-info validation failed.";
        return false;
      }

      if (not nexthop.has_value() or
          distance == std::numeric_limits<int64_t>::max()) {
        // skip disconnected node
        continue;
      }

      // add node
      if (floodDescriptors.count(node) == 0) {
        auto d = boost::add_vertex(Vertex{node}, floodTopo);
        floodDescriptors.emplace(node, d);
      }

      // add node(nexthop)
      if (floodDescriptors.count(*nexthop) == 0) {
        auto d = boost::add_vertex(Vertex{*nexthop}, floodTopo);
        floodDescriptors.emplace(*nexthop, d);
      }

      // add edge
      if (*nexthop == node) {
        // skip root
        continue;
      }
      boost::add_edge(
          floodDescriptors.at(node),
          floodDescriptors.at(*nexthop),
          EdgeProperty(1),
          floodTopo);
    }

    // 1. validate flooding topology is a SPT
    if (not isSpt(floodTopo)) {
      LOG(ERROR) << "flooding topology is not a SPT";
      return false;
    }

    // construct physical topology from vertices and edges
    Graph physicalTopo;
    std::unordered_map<std::string, VertexDescriptor> physicalDescriptors;
    // add node
    for (const auto& vertex : vertices) {
      auto d = boost::add_vertex(vertex, physicalTopo);
      physicalDescriptors.emplace(vertex.name, d);
    }
    // add edge
    for (const auto& edge : edges) {
      if (not edge.up) {
        continue; // skip down edge
      }
      if (physicalDescriptors.count(edge.name1) == 0 or
          physicalDescriptors.count(edge.name2) == 0) {
        continue; // skip edge if any-end is down
      }
      boost::add_edge(
          physicalDescriptors.at(edge.name1),
          physicalDescriptors.at(edge.name2),
          EdgeProperty(edge.weight),
          physicalTopo);
    }

    // 2. validate flooding topology matches expected shortest-path graph
    // predecessors vector
    std::vector<VertexDescriptor> preds(boost::num_vertices(physicalTopo));
    // distances vector
    std::vector<Weight> dists(boost::num_vertices(physicalTopo));

    // build spt from this vertex
    IndexMap indexMap = boost::get(boost::vertex_index, physicalTopo);
    PredecessorMap predMap(preds.data(), indexMap);
    DistanceMap distMap(dists.data(), indexMap);
    boost::dijkstra_shortest_paths(
        physicalTopo,
        physicalDescriptors.at(rootId),
        boost::distance_map(distMap).predecessor_map(predMap));

    // verify number of connected nodes
    unsigned int numConnectedNodes = 0;
    for (const auto& weight : dists) {
      if (weight == std::numeric_limits<Weight>::max()) {
        continue;
      }
      ++numConnectedNodes;
    }
    if (numConnectedNodes != boost::num_vertices(floodTopo)) {
      LOG(ERROR)
          << "numConnectedNodes: " << numConnectedNodes << " != "
          << "num of flooding topology: " << boost::num_vertices(floodTopo);
      return false;
    }

    // verify distances
    for (const auto& kv : results) {
      const auto& node = kv.first;
      const auto& distance = kv.second.at(rootId).distance;
      const auto& expDistance = dists[physicalDescriptors.at(node)];
      if (expDistance != distance) {
        LOG(ERROR) << node << " distance: " << distance << " != "
                   << "expDistance: " << expDistance;
        return false;
      }
    }
    return true;
  }

  // validate the correctness of Dual Algorithm for all roots
  bool
  validate() {
    // get all-infos and stauts-strings
    std::unordered_map<
        std::string,
        std::unordered_map<std::string, Dual::RouteInfo>>
        infos;
    std::unordered_map<std::string, StatusStrings> status;
    getResults(infos, status);

    if (rootIds.empty()) {
      VLOG(1) << "validate no-root case";
      if (not validateNoRoot(infos)) {
        LOG(ERROR) << "validate no root failed";
        printStatus(status);
        return false;
      }
      return true;
    }

    for (const auto& rootId : rootIds) {
      VLOG(1) << "validate root " << rootId;
      if (not validateOnRoot(rootId, infos)) {
        LOG(ERROR) << "validate root " << rootId << " failed";
        printStatus(status, rootId);
        return false;
      }
    }
    return true;
  }

  // Single Link Failure Test
  // if not flap:
  // for EACH link: bring it down, wait-and-validate, bring it up,
  // wait-and-validate
  // if flap:
  // for EACH link: bring it down, bring it up right
  // away without delay, validate all
  bool
  singleLinkFailureTest(bool flap = false) {
    // flap each edge one by one and validate
    for (auto& edge : edges) {
      if (flap) {
        // flap link test
        VLOG(1) << "===> link (" << edge.name1 << ", " << edge.name2
                << ") flap";
        linkFlap(edge.name1, edge.name2, edge.weight);

        /* sleep override */
        std::this_thread::sleep_for(syncms);
        if (not validate()) {
          LOG(ERROR) << "flap link " << edge.name1 << ", " << edge.name2
                     << " validation failed";
          return false;
        }
        continue;
      }

      // bring edge down and validate
      VLOG(1) << "===> link (" << edge.name1 << ", " << edge.name2 << ") down";
      peerDown(edge.name1, edge.name2);

      /* sleep override */
      std::this_thread::sleep_for(syncms);
      if (not validate()) {
        LOG(ERROR) << "down link " << edge.name1 << ", " << edge.name2
                   << " validation failed";
        return false;
      }

      // bring edge back up and validate
      VLOG(1) << "===> link (" << edge.name1 << ", " << edge.name2 << ") up";
      peerUp(edge.name1, edge.name2, edge.weight);

      /* sleep override */
      std::this_thread::sleep_for(syncms);
      if (not validate()) {
        LOG(ERROR) << "up link " << edge.name1 << ", " << edge.name2
                   << " validation failed";
        return false;
      }
    }
    return true;
  }

  // Single Node Failure Test
  // if not flap:
  // for EACH node: bring it down, wait-and-validate, bring it up,
  // wait-and-validate
  // if flap:
  // for EACH node: bring it down, bring it up right
  // away without delay, validate all
  bool
  singleNodeFailureTest(bool flap = false) {
    // flap each node and validate
    for (auto& vertex : vertices) {
      if (vertex.name != "n0") {
        continue;
      }
      if (flap) {
        // flap node test
        VLOG(1) << "===> node (" << vertex.name << ") flap";
        nodeFlap(vertex.name);

        /* sleep override */
        std::this_thread::sleep_for(syncms);
        if (not validate()) {
          LOG(ERROR) << "flap node " << vertex.name << " validation failed";
          return false;
        }
        continue;
      }

      // bring node down and validate
      VLOG(1) << "===> node (" << vertex.name << ") down";
      nodeDown(vertex.name);

      /* sleep override */
      std::this_thread::sleep_for(syncms);
      if (not validate()) {
        LOG(ERROR) << "down node " << vertex.name << " validation failed";
        return false;
      }

      // bring node up and validate
      VLOG(1) << "===> node (" << vertex.name << ") up";
      nodeUp(vertex.name);
      /* sleep override */
      std::this_thread::sleep_for(syncms);
      if (not validate()) {
        LOG(ERROR) << "up node " << vertex.name << " validation failed";
        return false;
      }
    }
    return true;
  }

  // Multiple Failure Test
  // randomly pick 20% links, capped between [2, 6]
  // bring them down/up and validate, same flap logic as above
  bool
  multiFailureTest(bool flap = false) {
    // pick 20% of edges to shut down
    auto edgeNum = std::max(2, int(edges.size() * 0.2));
    edgeNum = std::min(edgeNum, 6);
    auto links = gen::from(edges) |
        gen::sample(edgeNum, folly::ThreadLocalPRNG()) |
        gen::as<std::vector<Edge>>();

    if (flap) {
      // flap test
      // flap all chosen edges
      for (const auto& edge : links) {
        VLOG(1) << "===> link (" << edge.name1 << ", " << edge.name2
                << ") flap";
        linkFlap(edge.name1, edge.name2, edge.weight);
      }

      /* sleep override */
      std::this_thread::sleep_for(syncms);
      if (not validate()) {
        LOG(ERROR) << "multi links flap validation failed";
        return false;
      }
      return true;
    }

    // bring down links
    for (const auto& edge : links) {
      VLOG(1) << "===> link (" << edge.name1 << ", " << edge.name2 << ") down";
      peerDown(edge.name1, edge.name2);
    }

    /* sleep override */
    std::this_thread::sleep_for(syncms);
    if (not validate()) {
      LOG(ERROR) << "multi links down validation failed";
      return false;
    }

    // bring links back up
    for (const auto& edge : links) {
      VLOG(1) << "===> link (" << edge.name1 << ", " << edge.name2 << ") up";
      peerUp(edge.name1, edge.name2, edge.weight);
    }

    // validate
    /* sleep override */
    std::this_thread::sleep_for(syncms);
    if (not validate()) {
      LOG(ERROR) << "multi links up validation failed";
      return false;
    }
    return true;
  }

  // collect results and status for all nodes (blocking call)
  // output: map<node-id: <root-id: RouteInfo>>
  // output: map<node-id: <root-id: status-string>>
  void
  getResults(
      std::unordered_map<
          std::string,
          std::unordered_map<std::string, Dual::RouteInfo>>& infos,
      std::unordered_map<std::string, StatusStrings>& status) {
    CHECK(infos.empty());
    CHECK(status.empty());

    for (const auto& kv : nodes) {
      auto& node = kv.second;
      evb->runInEventBaseThreadAndWait([&, node]() {
        infos.emplace(kv.first, node->getInfos());
        status.emplace(kv.first, node->getStatusStrings());
      });
    }
  }

  std::shared_ptr<folly::EventBase> evb;
  std::thread thread;
  // map<node-id: DualTestNode*>
  std::map<std::string, std::shared_ptr<DualTestNode>> nodes;
  std::vector<std::string> rootIds{};

  std::vector<Vertex> vertices;
  std::vector<Edge> edges;
};

// Test Parameters
struct TestParam {
  int totalRoots; // number of roots
  bool flap; // flap link/node or not
  TestParam(int totalRoots, bool flap) : totalRoots(totalRoots), flap(flap) {}
};

class DualFixture : public DualBaseFixture,
                    public ::testing::WithParamInterface<TestParam> {};

// Test all following different cases for each topology
INSTANTIATE_TEST_CASE_P(
    DualInstance,
    DualFixture,
    ::testing::Values(
        TestParam(0, false),
        TestParam(0, true),
        TestParam(1, false),
        TestParam(1, true),
        TestParam(2, false),
        TestParam(2, true)));

/**
 *  Circular Topology
 *  root: n0, each-link-cost: 1
 *  n0 --- n1
 *  |      |
 *  |     ...
 *  |      |
 * n(N-1)- n(N-2)
 */
TEST_P(DualFixture, CircularTest) {
  const auto& param = GetParam();
  const auto& totalRoots = param.totalRoots;
  const auto& flap = param.flap;
  VLOG(1) << "test params: " << totalRoots << ", " << flap;

  int numNodes = 4;
  // add nodes
  for (int i = 0; i < numNodes; ++i) {
    bool isRoot = i < totalRoots;
    addNode(folly::sformat("n{}", i), isRoot);
  }
  // add links
  for (int i = 0; i < numNodes; ++i) {
    int j = (i + 1) % numNodes;
    addLink(folly::sformat("n{}", i), folly::sformat("n{}", j), 1);
  }

  /* sleep override */
  std::this_thread::sleep_for(syncms);
  EXPECT_TRUE(validate());

  EXPECT_TRUE(singleLinkFailureTest(flap));
  EXPECT_TRUE(singleNodeFailureTest(flap));
  EXPECT_TRUE(multiFailureTest(flap));
}

/**
 *  Fabric Topology (2 X 4)
 *  n0        n1
 *  |\        /\
 *  | \ ...  /  \
 *  |  \    /    \
 *  n2 n3 n4    n5
 */
TEST_P(DualFixture, FabricTest) {
  const auto& param = GetParam();
  const auto& totalRoots = param.totalRoots;
  const auto& flap = param.flap;
  VLOG(1) << "test params: " << totalRoots << ", " << flap;

  int m = 2;
  int n = 4;

  for (int i = 0; i < m + n; ++i) {
    bool isRoot = i < totalRoots;
    addNode(folly::sformat("n{}", i), isRoot);
  }

  for (int i = 0; i < m; ++i) {
    for (int j = m; j < m + n; ++j) {
      addLink(folly::sformat("n{}", i), folly::sformat("n{}", j), 1);
    }
  }

  /* sleep override */
  std::this_thread::sleep_for(syncms);
  EXPECT_TRUE(validate());

  EXPECT_TRUE(singleLinkFailureTest(flap));
  EXPECT_TRUE(singleNodeFailureTest(flap));
  EXPECT_TRUE(multiFailureTest(flap));
}

/**
 *  Full-Mesh Topology
 */
TEST_P(DualFixture, FullMeshTest) {
  const auto& param = GetParam();
  const auto& totalRoots = param.totalRoots;
  const auto& flap = param.flap;
  VLOG(1) << "test params: " << totalRoots << ", " << flap;

  int numNodes = 4;
  // add nodes
  for (int i = 0; i < numNodes; ++i) {
    bool isRoot = i < totalRoots;
    addNode(folly::sformat("n{}", i), isRoot);
  }

  // add links
  for (int i = 0; i < numNodes; ++i) {
    for (int j = 0; j < numNodes; ++j) {
      if (i == j) {
        continue;
      }
      addLink(folly::sformat("n{}", i), folly::sformat("n{}", j), 1);
    }
  }

  /* sleep override */
  std::this_thread::sleep_for(syncms);
  EXPECT_TRUE(validate());

  EXPECT_TRUE(singleLinkFailureTest(flap));
  EXPECT_TRUE(singleNodeFailureTest(flap));
  EXPECT_TRUE(multiFailureTest(flap));
}

/**
 * m X n Grid Topology
 */
TEST_P(DualFixture, GridTest) {
  const auto& param = GetParam();
  const auto& totalRoots = param.totalRoots;
  const auto& flap = param.flap;
  VLOG(1) << "test params: " << totalRoots << ", " << flap;

  int m = 2;
  int n = 3;
  // add nodes
  for (int i = 0; i < m * n; ++i) {
    bool isRoot = i < totalRoots;
    addNode(folly::sformat("n{}", i), isRoot);
  }

  // add links
  for (int i = 0; i < m; ++i) {
    for (int j = 1; j < n; ++j) {
      addLink(
          folly::sformat("n{}", i * n + j),
          folly::sformat("n{}", i * n + j - 1),
          1);
    }
  }
  for (int i = 1; i < m; ++i) {
    for (int j = 0; j < n; ++j) {
      addLink(
          folly::sformat("n{}", i * n + j),
          folly::sformat("n{}", i * n + j - n),
          1);
    }
  }

  /* sleep override */
  std::this_thread::sleep_for(syncms);
  EXPECT_TRUE(validate());

  EXPECT_TRUE(singleLinkFailureTest(flap));
  EXPECT_TRUE(singleNodeFailureTest(flap));
  EXPECT_TRUE(multiFailureTest(flap));
}

int
main(int argc, char* argv[]) {
  // Parse command line flags
  testing::InitGoogleTest(&argc, argv);
  gflags::ParseCommandLineFlags(&argc, &argv, true);
  google::InitGoogleLogging(argv[0]);
  google::InstallFailureSignalHandler();

  // Run the tests
  return RUN_ALL_TESTS();
}


#include "TimeIntegrator.h"

#include <exception>
#include <Eigen/Dense>
#include <Eigen/IterativeLinearSolvers>
#include <Eigen/SparseLU>
#include <cinder/Log.h>

namespace ar
{
    VectorX TimeIntegrator::solveDense(const MatrixX& A, const VectorX& b) const
    {
        switch (denseSolver)
        {
        case DenseLinearSolver::PartialPivLU: {return A.partialPivLu().solve(b); }
        case DenseLinearSolver::FullPivLU:
            {
                Eigen::FullPivLU<MatrixX> decomp = A.fullPivLu();
                decomp.setThreshold(1e-5);
                int rank = decomp.rank();
                if (rank == std::min(A.rows(), A.cols()))
                    CI_LOG_D("rank is " << decomp.rank() << " (of " << std::min(A.rows(), A.cols()) << ")");
                else
                    CI_LOG_W("Matrix is not of full rank, only " << rank << " of " << std::min(A.rows(), A.cols()));
                return decomp.solve(b);
            }
        case DenseLinearSolver::HouseholderQR: {return A.householderQr().solve(b); }
        case DenseLinearSolver::ColPivHousholderQR:
            {
                Eigen::ColPivHouseholderQR<MatrixX> decomp = A.colPivHouseholderQr();
                int rank = decomp.rank();
                if (rank == std::min(A.rows(), A.cols()))
                    CI_LOG_D("rank is " << decomp.rank() << " (of " << std::min(A.rows(), A.cols()) << ")");
                else
                    CI_LOG_W("Matrix is not of full rank, only " << rank << " of " << std::min(A.rows(), A.cols()));
                return decomp.solve(b);
            }
        case DenseLinearSolver::FullPivHouseholderQR:
            {
                Eigen::FullPivHouseholderQR<MatrixX> decomp = A.fullPivHouseholderQr();
                int rank = decomp.rank();
                if (rank == std::min(A.rows(), A.cols()))
                    CI_LOG_D("rank is " << decomp.rank() << " (of " << std::min(A.rows(), A.cols()) << ")");
                else
                    CI_LOG_W("Matrix is not of full rank, only " << rank << " of " << std::min(A.rows(), A.cols()));
                return decomp.solve(b);
            }
        case DenseLinearSolver::CompleteOrthogonalDecomposition: {return A.completeOrthogonalDecomposition().solve(b); }
        case DenseLinearSolver::LLT: {return A.llt().solve(b); }
        case DenseLinearSolver::LDLT: {return A.ldlt().solve(b); }
        default: throw std::exception("unsupported solver");
        }
    }

    VectorX TimeIntegrator::solveSparse(const SparseMatrixRowMajor & A, const VectorX & b, const VectorX& guess) const
    {
        //TODO: do I want to support custom number of iterations and thesholds?
        switch (sparseSolver)
        {
        case SparseLinearSolver::ConjugateGradient:
            {
                Eigen::ConjugateGradient<SparseMatrixRowMajor, Eigen::Lower | Eigen::Upper> cg;
                if (sparseSolverIterations > 0) cg.setMaxIterations(sparseSolverIterations);
                if (sparseSolverTolerance > 0) cg.setTolerance(sparseSolverTolerance);
                cg.compute(A);
                if (guess.size() > 0)
                    return cg.solveWithGuess(b, guess);
                else
                    return cg.solve(b);
            }
        case SparseLinearSolver::BiCGSTAB:
        {
            Eigen::BiCGSTAB<SparseMatrixRowMajor> cg;
            if (sparseSolverIterations > 0) cg.setMaxIterations(sparseSolverIterations);
            if (sparseSolverTolerance > 0) cg.setTolerance(sparseSolverTolerance);
            cg.compute(A);
            if (guess.size() > 0)
                return cg.solveWithGuess(b, guess);
            else
                return cg.solve(b);
        }
        case SparseLinearSolver::LU:
        {
            CI_LOG_E("Sparse LU does only work with column-major matrices");
            return VectorX::Zero(b.size());
        }
        default: throw std::exception("unsupported solver");
        }
    }

    VectorX TimeIntegrator::solveSparse(const SparseMatrixColumnMajor & A, const VectorX & b, const VectorX& guess) const
    {
        //TODO: do I want to support custom number of iterations and thesholds?
        switch (sparseSolver)
        {
        case SparseLinearSolver::ConjugateGradient:
        {
            Eigen::ConjugateGradient<SparseMatrixColumnMajor, Eigen::Lower | Eigen::Upper> cg;
            if (sparseSolverIterations > 0) cg.setMaxIterations(sparseSolverIterations);
            if (sparseSolverTolerance > 0) cg.setTolerance(sparseSolverTolerance);
            cg.compute(A);
            if (guess.size() > 0)
                return cg.solveWithGuess(b, guess);
            else
                return cg.solve(b);
        }
        case SparseLinearSolver::BiCGSTAB:
        {
            Eigen::BiCGSTAB<SparseMatrixColumnMajor> cg;
            if (sparseSolverIterations > 0) cg.setMaxIterations(sparseSolverIterations);
            if (sparseSolverTolerance > 0) cg.setTolerance(sparseSolverTolerance);
            cg.compute(A);
            if (guess.size() > 0)
                return cg.solveWithGuess(b, guess);
            else
                return cg.solve(b);
        }
        case SparseLinearSolver::LU:
        {
            Eigen::SparseLU<SparseMatrixColumnMajor, Eigen::COLAMDOrdering<int>> lu;
            lu.analyzePattern(A);
            lu.factorize(A);
            return lu.solve(b);
        }
        default: throw std::exception("unsupported solver");
        }
    }

    std::shared_ptr<TimeIntegrator> TimeIntegrator::createIntegrator(Integrator type, Eigen::Index dof)
    {
        switch (type)
        {
        case Integrator::Newmark1: return std::make_shared<TimeIntegrator_Newmark1>(dof);
        case Integrator::Newmark2: return std::make_shared<TimeIntegrator_Newmark2>(dof);
        case Integrator::ExplicitCentralDifferences: return std::make_shared<TimeIntegrator_ExplicitCentralDifferences>(dof);
        case Integrator::ImplicitLinearAcceleartion: return std::make_shared<TimeIntegrator_ImplicitLinearAcceleration>(dof);
        case Integrator::Newmark3: return std::make_shared<TimeIntegrator_Newmark3>(dof);
        case Integrator::HHTAlpha: return std::make_shared<TimeIntegrator_HHTalpha>(dof);
        default: throw std::exception("unknown integrator");
        }
    }
}


#include <CGAL/Polygon_mesh_processing/remesh.h>
#include <CGAL/Polygon_mesh_processing/border.h>

#include "catalog.h"
#include "remesh.h" // class implemented

#include <boost/function_output_iterator.hpp>
#include <fstream>
#include <vector>

typedef boost::graph_traits<Mesh>::halfedge_descriptor halfedge_descriptor;
typedef boost::graph_traits<Mesh>::edge_descriptor     edge_descriptor;

namespace PMP = CGAL::Polygon_mesh_processing;

using namespace tusk;

struct halfedge2edge
{
  halfedge2edge(const Mesh& m, std::vector<edge_descriptor>& edges)
    : m_mesh(m), m_edges(edges)
  {}
  void operator()(const halfedge_descriptor& h) const
  {
    m_edges.push_back(edge(h, m_mesh));
  }
  const Mesh& m_mesh;
  std::vector<edge_descriptor>& m_edges;
};

void
Remesh::usage()
{
  std::cerr << "remesh [target_edge_length] [infile] [outfile]\n\n"
            <<"   [target_edge_length] in millimters.\n"
            << "  Isotropic remeshing of [infile] saving to [outfile].\n"
            << "  Files in binary STL format.\n\n"
            << "  for example, remesh 0.04 0000.stl output.stl\n"
            << std::endl;
}// usage

int
Remesh::run(double length,
            const std::string& infile,
            const std::string& outfile)
{
  try {
    Mesh mesh;

    std::cout << "Load mesh from " << infile << "... ";
    Catalog catalog;
    catalog.read(infile, &mesh);
    std::cout << "done." << std::endl;

    unsigned int nb_iter = 3;
    std::cout << "Split border... ";
    std::vector<edge_descriptor> border;
    PMP::border_halfedges(faces(mesh),
                          mesh,
                          boost::make_function_output_iterator(halfedge2edge(mesh, border)));
    PMP::split_long_edges(border, length, mesh);
    std::cout << "done." << std::endl;

    std::cout << "Remeshing (" << num_faces(mesh) << " faces) by target edge length " << length << "mm ... ";
    PMP::isotropic_remeshing(faces(mesh), length, mesh, PMP::parameters::number_of_iterations(nb_iter));
    std::cout << "done." << std::endl;

    std::cout << "Write to " << outfile << "... ";
    catalog.write(mesh, outfile);
    std::cout << "done." << std::endl;

  } catch (std::exception& e) {
    std::cerr << e.what() << std::endl;
  }

  return 0;
}// run


/*

copyright (c) 2021, adrien blassiau and corentin juvigny

permission to use, copy, modify, and/or distribute this software
for any purpose with or without fee is hereby granted, provided
that the above copyright notice and this permission notice appear
in all copies.

the software is provided "as is" and the author disclaims all
warranties with regard to this software including all implied
warranties of merchantability and fitness. in no event shall the
author be liable for any special, direct, indirect, or
consequential damages or any damages whatsoever resulting from
loss of use, data or profits, whether in an action of contract,
negligence or other tortious action, arising out of or in
connection with the use or performance of this software.

*/

/** @file mip.cpp
 *
 * @brief Mixed integer algorithm to construct the optimal solution of the problem
 *
 */

#define TBB_SUPPRESS_DEPRECATED_MESSAGES 1

#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <algorithm>
#include <mip.hpp>
#include <utility>
#include <ilcplex/ilocplex.h>

#define EPSILON 10e-10

typedef std::map<Node<2>*,std::pair<IloNum,long>,Node<2>::NodeCmp> IloNumIndexMap;
typedef std::map<Node<2>*,IloNumVar,Node<2>::NodeCmp> IloNumVarMap;
typedef std::map<Node<2>*,IloNum,Node<2>::NodeCmp> IloNumMap;
typedef boost::adjacency_list < boost::vecS, boost::vecS, boost::undirectedS > graph_t;
typedef boost::graph_traits < graph_t >::vertex_descriptor vertex_descriptor;

ILOLAZYCONSTRAINTCALLBACK1( ValidityCallback
                          , const IloNumVarMap&
                          , X )
{
   IloNumIndexMap Xr;
   size_t index = 0;
#if 0
   for (const auto &[name,value] : X)
      Xr[name] = std::make_pair(getValue(value),index++);
#else
   for (const auto &[node,value] : X) {
      IloInt x = getValue(value);
      if ( abs(1 - x) <= EPSILON )
         Xr[node] = std::make_pair(x,index++);
      else
         Xr[node] = std::make_pair(x,-1);
   }
#endif

   graph_t graph;
#if 0
   for (struct {size_t i; IloNumIndexMap::const_iterator it;} ist = {0, Xr.cbegin()}; ist.it != Xr.cend(); ist.it++, ist.i++) {
      for (struct {size_t j; IloNumIndexMap::const_iterator jt;} jst = {ist.i, ist.it}; jst.jt != Xr.cend(); jst.jt++, jst.j++) {
         if ( abs(ist.it->second.first - 1) <= 10e-10
            && (std::find( ist.it->first->communication_queue().cbegin()
                         , ist.it->first->communication_queue().cend()
                         , jst.jt->first ) != ist.it->first->communication_queue().end()) ) {
            boost::add_edge(ist.i,jst.j,graph);
         }
      }
   }
#endif
#if 1
   for (const auto &[node,pair] : Xr) {
      const auto [value1,index1] = pair;
      if ( abs(value1 - 1) <= EPSILON ) {
         for (const auto &neighbour : node->communication_queue()) {
            const auto [value2,index2] = Xr[neighbour];
            if ( abs(value2 - 1) <= EPSILON )
               boost::add_edge(index1,index2,graph);
         }
      }
   }
#endif
#if 0
   std::vector<Node<2>*> nodes(X.size());
   std::transform( Xr.cbegin()
                 , Xr.cend()
                 , nodes.begin()
                 , [](const auto &node) { return node.first; });
   for (size_t i = 0; i < nodes.size(); i++) {
      for (size_t j = i+1; j < nodes.size(); j++) {
         if ( Xr[nodes[i]].value == 1
            && (std::find( nodes[i]->communication_queue().cbegin()
                         , nodes[i]->communication_queue().cend()
                         , nodes[j] ) != nodes[i]->communication_queue().end()) ) {
            boost::add_edge(i,j,graph);
         }
      }
   }

#endif

   std::vector<size_t> component(boost::num_vertices(graph));
   size_t num = boost::connected_components(graph,&component[0]);

   if ( num > 1 ) { // Graph is disconnected
      IloExpr e(getEnv());
      IloInt nbr = 0;
      for (const auto &[node,tuple] : Xr) {
         if ( tuple.first == 1 ) {
            e += X.find(node)->second;
            nbr++;
         } else {
            //e -= X.find(node)->second;
         }
      }
      addLocal(e <= nbr-1);
      e.end();
   }
}

void mip_resolution_2D(Grid<2> &g)
{
   IloEnv env;

   IloModel model(env);

   // Decision var
   IloNumVarMap X;
   for (const auto &s : g.nodes()) {
      std::stringstream ss;
      ss << "x_" << s->name();
      X[s.get()] = IloNumVar(env,0,1,ILOBOOL,ss.str().c_str());
   }

   // Objective
   IloExpr obj(env);
   for (auto &x_s : X)
      obj += x_s.second;
   model.add(IloMinimize(env,obj,"obj"));
   obj.end();

   // Constraints
   for (const auto &s : g.nodes()) {
      IloExpr e_com(env);
      IloExpr e_capt(env);
      for (const auto &v : s->communication_queue())
         e_com += X[v];
      for (const auto &v : s->capture_queue())
         e_capt += X[v];
      model.add(e_com >= 1);
      model.add(e_capt >= 1);
      e_capt.end();
      e_com.end();
   }
   model.add(X[g.well().get()] == 1);

   IloCplex cplex(model);

   cplex.setParam(IloCplex::Param::Threads,8);
   cplex.setParam(IloCplex::Param::MIP::Strategy::File,1);
   cplex.setParam(IloCplex::Param::MIP::Limits::Solutions,1000);
   cplex.use(ValidityCallback(env,X));
   cplex.solve();

   IloAlgorithm::Status status = cplex.getStatus();
   std::stringstream ss;
   ss << "Solution status:                   " << cplex.getStatus();
   std::string separator_line(ss.str().length(),'-');
   std::cout << separator_line << std::endl;
   std::cout << ss.str() << std::endl;
   IloNumIndexMap Xr;
   graph_t graph;
   size_t index = 0;
   switch ( status ) {
      case IloAlgorithm::Optimal:
         std::cout << "Nodes processed:                   " << cplex.getNnodes() << std::endl;
         std::cout << "Active user cuts/lazy constraints: " << cplex.getNcuts(IloCplex::CutUser) << std::endl;
         std::cout << "Time elapsed:                      " << cplex.getTime() << std::endl;
         std::cout << "Optimal value:                     " << cplex.getObjValue()-1 << std::endl;
         for (const auto &[node,value] : X) {
            IloInt x = cplex.getValue(value);
            if ( abs(1 - x) <= EPSILON )
               Xr[node] = std::make_pair(x,index++);
            else
               Xr[node] = std::make_pair(x,-1);
         }
         for (const auto &v : g.nodes()) {
            if ( Xr[v.get()].first == 1 ) {
               typename Node<2>::Queue queue;
               for (const auto &node : v->communication_queue())
                  if ( Xr[node].first == 1 ) {
                     queue.push_front(node);
                  }
               v->set_new_sensor(queue);
            }
         }
         {
            for (const auto &[node,pair] : Xr) {
               const auto [value1,index1] = pair;
               if ( abs(value1 - 1) <= EPSILON ) {
                  for (const auto &neighbour : node->communication_queue()) {
                     const auto [value2,index2] = Xr[neighbour];
                     if ( abs(value2 - 1) <= EPSILON ) {
                        boost::add_edge(index1,index2,graph);
                        std::cout << "Node "<<*node<<" connected to node "<<*neighbour<< " with indices : " << index1 << " " << index2 << std::endl;
                     }
                  }
               }
            }
            std::vector<size_t> component(boost::num_vertices(graph));
            long num = boost::connected_components(graph,&component[0]);
            std::cout << "Number of connected components " << num << std::endl;
            std::cout << "Number of vertices in the graph : " << boost::num_vertices(graph) << std::endl;

            for (const auto &[node,pair] : Xr) {
               const auto [value1,index1] = pair;
               if ( abs(value1 - 1) <= EPSILON ) {
                  std::cout << "Node " << *node << " in component " << component[index1] << std::endl;
               }
            }

         }
         break;
      default:
         break;
   }
   std::cout << separator_line << std::endl;

   model.end();
   cplex.end();
   env.end();
}


#pragma once

#include <cstdint>
#include <Eigen/Dense>

/**
 * Controller for wheel velocities
 */
class RobotController {
private:
    /**
     * Number of tracked states (number of states in state vector)
     *
     * X and Y: linear velocities of robot body (m/s)
     * W: angular velocity of robot body around z axis (rad/s)
     */
    const static int numStates = 3;

    /**
     * Number of wheels controlled
     */
    const static int numWheels = 4;

public:
    /**
     * Constructor
     *
     * @param dt_us Expected period of the controller (microseconds)
     */
    RobotController(uint32_t dt_us);

    /**
     * Updates the controller with the latest input and calculates
     * the correct wheel velocities to reach this target
     *
     * @param pv Current state (XYW vel in m/s or rad/s)
     * @param sp Current target (XYW vel in m/s or rad/s)
     * @param output Motor targets (rad/s)
     */
    void calculateBody(Eigen::Matrix<float, numStates, 1> pv,
                       Eigen::Matrix<float, numStates, 1> sp,
                       Eigen::Matrix<float, numWheels, 1>& outputs);

    /**
     * Updates the wheels such that they try to follow the target
     * Outputs the correct motor commands to do this
     *
     * @param pv Current state (W1-4 in rad/s)
     * @param sp Current target (W1-4 in rad/s)
     * @param output Motor duty cycles in % max (-1 to 1)
     */
    void calculateWheel(Eigen::Matrix<float, numWheels, 1> pv,
                         Eigen::Matrix<float, numWheels, 1> sp,
                         Eigen::Matrix<float, numWheels, 1>& outputs);

private:
    /**
     * Limits the difference between the previous target and the new target
     * such that the acceleration limits below are never broken
     */
    bool limitBodyAccel(const Eigen::Matrix<float, numStates, 1> finalTarget,
                        Eigen::Matrix<float, numStates, 1>& dampened);

    bool limitWheelAccel(const Eigen::Matrix<float, numWheels, 1> finalTarget,
                        Eigen::Matrix<float, numWheels, 1>& dampened);

    /**
     * Vector of proportional constants for body velocity PID control
     *
     * Weighting of current error terms
     */
    Eigen::Matrix<float, numStates, 1> BodyKp;

    /**
     * Vector of integral constants for body velocity PID control
     *
     * Weighting of total error terms (integral)
     */
    Eigen::Matrix<float, numStates, 1> BodyKi;

    Eigen::Matrix<float, numStates, 1> BodyErrorSum;

    /**
      * Vector of limits for integral constants of body velocity PID control
      *
      * Thresholds to prevent integral term from blowing up (e.g. if robot is stuck)
      */
    Eigen::Matrix<float, numStates, 1> BodyILimit;

    bool BodyUseILimit;
    bool BodyInputLimited;
    bool BodyOutputLimited;

    Eigen::Matrix<float, numStates, 1> BodyPrevTarget;

    /**
     * Vector of proportional constants for wheel velocity PID control
     *
     * Weighting of current error terms
     */
    Eigen::Matrix<float, numWheels, 1> WheelKp;

    Eigen::Matrix<float, numWheels, 1> WheelPrevTarget;

    /**
     * Interval of control calculations (seconds)
     */
    float dt;

    /**
     * Max wheel acceleration (rad/s^2)
     */
    static constexpr float maxWheelAccel = 160;

    /**
     * Max foward acceleration (m/s^2)
     */
    static constexpr float maxForwardAccel = 8;

    /**
     * Max side acceleration (m/s^2)
     */
    static constexpr float maxSideAccel = 8;

    /**
     * Max angular acceleration (rad/s^2)
     */
    static constexpr float maxAngularAccel = 40.0;
};


#pragma once

#include <boost/geometry.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <utility>
#include <cstdint>

typedef boost::geometry::model::point<double, 3, boost::geometry::cs::spherical_equatorial<boost::geometry::degree>> wgs84_point_t;
typedef boost::geometry::model::segment<wgs84_point_t> wgs84_segment_t;
typedef boost::geometry::model::linestring<wgs84_point_t> wgs84_linestring_t;
typedef boost::geometry::model::box<wgs84_point_t> wgs84_box_t;
typedef std::pair<std::uint64_t, std::uint64_t> nodepair_t;
typedef std::pair<wgs84_segment_t, nodepair_t> rtree_value_t;
typedef boost::geometry::index::rtree<rtree_value_t, boost::geometry::index::rstar<16>> line_rtree_t;

enum ValidDirections {
    Both,
    Forward,
    Reverse
};


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

#include <vector>
#include <boost/math/tools/random_vector.hpp>
#include <boost/math/statistics/bivariate_statistics.hpp>
#include <benchmark/benchmark.h>

using boost::math::generate_random_vector;

template<typename T>
void seq_covariance(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> u = generate_random_vector<T>(size, seed);
    std::vector<T> v = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::covariance(std::execution::seq, u, v));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void par_covariance(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> u = generate_random_vector<T>(size, seed);
    std::vector<T> v = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::covariance(std::execution::par, u, v));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void seq_correlation(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> u = generate_random_vector<T>(size, seed);
    std::vector<T> v = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::correlation_coefficient(std::execution::seq, u, v));
    }
    state.SetComplexityN(state.range(0));
}

template<typename T>
void par_correlation(benchmark::State& state)
{
    constexpr std::size_t seed {};
    const std::size_t size = state.range(0);
    std::vector<T> u = generate_random_vector<T>(size, seed);
    std::vector<T> v = generate_random_vector<T>(size, seed);

    for(auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::statistics::correlation_coefficient(std::execution::par, u, v));
    }
    state.SetComplexityN(state.range(0));
}

BENCHMARK_TEMPLATE(seq_covariance, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(par_covariance, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();

BENCHMARK_TEMPLATE(seq_correlation, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();
BENCHMARK_TEMPLATE(par_correlation, double)->RangeMultiplier(2)->Range(1 << 6, 1 << 20)->Complexity(benchmark::oN)->UseRealTime();

BENCHMARK_MAIN();


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <string>

#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

template <typename Coll>
void test(Coll& x, const char* name)
{
    std::cout << "\n" << name << " is set to:\n";
    mtl::seed<typename mtl::Collection<Coll>::value_type> s;
    random(x, s);
    std::cout << x;
}

int main(int, char**)
{
    using namespace mtl;

    const unsigned size= 3;

    dense_vector<double>                             dv(size);

    compressed2D<double>                             cdc(size, size);
    compressed2D<std::complex<double> >              ccc(size, size);
    dense2D<double>                                  dc(size, size);
    dense2D<double, mat::parameters<col_major> >  dcc(size, size);
    dense2D<float>                                   fc(size, size);
    morton_dense<double,  morton_mask>               mdc(size, size);
    morton_dense<double, doppled_32_col_mask>        mcc(size, size);

    test(dv, "dense_vector");
    test(cdc, "compressed2D");
    test(ccc, "compressed2D complex");
    test(dc, "dense2D");
    test(dcc, "dense2D col-major");
    test(mdc, "pure Morton");
    test(mcc, "Hybrid col-major");

    return 0;
}


#include "./../../header/tarjan.h"

//STL
#include <iostream>
#include <vector>
#include <string>
#include <fstream>
#include <sstream>
#include <iterator>
//Boost
#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/adjacency_list_io.hpp>
#include <boost/graph/property_iter_range.hpp>
#include <boost/graph/copy.hpp>
#include <boost/property_map/property_map.hpp>

using namespace boost;

void Tarjan::print_graph(){
    GraphComponent::print_graph_file(t);
}

void Tarjan::ApplyDFS() {
    typedef boost::graph_traits<theGraph>::vertex_descriptor Vertex;
    v_p id = get(&VertexProperty::index, t);
    typedef boost::graph_traits<theGraph>::vertex_iterator vertex_iter;
    std::pair<vertex_iter, vertex_iter> vp;
    Vertex_t theFirst = *(vertices(t).first);
    int Counter = 0;
    int SizeOfGraph = num_vertices(t);
    std::vector<int> visited(SizeOfGraph,666);
    DepthFirstSearch(theFirst, Counter, visited);
    std::cout << "\nTarjan Depth First Search Exited Successfully." << std::endl;
    std::cout << "Discovery Order is: " << std::endl;
     for(vp=vertices(t); vp.first != vp.second; vp.first++){
        Vertex_t v = *vp.first;
        std::cout << "Node(" << id[v] << ") --> " << visited[id[v]] << std::endl;
    }

}

void Tarjan::DepthFirstSearch( Vertex_t &v, int &Counter,std::vector<int> &visited) {
    v_p id = get(&VertexProperty::index, t);
    e_p arch_type =get(&EdgeProperty::name, t);
    Counter++;
    visited[id[v]] = static_cast<unsigned long>(Counter);
 //   std::cout << "Node(" << id[v] << ") --> " << visited[id[v]] << std::endl;
    typedef boost::graph_traits<theGraph>::edge_descriptor Edge;
    boost::graph_traits<theGraph>::out_edge_iterator out_i, out_end;
    for (boost::tie(out_i, out_end) = out_edges(v, t); out_i != out_end; ++out_i) {
        Edge e = *out_i;
        Vertex_t w = target(e, t);
        if (visited[id[w]] == 666) {
            arch_type[e] = "forward";
            //std::cout << "Node(" <<id[w] << ") --> " << visited[w] << std::endl;
            DepthFirstSearch(w, Counter,visited);
        } else if (visited[id[w]] < visited[id[v]]) {
            arch_type[e] = "backward";
        }
    }
}

bool Tarjan::isReachable(Vertex_t &source, Vertex_t &Target) {
    typedef graph_traits<theGraph>::vertices_size_type size_type;
    size_type n_vertices;
    n_vertices = num_vertices(t);
    v_p id = get(&VertexProperty::index, t);
    std::vector<bool> Visited(n_vertices, false);
    std::deque<Vertex_t> Checks;
    Visited[id[source]] = true;
    Checks.push_back(source);
    while (!Checks.empty()) {
        Vertex_t v = Checks.front();
        Checks.pop_front();
        boost::graph_traits<theGraph>::out_edge_iterator out_i, out_end;
        for (boost::tie(out_i, out_end) = out_edges(v, t); out_i != out_end; ++out_i) {
            Vertex_t w = target(*out_i, t);
            if (id[w] == id[Target]) {
                return true;
            } else if (!Visited[id[w]]) {
                Visited[id[w]] = true;
                Checks.push_back(w);
            }
        }
    }
    return false;
}
UtilityStructs::StorageItems Tarjan::applyBiconnectivity(){
    float ms_duration = 0;
    v_p id = get(&VertexProperty::index, t);
    typedef boost::graph_traits<theGraph>::vertex_descriptor Vertex_t;
    typedef boost::graph_traits<theGraph>::edge_descriptor Edge_t;
    typedef boost::graph_traits<theGraph>::vertex_iterator vertex_iter;
    std::pair<vertex_iter, vertex_iter> vp;
    std::vector<Edge_t> edges;
    std::vector<std::vector<Edge_t> > components;
    int Counter = 0;
    int sizeOfGraph = num_vertices(t);
    std::vector<int> visited(sizeOfGraph,999999);
    std::vector<int> lowPt(sizeOfGraph,999999);
    int stackCount = 0;
    {
        UtilityStructs::Timer timer;
        for(vp = vertices(t); vp.first != vp.second; vp.first++){
            Vertex_t v = *vp.first;
            if(visited[id[v]] == 999999) {
                biconnect(v, edges, Counter,components, visited, lowPt, stackCount);
            }
        }
        ms_duration = timer.stop();
    }
    size_t total_bytes = 0;
    total_bytes += sizeof(visited[0]) * visited.size();
    total_bytes += sizeof(lowPt[0]) * lowPt.size();
    total_bytes +=sizeof(edges[0]) * stackCount;
    UtilityStructs::StorageItems s;
    s.vertexCount = num_vertices(t);
    s.edgeCount = num_edges(t);
    s.duration = ms_duration;
    s.total_bytes = total_bytes;

    return s;
}

void Tarjan::biconnect(Vertex_t &v,
                  std::vector<Edge_t> &edges,
                  int &Counter,
                  std::vector<std::vector<Edge_t> > &components,
                  std::vector<int> &visited,
                  std::vector<int> &lowPt,
                  int &stackCount){
    v_p id = get(&VertexProperty::index,t);
    Counter++;
    visited[id[v]] = Counter;
    lowPt[id[v]] = Counter;
    typedef boost::graph_traits<theGraph>::edge_descriptor Edge;
    boost::graph_traits<theGraph>::out_edge_iterator out_i, out_end;
    for( boost::tie(out_i,out_end) = out_edges(v,t); out_i != out_end; ++out_i){
        Edge e = *out_i;
        Vertex_t w = target(e,t);
        if(visited[id[w]] == 999999){
            edges.push_back(e);
            biconnect(w,edges,Counter,components, visited,lowPt,stackCount);
            lowPt[id[v]] = lowPt[id[v]] < lowPt[id[w]]?lowPt[id[v]]:lowPt[id[w]];
            if(lowPt[id[w]] > lowPt[id[v]]){
                std::vector<Edge_t> temp;
                std::vector<Edge_t>::iterator it;
                for(it = edges.begin();it != edges.end(); it++){
                    Edge e2 = *it;
                    Vertex_t w2 = target(e2,t);
                    if(visited[id[w2]] > visited[id[v]]){
                        temp.push_back(e2);
                        edges.erase(std::remove(edges.begin(), edges.end(), e2), edges.end());
                    }
                }
                temp.push_back(e);
                edges.erase(std::remove(edges.begin(), edges.end(), e), edges.end());
                components.push_back(temp);
            }
        }else if(visited[id[w]] < visited[id[v]] & w != v){
            edges.push_back(e);
            lowPt[id[v]] = lowPt[id[v]] < lowPt[id[w]]?lowPt[id[v]]:visited[id[w]];
        }
    }
}

UtilityStructs::StorageItems Tarjan::ApplySCC(bool debugMode) {
    float ms_duration = 0;
    v_p id = get(&VertexProperty::index, t);
    typedef boost::graph_traits<theGraph>::vertex_descriptor Vertex_t;
    typedef boost::graph_traits<theGraph>::vertex_iterator vertex_iter;
    std::pair<vertex_iter, vertex_iter> vp;
    std::vector<Vertex_t> Points;
    int Counter = 0;
    int SizeOfGraph = num_vertices(t);
    std::vector<int> visited(SizeOfGraph,999999);
    std::vector<int> lowPt(SizeOfGraph,999999);
    std::vector<int> lowVine(SizeOfGraph,999999);
    int stackCount = 0;
   // std::cout << "\nTarjan version of the SCC Algorithm has processed the graph" << std::endl;
    //TIMER
    {
        UtilityStructs::Timer timer;
    for (vp = vertices(t); vp.first != vp.second; vp.first++) {
        Vertex_t v = *vp.first;
        if (visited[id[v]] == 999999) {
            StrongConnect(v,Points, Counter,visited,lowPt,lowVine,stackCount,debugMode);
        }
    }
    ms_duration = timer.stop();
    }
    //TIMER end
    //Storage info collection
    size_t total_bytes = 0;
    total_bytes += sizeof(visited[0]) * visited.size();
    total_bytes += sizeof(lowPt[0]) * lowPt.size();
    total_bytes += sizeof(lowVine[0]) * lowVine.size();
    total_bytes +=sizeof(Points[0]) * stackCount;
    UtilityStructs::StorageItems s;
    s.vertexCount = num_vertices(t);
    s.edgeCount = num_edges(t);
    s.duration = ms_duration;
    s.total_bytes = total_bytes;
    Visualize vis;
    if(debugMode){
        vis.printProgramBottom();
         std::stringstream buffer3;
        buffer3 << std::setw(40) << std::left <<"Completion Time" ;
        buffer3 << ": " << s.duration << " miliseconds" ;
        std::stringstream buffer4;
        buffer4 << std::setw(40)<< std::left << "Total Execution Size";
        buffer4 << ": " << s.total_bytes / 1024.f << " KBs";
        vis.printLine(buffer3.str());
        vis.printLine(buffer4.str());
}
    return s;
}

void Tarjan::StrongConnect(Vertex_t &v,
                            std::vector<Vertex_t> &Points,
                            int &Counter,
                            std::vector<int> &visited,
                            std::vector<int> &lowPt,
                            std::vector<int> &lowVine,
                            int &stackCount,
                            bool debugMode) {
    e_p arch_type = get(&EdgeProperty::name, t);
    v_p id = get(&VertexProperty::index,t);
    Visualize vis;
    Counter++;
    visited[id[v]]= Counter;
    lowPt[id[v]]= Counter;
    lowVine[id[v]] = Counter;
    Points.push_back(v);
    stackCount++;
    typedef boost::graph_traits<theGraph>::edge_descriptor Edge;
    boost::graph_traits<theGraph>::out_edge_iterator out_i, out_end;
    for (boost::tie(out_i, out_end) = out_edges(v, t); out_i != out_end; ++out_i) {
        Edge e = *out_i;
        Vertex_t w = target(e, t);
        if (visited[id[w]] == 999999) {
            arch_type[e] = "tree"; // like in tarjan's algorithm
            StrongConnect(w,Points, Counter,visited,lowPt,lowVine,stackCount,debugMode);
            lowPt[id[v]] = lowPt[id[v]]< lowPt[id[w]]?lowPt[id[v]]:lowPt[id[w]];
            lowVine[id[v]] = lowVine[id[v]]< lowVine[id[w]]?lowVine[id[v]]:lowVine[id[w]];
        } else if (!isReachable( w, v)) {
            if (visited[id[w]] < visited[id[v]]) {
                arch_type[e] = "vine";
                if (std::find(Points.begin(), Points.end(), w) != Points.end()) {
                    lowVine[id[v]] = lowVine[id[v]] < visited[id[w]] ? lowVine[id[v]] : visited[id[w]];
                }
            }
        } else {
            arch_type[e] = "frond";
            lowPt[id[v]] = lowPt[id[v]] < visited[id[w]] ? lowPt[id[v]] : visited[id[w]];
        }
    }
    if (lowPt[id[v]] == visited[id[v]] && lowVine[id[v]] == visited[id[v]]) {
            int count_component = 0;
            std::stringstream buffer ;
            if(debugMode){
                vis.printLine("");
                buffer << std::setw(40) << std::left << "The Strongly Connected Component is" ;
                 buffer << ": ";
                 count_component++;
            }

        while(!Points.empty() && visited[id[Points.back()]] >= visited[id[v]]){
            Vertex_t x = Points.back();
            Points.pop_back();
            buffer << id[x]+1 <<  " " ;
        }
       if(debugMode){
        vis.printLine(buffer.str());
        std::stringstream buffer2;
        buffer2 << std::setw(40) << std::left << "Number of elements in the component is" ;
        buffer2 << ": "<< count_component ;
        vis.printLine(buffer2.str());
       }
    }

}


/*
 * poh_color.hpp
 * Author: Aven Bross
 *
 * Implementation of Poh path 3-coloring algorithm for triangulated plane graphs
 * that traces a chordless path along the inside of the outer face.
 */

#ifndef __POH_COLOR_HPP
#define __POH_COLOR_HPP

// STL headers
#include <vector>
#include <queue>
#include <stdexcept>
#include <utility>

// Basic graph headers
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>

// Local project headers
#include "incidence_list_helpers.hpp"

/*
 * poh_color_recursive
 *
 * assumptions: There are two disjoint colored paths P=p_0...p_n and Q=q_0...q_m
 *     such that p_0...p_nq_m...q_0 is a cycle, and all vertices on the interior
 *     of the subgrapph bounded by this cycle with at least one neighbor in P
 *     have been marked with an identifying integer.
 *
 * inputs: A weakly triangulated planar graph with vertex indices (predfined
 *     boost property), a valid planar embedding of the graph modeling the boost
 *     PlanarEmbedding concept, a read-write-able vertex property map to store
 *     vertex colors, a read-write-able vertex property map to store integer
 *     marks for BFS, a read-write-able vertex property map to store start and
 *     stop iterators in each incidence list, the vertex u such that p_0uq_0
 *     is a triangle, the integer mark for vertices with neighbors in P, an
 *     unsigned integer count such that all vertex marks assigned so far are
 *     less than count, and finally the color for the path T to be constructed,
 *     the color of the path P, and the color of the path Q.
 *
 * output: The coloring vertex property will contain a valid path 3-coloring of
 *     the subgraph bounded by p_0...p_nq_m...q_0 such that no interior vertex
 *     shares a color with a neighbor in P or Q.
 */

namespace {
    template<
            typename graph_t, typename planar_embedding_t, typename color_map_t,
            typename mark_map_t, typename neighbor_range_map_t,
            typename color_t, typename vertex_t
                = typename boost::graph_traits<graph_t>::vertex_descriptor
        >
    void poh_color_recursive(
            const graph_t & graph, const planar_embedding_t & planar_embedding,
            color_map_t & color_map, mark_map_t & mark_map,
            neighbor_range_map_t & neighbor_range_map, vertex_t u,
            int face_mark, int count,
            color_t t_color, color_t p_color, color_t q_color
        )
    {
        // t_i will track the last vertex of T, and w the first interior vertex
        vertex_t t_i = u, w = u;

        // Initialize remaining variables for the loop constructing T
        auto edge_iter = neighbor_range_map[u].first;
        int below_t_mark = count++;
        color_map[u] = t_color;

        // If u is the only remaining interior vertex we are are done
        if(neighbor_range_map[u].first == neighbor_range_map[u].second){
            return;
        }

        // Construct the path T one vertex at a time, with current vertex t_i
        while(edge_iter++ != neighbor_range_map[t_i].second)
        {
            // If we hit the end of the incidence list, wrap to the start
            if(edge_iter == planar_embedding[t_i].end())
                edge_iter = planar_embedding[t_i].begin();

            vertex_t n = get_incident_vertex(t_i, *edge_iter, graph);

            // Note if the vertex n may be added to T
            bool continue_path = (mark_map[n] == face_mark
                && color_map[n] != p_color && color_map[n] != q_color);

            // If we will add n to T or we hit the end, color vertices above T
            if(continue_path || color_map[n] == p_color) {
                if(continue_path) {
                    color_map[n] = t_color;
                }

                vertex_t l = n;

                // Loop through neighbors of t_i that lie above T
                while(edge_iter++ != neighbor_range_map[t_i].second) {
                    // If we hit the end of the list, wrap to the start
                    if(edge_iter == planar_embedding[t_i].end())
                        edge_iter = planar_embedding[t_i].begin();

                    vertex_t v = get_incident_vertex(t_i, *edge_iter, graph);

                    // If v is in P and l is uncolored, make recursive call
                    if(color_map[v] == p_color)
                    {
                        if(color_map[l] != p_color && color_map[l] != q_color
                            && color_map[l] != t_color)
                        {
                            poh_color_recursive(
                                    graph, planar_embedding, color_map,
                                    mark_map, neighbor_range_map, l, face_mark,
                                    count, q_color, p_color, t_color
                                );
                        }
                    }

                    l = v;
                }

                // If we are adding n to T, setup next loop with t_i = n
                if(continue_path) {
                    auto n_back_iter = find_edge_iterator_restricted(
                            n, t_i, neighbor_range_map[n].first,
                            neighbor_range_map[n].second, planar_embedding,
                            graph
                        );

                    neighbor_range_map[n].first = n_back_iter;
                    edge_iter = n_back_iter;
                    t_i = n;
                }
                // Otherwise we have completed T and we are done
                else {
                    return;
                }
            }
            // If n is in Q we have an edge T to Q and may split along it
            else if(color_map[n] == q_color) {
                // Color the left cycle if it has uncolored vertices
                if(w != u) {
                    poh_color_recursive(
                            graph, planar_embedding, color_map, mark_map,
                            neighbor_range_map, w, below_t_mark, count,
                            p_color, t_color, q_color
                        );

                    u = t_i;
                    w = t_i;
                }
            }
            // If n is interior and has not been visited, mark and initialize it
            else if(mark_map[n] != below_t_mark) {
                auto back_iter = find_edge_iterator(
                        n, t_i, planar_embedding, graph
                    );

                // If this is the first interior vertex hit, save it as w
                if(w == u) {
                    w = n;
                }

                // Increment back_iter counterclockwise, wrapping in list
                if(++back_iter == planar_embedding[n].end()) {
                    back_iter = planar_embedding[n].begin();
                }

                initialize_neighbor_range(
                        n, back_iter, neighbor_range_map, planar_embedding
                    );
                mark_map[n] = below_t_mark;
            }
        }
    }
}

/*
 * poh_color
 *
 * inputs: A weakly triangulated planar graph with vertex indices (predfined
 *     boost property), a valid planar embedding of the graph modeling the boost
 *     PlanarEmbedding concept, a read-write-able vertex property map to store
 *     vertex colors, pairs of bidirectional iterators for lists of vertices
 *     of two disjoint colored paths P=p_0...p_n and Q=q_0...q_m such that
 *     p_0...p_nq_m...q_0 is a cycle, and three colors for the path 3-coloring.
 *
 * output: The coloring vertex property will contain a valid path 3-coloring of
 *     the subgraph bounded by p_0...p_nq_m...q_0 such that vertices in P are
 *     recieve the first color, vertices in Q the second color, and no interior
 *     vertex shares a color with a neighbor in P or Q.
 */

template<
        typename graph_t,
        typename planar_embedding_t,
        typename color_map_t,
        typename mark_map_t,
        typename neighbor_range_map_t,
        typename vertex_iterator_t,
        typename color_t
    >
void poh_color(
        const graph_t & graph,
        const planar_embedding_t & planar_embedding,
        vertex_iterator_t p_begin, vertex_iterator_t p_end,
        vertex_iterator_t q_begin, vertex_iterator_t q_end,
        color_t c_0, color_t c_1, color_t c_2,
        neighbor_range_map_t & neighbor_range_map,
        mark_map_t & mark_map,
        color_map_t & color_map
    )
{
    // Type definitions
    typedef typename boost::graph_traits<graph_t>::vertex_descriptor vertex_t;

    // Intitialize neighbor ranges for vertices in the path P
    vertex_t l = *q_begin;
    for(vertex_iterator_t p_iter = p_begin; p_iter != p_end; ++p_iter) {
        vertex_t v = *p_iter;

        auto back_iter = find_edge_iterator(v, l, planar_embedding, graph);
        initialize_neighbor_range(
                v, back_iter, neighbor_range_map, planar_embedding
            );

        l = v;
    }

    // Color the path Q
    for(vertex_iterator_t q_iter = q_begin; q_iter != q_end; ++q_iter) {
        vertex_t v = *q_iter;

        color_map[v] = c_1;
    }

    // Construct the path 3-coloring
    poh_color_recursive(
            graph, planar_embedding, color_map, mark_map,
            neighbor_range_map, *p_begin, 1, 2,
            c_0, c_2, c_1
        );
}

/*
 * A wrapper that automatically constructs fast property maps for the mark_map
 * and neighbor_range_map, but requires that graph_t is some definition of
 * boost::adjacency_list.
 */

template<
        typename graph_t,
        typename planar_embedding_t,
        typename color_map_t,
        typename vertex_iterator_t,
        typename color_t
    >
void poh_color(
        const graph_t & graph,
        const planar_embedding_t & planar_embedding,
        vertex_iterator_t p_begin, vertex_iterator_t p_end,
        vertex_iterator_t q_begin, vertex_iterator_t q_end,
        color_t c_0, color_t c_1, color_t c_2,
        color_map_t & color_map
    )
{
    // Vertex property map type to store vertex marks
    typedef boost::iterator_property_map<
            std::vector<int>::iterator,
            typename boost::property_map<
                    graph_t, boost::vertex_index_t
                >::const_type
        > integer_property_map_t;

    // Vertex property map type for the neighbor ranges of planar_embedding_t
    typedef typename boost::property_traits<planar_embedding_t>::value_type
            ::const_iterator embedding_iterator_t;
    typedef typename std::vector<
            std::pair<embedding_iterator_t, embedding_iterator_t>
        > neighbor_range_storage_t;
    typedef boost::iterator_property_map<
            typename neighbor_range_storage_t::iterator,
            typename boost::property_map<
                    graph_t, boost::vertex_index_t
                >::const_type
        > neighbor_range_map_t;

    // Construct a vertex property map to store vertex marks
    std::vector<int> mark_storage(boost::num_vertices(graph));
    integer_property_map_t mark_map(
            mark_storage.begin(), boost::get(boost::vertex_index, graph)
        );

    // Construct a vertex property map to store neighbor ranges
    neighbor_range_storage_t neighbor_range_storage(num_vertices(graph));
    neighbor_range_map_t neighbor_range_map(
            neighbor_range_storage.begin(),
            boost::get(boost::vertex_index, graph)
        );

    // Construct the path 3-coloring
    poh_color(
            graph,
            planar_embedding,
            p_begin, p_end,
            q_begin, q_end,
            1, 2, 3,
            neighbor_range_map,
            mark_map,
            color_map
        );
}

#endif


#pragma once

#include <boost/functional/hash.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/laguerre.hpp>
#include <stdexcept>

#include "spectral/basis/spectral_function/spectral_function_base.hpp"
#include "spectral/basis/spectral_function/spectral_weight_function.hpp"

namespace boltzmann {

namespace local_ {
struct laguerreKS_id_t
{
 private:
  constexpr const static double FUZZY = 1e6;

 public:
  typedef laguerreKS_id_t id_t;

  /// Default constructor
  laguerreKS_id_t()
      : j(std::nan("nan"))
      , k(std::nan("nan"))
      , fw(std::nan("nan"))
      , idw(std::nan("nan"))
  {
  }

  laguerreKS_id_t(int k_, int j_, double fw_)
      : j(j_)
      , k(k_)
      , fw(fw_)
      , idw(FUZZY * fw_)
  {
  }

  laguerreKS_id_t(const id_t& id)
      : j(id.j)
      , k(id.k)
      , fw(id.fw)
      , idw(id.idw)
  {
  }

  /// order parameter
  int j;
  /// Polynomial degree
  int k;
  /// weight exponent
  double fw;
  /// weight id
  long int idw;

  bool operator<(const id_t& other) const
  {
    return std::tie(j, k, idw) < std::tie(other.j, other.k, other.idw);
    //#warning "fix this!"
    //    return k < other.k;
  }

  // ----------------------------------------------------------------------
  inline bool operator==(const id_t& other) const
  {
    return std::tie(j, k, idw) == std::tie(other.j, other.k, other.idw);
    //#warning "fix this!"
    //    return k == other.k;
  }

  // ----------------------------------------------------------------------
  friend std::ostream& operator<<(std::ostream& stream, const id_t& x)
  {
    stream << x.to_string();
    return stream;
  }

  // ----------------------------------------------------------------------
  std::string to_string() const
  {
    return "(fw_" + boost::lexical_cast<std::string>(fw) + ", k_" +
           boost::lexical_cast<std::string>(k) + ", j_" + boost::lexical_cast<std::string>(j) +
           ") ";
  }

  // ----------------------------------------------------------------------
  inline std::tuple<int, int, long int> key() const { return std::make_tuple(j, k, idw); }
};
}  // end namespace local_
}  // end namespace boltzmann

namespace std {
// hash functions for id's
template <>
class hash<boltzmann::local_::laguerreKS_id_t>
{
 public:
  size_t operator()(const boltzmann::local_::laguerreKS_id_t& id) const
  {
    std::size_t current = std::hash<double>()(id.fw);
    boost::hash_combine(current, std::hash<int>()(id.k));
    boost::hash_combine(current, std::hash<int>()(id.j));
    return current;
  }
};
}  // end namespace std

namespace boltzmann {

/**
 * @brief Laguerre Radial Polynomial used by Kitzler & Schoeberl
 *
 * Normalization & Orthogonality
 * =============================
 *
 * For \f$ k_1 \equiv k_2 \operatorname{mod} 2 \f$, \f$k_1\f$ even:
 * \f[
 *   \int_0^{\infty} r^{2j}  r^{2j} L^{(2j)}_{\frac{k_1}{2}-j}(r^2)
 * L^{(2j)}_{\frac{k_2}{2}-j} (r^2) e^{-r^2}\; r \operatorname{d} r= \frac{1}{2} \delta_{k_1, k_2}
 * \f]
 *
 *  For \f$ k_1 \equiv k_2 \operatorname{mod} 2 \f$, \f$k_1\f$ odd:
 * \f[
 *   \int_0^{\infty} r^{2j+1}  r^{2j+1} L^{(2j+1)}_{\frac{k_1-1}{2}-j}(r^2)
 * L^{(2j+1)}_{\frac{k_2-1}{2}-j} (r^2) e^{-r^2}\; r \operatorname{d} r= \frac{1}{2} \delta_{k_1,
 * k_2}
 * \f]
 *
 */
class LaguerreKS : public weighted<LaguerreKS, true>,
                   public local_::index_policy<local_::laguerreKS_id_t>
{
 public:
  typedef double numeric_t;

 public:
  /**
   * @brief Basis functions of the form \f$ r^{2*j + k\mod2} L_{(k-k\mod2)/2-j}^{2*j + k\mod2}(r^2)
   * \f$
   *
   *
   * @param k  polynomial degree
   * @param j  angular parameter
   * @param w  weight
   *
   * @return
   */
  explicit LaguerreKS(int k, int j, double w = 0.5);
  explicit LaguerreKS(const id_t& id)
      : id_(id)
  {
  }
  LaguerreKS(){};

  /// evaluate polynomial part
  numeric_t evaluate(double r) const;

  /// evaluate weight
  numeric_t weight(double r) const;

  /// return weight
  numeric_t w() const { return id_.fw; }

  const id_t& get_id() const { return id_; }

  unsigned int get_order() const { return 2 * id_.j + id_.k % 2; }
  unsigned int get_degree() const { return id_.k / 2 - id_.j; }

 private:
  id_t id_;
};

// ----------------------------------------------------------------------
inline LaguerreKS::LaguerreKS(int k, int j, double w)
    : id_(k, j, w)
{
  if (j > k / 2) {
    std::runtime_error("invalid combination of parameters in LaguerreKS");
  }
}

// ----------------------------------------------------------------------
inline typename LaguerreKS::numeric_t
LaguerreKS::evaluate(double r) const
{
  int alpha = 2 * id_.j + (id_.k % 2);
  int n = id_.k / 2 - id_.j;

  // normalization factor
  double a = 1;
  for (int i = n + alpha; i > n; --i) {
    a *= i;
  }

  return std::pow(r, 2 * id_.j + (id_.k % 2)) * boost::math::laguerre(n, alpha, r * r) /
         std::sqrt(a);
}

// ----------------------------------------------------------------------
inline typename LaguerreKS::numeric_t
LaguerreKS::weight(double r) const
{
  return std::exp(-r * r * id_.fw);
}

}  // end namespace boltzmann


#pragma once
#include <random>
#include <bitset>
#include <fstream>
#include <ios>
#include <string>
#include <cassert>
#include <Eigen/Eigen>

#include <tbb/tbb.h>

#include <nlohmann/json.hpp>

#include "Utilities/type_traits.hpp"
#include "Utilities/Utility.hpp"
#include "Serializers/SerializeEigen.hpp"

namespace yannq
{
//! \ingroup Machines
//! RBM machine
template<typename T>
class RBM
{
	static_assert(std::is_floating_point<T>::value || is_complex_type<T>::value, "T must be floating or complex");
public:
	using Scalar = T;
	using RealScalar = typename yannq::remove_complex<T>::type;

	using Matrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
	using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
	using VectorRef = Eigen::Ref<Vector>;
	using VectorConstRef = Eigen::Ref<const Vector>;

	using RealVector = Eigen::Matrix<RealScalar, Eigen::Dynamic, 1>;

	using DataT = std::tuple<Eigen::VectorXi, Vector>;

private:
	uint32_t n_; //# of qubits
	uint32_t m_; //# of hidden units

	bool useBias_;

	Matrix W_; //W should be m by n
	Vector a_; //a is length n
	Vector b_; //b is length m

public:
	RBM(uint32_t n, uint32_t m, bool useBias = true) noexcept
		: n_(n), m_(m), useBias_(useBias), W_(m, n), a_(n), b_(m)
	{
		a_.setZero();
		b_.setZero();
		W_.setZero();
	}

	RBM() noexcept
		: useBias_{true}
	{
	}

	template<typename U, std::enable_if_t<std::is_convertible_v<U, T> && !std::is_same_v<U, T>, int> = 0>
	RBM(const RBM<U>& rhs)
		: n_{rhs.getN()}, m_{rhs.getM()}, useBias_{rhs.useBias()}
	{
		W_ = rhs.getW().template cast<T>();
		a_ = rhs.getA().template cast<T>();
		b_ = rhs.getB().template cast<T>();
	}

	RBM(const RBM& rhs) /* noexcept */ = default;
	RBM(RBM&& rhs) /* noexcept */ = default;

	template<typename U, std::enable_if_t<std::is_convertible_v<U, T> && !std::is_same_v<U, T>, int> = 0>
	RBM& operator=(const RBM<U>& rhs)
	{
		n_ = rhs.n_;
		m_ = rhs.m_;
		useBias_ = rhs.useBias_;

		W_ = rhs.W_.template cast<T>();
		a_ = rhs.a_.template cast<T>();
		b_ = rhs.b_.template cast<T>();

		return *this;
	}

	RBM& operator=(const RBM& rhs) /* noexcept */ = default;
	RBM& operator=(RBM&& rhs) /* noexcept */ = default;

	template<typename U, std::enable_if_t<std::is_convertible_v<T, U>, int> = 0>
	RBM<U> cast() const
	{
		RBM<U> res(n_, m_, useBias_);
		res.setA(a_.template cast<U>());
		res.setB(b_.template cast<U>());
		res.setW(W_.template cast<U>());
		return res;
	}

	nlohmann::json desc() const
	{
		return nlohmann::json
		{
			{"name", "RBM"},
			{"useBias", useBias_},
			{"n", n_},
			{"m", m_}
		};
	}

	inline uint32_t getN() const
	{
		return n_;
	}
	inline uint32_t getM() const
	{
		return m_;
	}

	inline uint32_t getDim() const
	{
		if(useBias_)
			return n_*m_ + n_ + m_;
		else
			return n_*m_;
	}

	inline bool useBias() const
	{
		return useBias_;
	}

	inline Vector calcTheta(const Eigen::VectorXi& sigma) const
	{
		Vector s = sigma.cast<T>();
		return W_*s + b_;
	}
	inline Vector calcGamma(const Eigen::VectorXi& hidden) const
	{
		Vector h = hidden.cast<T>();
		return W_.transpose()*h + a_;
	}

	void setUseBias(bool newBias)
	{
		useBias_ = newBias;
	}

	void resize(uint32_t n, uint32_t m)
	{
		n_ = n;
		m_ = m;

		a_.resize(n);
		b_.resize(m);
		W_.resize(m,n);

		if(!useBias_)
		{
			a_.setZero();
			b_.setZero();
		}
	}

	void conservativeResize(uint32_t newM)
	{
		Vector newB = Vector::Zero(newM);
		newB.head(m_) = b_;

		Matrix newW = Matrix::Zero(newM, n_);
		newW.topRows(m_) = W_;

		m_ = newM;
		b_ = std::move(newB);
		W_ = std::move(newW);
	}

	void setW(const Eigen::Ref<const Matrix>& m)
	{
		assert(m.rows() == W_.rows() && m.cols() == W_.cols());
		W_ = m;
	}

	void setA(const VectorConstRef& A)
	{
		assert(A.size() == a_.size());
		if(!useBias_)
			return ;
		a_ = A;
	}

	void setB(const VectorConstRef& B)
	{
		assert(B.size() == b_.size());
		if(!useBias_)
			return ;
		b_ = B;
	}

	inline const T& W(uint32_t j, uint32_t i) const
	{
		return W_.coeff(j,i);
	}
	inline const T& A(uint32_t i) const
	{
		return a_.coeff(i);
	}
	inline const T& B(uint32_t j) const
	{
		return b_.coeff(j);
	}

	inline T& W(uint32_t j, uint32_t i)
	{
		return W_.coeffRef(j,i);
	}
	inline T& A(uint32_t i)
	{
		return a_.coeffRef(i);
	}
	inline T& B(uint32_t j)
	{
		return b_.coeffRef(j);
	}

	const Matrix& getW() const & { return W_; }
	Matrix getW() && { return std::move(W_); }

	const Vector& getA() const & { return a_; }
	Vector getA() && { return std::move(a_); }

	const Vector& getB() const & { return b_; }
	Vector getB() && { return std::move(b_); }

	//! update Bias A by adding v
	void updateA(const VectorConstRef& v)
	{
		assert(useBias_);
		a_ += v;
	}
	//! update Bias B by adding v
	void updateB(const VectorConstRef& v)
	{
		assert(useBias_);
		b_ += v;
	}
	//! update the weight W by adding m
	void updateW(const Eigen::Ref<const Matrix>& m)
	{
		W_ += m;
	}

	//! update all parameters.
	void updateParams(const VectorConstRef& m)
	{
		assert(m.size() == getDim());
		W_ += Eigen::Map<const Matrix>(m.data(), m_, n_);
		if(!useBias_)
			return ;
		a_ += Eigen::Map<const Vector>(m.data() + m_*n_, n_);
		b_ += Eigen::Map<const Vector>(m.data() + m_*n_ + n_, m_);
	}

	Vector getParams() const
	{
		Vector res(getDim());
		res.head(n_*m_) = Eigen::Map<const Vector>(W_.data(), W_.size());
		if(!useBias_)
			return res;

		res.segment(n_*m_, n_) = a_;
		res.segment(n_*m_ + n_, m_) = b_;
		return res;
	}

	void setParams(const VectorConstRef& r)
	{
		assert(r.size() == getDim());
		Eigen::Map<Vector>(W_.data(), W_.size()) = r.head(n_*m_);
		if(!useBias_)
			return ;
		a_ = r.segment(n_*m_, n_);
		b_ = r.segment(n_*m_ + n_, m_);
	}

	bool hasNaN() const
	{
		return a_.hasNaN() || b_.hasNaN() || W_.hasNaN();
	}

	/* When T is real type */
	template <typename RandomEngine, class U=T,
            	std::enable_if_t < !is_complex_type<U>::value, int > = 0 >
	void initializeRandom(RandomEngine& re, T sigma = 1e-3)
	{
		std::normal_distribution<T> nd{0, sigma};
		if(useBias_)
		{
			for(uint32_t i = 0u; i < n_; i++)
			{
				a_.coeffRef(i) = nd(re);
			}
			for(uint32_t i = 0u; i < m_; i++)
			{
				b_.coeffRef(i) = nd(re);
			}
		}
		for(uint32_t j = 0u; j < n_; j++)
		{
			for(uint32_t i = 0u; i < m_; i++)
			{
				W_.coeffRef(i, j) = nd(re);
			}
		}
	}

	/* When T is complex type */
	template <typename RandomEngine, class U=T,
               std::enable_if_t < is_complex_type<U>::value, int > = 0 >
	void initializeRandom(RandomEngine& re, typename remove_complex<T>::type sigma = 1e-3)
	{
		std::normal_distribution<typename remove_complex<T>::type> nd{0, sigma};

		if(useBias_)
		{
			for(uint32_t i = 0u; i < n_; i++)
			{
				a_.coeffRef(i) = T{nd(re), nd(re)};
			}
			for(uint32_t i = 0u; i < m_; i++)
			{
				b_.coeffRef(i) = T{nd(re), nd(re)};
			}
		}
		for(uint32_t j = 0; j < n_; j++)
		{
			for(uint32_t i = 0u; i < m_; i++)
			{
				W_.coeffRef(i, j) = T{nd(re), nd(re)};
			}
		}
	}

	bool operator==(const RBM<T>& rhs) const
	{
		if(n_ != rhs.n_ || m_ != rhs.m_)
			return false;
		bool equalW = (W_ == rhs.W_);
		if(useBias_)
			return equalW && (a_ == rhs.a_) && (b_ == rhs.b_);
		else
			return equalW;
	}

	std::tuple<Eigen::VectorXi, Vector> makeData(const Eigen::VectorXi& sigma) const
	{
		return std::make_tuple(sigma, calcTheta(sigma));
	}

	T logCoeff(const std::tuple<Eigen::VectorXi, Vector>& t) const
	{
		using std::cosh;

		Vector ss = std::get<0>(t).template cast<T>();
		T s = a_.transpose()*ss;
		for(uint32_t j = 0u; j < m_; j++)
		{
			s += logCosh(std::get<1>(t).coeff(j));
		}
		return s;
	}

	T coeff(const std::tuple<Eigen::VectorXi, Vector>& t) const
	{
		using std::cosh;

		Vector ss = std::get<0>(t).template cast<T>();
		T s = a_.transpose()*ss;
		T p = exp(s) * std::get<1>(t).array().cosh().prod();
		return p;
	}

	Vector logDeriv(const std::tuple<Eigen::VectorXi, Vector>& t) const
	{
		Vector res(getDim());

		Vector tanhs = std::get<1>(t).array().tanh();
		Vector sigma = std::get<0>(t).template cast<Scalar>();

		for(uint32_t i = 0u; i < n_; i++)
		{
			res.segment(i*m_, m_) = sigma(i)*tanhs;
		}
		if(!useBias_)
			return res;
		res.segment(n_*m_, n_) = sigma;
		res.segment(n_*m_ + n_, m_) = tanhs;
		return res;
	}
};

template<typename T>
typename RBM<T>::Vector getPsi(const RBM<T>& qs, bool normalize)
{
	const uint32_t n = qs.getN();
	typename RBM<T>::Vector psi(1u<<n);
	tbb::parallel_for(0u, (1u << n),
		[n, &qs, &psi](uint32_t idx)
	{
		auto s = toSigma(n, idx);
		psi(idx) = qs.coeff(qs.makeData(s));
	});
	if(normalize)
		psi.normalize();
	return psi;
}

template<typename T, typename Iterable> //Iterable must be random access iterable
typename RBM<T>::Vector getPsi(const RBM<T>& qs, Iterable&& basis, bool normalize)
{
	const uint32_t n = qs.getN();
	typename RBM<T>::Vector psi(basis.size());

	tbb::parallel_for(std::size_t(0u), basis.size(),
		[n, &qs, &psi, &basis](std::size_t idx)
	{
		auto s = toSigma(n, basis[idx]);
		psi(idx) = qs.coeff(qs.makeData(s));
	});
	if(normalize)
		psi.normalize();
	return psi;
}
template<typename T>
typename RBM<T>::RealVector getProbs(const RBM<T>& qs, bool normalize)
{
	return getPsi(qs, normalize).cwiseAbs2();
}

template<typename T, typename Iterable> //Iterable must be random access iterable
typename RBM<T>::RealVector getProbs(const RBM<T>& qs, Iterable&& basis, bool normalize)
{
	return getPsi(qs, std::forward<Iterable>(basis), normalize).cwiseAbs2();
}
}//namespace yannq


#include <numpy_eigen/boost_python_headers.hpp>

#include <aslam/backend/test/SampleDvAndError.hpp>

#include <boost/shared_ptr.hpp>

using namespace boost::python;
using namespace aslam::backend;

void exportSampleDvAndError()
{

    class_<Point2d, boost::shared_ptr<Point2d>, bases<DesignVariable> >
      ("Point2d", init<const Eigen::Vector2d&>("Point2d(vector2d v): Constructor"))
      ;

    class_<LinearErr, boost::shared_ptr<LinearErr>, bases< aslam::backend::ErrorTermFs<2> > >
      ("LinearErr", init<Point2d*>("LinearErr(Point2d p): Constructor"))
      ;

    class_<LinearErr2, boost::shared_ptr<LinearErr2>, bases< aslam::backend::ErrorTermFs<2> > >
      ("LinearErr2", init<Point2d*,Point2d*>("LinearErr2(Point2d p0, Point2d p1): Constructor"))
      ;

    class_<LinearErr3, boost::shared_ptr<LinearErr3>, bases< aslam::backend::ErrorTermFs<4> > >
      ("LinearErr3", init<Point2d*,Point2d*,Point2d*>("LinearErr3(Point2d p0, Point2d p1, Point2d p2): Constructor"))
      ;

    class_<TestNonSquaredError, boost::shared_ptr<TestNonSquaredError>, bases<ScalarNonSquaredErrorTerm> >
      ("TestNonSquaredError", init<Point2d*, const TestNonSquaredError::grad_t&>("TestNonSquaredError(Point2d p, Vector2d grad): Constructor"))
      .def_readwrite("_p", &TestNonSquaredError::_p)
      .def_readwrite("_grad", &TestNonSquaredError::_grad)
      .def_readwrite("_p2d", &TestNonSquaredError::_p2d)
      ;

}


// Copyright Aleksey Gurtovoy 2001-2004
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id: integer.cpp 49268 2008-10-11 06:26:17Z agurtovoy $
// $Date: 2008-10-11 02:26:17 -0400 (Sat, 11 Oct 2008) $
// $Revision: 49268 $

#include <boost/mpl/multiplies.hpp>
#include <boost/mpl/list.hpp>
#include <boost/mpl/lower_bound.hpp>
#include <boost/mpl/transform_view.hpp>
#include <boost/mpl/sizeof.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/identity.hpp>
#include <boost/mpl/base.hpp>
#include <boost/mpl/eval_if.hpp>
#include <boost/mpl/deref.hpp>
#include <boost/mpl/begin_end.hpp>
#include <boost/mpl/assert.hpp>

#include <boost/type_traits/is_same.hpp>

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

template< int bit_size >
class big_int
{
    // ...
};

template< int bit_size >
struct integer
{
    typedef mpl::list<char,short,int,long> builtins_;
    typedef typename mpl::base< typename mpl::lower_bound<
          mpl::transform_view< builtins_
            , mpl::multiplies< mpl::sizeof_<_1>, mpl::int_<8> >
            >
        , mpl::int_<bit_size>
        >::type >::type iter_;

    typedef typename mpl::end<builtins_>::type last_;
    typedef typename mpl::eval_if<
          boost::is_same<iter_,last_>
        , mpl::identity< big_int<bit_size> >
        , mpl::deref<iter_>
        >::type type;
};

typedef integer<1>::type int1;
typedef integer<5>::type int5;
typedef integer<15>::type int15;
typedef integer<32>::type int32;
typedef integer<100>::type int100;

BOOST_MPL_ASSERT(( boost::is_same< int1, char > ));
BOOST_MPL_ASSERT(( boost::is_same< int5, char > ));
BOOST_MPL_ASSERT(( boost::is_same< int15, short > ));
BOOST_MPL_ASSERT(( boost::is_same< int32, int > ));
BOOST_MPL_ASSERT(( boost::is_same< int100, big_int<100> > ));