Робот, сажающий грядки

// Находим кратчайший путь между заданными клетками
cell_path(int p1, int p2)
{
    int dir;
    path_len = 0;
    vertices_len = 0;
    heap_len = 0;
    vertices_add_pos(p1, p2, 0, FROM_PARENT, DIR_NONE);
    while(heap_len > 0)
    {
        int node = heap_remove_max();
        int index = heap_get_index(heap_remove_max());
        int vertex = vertices[index];
        int curr_pos = vertex_get_pos(vertex);
        if(curr_pos == p2)
        {
            path_restore(vertex);
            break;
        }
        int dist = vertex_get_distance(vertex);
        for(dir = 0; dir < DIR_BOUND; dir = dir + 1)
        {                                                                      
            int next_pos = pos_add1(curr_pos, dir);
            int cell_index = cells_find_by_pos(next_pos);
            if(cell_index < 0)
                continue;
            int type = cell_get_type(cells[cell_index]);
            if(type >= CELL_EMPTY | type == CELL_FREE)
                vertices_add_pos(next_pos, p2, dist + 1, index, dir);
        }
    }
}
 
// Добавляем вершину в граф для поиска кратчайшего пути
vertices_add_pos(int position, int dest_position, int distance, int parent, int direction)
{
    int index = vertices_find_by_pos(position);
    if(index >= 0)
        return;
 
    int vertex = vertex_set(position, distance, parent, direction);
    int dist_ev = distance_ev(position, dest_position);
    int heap_node = heap_node_set(dist_ev + distance, vertices_len); //vertices_len == index of new vertex 
    vertices[vertices_len] = vertex;
    vertices_len = vertices_len + 1;
    heap_add(heap_node);
}
 
// Востанавливаем путь по вершинам графа
path_restore(int vertex)
{
    int parent = vertex_get_parent(vertex);
    while(parent != FROM_PARENT)
    {
        int dir = vertex_get_dir(vertex);
        path[path_len] = dir;
        path_len = path_len + 1;
        vertex = vertices[parent];
        parent = vertex_get_parent(vertex);
    }
}