//Totte Kolsi
//Student Number 258980
//tottek@student.uef.fi, tottekoo@gmail.com

//POLYGON BASED APPROXIMATION ALGORITHM
//based on the following research paper:

//"Trajectory approximation for resource constrained mobile sensor networks."
//In Distributed Computing in Sensor Systems (DCOSS), 2014 IEEE International Conference on, pp. 59-66. IEEE, 2014.
//Murtaza, Ghulam, Salil S. Kanhere, Aleksandar Ignjatovic, Raja Jurdak, and Sanjay Jha. 

#include <iostream>
#include <fstream> 
#include <string>
#include <iomanip>
#include <sstream>

#include "PBA.h";
#include "PBABitSet.h"

//Call PBA() for the encoding algorithm
//Call PBABitSet.decode() for decoding algorithm
//Example with file read/write functions in main()

//TEST USING:
//MOPSI DATASET		http://cs.joensuu.fi/~mchen/GPSTrajSimp.htm 
//GPS VISUALIZER	http://www.gpsvisualizer.com/map_input?form=google
												
PBABitSet PBA(std::vector<GPSPoint> trajectory, double spatialError);
PBABitSet PBA(std::vector<GPSPointEx> trajectory, double spatialError, double temporalError);

int main()
{
	/*
	std::vector<GPSPoint> trajectory;
	PBABitSet bs;
	std::vector<GPSPoint> pbaTrajectory;
	trajectory = readTrajectory("txt\\MOPSI\\0003.txt");
	writeTrajectory(trajectory, "txt\\PBA\\pre_pba3.txt");
	bs = PBA(trajectory, 10);
	pbaTrajectory = bs.decode(10);
	writeTrajectory(pbaTrajectory, "txt\\PBA\\pba3.txt");

	int original = sizeof(trajectory) + trajectory.size() * sizeof(GPSPoint);
	int compressed = sizeof(bs) + bs.getBitmaskSize() * sizeof(short);
	*/

	std::vector<GPSPointEx> trajectoryEx;
	PBABitSet bs2;
	std::vector<GPSPointEx> pbaTrajectoryEx;

	trajectoryEx = readTrajectoryEx("txt\\MOPSI\\0003.txt");
	writeTrajectoryEx(trajectoryEx, "txt\\PBA\\pre_pba3.txt");

	bs2 = PBA(trajectoryEx, 10, 60);
	pbaTrajectoryEx = bs2.decodeEx(10, 60);
	writeTrajectoryEx(pbaTrajectoryEx, "txt\\PBA\\pba3.txt");

	int original2 = sizeof(trajectoryEx) + trajectoryEx.size() * sizeof(GPSPointEx);
	int compressed2 = sizeof(bs2) + bs2.getBitmaskSize() * sizeof(short);
	
	

	return 0;
}

												// in meters
PBABitSet PBA(std::vector<GPSPoint> trajectory, double spatialError)
{
	GPSPoint cp = trajectory[0];

	PBABitSet bs;
	bs.startPoint = cp;

	double max = 0;
	for (int i = 1; i < trajectory.size(); i++)
	{
		GPSPoint np = trajectory[i];

		double d = distance(cp, np) * 1000; //km to meters

		if (d > max)
			max = d;

		PBACase result = nonRecorder;

		if (d < spatialError)
			continue;
		else if (spatialError < d && d <= (3 * spatialError))
			result = chase;
		else if (d > 3 * spatialError)
			result = jump;

		switch (result) 
		{
			case nonRecorder:
				continue;
				break;

			case chase:
			{
				//triangle: current point, next point, point on meridian(cp.lon, cp.lat+constant)
				//solve angle between line cp->meridian and cp->next point
				//record angle as 3 bits

				double angle = chaseAngle(cp, np);
				bs.storeChase(angle);

				cp = latlon_radial_distance(cp, angle, spatialError*2);
				break;
			}
			case jump:
			{
				//TODO:
				//direction calculation will be incorrect near 180(east) and -180(west). fix if time left.
				xDirection xDir = left;
				if (cp.lon < np.lon)
					xDir = right;

				yDirection yDir = down;
				if (cp.lat < np.lat)
					yDir = up;
				
				//intersection type == edge || v2v
				bool isEdge = false;

				unsigned short jx = jumpX(cp, np, spatialError, isEdge);
				unsigned short jy = jumpY(cp, np, spatialError, isEdge);

				bs.storeJump(jx, jy, xDir, yDir);
				
				//Calculate new current point
				//x-axis jump
				double base = horizontalLength(jx, spatialError);
				double xDegree = -90;
				if (xDir == right)
					xDegree = 90;
				cp = latlon_radial_distance(cp, xDegree, base);

				//y-axis jump
				double height = verticalLength(jy, spatialError, jx);
				double yDegree = 0;
				if (yDir == up)
					yDegree = 180;

				
				cp = latlon_radial_distance(cp, yDegree, height);
			
				break;
			}
		}		
	}

	return bs;
}

