#include "StreetsDatabaseAPI.h"
#include "OSMDatabaseAPI.h"
#include "math.h"
#include "mapTools.h"
#include <iostream>
#include <string>
#include <unordered_set>
#include <unordered_map>
#include <algorithm>
#include <utility>
#include <thread>



//global variables - original declaration in mapTools.h
bool load_successful = false;
std::vector<Node> nodes;
std::vector<std::vector<StreetSegmentIdx>> segmentsInIntersections; 
std::unordered_set<StreetSegmentIdx> allSegments;
std::vector<std::unordered_set<IntersectionIdx>> streetIntersections;
std::vector<std::vector<IntersectionIdx>> allStreetIntersections;
std::vector<std::pair<StreetIdx, std::string>> streetNames;
std::unordered_map<OSMID, LatLon> OSMContent; 
std::vector<double> streetLengths; 
std::vector<double> timeTravel; 
std::vector<std::pair<FeatureIdx,double>> sortedFeaturesArea; 
double maxSpeedLimit;

//gets the minimum and maximum y coordinate of the feature
std::unordered_map<FeatureIdx,double> featureMax; 
std::unordered_map<FeatureIdx,double> featureMin; 

class thread_guard{
    std::thread & t;
    public:
        explicit thread_guard(std::thread & t_):t(t_) {}
        ~thread_guard() {
            if(t.joinable()) 
                t.join();
        }
        thread_guard(thread_guard const&)=delete;
        thread_guard& operator=(thread_guard const &)=delete;
};




bool loadMap(std::string map_streets_database_filename) {
     
    std::cout << "loadMap: " << map_streets_database_filename << std::endl;

    std::string mapStreetsBIN = map_streets_database_filename;
    map_streets_database_filename.replace(map_streets_database_filename.length() - 11,7,"osm"); //replace streets with OSM
    
    //checks if loading succeeded for both loadStreetsDatabaseBIN and loadOSMDatabaseBIN
    if(loadStreetsDatabaseBIN(mapStreetsBIN) && loadOSMDatabaseBIN(map_streets_database_filename))
        load_successful = true; 
    else return false; //loading failed

    // populate and sort global vectors and initialize global variables
    std::thread t1(convertFeaturesLatLon);  
    thread_guard g1(t1);

    std::thread t2(populateSegments); 
    thread_guard g2(t2);
    // populateAllSegments();                
    std::thread t3(populateStreetIntersections);
    thread_guard g3(t3);
    // t3.detach();

    std::thread t4(populateStreetNames);
    thread_guard g4(t4);
    // t4.detach();
    populateOSMContent();

    std::sort(streetNames.begin(), streetNames.end(), sortbysec);

    nodes.reserve(getNumIntersections());
    for(int i = 0; i < getNumIntersections(); i++)
        nodes.push_back(Node());



    // typecast unordered set into vector
    allStreetIntersections.resize(streetIntersections.size());
    for (StreetIdx streetID = 0; streetID < streetIntersections.size(); streetID ++){
        std::unordered_set<IntersectionIdx> s = streetIntersections[streetID];
        std::vector<IntersectionIdx> v;

        v.reserve(s.size());
        std::copy(s.begin(), s.end(), std::back_inserter(v));
        allStreetIntersections[streetID] = v;
    }

    // findStreetLength
    streetLengths.resize(getNumStreets());
    std::fill(streetLengths.begin(), streetLengths.end(), 0.0);
    for(StreetSegmentIdx currSegment = 0; currSegment < getNumStreetSegments(); currSegment++){
        double currLength = findStreetSegmentLength(currSegment);

        StreetSegmentInfo currSegInfo = getStreetSegmentInfo(currSegment);
        streetLengths[currSegInfo.streetID] += currLength;
    }

    
    maxSpeedLimit = 0;
    // find streetSegmentTravelTime
    timeTravel.resize(getNumStreetSegments());
    std::fill(timeTravel.begin(), timeTravel.end(), 0.0);
    for(int street_segment_id = 0; street_segment_id < getNumStreetSegments(); street_segment_id++){
        // Get street segment info and get speed limit data from object
        StreetSegmentInfo streetSeg = getStreetSegmentInfo(street_segment_id);

        if(streetSeg.speedLimit > maxSpeedLimit) 
            maxSpeedLimit = streetSeg.speedLimit;

        // calculate travel time from one end or street segment to the other
        timeTravel[street_segment_id] = findStreetSegmentLength(street_segment_id)/streetSeg.speedLimit; 
    }

    //sort features by area
    int totFeatures = getNumFeatures(); 
    for(int curr=0; curr < totFeatures; curr++){
        double currArea = findFeatureArea(curr);
        sortedFeaturesArea.push_back(std::make_pair(curr, currArea));
    }
    std::sort(sortedFeaturesArea.begin(), sortedFeaturesArea.end(), sortfeaturebysec);


    //associate features with the min and max y coordinate
    for(int curr = 0; curr < totFeatures; curr++){
       //LatLon currentPosition = getFeaturePoint(0, curr);
        if (featuresInPoint2d.size() == 0) break;
        featureMax.insert({curr, featuresInPoint2d[curr][0].y});
        featureMin.insert({curr, featuresInPoint2d[curr][0].y});


        for(int pointNum = 0; pointNum < getNumFeaturePoints(curr); pointNum++){
            double currLong = (featuresInPoint2d[curr][pointNum]).y; 
            //set max
            if( currLong > featureMax[curr]) featureMax[curr] = currLong; 
            //set min 
            else if( currLong < featureMin[curr]) featureMin[curr] =  currLong; 

        }
    }

    // populate more functions
    avgLatFinder();
    intersection_to_point2d();
    loadOSMRoads();

    return load_successful; 
}

