#include "GPS_Sedlak.h"

char UTMZone[4];
int RefEllipsoid = 23;
Tserial *g_GPS; 
Tserial *ts_DGPS;
Tserial *g_Init;
Tserial *ts_Poloha;

int NacteniSoubor(void) {
	char c;
	string data = "";
	string veta = "";
	double Lat, Longi;

	ifstream soubor;
		soubor.open("Souradnice.txt");
		if(!soubor.is_open()) {
			cerr << "Nepodarilo se otevrit soubor s konfiguraci." << endl;
			return EXIT_FAILURE;
		}
		while(soubor.get(c)) {
			data.append(1, c);
		}
		soubor.close();
		cout<<data<<endl;
		return EXIT_SUCCESS;

}
double Vzdalenost(string Sbod1, string Sbod2) {
	double bodax, boday, bodbx, bodby;

	PrevedStrToUTM(Sbod1, boday, bodax);
	PrevedStrToUTM(Sbod2, bodby, bodbx);

	return(sqrt((bodby-boday)*(bodby-boday)+(bodbx-bodax)*(bodbx-bodax)));
}


void PrevedStrToLL(string Sbod, double &Lat, double &Longi) { 
	string SbodN, SbodE;

	SbodN = Sbod.substr (0,Sbod.find(','));
	SbodE = Sbod.substr (Sbod.find(',')+1);

	if(Sbod.find('°')!=string::npos) {
		Lat = atof(SbodN.substr(0,SbodN.find('°')).c_str());
		Lat += atof(SbodN.substr(SbodN.find('°')+1,SbodN.find('\'')-SbodN.find('°')-1).c_str())/60;
		Lat += atof(SbodN.substr(SbodN.find('\'')+1).c_str())/3600;

		Longi = atof(SbodE.substr(0,SbodE.find('°')).c_str());
		Longi += atof(SbodE.substr(SbodE.find('°')+1,SbodE.find('\'')-SbodE.find('°')-1).c_str())/60;
		Longi += atof(SbodE.substr(SbodE.find('\'')+1).c_str())/3600;
	}
	else {
		SbodN = Sbod.substr (0,Sbod.find(','));
		SbodE = Sbod.substr (Sbod.find(',')+1);
		Lat = atof(SbodN.c_str());
		Longi = atof(SbodE.c_str());
	}
}

void PrevedStrToUTM(string Sbod, double &N, double &E) { 
	double Lat,Longi;
	string SbodN, SbodE;

	SbodN = Sbod.substr (0,Sbod.find(','));
	SbodE = Sbod.substr (Sbod.find(',')+1);

	if(Sbod.find('°')!=string::npos) {
		Lat = atof(SbodN.substr(0,SbodN.find('°')).c_str());
		Lat += atof(SbodN.substr(SbodN.find('°')+1,SbodN.find('\'')-SbodN.find('°')-1).c_str())/60;
		Lat += atof(SbodN.substr(SbodN.find('\'')+1).c_str())/3600;

		Longi = atof(SbodE.substr(0,SbodE.find('°')).c_str());
		Longi += atof(SbodE.substr(SbodE.find('°')+1,SbodE.find('\'')-SbodE.find('°')-1).c_str())/60;
		Longi += atof(SbodE.substr(SbodE.find('\'')+1).c_str())/3600;
	}
	else {
		SbodN = Sbod.substr (0,Sbod.find(','));
		SbodE = Sbod.substr (Sbod.find(',')+1);
		Lat = atof(SbodN.c_str());
		Longi = atof(SbodE.c_str());
	}
	LLtoUTM(RefEllipsoid, Lat, Longi, E, N, UTMZone);
}

