/*
Google Polyline encoding/decoding class by M Phillips - 2010.
Based on work by Mark McClure http://facstaff.unca.edu/mcmcclur

This code is provided as is with no warranties or guarantees of 
any kind.

Please send an email to M Phillips (mbp2@i4free.co.nz)
  - if you use this file in a released product, or
  - if you find any bugs, or
  - if you have any suggestions
*/

#include "GooglePolyline.h"

PolylineEncoder::PolylineEncoder(int numLevels, double zoomFactor, double verySmall, bool forceEndpoints)
	: numLevels(numLevels), zoomFactor(zoomFactor), verySmall(verySmall), forceEndpoints(forceEndpoints),
	zoomLevelBreaks(numLevels)
{
	for (int i = 0; i < numLevels; i++)
		zoomLevelBreaks[i] = verySmall * pow(zoomFactor, static_cast<double>(numLevels-i-1));
}

void PolylineEncoder::dpEncode(const PolyLine &points, std::string &encodedPoints, std::string &encodedLevels)
{
	double absMaxDist = 0;
	std::vector<double> dists(points.size());
	std::fill(dists.begin(), dists.end(), undefined);

	if (points.size() > 2) {
		std::vector<Range> stack;
		stack.push_back(Range(0, points.size()-1));
		while (!stack.empty()) {
			Range current = stack.back();
			stack.pop_back();
			double maxDist = 0;
			size_t maxLoc = 0;
			double segmentLength = (points[current.second] - points[current.first]).lengthSquared();
			for (size_t i = current.first+1; i < current.second; i++) {
				double temp = distance(points[i], points[current.first], points[current.second], segmentLength);
				if (temp > maxDist) {
					maxDist = temp;
					maxLoc = i;
					if (maxDist > absMaxDist) {
						absMaxDist = maxDist;
					}
				}
			}
			if (maxDist > verySmall) {
				dists[maxLoc] = maxDist;
				stack.push_back(Range(current.first, maxLoc));
				stack.push_back(Range(maxLoc, current.second));
			}
		}
	}

	encodedPoints = createEncodings(points, dists);
	encodedLevels = encodeLevels(points, dists, absMaxDist);
}

PolyLine PolylineEncoder::simplifyPath(const PolyLine &points) {
	std::vector<double> dists(points.size());
	std::fill(dists.begin(), dists.end(), undefined);

	if (points.size() > 2) {
		std::vector<Range> stack;
		stack.push_back(Range(0, points.size()-1));
		while (!stack.empty()) {
			Range current = stack.back();
			stack.pop_back();
			double maxDist = 0;
			size_t maxLoc = 0;
			double segmentLength = (points[current.second] - points[current.first]).lengthSquared();
			for (size_t i = current.first+1; i < current.second; i++) {
				double temp = distance(points[i], points[current.first], points[current.second], segmentLength);
				if (temp > maxDist) {
					maxDist = temp;
					maxLoc = i;
				}
			}
			if (maxDist > verySmall) {
				dists[maxLoc] = maxDist;
				stack.push_back(Range(current.first, maxLoc));
				stack.push_back(Range(maxLoc, current.second));
			}
		}
	}

	PolyLine outpoints;
	for (size_t i = 0; i < points.size(); i++) {
		if (dists[i] != undefined || i == 0 || i == points.size()-1)
			outpoints.push_back(points[i]);
	}
	return outpoints;
}


// distance(p0, p1, p2) computes the distance between the point p0
// and the segment [p1,p2].	This could probably be replaced with
// something that is a bit more numerically stable.
double PolylineEncoder::distance(const Point &p0, const Point &p1, const Point &p2, double segLength) {
	double out;

	if (p1.lat() == p2.lat() && p1.lng() == p2.lng()) {
		out = (p2 - p0).lengthSquared();
	} else {
		double u = ((p0.lat()-p1.lat())*(p2.lat()-p1.lat())+(p0.lng()-p1.lng())*(p2.lng()-p1.lng())) / segLength;

		if (u <= 0) {
			out = (p0 - p1).lengthSquared();
		} else if (u >= 1) {
			out = (p0 - p2).lengthSquared();
		} else {
			out = (p0 - p1 - u*(p2 - p1)).lengthSquared();
		}
	}
	return sqrt(out);
}