void closeMap() {
    //Clean-up your map related data structures here
    if(!load_successful) return;
    clearMemory();
    
    closeStreetDatabase();
    closeOSMDatabase();
    
}

// Returns the distance between two (lattitude,longitude) coordinates in meters
// Speed Requirement --> moderate
double findDistanceBetweenTwoPoints(std::pair<LatLon, LatLon> points){
    
    //retrieve the lat, lon points of the pair, convert them into radians. 
    double lat1 = kDegreeToRadian * points.first.latitude();
    double lat2 = kDegreeToRadian * points.second.latitude(); 
    double lon1 = kDegreeToRadian * points.first.longitude();
    double lon2 = kDegreeToRadian * points.second.longitude(); 

    //convert lat/lon into x,y points so they are equidistant
    //(x,y)=(R·lon·cos(latavg), R·lat)
    double x1 = kEarthRadiusInMeters * lon1 * cos((lat1+lat2)/2); 
    double x2 = kEarthRadiusInMeters * lon2 * cos((lat1+lat2)/2); 
    double y1 = kEarthRadiusInMeters * lat1; 
    double y2 = kEarthRadiusInMeters * lat2; 
    
    //distance using pythagoras' formula
    double dist = sqrt(pow(y2-y1,2)+pow(x2-x1,2)); 

    return dist; 
}

// Returns the length of the given street segment in meters
// Speed Requirement --> moderate
double findStreetSegmentLength(StreetSegmentIdx street_segment_id){

    // find number of curves in street segment
    StreetSegmentInfo streetSeg = getStreetSegmentInfo(street_segment_id); 
    int numOfCurves = streetSeg.numCurvePoints;

    if (numOfCurves == 0){ // no curves
        LatLon point1 = getIntersectionPosition(streetSeg.from);
        LatLon point2 = getIntersectionPosition(streetSeg.to);
        
        // create pair of points
        std::pair <LatLon, LatLon> position (point1, point2);
        
        double segLength = findDistanceBetweenTwoPoints(position);
        return segLength; 
    }
    else{ // 1 or more curve points
        // starting segment id
        LatLon LatLon1 = getIntersectionPosition(streetSeg.from);
        
        // initialize variables
        double segmentLength = 0;
        LatLon LatLon2;

        for (int i = 0; i <= numOfCurves; i ++){
            // determine id of curves
            if (i < numOfCurves)
                LatLon2 = getStreetSegmentCurvePoint(i, street_segment_id);
            else // i = numOfCurves, no more curves left, get last instersection point
                LatLon2 = getIntersectionPosition(streetSeg.to);
            
            // create pair
            std::pair<LatLon, LatLon> position (LatLon1, LatLon2);

            // add segment lenth to total street segment length
            segmentLength += findDistanceBetweenTwoPoints(position);

            LatLon1 = LatLon2; // LatLon2 will become the start of next segment in street segment
        }
        return segmentLength;
    } 
}

// Returns the travel time to drive from one end of a street segment 
// to the other, in seconds, when driving at the speed limit
// Note: (time = distance/speed_limit)
// Speed Requirement --> high 
double findStreetSegmentTravelTime(StreetSegmentIdx street_segment_id){
    return timeTravel[street_segment_id];
}