void PrevedStrToUTMaLL(string Sbod, double &N, double &E, double &Lat, double &Longi) {
	string SbodN, SbodE;

	SbodN = Sbod.substr (0,Sbod.find(','));
	SbodE = Sbod.substr (Sbod.find(',')+1);

	if(Sbod.find('°')!=string::npos) {
		Lat = atof(SbodN.substr(0,SbodN.find('°')).c_str());
		Lat += atof(SbodN.substr(SbodN.find('°')+1,SbodN.find('\'')-SbodN.find('°')-1).c_str())/60;
		Lat += atof(SbodN.substr(SbodN.find('\'')+1).c_str())/3600;

		Longi = atof(SbodE.substr(0,SbodE.find('°')).c_str());
		Longi += atof(SbodE.substr(SbodE.find('°')+1,SbodE.find('\'')-SbodE.find('°')-1).c_str())/60;
		Longi += atof(SbodE.substr(SbodE.find('\'')+1).c_str())/3600;
	}
	else {
		SbodN = Sbod.substr (0,Sbod.find(','));
		SbodE = Sbod.substr (Sbod.find(',')+1);
		Lat = atof(SbodN.c_str());
		Longi = atof(SbodE.c_str());
	}

	LLtoUTM(RefEllipsoid, Lat, Longi, E, N, UTMZone);
}
string GetSentence(Tserial *Target, string identificator, string &data) {	//funkce pro nacitani vety z GPS prijimace
	int ready = 0;															//vystupni parametry jsou: Crc - kontrolni soucet, a data - nasctena veta
	int j = 0, k = 0;
	char c;
	string CrcSoucet;
	string CrcGps;
	char buffer[4] = "";
	
	while(!ready) {
		while((c = Target->getChar()) != '\n') {
			data.push_back(c);
			j++;
			if(j>200) {												//osetreni v pripade nenalezeni konce vety
				cout<<"ERROR1"<<endl;
				return "ERROR";
			}
		}
		if(data.find(identificator.c_str()) != string::npos) {		//nacitani probiha od nastaveneho identifikatoru			 
			ready = 1;
		}
		else {
			ready = 0;
			j = 0;
			data.clear();
		}
	}

	size_t position;
	position = data.find(identificator.c_str());							
	data.erase(0, position+1);											//odmazani dat pred identifikatorem
		
	
	if (j<80) {															//delka spravne vety
		
		return "ERROR";	
	}

	CrcGps = data.substr(data.find("*")+1,2);							//vycteni kontrolniho souctu z vety
	char soucet = data[0];
	for (size_t i=1; i<data.size()-4; i++) {
		soucet^=data[i];												//vypocet kontrolniho souctu
	}
	sprintf(buffer,"%X",soucet);
	CrcSoucet.append(buffer);
	if(CrcSoucet == CrcGps) {
		return CrcSoucet;
	}
	else {
		
		return "ERROR";													//vraci ERROR pri rodilne hodnote kontrolniho souctu
	}
}
string GetSentence(Tserial *Target, string identificator) {	
	int ready = 0;
	int j = 0;
	char c;
	string CrcSoucet;
	string CrcGps;
	string data;
	char buffer[4] = "";

	while(!ready) {
		while((c = Target->getChar()) != '\n') {
			data.push_back(c);
			j++;
		}
		if(data.find(identificator.c_str()) != string::npos)
			ready = 1;
	}

	size_t position;
	position = data.find(identificator.c_str());
	data.erase(0, position+1);

	CrcGps = data.substr(data.find("*")+1,2);
	char soucet = data[0];
	for (size_t i=1; i<data.size()-4; i++) {
		soucet^=data[i];
	}
	sprintf(buffer,"%X",soucet);
	CrcSoucet.append(buffer);
	if(CrcSoucet == CrcGps) {
		return CrcSoucet;
	}
	else {
		return "ERROR";		
	}
}

