

// emulate.c - a version of navsoft incorporating code from James Coxon, Steve Randall and Ed Moore
// removed high altitude slug routines, added Coords_to_bearing_and_distance, added gps emmulation

#define DEBUG	//debug version

#include    /* Standard input/output definitions */
#include   /* Standard stuff like exit */
#include 
#include 
#include   /* String function definitions */

#include    /* File control definitions */
#include    /* Error number definitions */

#ifndef DEBUG
#include  /* POSIX terminal control definitions */
#include   /* UNIX standard function definitions */
#endif // end of ifnfed DEBUG


 
typedef struct {
	float lat;
	float lon;
} coordinate;

typedef struct {
	float bearing;
	float distance;
} vector;


/*
 * Constants
 */

/*
 * Function-like macros
 */

#define DEG2KM(a)	(a * (float)111.226306)	// Degrees (of latitude) to Kilometers multiplier
#define DEG2RAD(a)	(a * (float)0.0174532925)	// Degrees to Radian multiplier
#define RAD2DEG(a) 	(a * (float)57.2957795)	// Radian to Degrees multiplier

int fdgps, fdservo; // global file descriptor of the gps and servo serial ports
int servo_pos; 
char buf[256];

float VTGtrueTrack, VTGmagTrack, VTGgroundSpeedKn, VTGgroundSpeedKm;
int GGAsats, GGAtime, GGAfix;
float GGAlat, GGAlong, GGAaltM, GGAhori, GGAgeoid;
static unsigned char servo_buf[10]; // holds the current RC servo data block - 8 channels + header and CRC

float targetLat;
float targetLon;
 
 
//Set parameters
 
float parachuteReleaseheight = 1000.0;
 
FILE * logFile; // Log file pointer

#define GPS_IN_FILE "d:/steve/rockrel/balloon/emulate/debug/gps.log"
#define GPS_OUT_FILE "gps_out.log"
#define LOG_FILE "gps.log"
#define GPS_PORT "/dev/ttyS2"

// *************************************** Dummy Routines for MSC++ ********************************************

// Dummy version of usleep (no usleep in msc)

#ifdef DEBUG

void usleep(int usecs)
{
	return;
}

#endif // end of #ifdef DEBUG

// **************************************************************************************

// This routine acts as if it is a serial port with a GPS attached
// It opens a GPS log file and plays it out as if the GPS was sending data

#ifdef DEBUG

FILE *fp_gps = NULL;
time_t epoch;
long gga_count; 

void open_gps_serial_port(void)

{

	fp_gps = fopen(GPS_IN_FILE,"r");
		
	if (fp_gps == NULL)
	{
		printf("%s failed to open for read\n",GPS_IN_FILE);
		exit(1); // abnormal exit
	}

	epoch = time(NULL); // the startup time
	gga_count = 0l;
}
	

void close_gps_serial_port(void)
{
	fclose(fp_gps);
}

void read_gps_serial_port(char *buf)
{
	time_t now; // the current time

	if (fgets(buf,255,fp_gps) == NULL)
	{
		printf("file read error\n");
		exit(2); // abnormal exit
	}
		
	// new line read (and terminated)
		
	if (strncmp(buf,"$GPGGA,",7) == 0)
	{ // a GPGGA message - should be one per second (for 1Hz GPS)
		gga_count++; // keep count of GGA messages

		do
		{
			now = time(NULL); 
		}
		while ((now - epoch) < gga_count); // wait until elapsed time in seconds catches up with number of gga messages
	}

}


void open_servo_serial_port(const char* name)
{
//	printf("Open Servo Port %s\n",name);
}
 
void close_servo_serial_port(void)
{
//	printf("Close Servo Port\n");
}

// write_servo_serial_port() is the lowest level routine - its is where the servo port serial I/O is done 

void write_servo_serial_port(unsigned char *buf,int len)
{
	int i;
	
	printf("Write Servo of %d bytes =",len);

	for (i = 0; i < len; i++)
	{
		printf(" %d",buf[i]);
	}

    printf("\n");
}

#endif // end of #ifdef DEBUG


// *******************************************************************************************
// ******************************* UNIX Serial Port Funtions *********************************

// ******************************* GPS Serial Port functions *********************************
 
#ifndef DEBUG