PBABitSet PBA(std::vector<GPSPointEx> trajectory, double spatialError, double temporalError)
{
	GPSPoint cp = trajectory[0];
	time_t ct = trajectory[0].time;

	PBABitSet bs;
	bs.startPoint = cp;
	bs.startTime = ct;

	for (int i = 1; i < trajectory.size(); i++)
	{
		GPSPoint np = trajectory[i];

		double d = distance(cp, np) * 1000; //km to meters

		PBACase result = nonRecorder;

		if (d < spatialError)
			continue;
		else if (spatialError < d && d <= (3 * spatialError))
			result = chase;
		else if (d > 3 * spatialError)
			result = jump;

		switch (result)
		{
		case nonRecorder:
			continue;
			break;

		case chase:
		{
			//triangle: current point, next point, point on meridian(cp.lon, cp.lat+constant)
			//solve angle between line cp->meridian and cp->next point
			//record angle as 3 bits

			bs.storeTimestamp(ct, trajectory[i].time, temporalError);
			double angle = chaseAngle(cp, np);
			bs.storeChase(angle);

			cp = latlon_radial_distance(cp, angle, spatialError * 2);
			break;
		}
		case jump:
		{
			bs.storeTimestamp(ct, trajectory[i].time, temporalError);

			//TODO:
			//direction calculation will be incorrect near 180(east) and -180(west). fix if time left.
			xDirection xDir = left;
			if (cp.lon < np.lon)
				xDir = right;

			yDirection yDir = down;
			if (cp.lat < np.lat)
				yDir = up;

			//intersection type == edge || v2v
			bool isEdge = false;

			unsigned short jx = jumpX(cp, np, spatialError, isEdge);
			unsigned short jy = jumpY(cp, np, spatialError, isEdge);

			bs.storeJump(jx, jy, xDir, yDir);

			//Calculate new current point
			//x-axis jump
			double base = horizontalLength(jx, spatialError);
			double xDegree = -90;
			if (xDir == right)
				xDegree = 90;
			cp = latlon_radial_distance(cp, xDegree, base);

			//y-axis jump
			double height = verticalLength(jy, spatialError, jx);
			double yDegree = 0;
			if (yDir == up)
				yDegree = 180;
			cp = latlon_radial_distance(cp, yDegree, height);

			break;
		}
		}
	}

	return bs;
}


//https://en.wikipedia.org/wiki/Law_of_cosines
// angle C = arccos(a^2 + b^2 - c^2 / 2ab)
// cp=current point, np=next point, mp = meridian point
// angle cp = arccos(cpnp^2 + cpmp^2 - npmp^2 / 2*cpnp*cpmp)
double chaseAngle(const GPSPoint &cp, const GPSPoint &np) {
	//TODO:
	//currently might produce rounding errors, consider this: https://math.stackexchange.com/questions/499310/law-of-cosines-for-very-acute-angles-round-off-error
	
	//vitusti erroria, distance  = NaN
	if (cp.lat == np.lat && cp.lon == np.lon)
		return 0;

	//point on meridian
	GPSPoint mp = cp;
	//maybe not the best offset
	double offset = (abs(cp.lat - np.lat) + abs(cp.lon - np.lon)) / 2;
	mp.lat -= offset;

	double cpnp, cpmp, npmp, cpnpcpmp;
	cpnp = std::pow(distance(cp, np), 2);
	cpmp = std::pow(distance(cp, mp), 2);
	npmp = std::pow(distance(np, mp), 2);
	cpnpcpmp = distance(cp, np)*distance(cp, mp);

	//rounding error can cause: temp> 1 or temp< -1, while acos() only accepts input between -1 and 1
	//--> ceil/floor to correct this error
	double temp = (cpnp + cpmp - npmp) / (2 * cpnpcpmp);
	if (temp < -1)
		temp = ceil(temp);
	else if (temp > 1)
		temp = floor(temp);

	double result = acos(temp);

	//_ASSERT(!isnan(result));

	result = rad2deg(result);

	if (np.lon < cp.lon)
		result = -result;

	return result;
}

