/* Fordham Robotics and Computer Vision Lab. dml 7/11 */ 

#include <stdio.h>   /* Standard input/output definitions */
#include "serialGPS.h"

static serialGPS *gps=0;

void gps_alarm_handler(int status) {
  gps->readGPS();
  alarm(1);
}

int serialGPS::initport(int fd) {
	struct termios options;
        gps=this;
	// Get the current options for the port...
	tcgetattr(fd, &options);
	// Set the baud rates to 19200...
	cfsetispeed(&options, B115200);
	cfsetospeed(&options, B115200);
	// Enable the receiver and set local mode...
	options.c_cflag |= (CLOCAL | CREAD);

	options.c_cflag &= ~PARENB;
	options.c_cflag &= ~CSTOPB;
	options.c_cflag &= ~CSIZE;
	options.c_cflag |= CS8;

	// Set the new options for the port...
	tcsetattr(fd, TCSANOW, &options);
	tcflush(fd, TCIFLUSH);
        signal(SIGALRM,gps_alarm_handler);
        alarm(1);

	return 1;
}
int serialGPS::writeport(int fd, char *chars) {
	int len = strlen(chars);
	chars[len] = 0x0d; // stick a <CR> after the command
	chars[len+1] = 0x00; // terminate the string properly
	int n = write(fd, chars, strlen(chars));
	if (n < 0) {
		fputs("serialGPS::write failed!\n", stderr);
		return 0;
	}
	return 1;
}

int serialGPS::readport(int fd, char *result) {
  int iIn = read(fd, result, 254);
  result[iIn-1] = 0x00;
  if (iIn < 0) {
    if (errno == EAGAIN) {
      printf("serialGPS:: EAGAIN ERROR\n");
      return 0;
    } else {
      printf("serialGPS:: read error %d, %s\n", errno, strerror(errno));
      return 0;
    }
  }                    
  return 1;
}

int serialGPS::getbaud(int fd) {
	struct termios termAttr;
	int inputSpeed = -1;
	speed_t baudRate;
	tcgetattr(fd, &termAttr);
	/* Get the input speed.  */
	baudRate = cfgetispeed(&termAttr);
	switch (baudRate) {
		case B0:      inputSpeed = 0; break;
		case B50:     inputSpeed = 50; break;
		case B110:    inputSpeed = 110; break;
		case B134:    inputSpeed = 134; break;
		case B150:    inputSpeed = 150; break;
		case B200:    inputSpeed = 200; break;
		case B300:    inputSpeed = 300; break;
		case B600:    inputSpeed = 600; break;
		case B1200:   inputSpeed = 1200; break;
		case B1800:   inputSpeed = 1800; break;
		case B2400:   inputSpeed = 2400; break;
		case B4800:   inputSpeed = 4800; break;
		case B9600:   inputSpeed = 9600; break;
		case B115200:  inputSpeed = 115200; break;
		case B19200:  inputSpeed = 19200; break;
		case B38400:  inputSpeed = 38400; break;
	}
	return inputSpeed;
}


/*
 NMEA COMMAND $GPGLL,4132.61502,N,07346.15105,W,204042.00,A,D*7C
 */

/*
 Example of how to write commands to the GPS 
	char sCmd[254];
	
	sprintf(sCmd,"$JASC,D1,0\r\n"); // turns on diagnostic messages -- don't do this it messes up the interface

	if (!writeport(fd, sCmd)) {
		printf("write failed\n");
		close(fd);
		return 1;
	}

	printf("written:%s\n", sCmd);
	
 */

void serialGPS::readGPS() {
  int i,count,left,timeout; char c=' ';
  char num[GPSNUMSIZE], *buffer;
  
  do read(fd,&c,1); while (c!='$'); // start of command
  do read(fd,&c,1); while (c!=','); // start of number

  left=GPSNUMSIZE-1; count=0; buffer= &num[0]; *buffer='\0'; timeout=0;
  do {
    count = read(fd,buffer,1); 
    if (count>0 && *buffer==',') break;
    if (count>0 && *buffer!='\0' )  { buffer++; left--; timeout=0; } else timeout++;
  } while (left>0 || timeout>MAXTIMEOUT);
  if (timeout>MAXTIMEOUT || num[0]==',' ) {
    locked=false;
    return;
  }
  *buffer='\0';

  sscanf(num,"%lf",&lastNorth);
  //  printf("SerialGPS: lastNorth=%s, %lf\n",num,lastNorth);
  
  do read(fd,&c,1); while (c!=','); // start of next number

  left=GPSNUMSIZE-1; count=0; buffer= &num[0]; *buffer='\0'; timeout=0;
  do {
    count = read(fd,buffer,1); 
    if (count>0 && *buffer==',') break;
    if (count>0 && *buffer!='\0' )  { buffer++; left--; timeout=0; } else timeout++;
  } while (left>0 || timeout>MAXTIMEOUT);
  if (timeout>MAXTIMEOUT || num[0]==',') {
    locked=false;
    return;
  }
  *buffer='\0';

  sscanf(num,"%lf",&lastWest);
  //printf("SerialGPS: lastWest=%s, %lf\n",num,lastWest);
  locked=true;

}