int Magellan_Init_Remote(string COM) {
	int i = 0;
	char Conf[40] = "";
	char buffer[5] = "";
	string data;
	string crc;
	string soucet;
	sprintf(buffer,"%s",COM.c_str());
	g_Init = new Tserial();	
	int i_InitErr = g_Init->connect(buffer, 9600, spNONE, 0);

	if (!i_InitErr) {
		cout<<"Probiha inicializace REMOTE STATION"<<endl;
		do {
			sprintf(Conf,"$PASHS,RST\r\n");						//reset zarizeni
			g_Init->sendArray(Conf,strlen(Conf));
			i++;
			if(i>5) {
				return EXIT_FAILURE;
			}
		}while(GetSentence(g_Init,"$PASHR") != "3D");
		cout<<"RESET"<<endl;

		i = 0;		
		do {
			sprintf(Conf,"$PASHS,RTC,REM,B\r\n");					//nastaveni remote station na port B - prijimani korekci
			g_Init->sendArray(Conf,strlen(Conf));
			i++;
			if(i>5) {
				return EXIT_FAILURE;
			}
		}while(GetSentence(g_Init,"$PASHR") != "3D");
		cout<<Conf<<endl;

		i = 0;	
		do {
			sprintf(Conf,"$PASHS,RTC,REM,ON\r\n");					//nastaveni remote station na port B - zapnuti prijimani korekci		
			g_Init->sendArray(Conf,strlen(Conf));
			i++;
			if(i>5) {
				return EXIT_FAILURE;
			}
		}while(GetSentence(g_Init,"$PASHR") != "3D");
		cout<<Conf<<endl;

		i = 0;	
		do {
			sprintf(Conf,"$PASHS,RTC,AUT,Y\r\n");					//nastaveni automatickeho rezimu DGPS
			g_Init->sendArray(Conf,strlen(Conf));
			i++;
			if(i>5) {
				return EXIT_FAILURE;
			}
		}while(GetSentence(g_Init,"$PASHR") != "3D");
		cout<<Conf<<endl;

		i = 0;	
		do {
			sprintf(Conf,"$PASHS,DYN,3\r\n");						//dynamicky model
			g_Init->sendArray(Conf,strlen(Conf));
			i++;
			if(i>5) {
				return EXIT_FAILURE;
			}
		}while(GetSentence(g_Init,"$PASHR") != "3D");
		cout<<Conf<<endl;

		i = 0;	
		do {
			sprintf(Conf,"$PASHS,NME,POS,A,ON,1\r\n");		//nastaveni posilani dat
			g_Init->sendArray(Conf,strlen(Conf));
			i++;
			if(i>5) {
				return EXIT_FAILURE;
			}
		}while(GetSentence(g_Init,"$PASHR") != "3D");
		cout<<Conf<<endl;

		i = 0;	
		do {
			sprintf(Conf,"$PASHS,KFP,ON\r\n");					//kalmanuv filtr
			g_Init->sendArray(Conf,strlen(Conf));
			i++;
			if(i>5) {
				return EXIT_FAILURE;
			}
		}while(GetSentence(g_Init,"$PASHR") != "3D");
		cout<<Conf<<endl;

		i = 0;	
		do {
			sprintf(Conf,"$PASHS,SAV,Y\r\n");					//ulozeni
			g_Init->sendArray(Conf,strlen(Conf));
			i++;
			if(i>5) {
				return EXIT_FAILURE;
			}
		}while(GetSentence(g_Init,"$PASHR") != "3D");
		cout<<"Ulozeni"<<endl;

		g_Init->disconnect();
		delete g_Init;
		cout<<"Dokoncena inicializace REMOTE STATION"<<endl;
		return EXIT_SUCCESS;
	}
	else {
		g_Init->disconnect();
		delete g_Init;
		cout<<"Dokoncena inicializace REMOTE STATION s CHYBOU"<<endl;
		return EXIT_FAILURE;
	}
}
void Magellan_NacteniGPS(GPSSTRUCT &gpsData, string data) {
	vector<string> values;
	values.clear();
	GPSSTRUCT GPSm;
	size_t cutAt;
	
	while((cutAt = data.find_first_of(",")) != data.npos) {
		if(cutAt > 0) {
			values.push_back(data.substr(0, cutAt));
		}
		data = data.substr(cutAt+1);
	}	
	if(data.length() > 0) {
		values.push_back(data);
	}

	double tmpFlt;
	ostringstream tmpStr;
	tmpStr.precision(20);

	// D_SIRKA
	tmpFlt = atof(values[D_SIRKA].substr(0,2).c_str()) + (atof(values[D_SIRKA].substr(2).c_str())/60.0);
	tmpStr.str("");
	tmpStr << tmpFlt;
	values[D_SIRKA] = tmpStr.str();

	// D_DELKA
	tmpFlt = atof(values[D_DELKA].substr(0,3).c_str()) + (atof(values[D_DELKA].substr(3).c_str())/60.0);
	tmpStr.str("");
	tmpStr << tmpFlt;
	values[D_DELKA] = tmpStr.str();

	// D_RYCHLOSTUZLY
	tmpFlt = atof(values[D_RYCHLOST].c_str()) * 1.852;
	tmpStr.str("");
	tmpStr << tmpFlt;
	values[D_RYCHLOST] = tmpStr.str();

	// UTM
	double utmX, utmY;
	LLtoUTM(RefEllipsoid, atof(values[D_SIRKA].c_str()), atof(values[D_DELKA].c_str()),
		utmY, utmX, UTMZone);

	gpsData.d_cas = atof(values[D_CAS].c_str());
	gpsData.d_delka = atof(values[D_DELKA].c_str());
	gpsData.d_rychlost = atof(values[D_RYCHLOST].c_str());
	gpsData.d_sirka = atof(values[D_SIRKA].c_str());
	gpsData.d_vyska = atof(values[D_VYSKA].c_str());
	gpsData.d_uhel = atof(values[D_UHEL].c_str());
	gpsData.d_utmx = utmX;
	gpsData.d_utmy = utmY;
	gpsData.i_korekce = atoi(values[I_KOREKCE].c_str());
	gpsData.i_pocetsat = atoi(values[I_POCETSAT].c_str());
	gpsData.s_delkasektor=values[S_DELKASEKTOR];
	gpsData.s_sirkasektor=values[S_SIRKASEKTOR];
}
int Magellan(GPSSTRUCT &gpsData) {
	int i_DGPS;
	string data;
	GPSSTRUCT GPSm;
	
	ts_DGPS = new Tserial;
	do {
		i_DGPS = ts_DGPS->connect(MAGELLANREMOTEGPS,9600,spNONE,0);
	}while (i_DGPS);

	if (!i_DGPS) {
		if (GetSentence(ts_DGPS, "$PASHR,POS", data)!="ERROR") {
			Magellan_NacteniGPS(GPSm, data);
		}
		else {
			ts_DGPS->disconnect();
			delete ts_DGPS;
			return EXIT_FAILURE;
		}
	}
	else { 
		delete ts_DGPS;
		ts_DGPS->disconnect();
		return EXIT_FAILURE;
	}
	gpsData.d_vyska = GPSm.d_vyska;
	gpsData.i_korekce = GPSm.i_korekce;
	gpsData.d_delka = GPSm.d_delka;
	gpsData.d_sirka = GPSm.d_sirka;
	gpsData.d_cas = GPSm.d_cas;
	gpsData.i_pocetsat = GPSm.i_pocetsat;
	gpsData.d_utmx = GPSm.d_utmx;
	gpsData.d_utmy = GPSm.d_utmy;
	gpsData.s_delkasektor = GPSm.s_delkasektor;
	gpsData.s_sirkasektor = GPSm.s_sirkasektor;

	delete ts_DGPS;	
	ts_DGPS->disconnect();
	return EXIT_SUCCESS;
}
void Navi_NacteniGPS(GPSSTRUCT &gpsData, string data) {
	vector<string> values;
	values.clear();
	double tmpFlt;
	ostringstream tmpStr;
	tmpStr.precision(20);
	size_t cutAt;
	while((cutAt = data.find_first_of(",")) != data.npos) {
		if(cutAt > 0) {
			values.push_back(data.substr(0, cutAt));
		}
		data = data.substr(cutAt+1);
	}	
	if(data.length() > 0) {
		values.push_back(data);
	}

	//D_SIRKA
	tmpFlt = atof(values[D_NSIRKA].substr(0,2).c_str()) + (atof(values[D_NSIRKA].substr(2).c_str())/60.0);
	tmpStr.str("");
	tmpStr << tmpFlt;
	values[D_NSIRKA] = tmpStr.str();
	// D_DELKA
	tmpFlt = atof(values[D_NDELKA].substr(0,3).c_str()) + (atof(values[D_NDELKA].substr(3).c_str())/60.0);
	tmpStr.str("");
	tmpStr << tmpFlt;
	values[D_NDELKA] = tmpStr.str();
	// UTM
	double utmX, utmY;
	LLtoUTM(RefEllipsoid, atof(values[D_NSIRKA].c_str()), atof(values[D_NDELKA].c_str()),
		utmY, utmX, UTMZone);

	gpsData.d_cas = atof(values[D_NCAS].c_str());
	gpsData.d_delka = atof(values[D_NDELKA].c_str());
	gpsData.i_kvalita = atoi(values[I_NKVALITA].c_str());
	gpsData.d_sirka = atof(values[D_NSIRKA].c_str());
	gpsData.d_utmx = utmX;
	gpsData.d_utmy = utmY;
	gpsData.i_pocetsat = atoi(values[I_NPOCETSAT].c_str());
	gpsData.s_delkasektor=values[S_NDELKASEKTOR];
	gpsData.s_sirkasektor=values[S_NSIRKASEKTOR];
}
int Navi(GPSSTRUCT &gpsData, string s_ReferGps) {
	Tserial *ts_DGPS;
	int i_DGPS;
	double d_KorX, d_KorY, d_RefX, d_RefY;
	string data;
	GPSSTRUCT Refer;
	GPSSTRUCT Mobil;

	ts_DGPS = new Tserial;
	i_DGPS = ts_DGPS->connect(NAVIREFERGPS,4800,spNONE,0);
	if (!i_DGPS) {
		if (GetSentence(ts_DGPS, "$GPGGA", data)!="ERROR") {
			Navi_NacteniGPS(Refer, data);
		}
	}
	else 
		Refer.d_cas = 9999;
	ts_DGPS->disconnect();

	i_DGPS = ts_DGPS->connect(NAVIREMOTEGPS,4800,spNONE,0);
	if (!i_DGPS) {
		if (GetSentence(ts_DGPS, "$GPGGA", data)!="ERROR") {
			Navi_NacteniGPS(Mobil, data);
		}
	}
	else 
		return EXIT_FAILURE;
	ts_DGPS->disconnect();	

	PrevedStrToLL(s_ReferGps, d_RefY, d_RefX); 

	if (abs(Refer.d_cas - Mobil.d_cas)<20) {
		d_KorX = d_RefX - Refer.d_delka;
		d_KorY = d_RefY - Refer.d_sirka;
		gpsData.d_delka = d_KorX + Mobil.d_delka;
		gpsData.d_sirka = d_KorY + Mobil.d_sirka;

		gpsData.i_korekce = 1;
		gpsData.d_cas = Mobil.d_cas;
		gpsData.i_pocetsat = Mobil.i_pocetsat;
		gpsData.d_utmx = Mobil.d_utmx;
		gpsData.d_utmy = Mobil.d_utmy;
		gpsData.s_delkasektor = Mobil.s_delkasektor;
		gpsData.s_sirkasektor = Mobil.s_sirkasektor;
	}
	else {
		gpsData.i_korekce = 0; 
		gpsData.d_delka = Mobil.d_delka;
		gpsData.d_sirka = Mobil.d_sirka;
		gpsData.d_cas = Mobil.d_cas;
		gpsData.i_pocetsat = Mobil.i_pocetsat;
		gpsData.d_utmx = Mobil.d_utmx;
		gpsData.d_utmy = Mobil.d_utmy;
		gpsData.s_delkasektor = Mobil.s_delkasektor;
		gpsData.s_sirkasektor = Mobil.s_sirkasektor;
	}
	delete ts_DGPS;	
	return EXIT_SUCCESS;
}