void open_gps_serial_port(void)
{
		// Open port for both reading ands writing (writes will not block)
        fdgps = open(GPS_PORT, O_RDWR | O_NOCTTY | O_NDELAY);
        if (fdgps == -1)
        {
            printf("open_port: Unable to open %s - \n", GPS_PORT);
			exit(3);
        }
        else
            fcntl(fdgps, F_SETFL, 0);
 
}
 
void close_gps_serial_port(void)
{
    close(fdgps);
}
 
// get a line of input from the GPS
//
 
void read_gps_serial_port(char *buf)

{
	int res; // set to length of line read

    res = read(fdgps,buf,255); // try to read a line of input from the gps, 
    buf[res] = 0; // termiante string, so we can printf etc.

}

// *************************** Servo serial Port functions **********************************

void open_servo_serial_port(const char* ttySx)
{
 	// Open port for both reading ands writing (writes will not block)   
    fdservo = open(ttySx, O_RDWR | O_NOCTTY | O_NDELAY);
    if (fdservo == -1)
    {
        printf("open_port: Unable to open %s - \n", ttySx);
    }
    else
        fcntl(fdservo, F_SETFL, 0);
}
 
void close_servo_serial_port(void)
{
    close (fdservo);
}

// write_servo_serial_port() is the lowest level routine - its is where the servo port serial I/O is done 

void write_servo_serial_port(unsigned char *buf,int len)
{
    //   WriteUART2(uc); // writes to the USART serving the servo controller
 
    int n = write(fdservo, buf, len); // Write the block to the USART
    printf("%d\n", n); // should be 10
}

#endif // end of #ifndef DEBUG



// ******************************************************************************************
// ***************************** platform independant code **********************************

void ReleaseChute(void)
{
	system("/root/scripts/Parachutecutdown");
}


// ********************************* Logging functions **************************************
 
void open_log_file(char *filename)
{
	logFile = fopen(filename, "a+");
}
 
void write_log()
{
	fprintf( logFile, "%d,%f,%f,%d,%d,%f,%f,%f\n", GGAtime, GGAlat, GGAlong, GGAfix, GGAsats, GGAhori, GGAaltM, GGAgeoid);
}
 
void close_log_file()
{
	fclose(logFile);
}

// ********************************* GPS functions **************************************
 
// read and parse NMEA sentances from GPS

// converts 2 ascii hex characters to a single unsigned character value

