#include "stdafx.h"
#include <vector>
#include <stdio.h>
#include "windows.h"
#include "map_rndf.h"
#include "math.h"
#include "LatLong-UTMconversion.h"

class CMapHolder {
public:
	std::vector<MAPPOINT> points;
	char name;
	int id;
};

std::vector<CMapHolder> RNDFFile;

std::vector<MAPPOINT> CurrentPath;


int map_init(void)
{
	RNDFFile.clear();
	return 0;
}

int map_load(const char *filename)
{
	FILE *fp = fopen(filename,"r");
	if (fp == NULL)
		return -1;

	

	int curr_segment = 0;
	int curr_lane    = 0;
	CMapHolder *h = NULL;

	while (!feof(fp))
	{
		char buffer[2000];
		fgets(buffer,2000,fp);

		char command[200];
		char params[200];
		

		int a,b,c;
		float lati=0,longi=0;

		if (sscanf(buffer,"%d.%d.%d %f %f",&a,&b,&c,&lati,&longi)==5)
		{
			if (curr_segment == 0)		continue;
			if (curr_lane != 1)			continue;
			if (h == NULL)				continue;

		    double UTMNorthing,UTMEasting;
			char UTMZone[4];
			
			LLtoUTM(23,lati,longi,UTMNorthing,UTMEasting,UTMZone);


			MAPPOINT pt;
			pt.x   =   (float)UTMEasting; // longi
			pt.y   =   (float)UTMNorthing; // lati
			
			pt.typ = 0;

			h->points.push_back(pt);
		}
		else if (sscanf(buffer,"%s %s",&command,&params) == 2)
		{
			if (strcmp(command,"segment") == 0)
			{
				curr_segment = atoi(params);
				curr_lane    = 0;

				CMapHolder val;
				val.id = curr_segment;

				RNDFFile.push_back(val);
				h = &(*(RNDFFile.end()-1));
			}
			else if (strcmp(command,"segment_name")==0)
			{
				if (h != NULL)
					h->name = params[0];
			}
			else if (strcmp(command,"lane")==0)
			{
				int e,f;
				if (sscanf(params,"%d.%d",&e,&f) != 2)
					return -1; /// error terror
				curr_lane = f;
			}
		}
		else
		{
			if (strcmp(buffer,"end_segment")==0)
			{
				h				= NULL;
				curr_segment	= 0;
				curr_lane		= 0;
			} 
			else if (strcmp(buffer,"end_lane")==0)
			{
				curr_lane		= 0;
			}
		}

	}

	fclose(fp);

	return 0;
}

int map_select(const char *waypoints)
{
	CurrentPath.clear();

	MAPPOINT last;
	last.typ = -1;
	last.x = 14.427808f;
	last.y = 50.105825f;

	while(*waypoints != 0)
	{
		for (std::vector<CMapHolder>::iterator i=RNDFFile.begin();i!=RNDFFile.end();i++)
		{
			if ((*i).name == waypoints[0])
			{
				MAPPOINT b = *((*i).points.begin());
				MAPPOINT e = *((*i).points.end()-1);

				float lb = fabs(last.x-b.x) + fabs(last.y-b.y);
				float le = fabs(last.x-e.x) + fabs(last.y-e.y);

				if ((lb < le) || (last.typ == -1))
				{
					// begin is closer
					for (std::vector<MAPPOINT>::iterator j=(*i).points.begin();j!=(*i).points.end();j++)
						CurrentPath.push_back(*j);

					last = e;
				}
				else
				{
					// end is closer
					for (std::vector<MAPPOINT>::reverse_iterator j=(*i).points.rbegin();j!=(*i).points.rend();j++)
						CurrentPath.push_back(*j);

					last = b;
				}

				break;
			}
		}
		waypoints++;
	}

	return 0;
}

int map_getpoints(MAPPOINT *pts,int max)
{
	if (pts == NULL) 
		return 0;

	int p = 0;
	for (std::vector<MAPPOINT>::iterator j=CurrentPath.begin();j!=CurrentPath.end();j++,pts++,p++)
	{
		if (p == max)
			return p;

		*pts = *j;
	}
	return p;
}