private void heapify(int index) {
        int left = left(index);
        int right = right(index);
        int smallest;
        
        if (right <= size) {
            if (heap[left].getCost() < heap[right].getCost()) {
                smallest = left;
            } else {
                smallest = right;
            }
            if (heap[index].getCost() > heap[smallest].getCost()) {
                swap(index, smallest);
                heapify(smallest);
            }
        } else if (left == size && heap[index].getCost() > heap[left].getCost()) {
            swap(index, left);
        }
    }