int hexchar(unsigned char *phc, char *pch)
{
	unsigned char hc;

	hc = 0;
	if ((*pch >= '0') && (*pch <= '9'))
	{
		hc += (*pch - '0');
	}
	else
	{
		if ((*pch  >= 'A') && (*pch <= 'F'))
			hc += (*pch - 'A' + 10);
		else
			return(0); // no characters processed
	}
	hc <<= 4;
	pch++;
	if ((*pch >= '0') && (*pch <= '9'))
	{
		hc += (*pch - '0');
	}
	else
	{
		if ((*pch  >= 'A') && (*pch <= 'F'))
			hc += (*pch - 'A' + 10);
		else
			return(1); // only 1 character processed
	}
	*phc = hc; // sucessful conversion
	return(2); // 2 characters processed
}

 
void read_and_parse_gps(void)
{
	char *token, *str1, *pch;
	unsigned char csa, csb;

	read_gps_serial_port(buf); // read a line of input

	pch = buf;
	if (*pch != '$') 
		return; // ignore - does not start with $

	pch++;
	csa = 0;
	while(*pch != '*')
	{ // checksum calcualtion done ofver characters between '$' and '*'
		if (*pch == '\0')
			return; // terminator encountered before '*'
		csa ^= *pch;
		pch++;
	}

	pch++; // point at checksum charcaters (skip over '*')

	if (hexchar(&csb,pch) != 2)
	{
		return; // not 2 checksum digits
	}

	if (csa != csb)
	{
		return; // checksum invalid
	}


    if (strncmp(buf, "$GPVTG", 6) == 0) 
	{	// NMEA GPVTG sentance read from GPS (contains ground course and speed)
	    printf("GPVTG found %s\n",buf);
	    str1 = strdup(buf);			// duplicate buffer so strtok can carve it up
	    token = strtok(str1, ",");
	    token = strtok(NULL, ",");
	    VTGtrueTrack = (float)atof(token);
	    token = strtok(NULL, ",");
	    token = strtok(NULL, ",");
	    VTGmagTrack = (float)atof(token);
	    token = strtok(NULL, ",");
	    token = strtok(NULL, ",");
	    VTGgroundSpeedKn = (float)atof(token);
	    token = strtok(NULL, ",");
	    token = strtok(NULL, ",");
	    VTGgroundSpeedKm = (float)atof(token);
	    printf("%f,%f,%f,%f\n", VTGtrueTrack, VTGmagTrack, VTGgroundSpeedKn, VTGgroundSpeedKm);
    }
 
    else if (strncmp(buf, "$GPGGA", 6) ==0) 
		{ // NMEA GPGGA sentance read from GPS (contains position and height information)
			printf ("GPGGA found%s\n",buf);
			/* Splits the GPGGA sequence into tokens and then converts them into respective ints or floats */
  			str1 = strdup(buf);			// duplicate buffer so strtok can carve it up
			token = strtok(str1, ",");
			token = strtok(NULL, ",");
			GGAtime = atoi(token);
			token = strtok(NULL, ",");
			GGAlat = (float)atof(token);
			/* Convert into decimal */
			GGAlat = GGAlat / (float)100.0;
			int deglat = (int)GGAlat;
			float minlat = GGAlat - (float)deglat;
			minlat = minlat / (float)0.6;
			GGAlat = (float)deglat + minlat;
			token = strtok(NULL, ",");
			token = strtok(NULL, ",");
			GGAlong = (float)atof(token);
			/* Convert into decimal */
			GGAlong = GGAlong / (float)100.0;
			int deglong = (int)GGAlong;
			float minlong = GGAlong - (float)deglong;
			minlong = minlong / (float)0.6;
			GGAlong = (float)deglong + minlong;
			token = strtok(NULL, ",");
			token = strtok(NULL, ",");
			GGAfix = atoi(token);
			token = strtok(NULL, ",");
			GGAsats = atoi(token);
			token = strtok(NULL, ",");
			GGAhori = (float)atof(token);
			token = strtok(NULL, ",");
			GGAaltM = (float)atof(token);
			token = strtok(NULL, ",");
			GGAgeoid = (float)atof(token);
			printf("%d,lat:%f,long:%f,fix:%d,sats:%d,hori:%f,alt:%f,geo:%f\n", GGAtime, GGAlat, GGAlong, GGAfix, GGAsats, GGAhori, GGAaltM, GGAgeoid);	
		}
		else 
		{ // neither a GPVTG or GPGGA sentance read
			//	    printf("No Lock\n");
		}
}


// ********************************* Servo functions **************************************
 

// contsructs and ouputs the servo data block to the servo controller
// creates header and CRC arround the data block to be sent to the servo controller
 
// servo_pos is the current absolute servo position
// change is the required difference from that value

// SR: probably need to add limits on servo positions.

void adjust_servo_position(int change)
{
	int i, pos;
	unsigned char Servo_CRC;
 
	/* Take data from calculate_differences and adjust servo position accordingly */

    pos = servo_pos + change;

	// range limit
	if (pos < 0)
		pos = 0;

	if (pos > 255)
		pos = 255;

    open_servo_serial_port("/dev/ttyS1");
//    printf("Serial port open\n");
 
	// construct the servo packet and send it

	servo_buf[0] = 0x5A; // header value
	Servo_CRC = servo_buf[0];
    for (i = 1; i < 9; i++)
    {
        servo_buf[i] = pos;	// set all servos the same
        Servo_CRC ^= servo_buf[i]; // calculate checksum 
    }
	servo_buf[i] = Servo_CRC;
 
	write_servo_serial_port(servo_buf,10); // write the data to the servo port

    close_servo_serial_port(); // force flush?
    servo_pos = pos;
}
 

void centre_servo(void)

{
	servo_pos = 128;

	adjust_servo_position(0); // drives servo to mid point
}

 
// ************************ Position and Direction Calculation functions **************************
 
// Distance and Bearings Calculations
//

 
// **************************************************************************************

vector Coords_to_bearing_and_distance(coordinate posn, coordinate dest)
{

	float delta_lat, delta_lon;
	vector result;
	
	delta_lat = dest.lat - posn.lat;
	delta_lon = (dest.lon - posn.lon) * (float)cos(DEG2RAD((dest.lat + posn.lat)/2));

	result.distance = DEG2KM((float)sqrt(((delta_lat) * (delta_lat)) + ((delta_lon) * (delta_lon)))); // pythagerious

	// calcualte compass bearing degrees clockwise from north (0 - 360)
	// atan2(y,x) produces the euclidean angle (+ve = counter-clockwise from x axis in radians)
	// atan2(d_lon,d_lat) produces the compass angle (+ve = clockwise from N /-ve counter-clockwise)

	result.bearing = RAD2DEG((float)atan2(delta_lon,delta_lat)); // atan2 argument inversion to get compass N based co-ordinates

	if (result.bearing < 0.0)
		result.bearing += 360; // convert to 0-360
	
	return(result);
}


