/*
================
reset_noe

Удаляет ноды
================
*/
void reset_node()
{
    size_t i;

    for ( i = 0; i < nodes.size(); i++ )
    {
        nodes[ i ].arcs.clear();
        nodes[ i ].dists.clear();
    }

    for ( i = 0; i < routes.size(); i++ )
        routes[ i ].clear();

    for ( i = 0; i < routedists.size(); i++ )
        routedists[ i ].clear();

    nodes.clear();
    routes.clear();
    routedists.clear();
}

/*
================
create_nodes_by_count

Находит ближайший нод для origin
================
*/
int get_num_closestnode( float *origin )
{
    size_t i;
    int ret = -1;

    if ( nodes.empty() )
        return ret;

    for ( i = 0; i < nodes.size(); i++ )
    {
        if ( IsVisible( nodes[ i ].origin, origin ) )
        {
            if ( ret == -1 )
                ret = i;
            else if ( closer( origin, nodes[ i ].origin, nodes[ ret ].origin ) )
                ret = i;
        }
    }

    return ret;
}

/*
================
create_nodes_by_count

Создает  ноды  распределяя  их  по ширине и высоте
Если  num  ==  -1,  то  кол-во нодов расчитывается
автоматически
================
*/
void create_nodes_by_count( float *origin, int width, int height, int range )
{
    int min_dist = 100; // минимум юнитов между нодами

    if ( !nodes.empty() )
        reset_node();

    count = 500;
    int crnt_count = 0;

    // добавляем первый нод
    node_t node;
    vec3_t start = { origin[ 0 ], origin[ 1 ], origin[ 2 ] };
    vec3_t end = { origin[ 0 ], origin[ 1 ], origin[ 2 ] - 1000000 };
    GetPositionBulletTrace( node.origin, start, end );
    node.origin[ 2 ] += 10;
    node.load = false;
    nodes.push_back( node );

    for ( size_t n = 0; crnt_count < count; n++ )
    {
        if ( n >= nodes.size() )
            break;

        if ( nodes[ n ].load )
            continue;

        float pw[ 4 ] = { -1, 1, 0, 0 };
        float ph[ 4 ] = { 0, 0, -1, 1 };

        for ( int i = 0; i < 4; i++ )
        {
            float to_pos[ 3 ] = {
                nodes[ n ].origin[ 0 ] + min_dist * pw[ i ],
                nodes[ n ].origin[ 1 ] + min_dist * ph[ i ],
                nodes[ n ].origin[ 2 ]
            };

            if ( IsVisible( nodes[ n ].origin, to_pos ) )
            {
                float new_to_pos[ 3 ];
                VectorCopy( to_pos, new_to_pos );
                new_to_pos[ 2 ] -= 1000000;

                GetPositionBulletTrace( new_to_pos, to_pos, new_to_pos );
                new_to_pos[ 2 ] += 10;

                if ( IsVisible( nodes[ n ].origin, new_to_pos ) )
                {
                    bool found = false;

                    for ( int j = 0; j < nodes.size(); j++ )
                    {
                        if ( GetDistance( nodes[ j ].origin, new_to_pos ) < 5 )
                        {
                            found = true;
                            break;
                        }
                    }

                    if ( !found )
                    {
                        node_t new_node;
                        new_node.load = false;
                        VectorCopy( new_to_pos, new_node.origin );

                        nodes.push_back( new_node );
                        crnt_count++;
                    }
                }
            }
        }

        nodes[ n ].load = true;
    }

    load_routes();
}

/*
================
load_routes

Дополняет ноды и строит все маршруты
================
*/
void load_routes()
{
    size_t i, j, n;

    for ( i = 0; i < nodes.size(); i++ )
    {
        for ( j = 0; j < nodes.size(); j++ )
        {
            if ( i != j && IsVisible( nodes[ i ].origin, nodes[ j ].origin ) )
            {
                nodes[ i ].arcs.push_back( j );
                nodes[ i ].dists.push_back( GetDistance( nodes[ i ].origin, nodes[ j ].origin ) );
            }
        }
    }

    for ( n = 0; n < nodes.size(); n++ )
    {
        std::vector<int> d, p, q;
        std::vector<bool> done;
        std::vector<int> invp;

        for ( i = 0; i < nodes.size(); i++ )
        {
            d.push_back( MAX_DIST_ROUTE );
            q.push_back( -1 );
            p.push_back( -1 );
            invp.push_back( -1 );
            done.push_back( false );

        }

        d[ n ] = 0;
        q[ n ] = 0;
        invp[ n ] = 0;

        while ( getSizeMass( q ) > 0 )
        {
            int min = MAX_DIST_ROUTE;
            int u = -1;

            for ( i = 0; i < nodes.size(); i++ )
            {
                if ( q[ i ] != -1 && q[ i ] < min )
                {
                    min = q[ i ];
                    u = i;
                }
            }

            done[ u ] = true;
            q[ u ] = -1;

            for ( i = 0; i < nodes[ u ].arcs.size(); i++ )
            {
                if ( !done[ nodes[ u ].arcs[ i ] ] && d[ nodes[ u ].arcs[ i ] ] > d[ u ] + nodes[ u ].dists[ i ] )
                {
                    d[ nodes[ u ].arcs[ i ] ] = d[ u ] + nodes[ u ].dists[ i ];
                    p[ nodes[ u ].arcs[ i ] ] = u;
                    q[ nodes[ u ].arcs[ i ] ] = d[ u ] + nodes[ u ].dists[ i ];
                }
            }
        }

        for ( i = 0; i < p.size(); i++ )
        {
            if ( i != n )
            {
                int last = i;
                int cur = i;

                while ( cur != n )
                {
                    last = cur;
                    cur = p[ cur ];
                }

                invp[ i ] = last;
            }
        }

        routes.push_back( invp );
        routedists.push_back( d );
    }
}