Problem p00528 (C++) — buggy submission, verdict: Time Limit Exceeded

#include <algorithm>
#include <climits>
#include <functional>
#include <iostream>
#include <queue>
#include <tuple>
#include <vector>
struct Node {
public:
  std::vector<std::tuple<int, Node *>> edges{};
  long long int minCost{LLONG_MAX};
  int x{0}, y{0};
};
struct MinCost {
  MinCost(long long int c, int h, Node *n) : cost{c}, heuristic{h}, node{n} {};
  long long int cost, heuristic;
  Node *node;
  bool operator>(const MinCost &other) const {
    return (cost + heuristic) > (other.cost + other.heuristic);
  }
  bool operator<(const MinCost &other) const {
    return (cost + heuristic) < (other.cost + other.heuristic);
  }
};
int main() {
  int m, n, k;
  std::cin >> m >> n >> k;
  std::vector<Node> nodes(k * 2);
  auto goal = Node();
  goal.x = m;
  goal.y = n;
  std::vector<std::vector<Node *>> orderedByX(m + 1, std::vector<Node *>());
  std::vector<std::vector<Node *>> orderedByY(n + 1, std::vector<Node *>());
  for (auto i = 0; i < k; ++i) {
    std::cin >> nodes[i * 2].x >> nodes[i * 2].y;
    nodes[i * 2 + 1].x = nodes[i * 2].x;
    nodes[i * 2 + 1].y = nodes[i * 2].y;
    nodes[i * 2].edges.push_back(std::make_tuple(1, &nodes[i * 2 + 1]));
    nodes[i * 2 + 1].edges.push_back(std::make_tuple(1, &nodes[i * 2]));
    orderedByX[nodes[i * 2].x].push_back(&nodes[i * 2]);
    orderedByY[nodes[i * 2 + 1].y].push_back(&nodes[i * 2 + 1]);
  }
  for (auto i = 1; i < orderedByX.size(); ++i) {
    std::sort(orderedByX[i].begin(), orderedByX[i].end(),
              [](const Node *a, const Node *b) -> bool { return a->y < b->y; });
  }
  for (auto i = 1; i < orderedByY.size(); ++i) {
    std::sort(orderedByY[i].begin(), orderedByY[i].end(),
              [](const Node *a, const Node *b) -> bool { return a->x < b->x; });
  }
  for (auto &node : orderedByX) {
    for (auto i = 1; i < node.size(); ++i) {
      node[i - 1]->edges.push_back(
          std::make_tuple(node[i]->y - node[i - 1]->y, node[i]));
      node[i]->edges.push_back(
          std::make_tuple(node[i]->y - node[i - 1]->y, node[i - 1]));
    }
  }
  for (auto &node : orderedByY) {
    for (auto i = 1; i < node.size(); ++i) {
      node[i - 1]->edges.push_back(
          std::make_tuple(node[i]->x - node[i - 1]->x, node[i]));
      node[i]->edges.push_back(
          std::make_tuple(node[i]->x - node[i - 1]->x, node[i - 1]));
    }
  }
  if (orderedByX[m].size() != 0) {
    orderedByX[m].back()->edges.push_back(
        std::make_tuple(n - orderedByX[m].back()->y, &goal));
  }
  if (orderedByY[n].size() != 0) {
    orderedByY[n].back()->edges.push_back(
        std::make_tuple(m - orderedByY[n].back()->x, &goal));
  }
  if (orderedByX[1].size() == 0) {
    std::cout << -1 << std::endl;
    return 0;
  }
  auto queue = std::priority_queue<MinCost, std::vector<MinCost>,
                                   std::greater<MinCost>>();
  orderedByX[1][0]->minCost = orderedByX[1][0]->y - 1;
  queue.push(MinCost{orderedByX[1][0]->minCost, 0, orderedByX[1][0]});
  while (!queue.empty() && queue.top().node != &goal) {
    auto top = queue.top();
    queue.pop();
    if (top.cost == top.node->minCost) {
      for (auto edge : top.node->edges) {
        if (std::get<1>(edge)->minCost > top.cost + std::get<0>(edge)) {
          std::get<1>(edge)->minCost = top.cost + std::get<0>(edge);
          queue.push(
              MinCost(std::get<1>(edge)->minCost,
                      m + n - std::get<1>(edge)->y - std::get<1>(edge)->x,
                      std::get<1>(edge)));
        }
      }
    }
  }
  if (queue.empty()) {
    std::cout << -1 << std::endl;
  } else {
    std::cout << goal.minCost << std::endl;
  }
}