std::string PolylineEncoder::createEncodings(const PolyLine &points, const std::vector<double> &dists) {
	int dlat, dlng;
	int plat = 0, plng = 0;
	std::string encoded_points;

	for (size_t i = 0; i < points.size(); i++) {
		if (dists[i] != undefined || i == 0 || i == points.size()-1) {
			const Point &point = points[i];
			double lat = point.lat();
			double lng = point.lng();
			int late5 = static_cast<int>(floor(lat * 1e5 + 0.5));
			int lnge5 = static_cast<int>(floor(lng * 1e5 + 0.5));
			dlat = late5 - plat;
			dlng = lnge5 - plng;
			plat = late5;
			plng = lnge5;
			encoded_points += encodeSignedNumber(dlat);
			encoded_points += encodeSignedNumber(dlng);
		}
	}
	return encoded_points;
}

// This computes the appropriate zoom level of a point in terms of it's 
// distance from the relevant segment in the DP algorithm.	Could be done
// in terms of a logarithm, but this approach makes it a bit easier to
// ensure that the level is not too large.
int PolylineEncoder::computeLevel(double dd) {
	int lev = 0;
	if (dd > verySmall) {
		while (dd < zoomLevelBreaks[lev]) {
			lev++;
		}
	}
	return lev;
}

// Now we can use the previous function to march down the list
// of points and encode the levels.	Like createEncodings, we
// ignore points whose distance (in dists) is undefined.
std::string PolylineEncoder::encodeLevels(const PolyLine &points, const std::vector<double> &dists, double absMaxDist) {
	std::string encoded_levels;
	if (forceEndpoints) {
		encoded_levels += encodeNumber(numLevels-1);
	} else {
		encoded_levels += encodeNumber(numLevels-computeLevel(absMaxDist)-1);
	}
	for (size_t i=1; i < points.size()-1; i++) {
		if (dists[i] != undefined) {
			encoded_levels += encodeNumber(numLevels-computeLevel(dists[i])-1);
		}
	}
	if (forceEndpoints) {
		encoded_levels += encodeNumber(numLevels-1);
	} else {
		encoded_levels += encodeNumber(numLevels-computeLevel(absMaxDist)-1);
	}
	return encoded_levels;
}

PolyLine PolylineEncoder::decodePoints(const std::string &encoded) {
	size_t len = encoded.size();
	size_t index = 0;
	PolyLine points;
	int lat = 0;
	int lng = 0;

	while (index < len) {
		char b;
		int shift = 0;
		int result = 0;
		do {
			b = encoded[index++] - 63;
			result |= (b & 0x1f) << shift;
			shift += 5;
		} while (b >= 0x20 && index < len);
		int dlat = ((result & 1) ? ~(result >> 1) : (result >> 1));
		lat += dlat;

		shift = 0;
		result = 0;
		do {
			b = encoded[index++] - 63;
			result |= (b & 0x1f) << shift;
			shift += 5;
		} while (b >= 0x20 && index < len);
		int dlng = ((result & 1) ? ~(result >> 1) : (result >> 1));
		lng += dlng;

		points.push_back(Point(lat * 1e-5, lng * 1e-5));
	}
	return points;
}

std::vector<unsigned int> PolylineEncoder::decodeLevels(const std::string &encoded) {
	size_t len = encoded.size();
	size_t index = 0;
	std::vector<unsigned int> levels;

	while (index < len) {
		char b;
		int shift = 0;
		unsigned int level = 0;
		do {
			b = encoded[index++] - 63;
			level |= (b & 0x1f) << shift;
			shift += 5;
		} while (b >= 0x20 && index < len);
		levels.push_back(level);
	}
	return levels;
}

std::string PolylineEncoder::encodeNumber(unsigned int num) {
	std::string encodeString;
	while (num >= 0x20) {
		char nextValue = (0x20 | (num & 0x1f)) + 63;
		encodeString += nextValue;
		num >>= 5;
	}
	char finalValue = static_cast<char>(num + 63);
	if (finalValue == '\\') {
		encodeString += finalValue;
	}
	encodeString += finalValue;
	return encodeString;
}

std::string PolylineEncoder::encodeSignedNumber(int num) {
	unsigned int sgn_num = num << 1;
	if (num < 0) {
		sgn_num = ~sgn_num;
	}
	return encodeNumber(sgn_num);
}