// Returns true if the two intersections are directly connected, meaning you can legally 
// drive from the first intersection to the second using only one streetSegment.
// Speed Requirement --> moderate 
bool intersectionsAreDirectlyConnected(std::pair<IntersectionIdx, IntersectionIdx> intersection_ids){
    if(intersection_ids.first == intersection_ids.second)
        return true;
        
    std::vector<StreetSegmentIdx> segmentVec1 = segmentsInIntersections[intersection_ids.first];
    std::vector<StreetSegmentIdx> segmentVec2 = segmentsInIntersections[intersection_ids.second];

    for (int i = 0; i < segmentVec1.size(); ++i) {
        if (std::find(segmentVec2.begin(), segmentVec2.end(), segmentVec1[i]) != segmentVec2.end()) {
            struct StreetSegmentInfo streetSeg = getStreetSegmentInfo(segmentVec1[i]);
            if(streetSeg.oneWay && streetSeg.from != intersection_ids.first) {
                return false;
            }

            return true;
        }
    }

    return false; // false, not connected
}

// Returns the geographically nearest intersection (i.e. as the crow flies) to the given position
// Speed Requirement --> none
IntersectionIdx findClosestIntersection(LatLon my_position){
    IntersectionIdx currClosest = 0;
    //placeholder distance variable
    double minDistance = -1; 

    //iterate through ALL intersections and calculate distance between the intersection / my position
    for(int currIdx = 0; currIdx < getNumIntersections(); currIdx++){
        std::pair<LatLon,LatLon> lineToPoi (my_position, getIntersectionPosition(currIdx));
        double currDist = findDistanceBetweenTwoPoints(lineToPoi); 

        if(minDistance == -1 || currDist < minDistance){
            minDistance = currDist; 
            currClosest = currIdx;
        } 
    }

    return currClosest;
}

// Returns the street segments that connect to the given intersection 
// Speed Requirement --> high
std::vector<StreetSegmentIdx> findStreetSegmentsOfIntersection(IntersectionIdx intersection_id){
    return segmentsInIntersections[intersection_id];
}

// Returns the street names at the given intersection (includes duplicate 
// street names in the returned vector)
// Speed Requirement --> high 
std::vector<std::string> findStreetNamesOfIntersection(IntersectionIdx intersection_id){
    std::vector<StreetSegmentIdx> streetSegments = segmentsInIntersections[intersection_id];
    std::vector<std::string> namesOfStreets;

    //get all segments for a given intersection, and add the StreetNames of the given segment
    for (int segmentName = 0; segmentName < streetSegments.size(); segmentName ++){
        StreetSegmentInfo streetSegInfo = getStreetSegmentInfo(streetSegments[segmentName]);
        StreetIdx streetID = streetSegInfo.streetID;

        // get and add street name to list of names
        namesOfStreets.push_back(getStreetName(streetID));
    }
    return namesOfStreets;
}

// Returns all intersections along the a given street.
// There should be no duplicate intersections in the returned vector.
// Speed Requirement --> high
std::vector<IntersectionIdx> findIntersectionsOfStreet(StreetIdx street_id){
    return allStreetIntersections[street_id]; 
}

// Return all intersection ids at which the two given streets intersect
// This function will typically return one intersection id for streets
// that intersect and a length 0 vector for streets that do not. For unusual 
// curved streets it is possible to have more than one intersection at which 
// two streets cross.
// There should be no duplicate intersections in the returned vector.
// Speed Requirement --> high
std::vector<IntersectionIdx> findIntersectionsOfTwoStreets(std::pair<StreetIdx, StreetIdx> street_ids){

    //find all intersections of the two streets and put them into 1 vector (firstStreet)    
    std::vector<IntersectionIdx> firstStreet = findIntersectionsOfStreet(street_ids.first); 
    std::vector<IntersectionIdx> secondStreet = findIntersectionsOfStreet(street_ids.second); 
    copy(secondStreet.begin(), secondStreet.end(),back_inserter(firstStreet));
    std::vector<IntersectionIdx> mutualIntersect; 
    
    //sort the vector - if there are mutual intersections, they will be placed adjacent to each other
    std::sort(firstStreet.begin(), firstStreet.end());
    for (int i = 1; i < firstStreet.size(); i ++){
        //if adjacent intersections are equal - add to final result
        if (firstStreet[i - 1] == firstStreet[i]) mutualIntersect.push_back(firstStreet[i]);
    }
    
    return mutualIntersect; 
}

