#include "RoboKop.h"

void Drawing(WPARAM wParam, LPARAM lParam)		//funkce pro vykreslovani okna na obrazovku
{
	HDC hdc;
	TCHAR chText[50];
	TCHAR chText1[50];
	TCHAR chText2[50];
	TCHAR chText3[50];
	TCHAR chText4[50];
	TCHAR chText5[50];
	TCHAR chText6[50];
	TCHAR chText7[50];
	TCHAR chText8[50];
	TCHAR chText9[50];
	TCHAR chText10[50];
	TCHAR chText11[50];
	TCHAR chText12[50];
	TCHAR chTextAut[16];

	
	if(g_bCamEnabled)
		sprintf_s(chText1,"Kamera:  %d              ",g_iCam);
	else
		sprintf_s(chText1,"Kamera:  Neaktivní");

	sprintf_s(chText5,"Vzdálenost A:    %d     ", g_iuzA);
	sprintf_s(chText6,"Vzdálenost B:    %d     ", g_iuzB);
	sprintf_s(chText7,"Vzdálenost C:    %d     ", g_iuzC);
	sprintf_s(chText10,"Úhel natočení:    %d     ", g_iAngleAct);
	if(g_bGPSEnabled)
		sprintf_s(chText11,"Úhel od GPS:    %d          ", g_iAngleReq);
	else
		sprintf_s(chText11,"Úhel od GPS:    Neaktivní");
	sprintf_s(chText,"Napětí baterií:  %.2f  V     ", g_fVoltage);
	//sprintf_s(chText,"Proud:  %lf  mA     ", g_fVoltage);
	
	sprintf_s(chText8,"Rychlost A:  %c  %d     ",  g_chSignDriveA, g_iSpeedA);
	sprintf_s(chText9,"Rychlost B:  %c  %d     ",  g_chSignDriveB, g_iSpeedB);
  
  
	sprintf_s(chTextAut,"Režim provozu:");
  
	hdc = GetDC(g_hwndMain);
  
	//zobrazeni obrazku
	BITMAP bitmap;
	RECT rect;
	GetClientRect(g_hwndMain, &rect);
	HDC hdcBitmap = CreateCompatibleDC(hdc);
	HBITMAP hBitmap = LoadBitmap(g_hInstance, MAKEINTRESOURCE(IDB_BITMAP1));
	GetObject(hBitmap, sizeof(bitmap), &bitmap);
	SelectObject(hdcBitmap, hBitmap);
	int xPos = (rect.right - bitmap.bmWidth)-10;
	int yPos = 10;
	BitBlt(hdc,xPos, yPos, bitmap.bmWidth, bitmap.bmHeight, hdcBitmap, 0,0, SRCCOPY);
	DeleteObject(hBitmap);
	DeleteDC(hdcBitmap);

	TextOut(hdc, 310, 210, chTextAut, lstrlen(chTextAut));
#ifdef TEST	
	//TextOut(hdc, 200, 190, g_chBufferDriveIn, lstrlen(g_chBufferDriveIn));
	TextOut(hdc, 200, 190, g_chtest, lstrlen(g_chBufferDriveIn));
#endif
	TextOut(hdc, 10, 10, "Stav:", lstrlen("Stav:"));
  
	TextOut(hdc, 10, 115, "Ultrazvuky:", lstrlen("Ultrazvuky:"));
	TextOut(hdc, 10, 210, chText1, lstrlen(chText1));//kamera
	TextOut(hdc, 10, 235, "Pohony:", lstrlen("Pohony:"));
  
	if(!g_iUltrasonicErr){
		sprintf_s(chText2,"Ultrazvuky  - COM1:  Připojeno.");
		SetTextColor(hdc, 0x00FF0000); // modrý text
	}
	else{
		sprintf_s(chText2,"Ultrazvuky  - COM1:  Odpojeno.");
		SetTextColor(hdc, 0x000000FF); //cervena
	}
		TextOut(hdc, 10, 30, chText2, lstrlen(chText2));
	
	if(!g_iDriveErr){
		sprintf_s(chText3,"Pohony      - COM4:  Připojeno.");
		SetTextColor(hdc, 0x00FF0000); // modrý text
	}
	else{
		sprintf_s(chText3,"Pohony      - COM4:  Odpojeno.");
		SetTextColor(hdc, 0x000000FF); //cervena
	}
	TextOut(hdc, 10, 50, chText3, lstrlen(chText3));
	
	if(!g_iCompassErr){
		sprintf_s(chText4,"Kompas    - COM2:  Připojeno.");
		SetTextColor(hdc, 0x00FF0000); // modrý text
	}
	else{
		sprintf_s(chText4,"Kompas    - COM2:  Odpojeno.");
		SetTextColor(hdc, 0x000000FF); //cervena
	}
	
	TextOut(hdc, 10, 70, chText4, lstrlen(chText4));

	if((!g_iGPS1Err) && (!g_iGPS2Err)){
		sprintf_s(chText12,"GPS  - COM3, COM5:  Připojeno.");
		SetTextColor(hdc, 0x00FF0000); // modrý text
	}
	else{
		sprintf_s(chText12,"GPS  - COM3, COM5:  Odpojeno.");
		SetTextColor(hdc, 0x000000FF); //cervena
	}
	
	TextOut(hdc, 10, 90, chText12, lstrlen(chText4));
	
	SetTextColor(hdc, 0x00FF0000); // modrý text
	
 	TextOut(hdc, 10, 260, chText8, lstrlen(chText8));//rychlost A
	TextOut(hdc, 10, 280, chText9, lstrlen(chText9));//rychlost B
	TextOut(hdc, 10, 300, chText10, lstrlen(chText10));//kompas
//#ifdef TEST	
	TextOut(hdc, 10, 320, chText11, lstrlen(chText11));//uhel pozadovany GPS
//#endif
	TextOut(hdc, 40, 140, chText5, lstrlen(chText5));
	TextOut(hdc, 40, 160, chText6, lstrlen(chText6));//vzdalenost B
	TextOut(hdc, 40, 180, chText7, lstrlen(chText7));
	

	SetTextColor(hdc, 0x000000FF); //cervena
	TextOut(hdc, 320, 230, g_chMode, lstrlen(g_chMode));		//rezim
	
	if(g_fVoltage > LOW_VOLTAGE)
		SetTextColor(hdc, 0x0000FF00); //zelena
	else if(g_fVoltage > LOWLOW_VOLTAGE)
		SetTextColor(hdc, 0x000066EE); //oranzova
	TextOut(hdc, 10, 355, chText, lstrlen(chText));//napeti
    
	ReleaseDC(g_hwndMain, hdc);
}

void OnWM_PAINT()
{
  Drawing(0,0);
}

