#include <stdio.h>
#include <stdlib.h>
#include <Windows.h>
#include <math.h>
#include <string>
#include <iostream>
#include <fstream>
#include <map>
#include <algorithm>
#include "OSMfunctions.h"

using namespace std;


//Ziskani pozice way
int getPositionWay(vector<int> &Ways_v, long wayID) {

	int i = 1;
	vector<int> Way;
	vector<int>::iterator itWay;
	for (itWay = Ways_v.begin(); itWay != Ways_v.end(); itWay++, i++) {
		if (*itWay == wayID) {
			return i;
		}
	}
	return -1;
}

// Ziskani nodu a jejich pozice dle way ID
NODES_t getNodesByWayId(DataFile_t &data, long wayId) {

	// Vraceni cesty a jeji souradic, pokud existuje
	if(data.find(wayId) != data.end()) {
		return data[wayId];
	}

	// Jinak prazdna struktura
	NODES_t empty;
	return empty;
}

// Ziskani souradnice nodu
void getNodePosition(DataFile_t &data, long nodeId, double &lat, double &lon) {

	// Nalezeni node ve ways
	for(DataFile_t::iterator wayIt = data.begin(); wayIt != data.end(); wayIt++) {

		NODES_t nodes = wayIt->second;

		for(NODES_t::iterator nodeIt = nodes.begin(); nodeIt != nodes.end(); nodeIt++) {

			// Pokud se node ID shoduje s pozadovanym, vratime jeho souradnice
			if(nodeIt->nodeId == nodeId) {
				lat = nodeIt->lat;
				lon = nodeIt->lon;
				return;
			}
		}
	}

	// Nic nebylo nalezeno
	lat = 0.0;
	lon = 0.0;
	return;
}

// Ziskani way ID podle nodu
void getWayIdByNodeId(DataFile_t &data, long nodeId, DataPos_t &PosNode) {

	// Preventivni vycisteni
	PosNode.clear();

	// Nalezeni node ve ways
	for(DataFile_t::iterator wayIt = data.begin(); wayIt != data.end(); wayIt++) {

		NODES_t nodes = wayIt->second;

		int pos = 1;
		for(NODES_t::iterator nodeIt = nodes.begin(); nodeIt != nodes.end(); nodeIt++, pos++) {

			// Pokud se node ID shoduje s pozadovanym,
			// zaznamename si jeho pozici a vrazime ho do vektoru vysledku
			if(nodeIt->nodeId == nodeId) {
				PosNode.push_back(pair<long, int>(wayIt->first,pos));
				// Najde pouze prvni vyskyt, aby se way ID neopakovali
				break;
			}
		}
	}

	return;
} 
//vypocet vzdalenosti
double getDistance(double PointLatA, double PointLatB, double PointLonA, double PointLonB) {

	double E1, E2, N1, N2;
	char UTMZone[4];
	int RefEllipsoid = 23;

	LLtoUTM(RefEllipsoid, PointLatA, PointLonA, E1, N1, UTMZone);
	LLtoUTM(RefEllipsoid, PointLatB, PointLonB, E2, N2, UTMZone);

	return(sqrt((N2-N1)*(N2-N1)+(E2-E1)*(E2-E1)));
}
//ziskani ID node za pomoci sirky a delky a jeho pozici ve ways
long getNodeByLL(DataFile_t &data, double lat, double lon, DataPos_t &PosNode) {
	
	for(DataFile_t::iterator wayIt = data.begin(); wayIt != data.end(); wayIt++) {

		NODES_t nodes = wayIt->second;

		for(NODES_t::iterator nodeIt = nodes.begin(); nodeIt != nodes.end(); nodeIt++) {

			// Pokud se node ID shoduje s pozadovanym, vratime jeho souradnice
			if(nodeIt->lat > lat - 0.000001 && nodeIt->lat < lat + 0.000001 
				&& nodeIt->lon > lon - 0.000001 && nodeIt->lon < lon + 0.000001) {
					getWayIdByNodeId(data, nodeIt->nodeId, PosNode);
					return nodeIt->nodeId;
			}
		}
	}

	//Nic nebylo nalezeno
	return 000000;
}

//Zjisteni nejblizsiho node od zadaneho bodu
double getNearestPoint(DataFile_t &data, double RoundLat, double RoundLon, 
					   double &NearestLat, double &NearestLon, long &Node, DataPos_t &PosNode) {
						 
	double distance = INFINITE;
	double tmpDist; 

	for(DataFile_t::iterator wayIt = data.begin(); wayIt != data.end(); wayIt++) {

		NODES_t nodes = wayIt->second;

		for(NODES_t::iterator nodeIt = nodes.begin(); nodeIt != nodes.end(); nodeIt++) {
			tmpDist = getDistance(RoundLat, nodeIt->lat, RoundLon, nodeIt->lon);
			if (distance > tmpDist) {
				distance = tmpDist;
				NearestLat = nodeIt->lat;
				NearestLon = nodeIt->lon;
			}
		}
	}
	Node = getNodeByLL(data, NearestLat, NearestLon, PosNode);

	return distance;
}