// Returns all street ids corresponding to street names that start with the 
// given prefix 
// The function should be case-insensitive to the street prefix. 
// The function should ignore spaces.
//  For example, both "bloor " and "BloOrst" are prefixes to 
// "Bloor Street East".
// If no street names match the given prefix, this routine returns an empty 
// (length 0) vector.
// You can choose what to return if the street prefix passed in is an empty 
// (length 0) string, but your program must not crash if street_prefix is a 
// length 0 string.
// Speed Requirement --> high 
std::vector<StreetIdx> findStreetIdsFromPartialStreetName(std::string street_prefix){
    //edge case - prefix must be given!
    if (street_prefix == "") return{};
    std::vector<StreetIdx> allStreets; 
    std::string parsedStreetPrefix = parseName(street_prefix);

    //find any index where partial prefix is contained in streetname. 
    int start = findPartialLocations(parsedStreetPrefix);
    allStreets.push_back(streetNames[start].first);

    //look left and right from the found index, and check if those indexes contain the partial prefix
    int goLeft = start-1;
    int goRight = start+1; 
    const char *str = parsedStreetPrefix.c_str();
    
    if(goLeft > 0){ // looking left, given curr index > 0
        while(strncmp(str, streetNames[goLeft].second.c_str(), parsedStreetPrefix.size()) == 0){
            allStreets.push_back(streetNames[goLeft].first);       
            goLeft--;          
            if (goLeft <0) break; // looking left, given curr index > 0
        }
    }
    if(goRight < getNumStreets()-1){//looking right, given curr index not at end
        while(strncmp(str, streetNames[goRight].second.c_str(), parsedStreetPrefix.size()) == 0){
            allStreets.push_back(streetNames[goRight].first);  
            goRight++;
            if (goRight > getNumStreets()-1) break; //looking right, given curr index not at end                
        }
    }
    //return empty if there are no matches
    if (allStreets.size() == 0) return{};
    
    return allStreets; 
}

// Returns the length of a given street in meters
// Speed Requirement --> high 
double findStreetLength(StreetIdx street_id){
    return streetLengths[street_id];
}

// Returns the nearest point of interest of the given type to the given position
// Speed Requirement --> none 
POIIdx findClosestPOI(LatLon my_position, std::string POIname){
    //similar function to findClosestIntersection() -- just iterate through POI instead of Intersection
    POIIdx currClosest = 0;
    double minDistance = -1; 

    for(int currIdx = 0; currIdx < getNumPointsOfInterest(); currIdx++){
        if(POIname == getPOIType(currIdx)){
            std::pair<LatLon,LatLon> lineToPoi (my_position, getPOIPosition(currIdx));
            double currDist = findDistanceBetweenTwoPoints(lineToPoi); 

            if(minDistance == -1 || currDist < minDistance){
                minDistance = currDist; 
                currClosest = currIdx;
            } 
        }
    }
    return currClosest;
}

// Returns the area of the given closed feature in square meters
// Assume a non self-intersecting polygon (i.e. no holes)
// Return 0 if this feature is not a closed polygon.
// Speed Requirement --> moderate
double findFeatureArea(FeatureIdx feature_id){
    //how many sides does the polygon have?
    int vertices = getNumFeaturePoints(feature_id);
    double totalArea = 0; 

    //if there are less than 3 vertices area is 0
    if(vertices <= 2) return 0;
    else{
        //calculate the coordinates of the start and end points. 
        LatLon startPoint = getFeaturePoint(0, feature_id);        
        LatLon endPoint = getFeaturePoint(vertices-1, feature_id);

        //if startpoint == endpoint, the polygon is closed and we calculate area. 
        //otherwise we return 0
        if(startPoint.latitude() == endPoint.latitude() && startPoint.longitude() == endPoint.longitude()){
            //calculate area using Riemann Sums - Taking Average Longitude Multiplied by Difference in Latitude. 
            for(int curr=0; curr < vertices-1; ++curr){
                //Convert Coordinates to x y coordinates for equidistant axes. 
                LatLon firstPoint = getFeaturePoint(curr, feature_id);
                double lat1 = kDegreeToRadian * firstPoint.latitude();
                double lon1 = kDegreeToRadian * firstPoint.longitude();        
                LatLon secondPoint = getFeaturePoint(curr+1, feature_id);
                double lat2 = kDegreeToRadian * secondPoint.latitude();
                double lon2 = kDegreeToRadian * secondPoint.longitude();

                double x1 = kEarthRadiusInMeters * lon1 * cos((lat1+lat2)/2); 
                double x2 = kEarthRadiusInMeters * lon2 * cos((lat1+lat2)/2); 
                double y1 = kEarthRadiusInMeters * lat1; 
                double y2 = kEarthRadiusInMeters * lat2; 

                //increment area each term of the Riemann sum. 
                totalArea += (y2-y1)*(x2+x1)/2;
            }
            return abs(totalArea);
        }
        else return 0; 
    }
}

// Return the LatLon of an OSM node; will only be called with OSM nodes (not ways or relations)
// Speed requirement --> high
LatLon findLatLonOfOSMNode (OSMID OSMid){
    return OSMContent[OSMid];
}