#include "get_route.h"

std::list<std::list<route_point_t> > get_routes(
        route_db_t* route_db,
        parameters_t & parameters,
        std::list< std::pair<double,double> > & geos)
{
    std::list<nid_t> nids;
    route_db->lookup(geos, nids);

    std::list<std::list<route_point_t> > ret;

    for(std::list<nid_t>::iterator nids_it = nids.begin();
            nids_it != nids.end();
            nids_it++)
    {
        std::list<nid_t>::iterator nids_it_next = nids_it;
        nids_it_next++;
        if(nids_it_next!=nids.end())
        {
            ret.push_back(
                    get_route_between_nids(
                        route_db,
                        parameters,
                        *nids_it,
                        *nids_it_next));
        }
    }
    return(ret);
}


std::list<route_point_t> get_route_between_nids(
        route_db_t* route_db,
        parameters_t & parameters,
        nid_t & nid_initial,
        nid_t & nid_final)
{
    std::priority_queue<edge_in_queue_t> queue; // This is the ordoned queue used to explore
    edge_map_t edges_map;


    // first we add edge starting from the initial node in the queue
    node_info_t* initial_node = route_db->get_node(nid_initial);
    node_info_t* final_node = route_db->get_node(nid_final);

    edge_t null_edge;
    null_edge.A = 0;
    null_edge.B = 0;
    for(unsigned int i=0; i< initial_node->fixed->neighbors_number; i++)
    {
        edge_t edge;
        edge.A=nid_initial;
        edge.B=initial_node->neighbors[i].to;
        status_t & status = edges_map[edge];
        status.criterion=0;
        status.velocity=0;
        status.energy=0;
        status.time=0;
        status.power=0;
        status.walk=false;
        status.from_edge=null_edge;
        status.freezed=false;
        status.initialized=true;
        edge_in_queue_t edge_in_queue;
        edge_in_queue.edge = edge;
        edge_in_queue.heuristic = rv_heuristic(parameters, initial_node->fixed, final_node->fixed, status);
        queue.push(edge_in_queue);
    }

    while(!queue.empty())
    {
        // We take the first
        edge_in_queue_t running_edge_in_queue = queue.top();
        queue.pop();

        status_t & running_status = edges_map[running_edge_in_queue.edge];

        if(running_status.freezed) // We have already foud the optimal for this edge
        {
            continue;
        }

        running_status.freezed = true;
       
        
        if(running_edge_in_queue.edge.A == nid_final) // It is finish
            break;

        node_info_t* A_node = route_db->get_node(running_edge_in_queue.edge.A);
        node_info_t* B_node = route_db->get_node(running_edge_in_queue.edge.B);


        if(running_edge_in_queue.edge.B == nid_final) // We are near the end
        {
            edge_in_queue_t following_edge_in_queue;
            following_edge_in_queue.edge.A = nid_final;
            following_edge_in_queue.edge.B = 0;
            status_t & following_status = edges_map[following_edge_in_queue.edge];
            status_t proposed_following_status;

            // We lookup the way_kind
            way_kind_t way_kind;
            for(unsigned int i=0;i<B_node->fixed->neighbors_number; i++)
            {
                if(B_node->neighbors[i].from == running_edge_in_queue.edge.A)
                {
                    way_kind = B_node->neighbors[i].way_kind;
                    break;
                }
            }

            rv_step(
                    parameters,
                    A_node->fixed,
                    B_node->fixed,
                    NULL,
                    way_kind,
                    running_status,
                    proposed_following_status);

            if(!following_status.initialized 
                    || proposed_following_status.criterion < following_status.criterion)
            {
                following_status = proposed_following_status;
                following_status.from_edge=running_edge_in_queue.edge;
                following_status.initialized=true;
                following_edge_in_queue.heuristic = rv_heuristic(
                        parameters,
                        B_node->fixed,
                        final_node->fixed,
                        following_status);
                queue.push(following_edge_in_queue);
            }
            continue;
        }

        for(unsigned int i=0; i<B_node->fixed->neighbors_number; i++)
        {
            if(B_node->neighbors[i].from == running_edge_in_queue.edge.A)
            {
                // We explore the following
                nid_t & nid_C = B_node->neighbors[i].to;
                if(!nid_C) // This is a dead end and we are not at the end
                    continue;
                way_kind_t & way_kind = B_node->neighbors[i].way_kind;

                node_info_t* C_node = route_db->get_node(nid_C);

                edge_in_queue_t following_edge_in_queue;
                following_edge_in_queue.edge.A = running_edge_in_queue.edge.B;
                following_edge_in_queue.edge.B = nid_C;
                status_t proposed_following_status;
                status_t & following_status = edges_map[following_edge_in_queue.edge];

                rv_step(
                        parameters,
                        A_node->fixed,
                        B_node->fixed,
                        C_node->fixed,
                        way_kind,
                        running_status,
                        proposed_following_status);

                if(!following_status.freezed
                        && ( !following_status.initialized 
                            || proposed_following_status.criterion < following_status.criterion))
                {
                    following_status = proposed_following_status;
                    following_status.from_edge=running_edge_in_queue.edge;
                    following_status.initialized=true;
                    following_edge_in_queue.heuristic = rv_heuristic(
                            parameters,
                            B_node->fixed,
                            final_node->fixed,
                            following_status);
                    queue.push(following_edge_in_queue);
                }
            }
        }
    }

    edge_t edge;
    edge.A = nid_final;
    edge.B =0;

    status_t status = edges_map[edge];
    if(!status.freezed) // FUCK YOU
    {
        fprintf(stderr,"Queue finished with last edge not freezed\n");
        abort();
    }

    // We only have to follow the edges in opposite order
    std::list<route_point_t> ret;
    while(true)
    {
        route_point_t routing_point;
        routing_point.nid = edge.A;
        routing_point.velocity = status.velocity;
        routing_point.distance = status.distance;
        routing_point.energy = status.energy;
        routing_point.time = status.time;
        routing_point.walk = status.walk;
        routing_point.power = status.power;
        ret.push_front(routing_point);

        if(edge.A == nid_initial)
            break;

        edge = status.from_edge;
        status = edges_map[edge];
    }

    return(ret);
}