#include "mapTools.h"
#include <iostream>
#include <list>
#include <queue>


//#define NO_EDGE -1

// Returns the time required to travel along the path specified, in seconds.

double computePathTravelTime(const double turn_penalty, const std::vector<StreetSegmentIdx>& path){
    //declare initial variables
    struct StreetSegmentInfo currSegInfo; 
    int prevStreet = NO_EDGE;
    double totalTime = 0;  

    //increment the total travel time by iterating through each street segment 
    for(auto currSeg : path){
        // get street ID of the current segment and
        currSegInfo = getStreetSegmentInfo(currSeg); 
        StreetIdx currStreet = currSegInfo.streetID; 

        // find the time it takes to travel across the current segment and add the time to total time
        double currTime = findStreetSegmentTravelTime(currSeg);
        totalTime += currTime; 

        //if there is a change in street; i.e. a turn, add a turn penalty
        if (prevStreet != currStreet && prevStreet != NO_EDGE)
            totalTime += turn_penalty; 
        
        //change the current street to prev street
        prevStreet = currStreet; 
    }
    return totalTime; 
}



// Returns a path (route) between the start intersection (1st object in intersect_ids pair)
// and the destination intersection(2nd object in intersect_ids pair)

std::vector<StreetSegmentIdx> findPathBetweenIntersections(const double turn_penalty, const std::pair<IntersectionIdx, IntersectionIdx> intersect_ids){
    // std::list<WaveElem> wavefront;
    // std::cout << "//////////////START OF NEW TEST//////////////" << std::endl;
    
    // set up return vector 
    std::vector<StreetSegmentIdx> segs; 

    // check if first second and second intersection are same and return empty vector if true
    if(intersect_ids.first == intersect_ids.second) {
        return segs;
    }

    // std::cout << "First intersection: " << intersect_ids.first << "\n Second intersection: " << intersect_ids.second << std::endl; 
    
    // set up queue to hold nodes to visit
    std::priority_queue<WaveElem, std::vector<WaveElem>, FriendElem> wavefront;

    // push the first node
    wavefront.push(WaveElem(intersect_ids.first, NO_EDGE, UNVISITED));

   // double currTime = 0;
    double found = false;

    // continue looping while there are more intersections to visit and destination hasn't been found
    while(wavefront.size() > 0 && !found) {

        // get a reference to the first node
        WaveElem wave = wavefront.top();

        //remove first node from wavefront
        wavefront.pop();
        

        IntersectionIdx currID = wave.nodeID;
        // std::cout << "currIntersectionID: " << currID << std::endl;

        // std::cout << "curr node bbest time: " << nodes[currID].bestTime << std::endl;

        // if(nodes[currID].bestTime < currTime) currTime = nodes[currID].bestTime;
        // else if(wave.travelTime != 0) currTime += findStreetSegmentTravelTime(outEdge);

        // if the current nodes'current mileage is better than its previous mileage
        if(nodes[currID].bestTime == UNVISITED || (wave.travelTime) < (nodes[currID].bestTime)) {
            // std::cout << "curr node bbest time: " << nodes[currID].bestTime << std::endl;
            nodes[currID].bestTime = wave.travelTime;
            nodes[currID].reachingEdge = wave.edgeID;
            // nodes[currID].bestMileage = wave.mileage;
            


            // if destination found
            if(currID == intersect_ids.second){
                // std::cout << "Intersection found!" << std::endl;
                // std::cout << "dest time:"<< nodes[currID].bestTime << std::endl;
                // std::cout << "dest reaching edge"<< nodes[currID].reachingEdge << std::endl;
                found = true;
                break;
            }

            //for each segment in current intersection
            for(auto segID : segmentsInIntersections[currID]) {
                // std::cout << "//////////////INSIDE SEGMENT LOOP//////////////" << std::endl;

                // std::cout << "curr time: "<< nodes[currID].bestTime << std::endl;
                // std::cout << "curr reaching edge: "<< nodes[currID].reachingEdge << std::endl;
                
                // std::cout << "test 1"<< std::endl;
                
                // time to travel segment
                double d_time = findStreetSegmentTravelTime(segID);
                // std::cout << "test 2"<< std::endl;

                StreetSegmentInfo currSegInfo = getStreetSegmentInfo(segID);
                // std::cout << "test 3"<< std::endl;


                StreetSegmentInfo prevSegInfo;
                IntersectionIdx prevSegID = -1; 

                IntersectionIdx nextID = (currSegInfo.from == currID) ? currSegInfo.to : currSegInfo.from;

                double nextSegTime; 
                bool noPrevNode = false; 
                // checks for non-existing edges
                if(nodes[currID].reachingEdge != NO_EDGE){
                    prevSegInfo = getStreetSegmentInfo(nodes[currID].reachingEdge);
                    prevSegID = (prevSegInfo.to == currID) ? prevSegInfo.from : prevSegInfo.to;

                    if(nodes[nextID].bestTime > (wave.travelTime + d_time) || (nodes[nextID].bestTime == UNVISITED)) 
                        nextSegTime = wave.travelTime + d_time;
                    else 
                        continue;
                        // nextSegTime = nodes[nextID].bestTime;

                    if(currSegInfo.streetID != prevSegInfo.streetID && nodes[currID].bestTime != UNVISITED)
                        nextSegTime += turn_penalty;
                }
                else{
                   noPrevNode = true; 
                   nextSegTime = d_time; 
                }
                

                // std::cout << "mileage:  " << mileage << std::endl;
                if(currSegInfo.oneWay && currSegInfo.from != currID) continue;
                // if path valid
                if(currID == intersect_ids.first)
                    wavefront.push(WaveElem(nextID, segID, nextSegTime)); 
                else if(!noPrevNode){
                    if(prevSegID != currSegInfo.to && prevSegID != currSegInfo.from){
                    // std::cout << "next seg pushed" << std::endl;
                        wavefront.push(WaveElem(nextID, segID, nextSegTime));
                    }
                }   
                // override current node values

            }
            // std::cout << "//////////////SEGMENT LOOP END//////////////" << std::endl;

        }
    }
    // std::cout << "after searching is done!" << std::endl;

    // if destiantion not found and queue exhausted
    if(!found) {
       // std::cout << "nothing found \n";
        return segs;
    }

    // populates path
    populatePath(segs, intersect_ids.second, intersect_ids.first);

    // clears all the nodes to prepare for next search
    clearNodes();

    return segs; 
    
}