// Prevod do RNDF
void ConvertToRNDF (DataFile_t &data, vector<int> &Ways_v, string RNDFoutFILE) {

	ofstream outRNDF(RNDFoutFILE.c_str());
	outRNDF.precision(6);
	outRNDF.setf(ios::fixed);

	SYSTEMTIME st;
	GetSystemTime(&st);

	outRNDF<<"RNDF_name\t"<<"\""<<RNDFoutFILE<<"\""<<endl;
	outRNDF<<"num_segments\t"<<data.size()<<endl;
	outRNDF<<"num_zones\t0"<<endl;
	outRNDF<<"creation_date\t"<<st.wMonth<<"/"<<st.wDay<<"/"<<st.wYear<<endl;

	int NmbrWay = 1;
	int cmplt = 0;
	for (DataFile_t::iterator WayIt = data.begin(); WayIt != data.end(); WayIt++, NmbrWay++) {
		if (NmbrWay%2) {
		cmplt = 100 * NmbrWay/ data.size();
		cout<<cmplt<<" % Prevedeno"<<endl;}

		NODES_t nodes = WayIt->second;
		outRNDF<<"segment\t"<<NmbrWay<<endl;
		outRNDF<<"num_lanes\t2"<<endl;
		outRNDF<<"segment_name\t"<<WayIt->first<<endl;
		outRNDF<<"lane\t"<<NmbrWay<<".1"<<endl;
		outRNDF<<"num_waypoints\t"<<nodes.size()<<endl;


		int NmbrNod = 1; 
		DataPos_t ways;
		vector <int> posNode;
		int PosActNode;

		for (NODES_t::iterator NodeIt = nodes.begin(); NodeIt != nodes.end(); NodeIt++) {

			getWayIdByNodeId(data, NodeIt->nodeId, ways);
			for (DataPos_t::iterator it = ways.begin(); it != ways.end(); it++) {
				if (WayIt->first == it->first) {
					PosActNode = it->second;
				}
			}

			for (DataPos_t::iterator it = ways.begin(); it != ways.end(); it++) {
				if (WayIt->first != it->first) {

					outRNDF<<"exit\t"<<NmbrWay<<".1."<<PosActNode<<"\t"
						<<getPositionWay(Ways_v, it->first)<<".1."<<it->second<<endl;
				}
			}
		}

		outRNDF<<"exit\t"<<NmbrWay<<".1."<<nodes.size()<<"\t"<<NmbrWay<<".2.1"<<endl;

		int nmbr = 1;
		for (NODES_t::iterator NodeIt = nodes.begin(); NodeIt != nodes.end(); NodeIt++, nmbr++) {
			//outRNDF<<it->first<<endl;
			outRNDF<<NmbrWay<<".1."<<nmbr<<"\t"<<NodeIt->lat<<"\t"<<NodeIt->lon<<endl;
		}

		outRNDF<<"end_lane"<<endl;
		outRNDF<<"lane\t"<<NmbrWay<<".2"<<endl;
		outRNDF<<"num_waypoints\t"<<nodes.size()<<endl;

		for (NODES_t::iterator NodeIt = nodes.begin(); NodeIt != nodes.end(); NodeIt++) {

			getWayIdByNodeId(data, NodeIt->nodeId, ways);
			for (DataPos_t::iterator it = ways.begin(); it != ways.end(); it++) {
				if (WayIt->first == it->first) {
					PosActNode = it->second;
				}
			}
			for (DataPos_t::iterator it = ways.begin(); it != ways.end(); it++) {
				if (WayIt->first != it->first) {

					outRNDF<<"exit\t"<<NmbrWay<<".2."<<nodes.size() - PosActNode + 1<<"\t"
						<<getPositionWay(Ways_v, it->first)<<".1."<<it->second<<endl;
				}
			}
		}

		outRNDF<<"exit\t"<<NmbrWay<<".2."<<nodes.size()<<"\t"<<NmbrWay<<".1.1"<<endl;

		nmbr = 1;
		for (NODES_t::reverse_iterator NodeIt = nodes.rbegin(); NodeIt != nodes.rend(); NodeIt++, nmbr++) {
			outRNDF<<NmbrWay<<".2."<<nmbr<<"\t"<<NodeIt->lat<<"\t"<<NodeIt->lon<<endl;
		}
		outRNDF<<"end_lane"<<endl;
		outRNDF<<"end_segment"<<endl;		
	}
	outRNDF<<"end_file"<<endl;
	cout<< "=== Prevedeno do RNDF ===" <<endl<<endl;

	return;
}