Поиск кратчайшего пути в графе

Дан неориентированный граф, длины ребер в котором равны 0 или 1. Необходимо найти длину кратчайшего пути из вершины A в вершину B.
В первой строке входа задано 4 целых числа n, m, a, b количество вершин и ребер графа, номер вершины A и номер вершины B соответственно. Вершины пронумерованы от 1 до n. 1 ≤ m, n ≤ 100 000, 1 ≤ a, b ≤ n. В каждой из следующих m строк по три целых числа, первое из которых означает номер начальной вершины ребра, второе номер конечной вершины ребра, третье длину ребра (0 или 1).
Выведите длину кратчайшего пути из вершины A в вершину B . Если пути из A в B не существует, выведите -1.

#include <stdio.h>
#include <vector>
#include <algorithm>

using std::vector;
using std::swap;

const int kMaxDistance = 1000000;

struct NodeLengthPair {
  int node_;
  int length_;

  NodeLengthPair(int node, int length):
    node_(node), length_(length) {}
};

struct DistancePositionPair {
  int distance_;
  int heap_position_;
  DistancePositionPair(int distance, int heap_position):
    distance_(distance), heap_position_(heap_position) {}
};

void ReadInput(vector< vector<NodeLengthPair> >& graph_edges,
               int& source_node,
               int& destination_node) {
  int nodes_num, edges_num, source, destination;
  scanf("%d %d %d %d", &nodes_num, &edges_num, &source, &destination);
  graph_edges.resize(nodes_num);
  source_node = source - 1;
  destination_node = destination - 1;
  int node_from, node_to, length;
  for (int counter = 0; counter < edges_num; ++counter) {
    scanf("%d %d %d", &node_from, &node_to, &length);
    graph_edges[node_from - 1].push_back(NodeLengthPair(node_to - 1, length));
    graph_edges[node_to - 1].push_back(NodeLengthPair(node_from - 1, length));
  }
}

void Heapify(int root_index,
             vector<int>& nodes_heap,
             vector<DistancePositionPair>& distance) {
  int left_son_index, right_son_index, smallest_index;
  left_son_index = 2 * root_index + 1;
  right_son_index = 2 * root_index + 2;
  smallest_index = root_index;
  if (left_son_index < nodes_heap.size() &&
      distance[nodes_heap[left_son_index]].distance_ <
          distance[nodes_heap[smallest_index]].distance_) {
    smallest_index = left_son_index;
  }
  if (right_son_index < nodes_heap.size() &&
      distance[nodes_heap[right_son_index]].distance_ <
          distance[nodes_heap[smallest_index]].distance_) {
    smallest_index = right_son_index;
  }
  if (smallest_index != root_index) {
    swap(distance[nodes_heap[root_index]].heap_position_,
         distance[nodes_heap[smallest_index]].heap_position_);
    swap(nodes_heap[root_index], nodes_heap[smallest_index]);
    Heapify(smallest_index, nodes_heap, distance);
  }
}

void MakeHeap(vector<int>& nodes_heap,
              vector<DistancePositionPair>& distance) {
  for (int counter = nodes_heap.size() / 2; counter >= 0; --counter) {
    Heapify(counter, nodes_heap, distance);
  }
}

int ExtractMin(vector<int>& nodes_heap,
               vector<DistancePositionPair>& distance) {
  int min = nodes_heap.front();
  nodes_heap.front() = nodes_heap.back();
  nodes_heap.pop_back();
  distance[nodes_heap.front()].heap_position_ = 0;
  Heapify(0, nodes_heap, distance);
  return min;
}

void ShiftUp(int index,
            vector<int>& nodes_heap,
            vector<DistancePositionPair>& distance) {
  if (index == 0) {
    return;
  }
  int parent_index = index % 2 == 0 ? (index - 2) / 2 : index / 2;
  if (distance[nodes_heap[parent_index]].distance_ >
          distance[nodes_heap[index]].distance_) {
    swap(distance[nodes_heap[parent_index]].heap_position_,
         distance[nodes_heap[index]].heap_position_);
    swap(nodes_heap[parent_index], nodes_heap[index]);
    ShiftUp(parent_index, nodes_heap, distance);
  }
}

int FindMinPath(const vector< vector<NodeLengthPair> >& graph_edges,
              int source_node,
              int destination_node) {
  vector<DistancePositionPair> distance(graph_edges.size(),
      DistancePositionPair(kMaxDistance, 0));
  distance[source_node].distance_ = 0;
  vector<int> nodes_heap(graph_edges.size());
  for (int counter = 0; counter < nodes_heap.size(); ++counter) {
    nodes_heap[counter] = counter;
    distance[counter].heap_position_ =  counter;
  }
  MakeHeap(nodes_heap, distance);
  int current_node;
  while (!nodes_heap.empty()) {
    current_node = ExtractMin(nodes_heap, distance);
    if (current_node == destination_node) {
      break;
    }
    for (vector<NodeLengthPair>::const_iterator adjanced_it =
             graph_edges[current_node].begin();
         adjanced_it != graph_edges[current_node].end();
         ++adjanced_it) {
      if (distance[adjanced_it->node_].distance_ >
              distance[current_node].distance_ + adjanced_it->length_) {
        distance[adjanced_it->node_].distance_ =
            distance[current_node].distance_ + adjanced_it->length_;
        ShiftUp(distance[adjanced_it->node_].heap_position_,
                nodes_heap,
                distance);
      }
    }
  }
  return distance[destination_node].distance_ == kMaxDistance ?
        -1 : distance[destination_node].distance_;
}

int main() {
  vector< vector<NodeLengthPair> > graph_edges;
  int source_node, destination_node;
  ReadInput(graph_edges, source_node, destination_node);
  printf("%d\n", FindMinPath(graph_edges, source_node, destination_node));
  return 0;
}