Error: TLE

Fixed submission:

#include <algorithm>
#include <climits>
#include <functional>
#include <iostream>
#include <queue>
#include <tuple>
#include <vector>
struct Node {
public:
  std::vector<std::tuple<int, Node *>> edges{};
  int minCost{INT_MAX};
  int x{0}, y{0};
};
struct MinCost {
  int cost, heuristic;
  Node *node;
  bool operator>(const MinCost &other) const {
    return (cost + heuristic) > (other.cost + other.heuristic);
  }
  bool operator<(const MinCost &other) const {
    return (cost + heuristic) < (other.cost + other.heuristic);
  }
};
int main() {
  int m, n, k;
  std::cin >> m >> n >> k;
  std::vector<Node> nodes(k * 2);
  std::vector<std::vector<Node *>> orderedByX(m + 1, std::vector<Node *>());
  std::vector<std::vector<Node *>> orderedByY(n + 1, std::vector<Node *>());
  for (auto i = 0; i < k; ++i) {
    std::cin >> nodes[i * 2].x >> nodes[i * 2].y;
    nodes[i * 2 + 1].x = nodes[i * 2].x;
    nodes[i * 2 + 1].y = nodes[i * 2].y;
    nodes[i * 2].edges.push_back(std::make_tuple(1, &nodes[i * 2 + 1]));
    nodes[i * 2 + 1].edges.push_back(std::make_tuple(1, &nodes[i * 2]));
    orderedByX[nodes[i * 2].x].push_back(&nodes[i * 2]);
    orderedByY[nodes[i * 2 + 1].y].push_back(&nodes[i * 2 + 1]);
  }
  for (auto i = 1; i < orderedByX.size(); ++i) {
    std::sort(orderedByX[i].begin(), orderedByX[i].end(),
              [](const Node *a, const Node *b) -> bool { return a->y < b->y; });
  }
  for (auto i = 1; i < orderedByY.size(); ++i) {
    std::sort(orderedByY[i].begin(), orderedByY[i].end(),
              [](const Node *a, const Node *b) -> bool { return a->x < b->x; });
  }
  for (auto &node : orderedByX) {
    for (auto i = 1; i < node.size(); ++i) {
      node[i - 1]->edges.push_back(
          std::make_tuple(node[i]->y - node[i - 1]->y, node[i]));
      node[i]->edges.push_back(
          std::make_tuple(node[i]->y - node[i - 1]->y, node[i - 1]));
    }
  }
  for (auto &node : orderedByY) {
    for (auto i = 1; i < node.size(); ++i) {
      node[i - 1]->edges.push_back(
          std::make_tuple(node[i]->x - node[i - 1]->x, node[i]));
      node[i]->edges.push_back(
          std::make_tuple(node[i]->x - node[i - 1]->x, node[i - 1]));
    }
  }
  auto queue = std::priority_queue<MinCost>();
  if (orderedByX[1].size() == 0) {
    std::cout << -1 << std::endl;
    return 0;
  }
  orderedByX[1][0]->minCost = orderedByX[1][0]->y - 1;
  queue.push(MinCost{orderedByX[1][0]->minCost, m + n - 2, orderedByX[1][0]});
  while (!queue.empty()) {
    auto top = queue.top();
    queue.pop();
    if (top.cost == top.node->minCost) {
      for (auto edge : top.node->edges) {
        if (std::get<1>(edge)->minCost > top.cost + std::get<0>(edge)) {
          std::get<1>(edge)->minCost = top.cost + std::get<0>(edge);
          queue.push(
              MinCost{std::get<1>(edge)->minCost,
                      m + n - std::get<1>(edge)->y - std::get<1>(edge)->x,
                      std::get<1>(edge)});
        }
      }
    }
  }
  int min = INT_MAX;
  if (orderedByX[m].size() != 0 && orderedByX[m].back()->minCost != INT_MAX) {
    if (min > orderedByX[m].back()->minCost - orderedByX[m].back()->y + n)
      min = orderedByX[m].back()->minCost - orderedByX[m].back()->y + n;
  }
  if (orderedByY[n].size() != 0 && orderedByY[n].back()->minCost != INT_MAX) {
    if (min > orderedByY[n].back()->minCost - orderedByY[n].back()->x + m)
      min = orderedByY[n].back()->minCost - orderedByY[n].back()->x + m;
  }
  if (min == INT_MAX) {
    std::cout << -1 << std::endl;
  } else {
    std::cout << min << std::endl;
  }
}