unsigned short jumpX(const GPSPoint &p, const GPSPoint &p1, double spatialError, bool &isEdge) {

	float dy = p1.lat - p.lat;
	float dx = cosf(M_PI / 180 * p.lat)*(p1.lon - p1.lon);
	float angle = atan2f(dy, dx);

	GPSPoint mp(p.lat, p1.lon);
	
	//TODO:
	//calculate base using pythagoran theorem instead (?)
	double base = distance(p, mp);
	//km to meters
	base *= 1000;

	//round down
	int aHor = base / spatialError;

	
	aHor -= 1;
	unsigned short jx = 0;
	if (aHor < 0)
	{
		jx = 0;
		
	}
	else if (aHor % 3 == 0)
	{
		jx = 2 * (aHor / 3 + 1) - 1;
		isEdge = true;
	}
	else if (aHor % 3 != 0)
	{
		jx = 2 * ((aHor + 3 - (aHor % 3)) / 3);
	}

	return jx;
}
unsigned short jumpY(const GPSPoint &p, const GPSPoint &p1, double spatialError, bool isEdge) {
	GPSPoint mp(p.lat, p1.lon);

	//TODO:
	//calculate height using pythagoran theorem instead (?)
	double d = distance(p, p1);
	double dx = distance(p, mp);

	double height = std::sqrt(std::pow(d, 2) + std::pow(dx, 2));
	//km to meters
	height *= 1000;

	//round down
	int aVer = height / spatialError;

	unsigned short jy = 0;
	if(isEdge)
	{
		//edge intersection case
		jy = aVer / 2 + 1;
	}
	else if (aVer % 2 == 0)
	{
		jy = aVer / 2;
	}
	else if (aVer % 2 != 0)
	{
		jy = (aVer + 1) / 2;
	}

	return jy;
}


//current distance formula
double distance(const GPSPoint &p1, const GPSPoint &p2) {
	return haversineDistance(p1, p2);
}

double haversine2(double lat1, double lon1, double lat2, double lon2) {
	double R = 6371;
	lat1 = deg2rad(lat1);
	lon1 = deg2rad(lon1);
	lat2 = deg2rad(lat2);
	lon2 = deg2rad(lon2);

	double dlon = lon2 - lon1;
	double dlat = lat2 - lat1;

	double a = std::pow(sin(dlat / 2), 2) + cos(lat1) * cos(lat2) * std::pow(sin(dlon/2), 2);
	double c = 2 * asin(std::min(1.0, sqrt(a)));
	double d = R * c;

	return d;
}

double haversine2(GPSPoint &p1, GPSPoint &p2) {
	return haversine2(p1.lat, p1.lon, p2.lat, p2.lon);
}

double haversineDistance(double lat1, double lon1, double lat2, double lon2)
{
	double earthRadius = 6371;
	lat1 = deg2rad(lat1);
	lon1 = deg2rad(lon1);
	lat2 = deg2rad(lat2);
	lon2 = deg2rad(lon2);

	double a = std::pow(sin((lat2 - lat1)/2), 2) + std::pow(sin((lon2 - lon1)/2), 2) * cos(lat1) * cos(lat2);
	double distance = 2 * earthRadius * asin(std::sqrt(a));

	return distance;
}

double haversineDistance(const GPSPoint &p1, const GPSPoint &p2) {
	return haversineDistance(p1.lat, p1.lon, p2.lat, p2.lon);
}


double deg2rad(double degree) {
	return degree * (M_PI / 180.0);
}

double rad2deg(double radian) {
	return radian * (180.0 / M_PI);
}

//www.edwilliams.org/avform.htm#Math
double aviation_modulo(double y, double x) {
	return y - x * floor(y / x);
}

/*
A point {lat,lon} is a distance d out on the tc radial from point 1 if:

lat=asin(sin(lat1)*cos(d)+cos(lat1)*sin(d)*cos(tc))
IF (cos(lat)=0)
lon=lon1      // endpoint a pole
ELSE
lon=mod(lon1-asin(sin(tc)*sin(d)/cos(lat))+pi,2*pi)-pi
ENDIF
*/

GPSPoint latlon_radial_distance(const GPSPoint &basePoint, double deg, double d) {
	GPSPoint point = GPSPoint();
	
	//for some reason gps coordinates were reversed before this. 0 was north, 180 south etc.
	//bandaid? TODO: figure out if the correct way to fix this.
	deg +=180;

	//CONVERT ALL UNIT TO RADIANS
	d = d/1852; //meters to nautical miles
	d = (M_PI/(180*60))* d; //nautical miles to radian

	point.lat = asin(sin(deg2rad(basePoint.lat))*cos(d) + cos(deg2rad(basePoint.lat))*sin(d)*cos(deg2rad(deg)));
	if (cos(point.lat) == 0)
		point.lon = deg2rad(basePoint.lon);      // endpoint a pole
	else
		point.lon = aviation_modulo(deg2rad(basePoint.lon) - asin(sin(deg2rad(deg))*sin(d) / cos(point.lat)) + M_PI, 2 * M_PI) - M_PI;

	point.lat = rad2deg(point.lat);
	point.lon = rad2deg(point.lon);

	return point;
}
/*
This algorithm is limited to distances such that dlon <pi / 2, i.e those that extend
	around less than one quarter of the circumference of the earth in longitude.
	A completely general, but more complicated algorithm is necessary if
	greater distances are allowed :

lat = asin(sin(lat1)*cos(d) + cos(lat1)*sin(d)*cos(tc))
dlon = atan2(sin(tc)*sin(d)*cos(lat1), cos(d) - sin(lat1)*sin(lat))
lon = mod(lon1 - dlon + pi, 2 * pi) - pi
*/

