maze_with_A_star

#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <queue>
#include <sstream>
#include <iterator>
#include <functional>
#include <cmath>

int INF = 1000000;

std::vector<std::vector<int> > maze;
std::vector<std::vector<int> > length;
std::vector<std::vector<int> > visited;
std::priority_queue<std::pair<int, std::pair<int, int> >, std::vector<std::pair<int, std::pair<int, int> > >, std::greater<std::pair<int, std::pair<int, int> > > > queue;

int h(int x, int y, int finish_x, int finish_y)
{
    //return 10 * static_cast<int>(ceil(sqrt((x - finish_x)*(x - finish_x) + (y - finish_y)*(y - finish_y))));
    return 10 * (abs(x - finish_x) + abs(y - finish_y));
}

void dijkstra(int finish_x, int finish_y, int n)
{
    while (!(queue.empty()))
    {
        int x = (queue.top().second).first;
        int y = (queue.top().second).second;
        queue.pop();
        visited[x][y] = 1;
        int delta[][2] = { {-1, 0}, {1, 0}, {0, 1}, {0, -1} };
        for (int i = 0; i < 4; i++)
        {
            if (x + delta[i][0] < n && y + delta[i][1] < n && x + delta[i][0] > -1 && y + delta[i][1] > -1 &&
                !(maze[x + delta[i][0]][y + delta[i][1]]) && length[x + delta[i][0]][y + delta[i][1]] > length[x][y] + 1 && !(visited[x + delta[i][0]][y + delta[i][1]]))
            {
                length[x + delta[i][0]][y + delta[i][1]] = length[x][y] + 1;
                queue.push(std::make_pair(length[x + delta[i][0]][y + delta[i][1]] + h(x, y, finish_x, finish_y), std::make_pair(x + delta[i][0], y + delta[i][1])));
            }
        }
        if (x == finish_x && y == finish_y)
            return;
    }
}

int main()
{
    std::ifstream finp;
    std::string s;
    finp.open("input.txt");
    int n, x1, x2, y1, y2;
    finp >> n;
    maze.resize(n);
    length.resize(n, std::vector<int>(n, INF));
    visited.resize(n, std::vector<int>(n));
    std::getline (finp, s);
    for (int i = 0; i < n; i++)
    {
        std::getline(finp, s);
        std::istringstream buf(s);
        std::istream_iterator<std::string> beg(buf), end;
        std::vector<std::string> arr(beg, end);

        for (int j = 0; j < arr.size(); j += 2)
        {
            maze[i].resize(std::stoi(arr[j]));
            maze[i].resize(std::stoi(arr[j + 1]), 1);
        }
        maze[i].resize(n);
    }
    finp >> x1 >> y1;
    finp >> x2 >> y2;
    length[x1][y1] = 0;
    queue.push(std::make_pair(0, std::make_pair(x1, y1)));
    dijkstra(x2, y2, n);
    std::cout << length[x2][y2];
    return 0;
}