//******************************************************************************************


/* Compare required bearing to present ground track (heading) and calculate difference, also trigger alarms if getting near */
/* to the landing site and finally look at altitude and how much height we have left before landing */
/* Rough example - will update with edwardmoores' version */

void calculate_differences(float bearing, float groundTrack)
{
	float bearingDifference;

    printf("Target Bearing = %f\n", bearing);
    printf("True Track = %f\n", groundTrack);

	// Calculate the difference between the baering to target and the current ground track
    bearingDifference = bearing - groundTrack;

	// since bearing and groundTrack can take any value from 0 - 360 then
	// bearingDifference can vary from +360 to -360
	// values -360 to -180 mapped to 0 to +180
	// values +360 to +180 mapped to 0 to -180

	if (bearingDifference <= -180.0)
		bearingDifference += 360.0;

	if(bearingDifference >= 180.0) 
		bearingDifference -= 360.0;

	// Bearing difference now adjusted to range -180 to +180

    printf("Bearing Difference = %f\n", bearingDifference);
    if (bearingDifference < 0.0) 
	{ /*Turn Left*/
		adjust_servo_position(-5); // nudge servo ccw
    }

    if (bearingDifference > 0.0) 
	{ /* Turn Right */
		adjust_servo_position(5); // nudge servo cw
    }
}
 
float prevAlt = 0.0;
int apogee = 0;

void check_against_paramters(float altitude)
{
	float altDifference;
    /* This compares data against various set paramaters eg. altitude and decides whether to cut down secondary parachute */
 
    // Calulates if the payload is descending
    altDifference = altitude - prevAlt;
    prevAlt = altitude;

    if ( altDifference < 0) 
	{
        apogee++; // counts number of descending samples
    }

    // Decide whether to release parachute.
    if (altitude < parachuteReleaseheight && apogee > 2) 
	{
		ReleaseChute();
    }
}
 
 
// ********************************* The Main loop **************************************
 
// main expects two commend line parameters: Latitude and Longitude of the target loaction
// the code echos the target loaction, opens the gps serial port and logging files and then
// enters the main loop. The main loop then
//		reads the position and direction from the GPS - read_gps_serial_port()
//		calculates bearing and distance to the target loaction
//		adjusts the servo
 
 
 
int main (int argc, char **argv) 
{
	vector target_vector;
	coordinate presentCoords;
	coordinate targetCoords;
	int line;

	line = 0;

	if (argc == 3)
	{
		targetLat = (float)atof(argv[1]);
		targetLon = (float)atof(argv[2]);
	}
	else
	{
		targetLat = (float)52.008565; // Kirton
		targetLon = (float)1.317307;

		printf("Debug mode\n");
	}



    printf("HARVe Lifting Body Navigation Software\n");
    printf("By James Coxon, Ed Moore and Steve Randall\n");
    printf("Target Coords = %f, %f\n", targetLat, targetLon);

    open_gps_serial_port();
    open_log_file(LOG_FILE);

	centre_servo(); // Start Servo at midway position
 
    while (1) // Loop forever
    {	
		read_and_parse_gps();
		printf("Parsed line %d\n",line++);
        // Do we have a GPS lock?
        if (GGAfix == 1) 
		{	// Valid GPS fix
			write_log();
			presentCoords.lat = GGAlat;
			presentCoords.lon = GGAlong;

			targetCoords.lat = targetLat;
			targetCoords.lon = targetLon;

			target_vector = Coords_to_bearing_and_distance(presentCoords, targetCoords);

			check_against_paramters(GGAaltM);
			printf("Target beraing = %fdeg, distance %fKm\n",target_vector.bearing,target_vector.distance);

			calculate_differences(target_vector.bearing,VTGtrueTrack); //   adjust_servo_position();

        }
        else   
        {	// No GPS lock.
			centre_servo(); // Centre servo if GPS lock lost

            usleep(500); // SR: 500 micro seconds - too short??
        }
 
		usleep(500); // SR: 2 lots of usleep(500) if no lock ??
 
    }
 
    close_log_file();
	close_gps_serial_port();

	return(0); // Normal termination
}