double horizontalLength(unsigned short jx, double spatialError) {
	double horizontalLength = 0;
	if (jx % 2 == 0)
	{
		horizontalLength = spatialError * (jx + (jx / 2.0));
	}
	else if (jx % 2 != 0)
	{
		horizontalLength = spatialError * (jx + ((jx / 2.0) + 0.5));
	}

	return horizontalLength;
}


double verticalLength(unsigned short jy, double spatialError, unsigned short jx) {
	double verticalLength = 0;
	if (jx % 2 == 0)
	{
		verticalLength = jy * 2 * spatialError;
	}
	else if (jx % 2 != 0)
	{
		verticalLength = jy * 2 * spatialError + spatialError;
	}

	return verticalLength;
}


std::vector<GPSPoint> readTrajectory(std::string filename, std::string delimeter)
{
	std::vector<GPSPoint> trajectory;

	std::ifstream myfile;
	myfile.open (filename, std::ios::in); 

	if (myfile.is_open())
	{
		std::string line;
		while (std::getline (myfile,line) )
		{
			GPSPoint pt = GPSPoint();
			std::string temp;

			//lat
			size_t start = 0;
			size_t len = line.std::string::find_first_of(delimeter, start);
			temp = line.std::string::substr(start, len);
			pt.lat = std::stod(temp);

			//lon
			start = start + len + 1;
			len = line.std::string::find_first_of(delimeter, start) - start;
			temp = line.std::string::substr(start, len);
			pt.lon = std::stod(temp);


			trajectory.push_back(pt);
			//store timestamp into a container
		}
	}

	myfile.close();

	return trajectory;
}

std::vector<GPSPointEx> readTrajectoryEx(std::string filename, std::string delimeter)
{
	std::vector<GPSPointEx> trajectory;

	std::ifstream myfile;
	myfile.open (filename, std::ios::in); 

	if (myfile.is_open())
	{
		std::string line;
		while (std::getline (myfile,line) )
		{
			GPSPointEx pt = GPSPointEx();
			std::string temp;

			//lat
			size_t start = 0;
			size_t len = line.std::string::find_first_of(delimeter, start);
			temp = line.std::string::substr(start, len);
			pt.lat = std::stod(temp);

			//lon
			start = start + len + 1;
			len = line.std::string::find_first_of(delimeter, start) - start;
			temp = line.std::string::substr(start, len);
			pt.lon = std::stod(temp);

			//timestamp yyyy-mm-dd
			start = start + len + 1;
			len = line.std::string::find_first_of(delimeter, start) - start;
			temp = line.std::string::substr(start, len);

			//timestamp hh:mm:ss
			start = start + len + 1;
			len = line.std::string::find_first_of(delimeter, start) - start;
			temp += " " + line.std::string::substr(start, len);

			struct std::tm tm;
			std::istringstream ss(temp);
			ss >> std::get_time(&tm, "%Y-%m-%d %H:%M:%S");
			pt.time = mktime(&tm);

			
			trajectory.push_back(pt);
		}

	}

	myfile.close();

	return trajectory;
}

void writeTrajectory(std::vector<GPSPoint> trajectory, std::string filename, std::string delimeter, bool writeHeader){

	std::ofstream myfile(filename);

	if (myfile.is_open())
	{
		std::string header = "latitude" "\t" "longitude" "\n";
		if(writeHeader)
			myfile << header;

		for(int i=0; i< trajectory.size(); i++)
		{
			myfile << std::to_string(trajectory[i].lat) + delimeter + std::to_string(trajectory[i].lon) + '\n';
		}
	}

	myfile.close();
}

void writeTrajectoryEx(std::vector<GPSPointEx> trajectory, std::string filename, std::string delimeter, bool writeHeader){

	std::ofstream myfile(filename);

	if (myfile.is_open())
	{
		std::string header = "latitude" + delimeter + "longitude" + delimeter + "yyyy-mm-dd" + delimeter + "hh:mm:ss" "\n";
		if (writeHeader)
			myfile << header;

		struct tm time_tm;
		for(int i=0; i< trajectory.size(); i++)
		{
			localtime_s(&time_tm, &trajectory[i].time);

			myfile << std::to_string(trajectory[i].lat) + delimeter + std::to_string(trajectory[i].lon) + delimeter;
			myfile << std::put_time((&time_tm), "%Y-%m-%d");
			myfile << delimeter;
			myfile << std::put_time((&time_tm), "%H:%M:%S");
			myfile << '\n';
		}
	}

	myfile.close();
}