#include <iostream>
#include <math.h>
#include <sstream>
#include <stdint.h>

#include "polyline.hpp"
#include "split.hpp"
#include "followtherider/srtm/SRTMHandler.hpp"

using namespace FollowTheRider::Element;
using namespace FollowTheRider::SRTM;

namespace FollowTheRider {
namespace Utils {
namespace Polyline {

WayCoordinates decode(const std::string& polyline, const int factor) {
    int count = 0;
    
    std::vector<NodeCoordinates> line;
    std::array<int, 2> tempLatLng = {0, 0};
    
    std::size_t length = polyline.length();
    std::size_t index = 0;
    
    while (index < length) {
        int result = 0;
        int b = 0x20;
        uint32_t shift = 0;
        
        while (b >= 0x20) {
            b = int(polyline[index]) - 63;
            index++;
            
            result |= (b & 0x1f) << shift;
            shift += 5;
        }
        
        // sign dection
        if ((result&1) != 0) {
            result = ~(result >> 1); // bitwise completement
        } else {
            result = result >> 1;
        }
        
        if (count%2 == 0) {
            result += tempLatLng[0];
            tempLatLng[0] = result;
        } else {
            result += tempLatLng[1];
            tempLatLng[1] = result;
            
            line.push_back({static_cast<double>(tempLatLng[0]) / factor, static_cast<double>(tempLatLng[1]) / factor});
        }
        
        count++;
    }
    
    return line;
}

std::string encode(const WayCoordinates& coordsCollection) {
    std::stringstream ss;
    
    auto encodeDiff = [&] (int diff) {
        int shifted = diff << 1;
        if (diff < 0) {
            shifted = ~shifted;
        }
        int rem = shifted;
        while (rem >= 0x20) {
            ss << (char)((0x20 | (rem & 0x1f)) + 63);
            rem >>= 5;
        }
        ss << (char)(rem + 63);
    };
    
    int lastLat = 0;
    int lastLon = 0;
    
    for (const NodeCoordinates& coordinates : coordsCollection) {
        int lat = (int)round(coordinates.latitude * 1E5);
        int lon = (int)round(coordinates.longitude * 1E5);
        encodeDiff(lat - lastLat);
        encodeDiff(lon - lastLon);
        lastLat = lat;
        lastLon = lon;
    }
    
    return ss.str();
}
    
void setElevationsFromString(/* out */ Element::WayCoordinates& coordinates, const std::string& elevations) {
    if (!elevations.empty()) {
        const std::vector<std::string>& elevationsArray = Split::split<std::string>(elevations);
        assert(elevationsArray.size() == coordinates.size());
        for (int i = 0; i < elevationsArray.size(); i++) {
            coordinates[i].elevation = std::stod(elevationsArray[i]);
        }
    }
}
    
void setElevationsFromSRTM(/* out */ Element::WayCoordinates& coordinates, const SRTMHandlerSP srtmHandler) {
    for (NodeCoordinates& coord : coordinates) {
        const int elevation = srtmHandler->getElevation(coord.latitude, coord.longitude);
        if (elevation == SRTMUtil::HGTParser::SRTM_ERROR) {
            // no need to retrieve elevations
            // tile does not exist
            break;
        }
        coord.elevation = elevation;
    }
}

void setElevationsFromSRTM(/* out */ Element::NodeCoordinates& coordinate, const SRTMHandlerSP srtmHandler) {
    const int elevation = srtmHandler->getElevation(coordinate.latitude, coordinate.longitude);
    if (elevation == SRTMUtil::HGTParser::SRTM_ERROR) {
        // tile does not exist
        return;
    }
    coordinate.elevation = elevation;
}
    
std::string elevationsToString(const Element::WayCoordinates& coordinates) {
    std::string strElevations;
    
    for (const NodeCoordinates& coord : coordinates) {
        strElevations += std::to_string(coord.elevation) + ",";
    }
    strElevations.pop_back();
    
    return strElevations;
}

} // namespace Polyline
} // namespace Utils
} // namespace FollowTheRider