static DWORD WINAPI threadSupersonic(LPVOID pthreadID)	// definovani vlakna
{
	char buffer2[10];
	char buffer[2];
	FILE *file;
	int error;

	g_tsUltrasonic = new Tserial();
	
	g_iUltrasonicErr = g_tsUltrasonic->connect("COM1", 9600, spNONE, 0);
			
	if(!g_iUltrasonicErr){

		while(!g_bStop){
        				
			if(!(error = fopen_s(&file, "zahni.txt", "r"))){
				fscanf_s(file, "%d", &g_iCam);
  				fclose (file);
			}
			g_tsUltrasonic->getArray(buffer,2);
			
			if((buffer[0] == '1') && (buffer[1] == ' ')){ //zkusit zmenit na strcmp()
				g_tsUltrasonic->getArray(buffer2,4);
				g_iuzA = atoi(buffer2);
			}

			g_tsUltrasonic->getArray(buffer,2);
			if((buffer[0] == '2') && (buffer[1] == ' ')){
				g_tsUltrasonic->getArray(buffer2,4);
				g_iuzB = atoi(buffer2);
			}

			g_tsUltrasonic->getArray(buffer,2);
			if((buffer[0] == '3') && (buffer[1] == ' ')){
				g_tsUltrasonic->getArray(buffer2,4);
				g_iuzC = atoi(buffer2);
			}
			Sleep(DELAY_SUPERSONIC);
		}

		
		
	}
	else{
			MessageBox(g_hwndMain, TEXT("Nepodařilo se otevřít sériový port COM1 pro subsystém ultrazvukových snímačů!"), TEXT("Chyba!"),
                MB_ICONWARNING);			
		}
	g_iUltrasonicErr = 1;
	g_tsUltrasonic->disconnect();
	delete g_tsUltrasonic;
	return 0;
	
}

static DWORD WINAPI threadCompass(LPVOID pthreadID)	//vlakno pro pohony
{
	char buffer[2] = " ";
	char buffer2[4] = "000";
	g_tsCompass = new Tserial();		
	
	g_iCompassErr = g_tsCompass->connect("COM2", 9600, spNONE, 0);

	if(!g_iCompassErr){

		while(!g_bStop){
		
			g_tsCompass->getArray(buffer,1);
			if(buffer[0] == 'U'){
				g_tsCompass->getArray(buffer2,3);
				g_iAngleAct = atoi(buffer2);
			}
			g_tsCompass->getArray(buffer,1);
			if(buffer[0] == 'X'){
				g_tsCompass->getArray(buffer2,3);
				g_iX = atoi(buffer2);
			}
			g_tsCompass->getArray(buffer,1);
			if(buffer[0] == 'Y'){
				g_tsCompass->getArray(buffer2,3);
				g_iY = atoi(buffer2);
			}
			g_tsCompass->getArray(buffer,1);
			if(buffer[0] == 'Z'){
				g_tsCompass->getArray(buffer2,3);
				g_iZ = atoi(buffer2);
			}

			Sleep(DELAY_COMPASS);
		}
		
	}
	else{
			MessageBox(g_hwndMain, TEXT("Nepodařilo se otevřít sériový port COM2 pro subsystém kompasu!"), TEXT("Chyba!"),
				    MB_ICONWARNING);
		}
	g_iCompassErr = 1;
	g_tsCompass->disconnect();
	delete g_tsCompass;			
	return 0;

}

static DWORD WINAPI threadDrive(LPVOID pthreadID)	//vlakno pro pohony
{
	int iSpeedLastA = 0;	
	int iSpeedLastB = 0;	
	int iSpeedGPSA = 0;
	int iSpeedGPSB = 0;
	char chSignLastA = '+';
	char chSignLastB = '+';

	g_tsDrive = new Tserial();	
	
	g_iDriveErr = g_tsDrive->connect("COM4", 9600, spNONE, 10);

	if(!g_iDriveErr){

		while(!g_bStop){
				
			//osetreni maximalni rychlosti
			if(g_iSpeedA > MAX_SPEED)
				g_iSpeedA = MAX_SPEED;
			if(g_iSpeedB > MAX_SPEED)
				g_iSpeedB = MAX_SPEED;

			//vyslani dat pohonum rozsah 0-255
			if(!((iSpeedLastA == g_iSpeedA) && (iSpeedLastB == g_iSpeedB) && (chSignLastA == g_chSignDriveA) && (chSignLastB == g_chSignDriveB))){
				//nacteni rychlosti do retezce pro vysilani
				g_bSend = true;
			}
			if(g_bSend == true){
				sprintf_s(g_chBufferDriveSend, "A%c%.3dB%c%.3d",g_chSignDriveA, g_iSpeedA, g_chSignDriveB, g_iSpeedB );
				g_tsDrive->sendArray(g_chBufferDriveSend,10);
				iSpeedLastA = g_iSpeedA;	
				iSpeedLastB = g_iSpeedB;	
				chSignLastA = g_chSignDriveA;
				chSignLastB = g_chSignDriveB;
				g_bSend = false;
			}
			
			if(!g_bManual){	//MANUALNI REZIM
				
				//*GPS
								
			  if(g_bGPSEnabled){
				if((g_iAngleAct - g_iAngleReq) > GPS_DEAD_ANGLE){
					//doprava
					iSpeedGPSA = AUT_SPEED_1;
					iSpeedGPSB = AUT_SPEED_1 / 2;
				}
				else if((g_iAngleAct - g_iAngleReq) < -GPS_DEAD_ANGLE){
					//doleva
					iSpeedGPSA = AUT_SPEED_1 / 2;
					iSpeedGPSB = AUT_SPEED_1;
				}
				else{ 
					//rovne
					iSpeedGPSA = AUT_SPEED_1;
					iSpeedGPSB = AUT_SPEED_1;				
				}
				//*/
			  }
			  else{
				iSpeedGPSA = AUT_SPEED_1;
				iSpeedGPSB = AUT_SPEED_1; 
			  }
				
			  if(g_bCamEnabled){

				switch(g_iCam){
					
					case 0: //rovne
						g_iSpeedCamA = iSpeedGPSA;
						g_iSpeedCamB = iSpeedGPSB;
						g_chSignCamA = '+';
						g_chSignCamB = '+';
					break;

					case 1: //
						g_iSpeedCamA = (AUT_SPEED_1 - AUT_SPEED_1 / 4);
						g_iSpeedCamB = AUT_SPEED_1;
						g_chSignCamA = '+';
						g_chSignCamB = '+';
					break;

					case 2: //
						g_iSpeedCamA = AUT_SPEED_1 / 4;
						g_iSpeedCamB = AUT_SPEED_1;
						g_chSignCamA = '+';
						g_chSignCamB = '+';
					break;

					case 3: //
						g_iSpeedCamA = AUT_SPEED_1 / 8;
						g_iSpeedCamB = AUT_SPEED_1;
						g_chSignCamA = '+';
						g_chSignCamB = '+';
					break;

					case -1: //
						g_iSpeedCamA = AUT_SPEED_1;
						g_iSpeedCamB = (AUT_SPEED_1 - AUT_SPEED_1 / 4);
						g_chSignCamA = '+';
						g_chSignCamB = '+';
					break;

					case -2: //
						g_iSpeedCamA = AUT_SPEED_1;
						g_iSpeedCamB = AUT_SPEED_1 / 4;
						g_chSignCamA = '+';
						g_chSignCamB = '+';
					break;

					case -3: //
						g_iSpeedCamA = AUT_SPEED_1;
						g_iSpeedCamB = AUT_SPEED_1 / 8;
						g_chSignCamA = '+';
						g_chSignCamB = '+';
					break;
				
				}//konec switche od kamery
			  }
			  else{
				g_iSpeedCamA = iSpeedGPSA;
				g_iSpeedCamB = iSpeedGPSB;
				g_chSignCamA = '+';
				g_chSignCamB = '+';
			  }

				//nejmensi mozna vydalenost pro otaceni
				if((g_iuzA < SLEPA_VZD) || (g_iuzB < SLEPA_VZD)  || (g_iuzC < SLEPA_VZD)){
					g_iSpeedA = AUT_SPEED_2;
					g_iSpeedB = AUT_SPEED_2;
					g_chSignDriveA = '-';
					g_chSignDriveB = '-';
					g_iStatUs = 2;
				}//konec osetreni nejmensi vzdalenosti pro otaceni

			 switch(g_iStatUs){
			 case 0: 
					
				if((g_iuzA < MIN_VZD) || (g_iuzB < MIN_VZD) || (g_iuzC < MIN_VZD)){		//otaceni na miste pri prekazce
					if(g_iuzA < g_iuzC){
						g_iSpeedA = AUT_SPEED_2;
						g_iSpeedB = AUT_SPEED_2;
						g_chSignDriveA = '-';
						g_chSignDriveB = '+';
					}
				
					else{
						g_iSpeedA = AUT_SPEED_2;
						g_iSpeedB = AUT_SPEED_2;
						g_chSignDriveA = '+';
						g_chSignDriveB = '-';
				 	}
					g_iStatUs = 1;
				}
				else{
					if((g_iuzA > VYHYBACI_VZD) && (g_iuzB > VYHYBACI_VZD) && (g_iuzC > VYHYBACI_VZD)){	//jizda rovne pokud je volno
						
						if((g_iuzA < (MIN_VZD * 2)) || (g_iuzB < (MIN_VZD * 2)) || (g_iuzC < (MIN_VZD * 2))){	//zmena rychlosti podle vzdalenosti B	
							g_iSpeedA = g_iSpeedCamA / 2;
							g_iSpeedB = g_iSpeedCamB / 2;
							g_chSignDriveA = g_chSignCamA;
							g_chSignDriveB = g_chSignCamB;
						}
						else if((g_iuzA > (MIN_VZD * 4)) && (g_iuzB > (MIN_VZD * 4)) && (g_iuzC > (MIN_VZD * 4))){
							g_iSpeedA = g_iSpeedCamA * 2;
							g_iSpeedB = g_iSpeedCamB * 2;
							g_chSignDriveA = g_chSignCamA;
							g_chSignDriveB = g_chSignCamB;
							if((g_iuzA > (MIN_VZD * 8)) && (g_iuzB > (MIN_VZD * 8)) && (g_iuzC > (MIN_VZD * 8))){
								g_iSpeedA = g_iSpeedCamA * 4;
								g_iSpeedB = g_iSpeedCamB * 4;
								g_chSignDriveA = g_chSignCamA;
								g_chSignDriveB = g_chSignCamB;
							}
						}
						
						else{
							g_iSpeedA = g_iSpeedCamA;
							g_iSpeedB = g_iSpeedCamB;
							g_chSignDriveA = g_chSignCamA;
							g_chSignDriveB = g_chSignCamB;
						}// konec zmeny rychlosti
						
					} //konec jizdy rovne
#ifdef OUTDOOR
					else if((g_iuzA > PROV_VZD) && (g_iuzC > PROV_VZD)){//objizdeni vzdalenejsich prekazek
						if((g_iuzA < VYHYBACI_VZD) && (g_iuzC > VYHYBACI_VZD)){
							g_iSpeedA = AUT_SPEED_1;
							g_iSpeedB = AUT_SPEED_1 / 2;
							g_chSignDriveA = '+';
							g_chSignDriveB = '+';
						}
						if((g_iuzC < VYHYBACI_VZD) && (g_iuzA > VYHYBACI_VZD)){
							g_iSpeedA = AUT_SPEED_1 / 2;
							g_iSpeedB = AUT_SPEED_1;
							g_chSignDriveA = '+';
							g_chSignDriveB = '+';
						}					
	
					}
#endif
					else{ //vyhybani se prekazkam
						if((g_iuzA < PROV_VZD) && (g_iuzC > PROV_VZD)){
							g_iSpeedA = AUT_SPEED_1;
							g_iSpeedB = AUT_SPEED_2;
							g_chSignDriveA = '+';
							g_chSignDriveB = '-';
						}
						if((g_iuzC < PROV_VZD) && (g_iuzA > PROV_VZD)){
							g_iSpeedA = AUT_SPEED_2;
							g_iSpeedB = AUT_SPEED_1;
							g_chSignDriveA = '-';
							g_chSignDriveB = '+';
						}
					}
				}
			 
				break;

			 case 1:
				
				 if((g_iuzA > PRUJEZDNA_VZD) && (g_iuzB > PRUJEZDNA_VZD) && (g_iuzC > PRUJEZDNA_VZD)){
					g_iStatUs = 0;
				 }

				break;

			 case 2:	//couvani
				 g_chMelody = 'c';
				 ResumeThread(hThreadSound);
				 if((g_iuzA > SLEPA_VZD) && (g_iuzB > SLEPA_VZD) && (g_iuzC > SLEPA_VZD)){
					/*
					 if(g_iuzA < g_iuzC){
						g_iSpeedA = AUT_SPEED_2;
						g_iSpeedB = AUT_SPEED_2;
						g_chSignDriveA = '+';
						g_chSignDriveB = '-';
					}
				
					else{
						g_iSpeedA = AUT_SPEED_2;
						g_iSpeedB = AUT_SPEED_2;
						g_chSignDriveA = '-';
						g_chSignDriveB = '+';
				 	}
					*/
					g_iStatUs = 0;//byl 1
				 }
				 
			 break;

			 }//konec switche
			 			 
			 }

			 Sleep(DELAY_DRIVE_OUT);
		}

	}
	else{
			MessageBox(g_hwndMain, TEXT("Nepodařilo se otevřít sériový port COM4 pro pohonný subsystém!"), TEXT("Chyba!"),
				    MB_ICONWARNING);
		}
	g_iDriveErr = 1;
	g_tsDrive->disconnect();
	delete g_tsDrive;			
	return 0;

}

static DWORD WINAPI threadDrive2(LPVOID pthreadID)	//nacitani dat od pohonu
{
	char buffer[2] = " ";
	char buff2[4] = "   ";
	int inizke_napeti = 0;
	bool bmanualPrev = true;

	while(!g_bStop){

	  if(!g_iDriveErr){
		// prijem dat od pohonu
		g_tsDrive->getArray(buffer, 1);

		if(buffer[0] == 'O'){	//ok
			g_tsDrive->getArray(buffer, 1);
			sprintf_s(g_chBufferDriveIn, "OK   ");
			g_bSend = true;
			sprintf_s(g_chtest, "OK   ");

		}
		
		if(buffer[0] == 'N'){	//napeti
			g_tsDrive->getArray(g_chBufferDriveIn, 5);
			if(((atof(g_chBufferDriveIn)) / 1000) > 10)	
				g_fVoltage = (atof(g_chBufferDriveIn)) / 1000;
			
		}

		if(buffer[0] == 'C'){			//osereni nadproudu

			g_tsDrive->getArray(g_chBufferDriveIn, 3);
			sprintf_s(g_chtest, "CERR ");
			
			if(!g_bManual){
				g_bManual = true;
				bmanualPrev = false;
			}
			
			if((g_chSignDriveA == '+') && (g_chSignDriveB == '-')){
				g_chSignDriveA = '-';
				g_chSignDriveB = '+';
			}
			else if((g_chSignDriveA == '-') && (g_chSignDriveB == '+')){
				g_chSignDriveA = '+';
				g_chSignDriveB = '-';
			}
			else if((g_chSignDriveA == '+') && (g_chSignDriveB == '+')){
				g_chSignDriveA = '-';
				g_chSignDriveB = '-';
			}
			else{
				g_chSignDriveA = '+';
				g_chSignDriveB = '+';
			}
			g_iSpeedA = 50;
			g_iSpeedB = 50;
			g_bSend = true;
			Sleep(1000);				
			g_iSpeedA = 0;
			g_iSpeedB = 0;

			if(!bmanualPrev)		//pokud byl predchozi rezim manual zastavime, kdyz ne tak opet pustime aut	
				g_bManual = false;

		}
	
		if(g_fVoltage < LOWLOW_VOLTAGE){
			Beep(500, 1000);     
			inizke_napeti ++;
		}
		else{
			inizke_napeti = 0;
		}
		
		if(inizke_napeti > LOWVOLT_LIMIT){
			if(g_bLowVoltEnabled){
				Beep(HIGH, LONG);
				Beep(LOW, LONG);
				system("shutdown -s -f -t 5");
				g_iSpeedA = 0;
				g_iSpeedB = 0;
				g_bManual = true;
				Sleep(500);
				g_bEnd = true;
				g_bStop = true;
			}
		}
		
		
	  }	
	  Sleep(DELAY_DRIVE_IN);
	}
	
	return 0;
}

static DWORD WINAPI ThreadSound(LPVOID pthreadID)	// definovani vlakna
{
	while(!g_bEnd){
		switch(g_chMelody){
			case 'r':
				Beep(1500, TECKA);
				Sleep(MALA);
				Beep(1500, CARKA);
				Sleep(MALA);
				Beep(1500, TECKA);
				Sleep(MEZERA);
				///
				Beep(1500, CARKA);
				Sleep(MALA);
				Beep(1500, CARKA);
				Sleep(MALA);
				Beep(1500, CARKA);
				Sleep(MEZERA);
				///
				Beep(1500, CARKA);
				Sleep(MALA);
				Beep(1500, TECKA);
				Sleep(MALA);
				Beep(1500, TECKA);
				Sleep(MALA);
				Beep(1500, TECKA);
				Sleep(MEZERA);
				///
				Beep(1500, CARKA);
				Sleep(MALA);
				Beep(1500, CARKA);
				Sleep(MALA);
				Beep(1500, CARKA);
				Sleep(MEZERA);
				///
				Beep(1500, CARKA);
				Sleep(MALA);
				Beep(1500, TECKA);
				Sleep(MALA);
				Beep(1500, CARKA);
				Sleep(MEZERA);
				///
				Beep(1500, CARKA);
				Sleep(MALA);
				Beep(1500, CARKA);
				Sleep(MALA);
				Beep(1500, CARKA);
				Sleep(MEZERA);
				///
				Beep(1500, TECKA);
				Sleep(MALA);
				Beep(1500, CARKA);
				Sleep(MALA);
				Beep(1500, CARKA);
				Sleep(MALA);
				Beep(1500, TECKA);
				Sleep(MEZERA);
				///
				break;

			case 'o':
				Beep(262, 250);
				Beep(332, 250);
				Beep(393, 250);
	
				Sleep(250);
				Beep(262, 250);
				Beep(332, 250);
				Beep(393, 250);
				Sleep(250);
				Beep(332, 125);
				Beep(332, 125);
				Beep(295, 125);
				Beep(332, 125);
				Beep(349, 250);
				Beep(295, 250);

				Beep(332, 125);
				Beep(332, 125);
				Beep(295, 125);
				Beep(332, 125);
				Beep(349, 250);
				Beep(295, 250);

				Beep(332, 250);
				Beep(295, 250);
				Beep(262, 250);
			break;
	
		case 't':
	
			Sleep(125);
			Beep(442, 125);
			Sleep(3*125);
			Beep(442, 125);
			Sleep(125);
			Beep(524, 125);
			Sleep(2*125);
			Beep(442, 125);
			Sleep(4*125);

			Sleep(125);
			Beep(442, 125);
			Sleep(3*125);
			Beep(442, 125);
			Sleep(125);
			Beep(594, 125);
			Sleep(2*125);
			Beep(442, 125);
			Sleep(4*125);

			break;
		
		case 'c':
			Beep(1200,500);
			Sleep(300);
			break;
		}

	SuspendThread(hThreadSound);
	}

	return 0;
}

static DWORD WINAPI threadGPS(LPVOID pthreadID)	// vlakno GPS
{

	//Zdenek Kuna - navigacni algoritmus
	g_tsGPS1 = new Tserial();	
	g_tsGPS2 = new Tserial();
	
	g_iGPS1Err = g_tsGPS1->connect("COM5", 4800, spNONE, 0);	//referencni
	g_iGPS2Err = g_tsGPS2->connect("COM3", 4800, spNONE, 0);	//pohybliva

	char buffer_read[2];// buffer pro cteni dat
	char buffer_read2[2];
	char idn_vet[6] = "     ";
	char idn_vet2[6] = "     ";
	

	double pol_rob_utm_y;
	double pol_rob_utm_x;

	int pom=1;
	double v=100000;
	double newpoint_x=0;
	double newpoint_y=0;
	double uhel=0;
	double pol_rob_x0=0;
	double pol_rob_y0=0;
	stack a;
	a.push(49.21944,16.59718);//souradnice bodu A (north,east)49°13'11.477"N, 16°35'40.255"E
	a.push(49.21911,16.593634);//souradnice bodu B
	a.push(49.21865,16.595287);//souradnice bodu C
	//a.push(49.21985,16.5945);//souradnice bodu D

	string gps_ref("");
	string ref_poloha("");

	string veta_a("");
	string veta_b("");
	
	string northwide_s("");
	string northwide_gps2_s("");
	
	string eastlength_s("");
	string eastlength_gps2_s("");
	
	string cas_s("");
	string cas_s_gps2("");
	
	string northwide_stupne("");
	
	string northwide_minuty("");
	
	string northwide_vteriny_s("");
	
	string eastlength_stupne("");
	
	string eastlength_minuty("");
	string eastlength_vteriny_s("");

	string ref_poloha_north("4913.155");//format z gps/známé
	string ref_poloha_east("1635.715");//format z gps/známé

	
	if((!g_iGPS1Err) && (!g_iGPS2Err)){

		while(!g_bStop){

			float cor_n=0;
			float cor_e=0;
			double UTMNorthing;
			double UTMEasting;
			g_tsGPS1->getArray(buffer_read,1);
			g_tsGPS2->getArray(buffer_read2,1);
			
			//sprintf(precteno_gps, buffer_read);
			if(buffer_read[0] == '$')
			{
				g_tsGPS1->getArray(idn_vet, 5);
				
				if(!(strcmp(idn_vet, "GPRMC")))
				{
					if (g_tsGPS1->getArray(precteno_gps, 62)){
						veta_a = precteno_gps;
								
						//cykluj dokud neni konec souboru,cti cely radek ze souboru data_a a tento radek uloz do veta_a
						cas_s=veta_a.substr(1,10);//nalezeni ve vete GPRMC-cas
			
						northwide_s=veta_a.substr(14,9);//nalezeni ve vete GPRMC-severni sirku
	
						int cas=atoi(cas_s.c_str());
						northwide_stupne=northwide_s.substr(0,2);//nalezeni v retezci severni sirka-stupne ve stupnich
						northwide_minuty=northwide_s.substr(2,2);//nalezeni v retezci severni sirka-minuty v minutach
						northwide_vteriny_s=northwide_s.substr(4,5);//nalezeni v retezci severni sirka-vteřiny
						float northwide_vteriny=(float)(atof(northwide_vteriny_s.c_str()))*60;//severni sirka-vteriny ve vterinach
			
						eastlength_s=veta_a.substr(27,9);//nalezeni ve vete GPRMC-zapadni delku
						eastlength_stupne=eastlength_s.substr(0,2);
						eastlength_minuty=eastlength_s.substr(2,2);
						eastlength_vteriny_s=eastlength_s.substr(4,5);
						float eastlength_vteriny=(float)(atof(eastlength_vteriny_s.c_str()))*60;
				
						float gps1_n=degree_to_sec(northwide_s);//prevod severni sirky gps1 na vteriny
						
						float gps1_e=degree_to_sec(eastlength_s);//prevod zapadni delky gps1 na vteriny
						
						float ref_n=degree_to_sec(ref_poloha_north);//prevod referencni polohy north-vteriny
						float ref_e=degree_to_sec(ref_poloha_east);//prevod referencni polohy east-vteriny
						
						cor_n=correction(northwide_s,ref_poloha_north);//vypocet korekce v north
						cor_e=correction(eastlength_s,ref_poloha_east);//vypocet korekce v east
						
						float kontrola_gps1_n=sec_to_degree(gps1_n);
	
						float kontrola_gps1_e=sec_to_degree(gps1_e);

					}
				}
			}
		

			if(buffer_read2[0] == '$'){
				g_tsGPS2->getArray(idn_vet2, 5);
					
			
				if(!(strcmp(idn_vet2, "GPRMC")))
				{
					if (g_tsGPS2->getArray(precteno_gps2, 62)){

						veta_b = precteno_gps2;

						cas_s_gps2=veta_b.substr(1,10);
						int cas_gps2=atoi(cas_s_gps2.c_str());
						northwide_gps2_s=veta_b.substr(14,9);
						eastlength_gps2_s=veta_b.substr(27,9);
			
						float gps2_n=degree_to_sec(northwide_gps2_s);//prevod severni sirky gps2 na vteriny
						float gps2_e=degree_to_sec(eastlength_gps2_s);//prevod zapadni delky gps2 na vteriny
			
						float corig_north=gps2_correction(northwide_gps2_s,cor_n);
						float corig_north_stupne=sec_to_degree(corig_north);
						
						float corig_east=gps2_correction(eastlength_gps2_s,cor_e);
						float corig_east_stupne=sec_to_degree(corig_east);
					
						char UTMZone[4];
						int RefEllipsoid = 23;//WGS-84. See list with file "LatLong- UTM conversion.cpp" for id numbers
	
						double pocatek_y=49.219250;//pocatecni bod...referencni stacionarni souradnice y=pocatek souradneho systemu
						double pocatek_x=16.59525;//pocatecni bod...referencni stacionarni souradnice x=pocatek souradneho systemu

						LLtoUTM(RefEllipsoid, corig_north_stupne, corig_east_stupne, UTMNorthing, UTMEasting, UTMZone);
						pol_rob_utm_y=UTMNorthing;//poloha robota v UTM systemu,northing
						pol_rob_utm_x=UTMEasting;//poloha robota v UTM systemu,easting
						
						LLtoUTM(RefEllipsoid, pocatek_y, pocatek_x, UTMNorthing, UTMEasting, UTMZone);//prevod UTM-počátek souradneho systemu
						pocatek_y=UTMNorthing;
						pocatek_x=UTMEasting;

						pol_rob_x0=pol_rob_utm_x-pocatek_x;//poloha robota v souradnem systemu
						pol_rob_y0=pol_rob_utm_y-pocatek_y;//poloha robota v souradnem systemu
							
						if(pom==1)//pocatecni bod
						{
							newpoint_x=a.popEast();
							newpoint_y=a.popNorth();
							pom=0;
			
						}
	
						double boday,bodax,boday0,bodax0;

						LLtoUTM(RefEllipsoid,newpoint_y,newpoint_x, UTMNorthing, UTMEasting, UTMZone);
						boday=UTMNorthing;
						bodax=UTMEasting;
	
						boday0=boday-pocatek_y;//souradnice ciloveho bodu v souradnem systemu
						bodax0=bodax-pocatek_x;//souradnice ciloveho bodu v souradnem systemu
						//////cout<<"cilovy bod_A y"<<boday0<<"cilovy bod_A x"<<bodax0<<endl;
						v=sqrt((bodax-pol_rob_x0)*(bodax-pol_rob_x0)+(boday-pol_rob_y0)*(boday-pol_rob_y0));//vzdalenost k cilovemu bodu od aktualni polohy robotu
						if(v<5)
						{
							newpoint_x=a.popEast();
							newpoint_y=a.popNorth();
							g_iAngleReq =(int) ((atan2( (double) (boday0-pol_rob_y0), (double) (bodax0-pol_rob_x0) ))*(180/PI));
						}
						else
							g_iAngleReq =(int) ((atan2( (double) (boday0-pol_rob_y0), (double) (bodax0-pol_rob_x0) ))*(180/PI));
						
						if(g_iAngleReq < 0)
							g_iAngleReq = 360 + g_iAngleReq;

						//konec casti Zdenka Kuny
					}
				}
				
			}
		}
	}
	else{
			MessageBox(g_hwndMain, TEXT("Nepodařilo se otevřít sériové porty COM3 a COM5 pro GPS přijímače!"), TEXT("Chyba!"),
                MB_ICONWARNING);			
	}
	g_iGPS1Err = 1;
	g_iGPS2Err = 1;
	g_tsGPS1->disconnect();
	g_tsGPS2->disconnect();
	delete g_tsGPS1;
	delete g_tsGPS2;

	return 0;
}

LRESULT CALLBACK WindowProcMain(HWND hwnd, UINT message, WPARAM wParam, LPARAM lParam) 
{ 
	TCHAR chText[200]; 
	SYSTEMTIME sysTime;

	switch ( message )
	{
		case WM_CLOSE:
			g_iSpeedA = 0;
			g_iSpeedB = 0;
			g_bManual = true;
			sprintf_s(g_chMode, "Manuální     ");
			Sleep(500);
			if ( MessageBox(hwnd, "Opravdu ukončit?", _AppName,
				MB_YESNO | MB_ICONQUESTION) != IDYES )
				return 0;				
			break;

		case WM_CREATE:
			Beep(LOW, LONG);	// jednotkz (Hz, ms)
			Sleep(SHORT);
			Beep(HIGH, LONG);	// jednotkz (Hz, ms)
		break;

		case WM_COMMAND:
			if ( lParam == (LPARAM)g_hwndAut )		//tlacitko pro automaticky rezim
			{
				g_bManual = false;
				sprintf_s(g_chMode, "Automatický");
				SetFocus(g_hwndMain);
			}

			if ( lParam == (LPARAM)g_hwndMan )	//tlacitko pro manualni rezim
			{
				g_bManual = true;
				sprintf_s(g_chMode, "Manuální     ");
				SetFocus(g_hwndMain);
				
				g_iSpeedA = 0;
				g_iSpeedB = 0;

			}

		switch ( LOWORD(wParam) )
		{
			case ID_ABOUT:
				MessageBox(hwnd, "Robot ROBO-KOP\n-řídicí software\nBakalářská práce\n\nMartin Kopecký, 2008",
				"O programu ROBO-KOP", MB_ICONINFORMATION);
	
			break;

			case ID_PRIPOJIT:

				g_bStop = false;
				if(g_iUltrasonicErr)
					_beginthread((void (__cdecl *)(void *))threadSupersonic, NULL, NULL);
				
				if(g_iDriveErr)	
					_beginthread((void (__cdecl *)(void *))threadDrive, NULL, NULL);
			
				if(g_iCompassErr)
					_beginthread((void (__cdecl *)(void *))threadCompass, NULL, NULL);

				if(g_iGPS1Err && g_iGPS2Err)
					_beginthread((void (__cdecl *)(void *))threadGPS, NULL, NULL);
				
				break;
			
			case ID_ODPOJIT:
				
				g_bStop = true;
				
				break;

			case ID_END:
				SendMessage(hwnd, WM_CLOSE, 0, 0);
				break;
		}
		break;

		case WM_PAINT:
			OnWM_PAINT();
			break;

		case WM_KEYDOWN:
			
			if(g_bManual){

				switch(wParam){
					case VK_DOWN:
						
						if((g_iSpeedA >= STEP) && (g_chSignDriveA == '+')){
							g_iSpeedA = g_iSpeedA - STEP;
							g_iSpeedB = g_iSpeedB - STEP;
							g_chSignDriveA = '+';
							g_chSignDriveB = '+';
						}
						
						else if(g_iSpeedA < (MAX_SPEED - STEP)){
							g_iSpeedA = g_iSpeedA + STEP;
							g_iSpeedB = g_iSpeedB + STEP;
							g_chSignDriveA = '-';
							g_chSignDriveB = '-';
						
						}

						if((g_iSpeedA == 0) && (g_iSpeedB == 0))
							g_iStat = 0;
						else
							g_iStat = 1;

						g_chFlagDir = 'D';

					break;

					case VK_UP:

						if(g_chFlagDir == 'R'){
							g_iSpeedA = g_iSpeedB;
							g_chSignDriveA = '+';
						}

						if(g_chFlagDir == 'L'){
							g_iSpeedB = g_iSpeedA;
							g_chSignDriveB = '+';
						

						}
						
						if((g_iSpeedA >= STEP) && (g_chSignDriveA == '-')){
							g_iSpeedA = g_iSpeedA - STEP;
							g_iSpeedB = g_iSpeedB - STEP;
							g_chSignDriveA = '-';
							g_chSignDriveB = '-';
						}
						
						else if(g_iSpeedA < (MAX_SPEED - STEP)){
							g_iSpeedA = g_iSpeedA + STEP;
							g_iSpeedB = g_iSpeedB + STEP;
							g_chSignDriveA = '+';
							g_chSignDriveB = '+';
						
						}
						
						if((g_iSpeedA == 0) && (g_iSpeedB == 0))
							g_iStat = 0;
						else
							g_iStat = 1;

						g_chFlagDir = 'U';	

					break;

					case VK_RIGHT:

						switch(g_iStat)	{
						
						case 0:

							if((g_iSpeedA >= STEP) && (g_chSignDriveA == '+') && (g_chSignDriveB == '-')){
								g_iSpeedA = g_iSpeedA - STEP;
								g_iSpeedB = g_iSpeedB - STEP;
								g_chSignDriveA = '+';
								g_chSignDriveB = '-';							
							}
						
							else if(g_iSpeedA < (MAX_SPEED - STEP)){
								g_iSpeedA = g_iSpeedA + STEP;
								g_iSpeedB = g_iSpeedB + STEP;
								g_chSignDriveA = '-';
								g_chSignDriveB = '+';
							}

						break;
						
						case 1:
							if(g_chFlagDir == 'L' )	//kdyz pri zataceni zmacknu opacnou sipku
								g_iStat = 2;
							//nebyl else
							else if((g_iSpeedA >= STEP) && (g_chSignDriveA == '+')){		//dopredu
								g_iSpeedA = g_iSpeedA - STEP;
								g_chSignDriveA = '+';
							}
						
							else if(g_iSpeedA < (MAX_SPEED - STEP)){	
								g_iSpeedA = g_iSpeedA + STEP;
								g_chSignDriveA = '-';
								
							}
							if(g_iSpeedA == g_iSpeedB)
								g_iStat = 0;
							break;
						
						case 2:
							if((g_iSpeedA == g_iSpeedB) && (g_chSignDriveA == g_chSignDriveB))
								g_iStat = 1;
	
							else{
								if((g_iSpeedB >= STEP) && (g_chSignDriveB == '-')){
									g_iSpeedB = g_iSpeedB - STEP;
									g_chSignDriveB = '-';
								}
								else if(g_iSpeedB < (MAX_SPEED - STEP)){
									g_iSpeedB = g_iSpeedB + STEP;
									g_chSignDriveB = '+';
								}
							}
											
						break;
						}	

						g_chFlagDir = 'R';

					break;

					case VK_LEFT:

						switch(g_iStat){
						case 0:

							if((g_iSpeedA >= STEP) && (g_chSignDriveA == '-') && (g_chSignDriveB == '+')){
								g_iSpeedA = g_iSpeedA - STEP;
								g_iSpeedB = g_iSpeedB - STEP;
								g_chSignDriveA = '-';
								g_chSignDriveB = '+';
							
							}
						
							else if(g_iSpeedA < (MAX_SPEED - STEP)){
								g_iSpeedA = g_iSpeedA + STEP;
								g_iSpeedB = g_iSpeedB + STEP;
								g_chSignDriveA = '+';
								g_chSignDriveB = '-';
						
							}
							break;

						case 1:
							if(g_chFlagDir == 'R')	//kdyz pri zataceni zmacknu opacnou sipku
								g_iStat = 2;
							//nebyl else
							else if((g_iSpeedB >= STEP) && (g_chSignDriveB == '+')){		//dopredu
								g_iSpeedB = g_iSpeedB - STEP;
								g_chSignDriveB = '+';
							}
						
							else if(g_iSpeedA < (MAX_SPEED - STEP)){
								g_iSpeedB = g_iSpeedB + STEP;
								g_chSignDriveB = '-';
						
							}
							
							if(g_iSpeedA == g_iSpeedB)
								g_iStat = 0;
							
							break;

						case 2:
							if((g_iSpeedA == g_iSpeedB) && (g_chSignDriveA == g_chSignDriveB))
								g_iStat = 1;
	
							else{
								if((g_iSpeedA >= STEP) && (g_chSignDriveA == '-')){
									g_iSpeedA = g_iSpeedA - STEP;
									g_chSignDriveA = '-';
								}
								else if(g_iSpeedA < (MAX_SPEED - STEP)){
									g_iSpeedA = g_iSpeedA + STEP;
									g_chSignDriveA = '+';
								}
	
							}
									
						break;
	
					}

					g_chFlagDir = 'L';
					
						break;
					
					case VK_SPACE:
						g_iSpeedA = 0;
						g_iSpeedB = 0;
						g_iStat = 0;
						g_bSend = true;
						break;

					}
					
			break;
			}

		//odeslani zpravy stavovemu radku pri zmene velikosti okna zmeni i svoji velikost
		case WM_SIZE:
			SendMessage(g_hwndStatusBar, WM_SIZE, wParam, lParam);
		break;

		//zpracovani zpravy od casovace
		case WM_TIMER:
			GetLocalTime(&sysTime);
			if((!g_iUltrasonicErr) && (!g_iDriveErr))
				sprintf_s(g_chStat, TEXT("Online."));
			else
				sprintf_s(g_chStat, TEXT("Chyba!"));

			sprintf_s(chText, TEXT(" Dnes je: %d.%d.%d, %d:%.2d:%.2d hod. Stav subsystémů: %s"),
				sysTime.wDay, sysTime.wMonth, sysTime.wYear,
				sysTime.wHour, sysTime.wMinute, sysTime.wSecond, g_chStat);
			SetWindowText(g_hwndStatusBar, chText);
			Drawing(0,0);
		break;


		case WM_CHAR:

			switch(wParam){
				
			case 'm':
				g_bManual = true;
				g_iSpeedA = 0;
				g_iSpeedB = 0;
				g_iStat = 0;
				sprintf_s(g_chMode, "Manuální     ");
				Beep(HIGH, SHORT);

			break;

			case 'a':
				g_bManual = false;
				g_bSend = true;
				sprintf_s(g_chMode, "Automatický");
				Beep(HIGH, SHORT);

			break;

			case 'o':
				g_chMelody = 'o';
				ResumeThread(hThreadSound);
			
			break;

			case 'r':
				g_chMelody = 'r';
				ResumeThread(hThreadSound);
			
			break;

			case 't':
				g_chMelody = 't';
				ResumeThread(hThreadSound);
			
			break;

			case 'g':
				if(g_bGPSEnabled){
					g_bGPSEnabled = false;
					Beep(HIGH, SHORT);
					Sleep(50);
					Beep(HIGH, SHORT);
				}
				else{
					g_bGPSEnabled = true;
					Beep(HIGH, SHORT);
				}
				break;

			case 'k':
				if(g_bCamEnabled){
					g_bCamEnabled = false;
					Beep(HIGH, SHORT);
					Sleep(50);
					Beep(HIGH, SHORT);
				}
				else{
					g_bCamEnabled = true;
					Beep(HIGH, SHORT);	
				}
				break;


			case 'n':
				if(g_bLowVoltEnabled){
					g_bLowVoltEnabled = false;
					Beep(HIGH, SHORT);
					Sleep(50);
					Beep(HIGH, SHORT);
				}
				else{
					g_bLowVoltEnabled = true;
					Beep(HIGH, SHORT);	
				}
				break;

			case '\n':	//CTRL+ENTER
				Beep(HIGH, LONG);
				Beep(LOW, SHORT);
				g_iSpeedA = 0;
				g_iSpeedB = 0;
				system("shutdown -s -f -t 5");

			break;

			}
			
		break;

		case WM_DESTROY:
			g_bStop = true;
			g_bEnd = true;
			CloseHandle(hThreadUltrasonic);
			CloseHandle(hThreadCompass);
			CloseHandle(hThreadDrive);
			CloseHandle(hThreadDrive2);
			CloseHandle(hThreadSound);
			CloseHandle(hThreadGPS);
			PostQuitMessage(0);
			break;
		
	}
	return DefWindowProc(hwnd, message, wParam, lParam);
}

