Wave Algorithm (Lee's Algorithm): incorrect final matrix - c

Question

I'm writing a program calculating the shortest way from point A to point B.
I have a map (matrix) with values:
0 is block (wall, no way to pass);
1 is free way (you can pass);
2 is start point;
In the code below I declare 2 arrays: an array " map"and changed array "visited" while running program demonstrating visited points.
I check the cells in 4 directions (not diagonals) for 1 or 0. If it's 1 (possible to pass), I increase the counter for 1. For do not count the previous cell I'm trying to avoid it by the condition. I realized that in two one-dimensional arrays {1 0 -1 0} and { 0, 1, 0, -1 } to check neighbor points (what mean i check [i+1][j], [i-1][j], [i][j+1] and [i][j-1]).
As a result I wanna see "visited" matrix with a few lines which shows the way to reach to the point B (1, 2, 3, ... 15). I wanna find the way to map[7][7] point.
Right now the error here that I do count++ for the previous position. How to avoid that?
Thank you.
P.S. I wrote a few functions implementing a new array with 0 values, counting free to go cells and printing arrays.
main.c:
#include <stdbool.h>
#include <stdlib.h>
#include <stdio.h>
#define WIDTH 8
#define HEIGHT 8
int mapZero(int map[WIDTH][HEIGHT]);
int mapPrint(int map[WIDTH][HEIGHT]);
int mapInit(int map[WIDTH][WIDTH]);
int findFreeToGoCells(int map[WIDTH][HEIGHT]);
int main(int argc, char * argv[])
{
bool stop;
unsigned int count;
unsigned int max;
int visited[WIDTH][HEIGHT];
int map[WIDTH][HEIGHT] =
{
{ 1, 1, 1, 1, 1, 0, 0, 1 },
{ 0, 1, 1, 1, 1, 1, 0, 1 },
{ 0, 0, 1, 0, 1, 1, 1, 0 },
{ 1, 0, 1, 1, 1, 0, 1, 1 },
{ 0, 0, 0, 1, 0, 0, 0, 1 },
{ 1, 0, 1, 1, 1, 0, 0, 1 },
{ 0, 0, 0, 0, 1, 0, 0, 1 },
{ 0, 1, 1, 1, 1, 1, 1, 1 },
};
mapZero(visited);
printf("Matrix of zeroed-visited cells:\n\n");
mapPrint(visited);
printf("Matrix of the map:\n\n");
mapPrint(map);
printf("Free to go cells: %d\n\n", findFreeToGoCells(map));
max = WIDTH * HEIGHT - 1;
visited[0][0] = map[0][0];
count = 0;
visited[0][0] = 0;
int di[4] = { 1, -1, 0, 0 };
int dj[4] = { 0, 0, 1, -1 };
//do
{
for (int i = 0; i < WIDTH; ++i)
{
for (int j = 0; j < HEIGHT; ++j)
{
if (visited[i][j] == count)
{
for (int k = 0; k < 4; ++k)
{
int i_check = i + di[k];
int j_check = j + dj[k];
if ((i_check >= 0 && i_check < WIDTH) && (j_check >= 0 && j_check < HEIGHT) && (map[i_check][j_check] != 0))
{
visited[i_check][j_check] = count + 1;
}
}
count++;
}
}
}
}// while (visited[7][7] == 0);
if (count > max + 99999)
printf("The way couldn't be found\n");
else
{
printf("Matrix of visited cells:\n\n");
mapPrint(visited);
printf("Free to go cells from [0][0] to [7][7]: %d\n", findFreeToGoCells(visited));
}
/*************************************************************************************/
/*************************************************************************************/
int len;
int x = 7;
int y = 7;
int x_path[WIDTH * HEIGHT];
int y_path[WIDTH * HEIGHT];
len = visited[7][7];
count = len;
while (count > 0)
{
x_path[count] = x;
y_path[count] = y;
count--;
for (int k = 0; k < 4; ++k)
{
int i_check = x + di[k];
int j_check = y + dj[k];
if ((i_check >= 0 && i_check < WIDTH) && (j_check >= 0 && j_check < HEIGHT) && (map[i_check][j_check] == count))
{
x = x + di[k];
y = y + dj[k];
break;
}
}
}
x_path[0] = 0;
y_path[0] = 0;
printf("\nThe shortest way consist of %d cells\nThere are %d the shortest way to reach th the final point\n\n", len, findFreeToGoCells(visited)-len);
system("pause");
return 0;
}
int mapZero(int map[WIDTH][HEIGHT])
{
for (int i = 0; i < WIDTH; ++i)
{
for (int j = 0; j < HEIGHT; ++j)
{
map[i][j] = 0;
}
}
return 0;
}
int mapPrint(int map[WIDTH][HEIGHT])
{
for (int i = 0; i < WIDTH; ++i)
{
for (int j = 0; j < HEIGHT; ++j)
{
printf("%2d ", map[i][j]);
}
printf("\n\n");
}
printf("\n");
return 0;
}
int findFreeToGoCells(int map[WIDTH][HEIGHT])
{
int count = 0;
for (int i = 0; i < WIDTH; ++i)
{
for (int j = 0; j < HEIGHT; ++j)
{
if (map[i][j] != 0) count++;
}
}
return count;
}
Result: