#include <iostream>
#include <string>
#include <vector>
#include <sstream>
#include <fstream>
#include <math.h>
using namespace std;


/*
//This is a way of converting a double to a string!

double mynumber;
mynumber = 123.456;


char str[256];
sprintf(str, "%.8f", mynumber);
string outputstring;
outputstring = str;


*/

/*
//This is a way of converting a string to a double!

string test;
test = "123456";

float mynumber;
mynumber = strtod(test.c_str(), NULL);


*/

  /////////////////////////////
  /////////////////////////////
 //This function will take a longitude and latitude and then check our database to find the closest camera
 int checker (float longitude1, float latitude1, float time) 
 {
 
 //Open our camera database and run through it checking how close each camera is
 string line;
 ifstream myfile ("/cams.txt");

 //Setup holders ready for our closest camera info
 float closestcamera;
 closestcamera = 9999999;
 string closestcameratype;


 //Main loop going through camera database
 while(!myfile.eof()){

 getline (myfile,line);

 
 
////////////////////////////////////	   
	   string templongitude2;        
	   string templatitude2;
	   string cameratype;
	   templongitude2 = line.substr(0, line.find(","));
	   line = line.substr(line.find(",")+1, line.find("\r"));
	   templatitude2 = line.substr(0, line.find(","));
	   line = line.substr(line.find(",")+1, line.find("\r"));
	   cameratype = line.substr(line.find("\"")+1, line.find(":")-1);
////////////////////////////////////

	   
////////////////////////////////////
//Convert our camera longitude from a string to a double
float longitude2;
longitude2 = strtod(templongitude2.c_str(), NULL);

//Convert our camera latitude from a string to a double
float latitude2;
latitude2 = strtod(templatitude2.c_str(), NULL);
////////////////////////////////////


////////////////////////////////////
//circula function to work out distance between points
float distance;
distance = (3958*3.1415926*sqrt((latitude2-latitude1)*(latitude2-latitude1) + cos(latitude2/57.29578)*cos(latitude1/57.29578)*(longitude2-longitude1)*(longitude2-longitude1))/180);
////////////////////////////////////	   

////////////////////////////////////
//Convert distance from km to meters
distance = distance * 1000; 
////////////////////////////////////


////////////////////////////////////
//Work out our closest camera
if(distance < closestcamera){
closestcamera = distance;
closestcameratype = cameratype;
}
////////////////////////////////////
	
}


//cout << "Time : " << time << " Closest Camera: " << closestcameratype << " " << closestcamera << " meters" << endl;

cout << "\33[2J"; //Clear console

//Set a marker to check if we have found the camera type
float camerafound;
camerafound = 0;

//Set the distance when we dont want alerts
float requireddistance;
requireddistance = 401;

if(closestcameratype == "MOBILE"){
if(closestcamera < requireddistance){
camerafound = 1;
cout << "888b     d888  .d88888b.  888888b.  8888888 888      8888888888 " << endl; 
cout << "8888b   d8888 d88P' 'Y88b 888  '88b   888   888      888        " << endl; 
cout << "88888b.d88888 888     888 888  .88P   888   888      888        " << endl; 
cout << "888Y88888P888 888     888 8888888K.   888   888      8888888    " << endl; 
cout << "888 Y888P 888 888     888 888  'Y88b  888   888      888        " << endl; 
cout << "888  Y8P  888 888     888 888    888  888   888      888        " << endl; 
cout << "888   .   888 Y88b. .d88P 888   d88P  888   888      888        " << endl; 
cout << "888       888  'Y88888P'  8888888P' 8888888 88888888 8888888888 " << endl; 
}
}

if(closestcameratype == "GATSO"){
if(closestcamera < requireddistance){
camerafound = 1;
cout << " .d8888b.         d8888 88888888888 .d8888b.   .d88888b.  " << endl; 
cout << "d88P  Y88b       d88888     888    d88P  Y88b d88P' 'Y88b " << endl; 
cout << "888    888      d88P888     888    Y88b.      888     888 " << endl; 
cout << "888            d88P 888     888     'Y888b.   888     888 " << endl; 
cout << "888  88888    d88P  888     888        'Y88b. 888     888 " << endl; 
cout << "888    888   d88P   888     888          '888 888     888 " << endl; 
cout << "Y88b  d88P  d8888888888     888    Y88b  d88P Y88b. .d88P " << endl; 
cout << " 'Y8888P88 d88P     888     888     'Y8888P'   'Y88888P'  " << endl; 
 }
 }

if(closestcameratype == "REDLIGHT"){
if(closestcamera < requireddistance){
camerafound = 1;
cout << "888      8888888 .d8888b.  888    888 88888888888 .d8888b.  " << endl; 
cout << "888        888  d88P  Y88b 888    888     888    d88P  Y88b " << endl; 
cout << "888        888  888    888 888    888     888    Y88b.      " << endl; 
cout << "888        888  888        8888888888     888     'Y888b.   " << endl; 
cout << "888        888  888  88888 888    888     888        'Y88b. " << endl; 
cout << "888        888  888    888 888    888     888          '888 " << endl; 
cout << "888        888  Y88b  d88P 888    888     888    Y88b  d88P " << endl; 
cout << "88888888 8888888 'Y8888P88 888    888     888     'Y8888P'  " << endl; 
}
}

if(closestcameratype == "SPECS"){
if(closestcamera < requireddistance){
camerafound = 1;
cout << " .d8888b.  8888888b.  8888888888 .d8888b.   .d8888b.  " << endl; 
cout << "d88P  Y88b 888   Y88b 888       d88P  Y88b d88P  Y88b " << endl; 
cout << "Y88b.      888    888 888       888    888 Y88b.      " << endl; 
cout << " 'Y888b.   888   d88P 8888888   888         'Y888b.   " << endl; 
cout << "    'Y88b. 8888888P'  888       888            'Y88b. " << endl; 
cout << "      '888 888        888       888    888       '888 " << endl; 
cout << "Y88b  d88P 888        888       Y88b  d88P Y88b  d88P " << endl; 
cout << " 'Y8888P'  888        8888888888 'Y8888P'   'Y8888P'  " << endl; 
}
}

if(closestcamera < requireddistance){
if(camerafound == 0){
cout << " .d8888b.        d8888 888b     d888 8888888888 8888888b.         d8888 " << endl;
cout << "d88P  Y88b      d88888 8888b   d8888 888        888   Y88b       d88888 " << endl;
cout << "888    888     d88P888 88888b.d88888 888        888    888      d88P888 " << endl;
cout << "888           d88P 888 888Y88888P888 8888888    888   d88P     d88P 888 " << endl;
cout << "888          d88P  888 888 Y888P 888 888        8888888P'     d88P  888 " << endl;
cout << "888    888  d88P   888 888  Y8P  888 888        888 T88b     d88P   888 " << endl;
cout << "Y88b  d88P d8888888888 888   '   888 888        888  T88b   d8888888888 " << endl;
cout << " 'Y8888P' d88P     888 888       888 8888888888 888   T88b d88P     888 " << endl;
}
}

//Add a break between camera type and meters closeness
cout << endl << endl;

if(closestcamera >= 401){
cout << " .d8888b.        d8888 8888888888 8888888888 " << endl;
cout << "d88P  Y88b      d88888 888        888        " << endl;
cout << "Y88b.          d88P888 888        888        " << endl;
cout << " 'Y888b.      d88P 888 8888888    8888888    " << endl;
cout << "    'Y88b.   d88P  888 888        888        " << endl;
cout << "      '888  d88P   888 888        888        " << endl;
cout << "Y88b  d88P d8888888888 888        888        " << endl;
cout << " 'Y8888P' d88P     888 888        8888888888 " << endl;
}


if(closestcamera <= 400){
if(closestcamera > 350){
cout << "    d8888   .d8888b.   .d8888b.  " << endl;
cout << "   d8P888  d88P  Y88b d88P  Y88b " << endl;
cout << "  d8P 888  888    888 888    888 " << endl;
cout << " d8P  888  888    888 888    888 " << endl;
cout << "d88   888  888    888 888    888 " << endl;
cout << "8888888888 888    888 888    888 " << endl;
cout << "      888  Y88b  d88P Y88b  d88P " << endl;
cout << "      888   'Y8888P'   'Y8888P'  " << endl;
}
}

if(closestcamera <= 350){
if(closestcamera > 300){
cout << " .d8888b.  888888888   .d8888b.  " << endl;
cout << "d88P  Y88b 888        d88P  Y88b " << endl;
cout << "     .d88P 888        888    888 " << endl;
cout << "    8888'  8888888b.  888    888 " << endl;
cout << "     'Y8b.      'Y88b 888    888 " << endl;
cout << "888    888        888 888    888 " << endl;
cout << "Y88b  d88P Y88b  d88P Y88b  d88P " << endl;
cout << " 'Y8888P'   'Y8888P'   'Y8888P'  " << endl;
}
}

if(closestcamera <= 300){
if(closestcamera > 250){
cout << " .d8888b.   .d8888b.   .d8888b.  " << endl;
cout << "d88P  Y88b d88P  Y88b d88P  Y88b " << endl;
cout << "     .d88P 888    888 888    888 " << endl;
cout << "    8888'  888    888 888    888 " << endl;
cout << "     'Y8b. 888    888 888    888 " << endl;
cout << "888    888 888    888 888    888 " << endl;
cout << "Y88b  d88P Y88b  d88P Y88b  d88P " << endl;
cout << " 'Y8888P'   'Y8888P'   'Y8888P'  " << endl;
}
}

if(closestcamera <= 250){
if(closestcamera > 200){
cout << " .d8888b.  888888888   .d8888b.  " << endl;
cout << "d88P  Y88b 888        d88P  Y88b " << endl;
cout << "       888 888        888    888 " << endl;
cout << "     .d88P 8888888b.  888    888 " << endl;
cout << " .od888P'       'Y88b 888    888 " << endl;
cout << "d88P'             888 888    888 " << endl;
cout << "888'       Y88b  d88P Y88b  d88P " << endl;
cout << "888888888   'Y8888P'   'Y8888P'  " << endl;
}
}

if(closestcamera <= 200){
if(closestcamera > 150){
cout << " .d8888b.   .d8888b.   .d8888b.  " << endl;
cout << "d88P  Y88b d88P  Y88b d88P  Y88b " << endl;
cout << "       888 888    888 888    888 " << endl;
cout << "     .d88P 888    888 888    888 " << endl;
cout << " .od888P'  888    888 888    888 " << endl;
cout << "d88P'      888    888 888    888 " << endl;
cout << "888'       Y88b  d88P Y88b  d88P " << endl;
cout << "888888888   'Y8888P'   'Y8888P'  " << endl;
}
}

if(closestcamera <= 150){
if(closestcamera > 100){
cout << " d888  888888888   .d8888b.  " << endl;
cout << "d8888  888        d88P  Y88b " << endl;
cout << "  888  888        888    888 " << endl;
cout << "  888  8888888b.  888    888 " << endl;
cout << "  888       'Y88b 888    888 " << endl;
cout << "  888         888 888    888 " << endl;
cout << "  888  Y88b  d88P Y88b  d88P " << endl;
cout << "8888888 'Y8888P'   'Y8888P'  " << endl;
}
}

if(closestcamera <= 100){
if(closestcamera > 50){
cout << " d888   .d8888b.   .d8888b.  " << endl;
cout << "d8888  d88P  Y88b d88P  Y88b " << endl;
cout << "  888  888    888 888    888 " << endl;
cout << "  888  888    888 888    888 " << endl;
cout << "  888  888    888 888    888 " << endl;
cout << "  888  888    888 888    888 " << endl;
cout << "  888  Y88b  d88P Y88b  d88P " << endl;
cout << "8888888 'Y8888P'   'Y8888P'  " << endl;
}
}

if(closestcamera <= 50){
if(closestcamera > 25){
cout << "888888888   .d8888b.  " << endl;
cout << "888        d88P  Y88b " << endl;
cout << "888        888    888 " << endl;
cout << "8888888b.  888    888 " << endl;
cout << "     'Y88b 888    888 " << endl;
cout << "       888 888    888 " << endl;
cout << "Y88b  d88P Y88b  d88P " << endl;
cout << " 'Y8888P'   'Y8888P'  " << endl;
}
}

if(closestcamera <= 25){
if(closestcamera > 10){
cout << " .d8888b.  888888888  " << endl;
cout << "d88P  Y88b 888        " << endl;
cout << "       888 888        " << endl;
cout << "     .d88P 8888888b.  " << endl;
cout << " .od888P'       'Y88b " << endl;
cout << "d88P'             888 " << endl;
cout << "888'       Y88b  d88P " << endl;
cout << "888888888   'Y8888P'  " << endl;
}
}

if(closestcamera <= 10){
if(closestcamera > 5){
cout << " d888   .d8888b.  " << endl;
cout << "d8888  d88P  Y88b " << endl;
cout << "  888  888    888 " << endl;
cout << "  888  888    888 " << endl;
cout << "  888  888    888 " << endl;
cout << "  888  888    888 " << endl;
cout << "  888  Y88b  d88P " << endl;
cout << "8888888 'Y8888P'  " << endl;
}
}

if(closestcamera <= 5){
if(closestcamera > 0){
cout << "888888888  " << endl;
cout << "888        " << endl;
cout << "888        " << endl;
cout << "8888888b.  " << endl;
cout << "     'Y88b " << endl;
cout << "       888 " << endl;
cout << "Y88b  d88P " << endl;
cout << " 'Y8888P'  " << endl;
}
}



//Output exact distance from speed camera
cout << "Time: " << time << " Exact distance from camera: " << closestcamera << " Meters" << endl;

return 0;
}
/////////////////////////////
///////////////////////////// 


///////////////////////////// 
/////////////////////////////
//This function connects to the GPS using /dev/tty.iBT-GPS-SPPslave-1 and processes the data 
int main ()
{

 string line;
 ifstream myfile ("/dev/tty.iBT-GPS-SPPslave-1");

 //Setup holders ready for our closest camera info
 float closestcamera;
 closestcamera = 9999999;
 string closestcameratype;

 //Main loop going through camera database
 while(!myfile.eof()){
 
 getline (myfile,line);


//Work out which type of packet is being passed by the GPS
string check;
check = line.substr(0, line.find(","));

//If it is GPGGA then we will extract the time, latitude, north or south, longitude and east or west
if(check == "$GPGGA"){
//cout << "Original GPS Line: " << line << endl;

string time;
string templatitude1;
string nors;
string templongitude1;
string eorw;

		//Grab the data from our GPS and put into strings
	   line = line.substr(line.find(",")+1, line.find("\r"));
	   time = line.substr(0, line.find(","));
	   line = line.substr(line.find(",")+1, line.find("\r"));
	   templatitude1 = line.substr(0, line.find(","));
	   line = line.substr(line.find(",")+1, line.find("\r"));
	   nors = line.substr(0, line.find(","));
	   line = line.substr(line.find(",")+1, line.find("\r"));	   
	   templongitude1 = line.substr(0, line.find(","));	   
	   line = line.substr(line.find(",")+1, line.find("\r"));	   
	   eorw = line.substr(0, line.find(","));	   

//templatitude1 = "0000";
//templongitude1 = "0000";

//cout << "Step 1: Latitude: " << templatitude1 << endl;	   
	   
//cout << "Time: " << time << " Latitude: " << templatitude1 << " Longitude " << templongitude1 << endl;
	   
//Convert our gps time to a float	   
double gpstime;
gpstime = strtod(time.c_str(), NULL);
	   
	   
//Convert our gps latitude to decimal	   
double latitude1;
latitude1 = strtod(templatitude1.c_str(), NULL);
latitude1 = latitude1 / 100; //When debugging this will output rounded but its not!


//Convert our latitiude back to a string so we can break it up
char str[256];
sprintf(str, "%.8f", latitude1);
string latitude_data;
latitude_data = str;

//cout << "Step 2: Devide lattude by 100. Latitude: " << latitude_data << endl;	 

string latitudesection1;
string latitudesection2;

latitudesection1 = latitude_data.substr(0,latitude_data.find("."));
latitudesection2 = latitude_data.substr(latitude_data.find(".")+1,latitude_data.find("\r"));

//cout << "Step 3: Break latitude up before and after . " <<  latitudesection1 << " AND " << latitudesection2 << endl;

//Work out the degrees, minutes and seconds of our GPS latitude

//Work out latitude degrees
string tlatitude_degrees;
tlatitude_degrees = latitudesection1;

//cout << "Step 4: Lat degrees is data before the . which is " << tlatitude_degrees << endl;

//Work out latitude minutes
string tlatitude_minutes;
tlatitude_minutes = latitudesection2.substr(latitudesection2.find(".")+1,2);

//cout << "Step 5: Use our data after the . and use the first 2 chars as latitude minutes: " << tlatitude_minutes << endl;

//Work out latitude seconds
string tlatitude_seconds;
tlatitude_seconds = latitudesection2.substr(2, latitudesection2.find("\r"));

//Convert out tlatitude_seconds to a float ready to be devided by 100 and times by 60
float atlatitude_seconds;
atlatitude_seconds = strtod(tlatitude_seconds.c_str(), NULL);
atlatitude_seconds = (atlatitude_seconds / 100) * 60; //We will have a bit of rounding here but its minute

//cout << "Step 6: " << atlatitude_seconds << endl;

//Convert atlatitude_seconds to string
sprintf(str, "%.8f", atlatitude_seconds);
string aalatitude_seconds;
aalatitude_seconds = str;

//Use just the first 2 chars of atlatitude_seconds to give us our latitude seconds
aalatitude_seconds = aalatitude_seconds.substr(0,2);

//cout << "Step 7: " << aalatitude_seconds << endl;

//Convert degrees, minutes and seconds of our latitude back into floats
float float_latitude_degrees;
float float_latitude_minutes;
float float_latitude_seconds;
float_latitude_degrees = strtod(tlatitude_degrees.c_str(), NULL);
float_latitude_minutes = strtod(tlatitude_minutes.c_str(), NULL);
float_latitude_seconds = strtod(aalatitude_seconds.c_str(), NULL);

//Work out real latitude based on the latitude degrees, minutes and seconds
float decimal_latitude;
decimal_latitude = float_latitude_degrees + (float_latitude_minutes/60) + (float_latitude_seconds/3600);

//cout << "Step 8: " << decimal_latitude << endl << endl;
	   
//Convert our gps longitude to decimal	   
//cout << "Step 1: Longitude: " << templongitude1 << endl;	   

double longitude1;
longitude1 = strtod(templongitude1.c_str(), NULL);
longitude1 = longitude1 / 100; //When debugging this will output rounded but its not!

//Convert our longitude back to a string so we can break it up
sprintf(str, "%.8f", longitude1);
string longitude_data;
longitude_data = str;

//cout << "Step 2: Devide longitude by 100. longitude: " << longitude_data << endl;	 

string longitudesection1;
string longitudesection2;

longitudesection1 = longitude_data.substr(0,longitude_data.find("."));
longitudesection2 = longitude_data.substr(longitude_data.find(".")+1,longitude_data.find("\r"));

//cout << "Step 3: Break longitude up before and after . " <<  longitudesection1 << " AND " << longitudesection2 << endl;

//Work out the degrees, minutes and seconds of our GPS longitude

//Work out longitude degrees
string tlongitude_degrees;
tlongitude_degrees = longitudesection1;

//cout << "Step 4: Lat degrees is data before the . which is " << tlongitude_degrees << endl;

//Work out longitude minutes
string tlongitude_minutes;
tlongitude_minutes = longitudesection2.substr(longitudesection2.find(".")+1,2);

//cout << "Step 5: Use our data after the . and use the first 2 chars as longitude minutes: " << tlongitude_minutes << endl;

//Work out longitude seconds
string tlongitude_seconds;
tlongitude_seconds = longitudesection2.substr(2, longitudesection2.find("\r"));

//Convert out tlongitude_seconds to a float ready to be devided by 100 and times by 60
float atlongitude_seconds;
atlongitude_seconds = strtod(tlongitude_seconds.c_str(), NULL);
atlongitude_seconds = (atlongitude_seconds / 100) * 60; //We will have a bit of rounding here but its minute

//cout << "Step 6: " << atlongitude_seconds << endl;

//Convert atlongitude_seconds to string
sprintf(str, "%.8f", atlongitude_seconds);
string aalongitude_seconds;
aalongitude_seconds = str;

//Use just the first 2 chars of atlongitude_seconds to give us our longitude seconds
aalongitude_seconds = aalongitude_seconds.substr(0,2);

//cout << "Step 7: " << aalongitude_seconds << endl;

//Convert degrees, minutes and seconds of our longitude back into floats
float float_longitude_degrees;
float float_longitude_minutes;
float float_longitude_seconds;
float_longitude_degrees = strtod(tlongitude_degrees.c_str(), NULL);
float_longitude_minutes = strtod(tlongitude_minutes.c_str(), NULL);
float_longitude_seconds = strtod(aalongitude_seconds.c_str(), NULL);

//Work out real longitude based on the longitude degrees, minutes and seconds
float decimal_longitude;
decimal_longitude = float_longitude_degrees + (float_longitude_minutes/60) + (float_longitude_seconds/3600);

//cout << "Step 8: " << decimal_longitude;

//End of processing for this line from the GPS
checker (decimal_longitude, decimal_latitude, gpstime);
}

}//Keep looping through every line of our GPS looking for GPGGA

return 0;
} //End of our MAIN