BOOL InitApp()
{
	INITCOMMONCONTROLSEX icc;
	icc.dwSize = sizeof(INITCOMMONCONTROLSEX);
	icc.dwICC = ICC_WIN95_CLASSES;
	if ( !InitCommonControlsEx(&icc) )
		return FALSE;

	WNDCLASSEX wc;
	wc.cbSize = sizeof(WNDCLASSEX);
	wc.cbClsExtra = 0;
	wc.cbWndExtra = 0;
	wc.hbrBackground = (HBRUSH)(COLOR_WINDOW + 1);
	wc.hCursor = LoadCursor(NULL, IDC_ARROW);
	wc.hIcon = LoadIcon(g_hInstance, MAKEINTRESOURCE(IDR_MAINICON));
	wc.hIconSm = LoadIcon(g_hInstance, MAKEINTRESOURCE(IDR_MAINICON));
	wc.hInstance = g_hInstance;
	wc.lpfnWndProc = WindowProcMain;
	wc.lpszClassName = _MainClassName;
	wc.lpszMenuName = NULL;
	wc.style = CS_HREDRAW | CS_VREDRAW;
	if ( !RegisterClassEx(&wc) )
		return FALSE;
	//vytvoreni hlavniho okna
	g_hwndMain = CreateWindowEx(0, _MainClassName,
		_AppName,
		WS_CAPTION | WS_VISIBLE | WS_BORDER | WS_SYSMENU | WS_MINIMIZEBOX,
		100, 100, 450, 450,
		NULL,
		LoadMenu(g_hInstance, MAKEINTRESOURCE(IDR_MAINMENU)),
		g_hInstance, NULL);
	if ( g_hwndMain == NULL )
		return FALSE;
	
	// vytvoření dětských oken	
	g_hwndAut = CreateWindowEx(0,
	  TEXT("BUTTON"),
	  TEXT("AUT"),
	  WS_CHILD | WS_VISIBLE | BS_PUSHBUTTON,
	  320, 260, 75, 25,
	  g_hwndMain,
	  (HMENU)NULL,
	  g_hInstance,
	  NULL);
	if ( g_hwndAut == NULL )
		return FALSE;

	g_hwndMan = CreateWindowEx(0,
	  TEXT("BUTTON"),
      TEXT("MAN"),
      WS_CHILD | WS_VISIBLE | BS_PUSHBUTTON,
      320, 300, 75, 25,
      g_hwndMain,
      (HMENU)NULL,
      g_hInstance,
      NULL);
	if ( g_hwndMan == NULL )
      return FALSE;

	g_hwndStatusBar = CreateWindowEx(0,
		STATUSCLASSNAME,
		TEXT("Stavový řádek"),
		WS_CHILD | WS_VISIBLE,
		0, 0, 0, 0,
		g_hwndMain,
		(HMENU)NULL,
		g_hInstance,
		NULL);
	if ( g_hwndStatusBar == NULL )
		return FALSE;
		
	HFONT hFont = (HFONT)GetStockObject(DEFAULT_GUI_FONT);
	SendMessage(g_hwndEdit, WM_SETFONT, (WPARAM)hFont, (LPARAM)TRUE);
	SendMessage(g_hwndAut, WM_SETFONT, (WPARAM)hFont, (LPARAM)TRUE);
	SendMessage(g_hwndMan, WM_SETFONT, (WPARAM)hFont, (LPARAM)TRUE);
	SetTimer(g_hwndMain, _TimerClock, 200, NULL); //atkivace casovace 
	SendMessage(g_hwndMain, WM_TIMER, _TimerClock, NULL);

	//vytvareni vlaken
	//vlakno pro ultrazvuky
	hThreadUltrasonic = CreateThread(	NULL, 0, &threadSupersonic, NULL, 0, &idThreadUltrasonic);	
	// testovani otevreni vlakna
	if (hThreadUltrasonic  == INVALID_HANDLE_VALUE)	
	{
		MessageBox(g_hwndMain, TEXT("Nepodařilo se otevřít vlákno ultrazvuku"), TEXT("Chyba"), MB_ICONWARNING);
		return FALSE;
	}

	//vlakno pro posilani dat pohonum
	hThreadDrive = CreateThread(NULL, 0, &threadDrive, NULL, 0, &idThreadDrive);
	// testovani otevreni vlakna
	if (hThreadDrive  == INVALID_HANDLE_VALUE)	
	{
		MessageBox(g_hwndMain, TEXT("Nepodařilo se otevřít vlákno pohonu"), TEXT("Chyba"), MB_ICONWARNING);
		return FALSE;
	}

	//vlakno pro cteni dat od pohonu
	hThreadDrive2 = CreateThread(NULL, 0, &threadDrive2, NULL, 0, &idThreadDrive2);
	// testovani otevreni vlakna
	if (hThreadDrive2  == INVALID_HANDLE_VALUE)	
	{
		MessageBox(g_hwndMain, TEXT("Nepodařilo se otevřít druhé vlákno pohonu"), TEXT("Chyba"), MB_ICONWARNING);
		return FALSE;
	}

	//vlakno pro cteni dat od kompasu
	hThreadCompass = CreateThread(NULL, 0, &threadCompass, NULL, 0, &idThreadCompass);
	// testovani otevreni vlakna
	if (hThreadCompass  == INVALID_HANDLE_VALUE)	
	{
		MessageBox(g_hwndMain, TEXT("Nepodařilo se otevřít vlákno kompasu"), TEXT("Chyba"), MB_ICONWARNING);
		return FALSE;
	}

	//vlakno pro zvuky
	hThreadSound = CreateThread(	NULL, 0, &ThreadSound, NULL, CREATE_SUSPENDED, &idThreadSound);
	// testovani otevreni vlakna
	if (hThreadSound  == INVALID_HANDLE_VALUE)	
	{
		MessageBox(g_hwndMain, TEXT("Nepodařilo se otevřít vlákno sound"), TEXT("Chyba"), MB_ICONWARNING);
		return FALSE;
	}

	// vlakno pro GPS
	hThreadGPS = CreateThread(	NULL, 0, &threadGPS, NULL, 0, &idThreadGPS);
	// testovani otevreni vlakna
	if (hThreadGPS  == INVALID_HANDLE_VALUE)	
	{
		MessageBox(g_hwndMain, TEXT("Nepodařilo se otevřít vlákno pro GPS"), TEXT("Chyba"), MB_ICONWARNING);
		return FALSE;
	}
	
	return TRUE;
}

MSG msg;

int WINAPI WinMain(HINSTANCE hInstance, HINSTANCE hPrevInst, LPSTR lpCmdLine, int nShow)
{
	g_hInstance = hInstance;
	if ( !InitApp() )
		return FALSE;
	
	while ( GetMessage(&msg, NULL, 0, 0) )
	{
		TranslateMessage(&msg);
		DispatchMessage(&msg);
	}
	return msg.wParam;
}