#include <stdlib.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h> // IO Control of socket.
#include <fcntl.h>     // IO Control of socket.
#include <unistd.h> // close
#include <time.h>
#include <sys/time.h>
#include <signal.h>
#include <netinet/in.h>
#include <errno.h>
#include <stdio.h>
#include <string.h>
#include <math.h>

#define PORT (8080)
#define MAXLINE (1024)

struct shipData {
  float zdaSec;
  int zdaDay, zdaMonth, zdaYear, zdaHour, zdaMin;
  time_t zdaTime;
  int ggaHour, ggaMin; float ggaSec;
  double lat, lon;
  int numSats;
  int qual;
  float alt;
  char aunits; //altitude units
  double hvel, hdop;
  float cog, sog;
  long headingTime;
  double heading;
};

struct rovData {
  int hour, min, sec;
  float fixTime;
  double lat,lon, depth,heading;
};

void parseShipNMEA(char *buf, struct shipData *s)
{
  char *p = buf, *ptr;
  int deg; double min;
  struct tm t;
  ptr = strtok(buf,",");
  if (!strncmp(ptr, "$GPZDA", 6)) {
    if (ptr = strtok(NULL, ",")){
      sscanf(ptr,"%2d%2d%f", &s->zdaHour, &s->zdaMin, &s->zdaSec);
    }
    if (ptr = strtok(NULL, ",")) s->zdaDay = atoi(ptr);
    if (ptr = strtok(NULL, ",")) s->zdaMonth = atoi(ptr);
    if (ptr = strtok(NULL, ",")) s->zdaYear = atoi(ptr);
    memset(&t, 0, sizeof(t));
    t.tm_year  = s->zdaYear-1900;
    t.tm_mon = s->zdaMonth-1;
    t.tm_mday = s->zdaDay;
    t.tm_hour = s->zdaHour;
    t.tm_min = s->zdaMin;
    t.tm_sec = s->zdaSec;
    t.tm_isdst = 0;
    s->zdaTime = mktime(&t);
    printf("zda:%d system:%d\n", s->zdaTime, time(NULL));
  } else if (!strncmp(ptr, "$GPGGA", 6)) {
    if (ptr = strtok(NULL, ",")) {
      sscanf(ptr,"%2d%2d%2.2f", &s->ggaHour, &s->ggaMin, &s->ggaSec);
    }
    if (ptr = strtok(NULL, ",")) {
      sscanf(ptr,"%2d%lf",&deg, &min);
      s->lat = (double)deg + min/(double)60.0;
    }
    if (ptr = strtok(NULL, ",")) {
      if (*ptr == 'S') s->lat *= -1.0;
    }
    if (ptr = strtok(NULL, ",")) {
      sscanf(ptr,"%3d%lf",&deg, &min);
      s->lon = (double)deg + min/(double)60.0;
    }
    if (ptr = strtok(NULL, ",")) {
      if (*ptr == 'W') s->lon *= -1.0;
    }
    if (ptr = strtok(NULL,",")) {
      s->qual = atoi(ptr);
    }
    if (ptr = strtok(NULL,",")) {
      s->numSats = atoi(ptr);
    }
    if (ptr = strtok(NULL,",")) {
      s->hdop = atof(ptr);
    }
    if (ptr = strtok(NULL,",")) {
      s->alt  = atof(ptr);
    }
    if (ptr = strtok(NULL,",")) {
      s->aunits = *ptr;
    }
  } else if (!strncmp(ptr, "$GPVTG", 6)) {
    if (ptr = strtok(NULL,",")) {
      s->cog = atof(ptr);
    }
    if (ptr = strtok(NULL,",")) {
    } //should be T for true heading
    if (ptr = strtok(NULL,",")) {
    } //cog in magnetic heading
    if (ptr = strtok(NULL,",")) {
      s->sog = atof(ptr); //speed over ground in kph
    }
  } else if (!strncmp(ptr, "$GPHDT", 6)) {
    if (ptr = strtok(NULL, ",")) s->heading = atof(ptr);
  }
}

void parseROVData(char *buf, struct rovData *r)
{
  char *p = buf, *ptr;
  int deg; double min;
  struct tm t;
  time_t now;

  ptr = strtok(buf,",");
  if (!strncmp(ptr, "$GPGGA", 6)) {
    if (ptr = strtok(NULL, ",")) {
      sscanf(ptr,"%2d%2d%2.2f", &r->hour, &r->min, &r->sec);
      now = time(NULL);
      r->fixTime = (now-now%86400) + r->hour*3600 + r->min*60 + r->sec;
    }
    if (ptr = strtok(NULL, ",")) {
      sscanf(ptr,"%2d%lf",&deg, &min);
      r->lat = (double)deg + min/(double)60.0;
    }
    if (ptr = strtok(NULL, ",")) {
      if (*ptr == 'S') r->lat *= -1.0;
    }
    if (ptr = strtok(NULL, ",")) {
      sscanf(ptr,"%3d%lf",&deg, &min);
      r->lon = (double)deg + min/(double)60.0;
    }
    if (ptr = strtok(NULL, ",")) {
      if (*ptr == 'W') r->lon *= -1.0;
    }
    if (ptr = strtok(NULL,",")) {
    }
    if (ptr = strtok(NULL,",")) {
    }
    if (ptr = strtok(NULL,",")) {
    }
    if (ptr = strtok(NULL,",")) {
      r->depth  = atof(ptr);
    }
  }
}

int createUDPListenSocket(int port, int rcvtimeout)
{
  int fd;
  struct sockaddr_in clientaddr;
  struct timeval tv;

  fd = socket(AF_INET, SOCK_DGRAM, 0);
  tv.tv_sec = rcvtimeout;
  setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO,
	     (struct timeval *)&tv,sizeof(struct timeval));

  clientaddr.sin_family    = AF_INET; // IPv4 
  clientaddr.sin_addr.s_addr = INADDR_ANY; 
  clientaddr.sin_port = htons(port); 

  if ( bind(fd, (const struct sockaddr *)&clientaddr,  
            sizeof(clientaddr)) < 0 ) 
    { 
        perror("bind failed"); 
        exit(EXIT_FAILURE); 
    } 

  return fd;
}

main()
{
struct sockaddr_in servaddr, cliaddr; 
int serverfd, shipfd, rovfd;
fd_set active_fd_set, read_fd_set;
struct shipData ship;
struct rovData rov;
char arcnav_buffer[MAXLINE], ship_buffer[MAXLINE], rov_buffer[MAXLINE];
char *headerString =
"LOGHOST.SYSTEM.UTC,SHIP.GPS.TIME,ROV.PRESSURE,ROV.CTD.PRESSURE,"
"SHIP.GPS.LAT,SHIP.GPS.LON,SHIP.GPS.DIFFERENTIAL,"
"SHIP.GPS.HORIZONTAL_VELOCITY,SHIP.GPS.HDOP,SHIP.GYRO.DEGREES.UTC,"
"SHIP.GYRO.DEGREES,SHIP.WIND.TRUE_SPEED.UTC,SHIP.WIND.TRUE_SPEED,"
"SHIP.WIND.TRUE_DIRECTION,ROV.POSITION.LAT.UTC,ROV.POSITION.LAT,"
"ROV.POSITION.LON,ROV.MBARI.DEPTH,ROV.HEADING,ROV.PITCH,"
"ROV.ROLL,ROV.ALTITUDE,LOGHOST.SYSTEM.CLOCKTIME.YEAR,"
"LOGHOST.SYSTEM.CLOCKTIME.YEARDAY,LOGHOST.SYSTEM.CLOCKTIME.TIMESTRING,"
"LOGHOST.SYSTEM.CLOCKTIME.TIMEZONE";
char dataString[MAXLINE];

    shipfd = createUDPListenSocket(38002, 10);
    rovfd = createUDPListenSocket(20289, 10);

    // Creating socket file descriptor 
    if ( (serverfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0 ) { 
        perror("socket creation failed"); 
        exit(EXIT_FAILURE); 
    }

    // Filling server information 
    servaddr.sin_family    = AF_INET; // IPv4 
    servaddr.sin_addr.s_addr = INADDR_ANY; 
    servaddr.sin_port = htons(PORT); 
      
    // Bind the socket with the server address 
    if ( bind(serverfd, (const struct sockaddr *)&servaddr,  
            sizeof(servaddr)) < 0 ) 
    { 
        perror("bind failed"); 
        exit(EXIT_FAILURE); 
    } 

    memset(&ship, 0, sizeof(ship));
    memset(&rov, 0, sizeof(ship));

    FD_ZERO(&active_fd_set);
    FD_ZERO(&read_fd_set);
    FD_SET(serverfd, &active_fd_set);
    FD_SET(shipfd, &active_fd_set);
    FD_SET(rovfd, &active_fd_set);

    while (1) {
      memcpy(&read_fd_set, &active_fd_set, sizeof(fd_set));

      if (select(10, &read_fd_set, NULL, NULL, NULL) < 0) {
	perror("select");
	exit(-1);
      }

      if (FD_ISSET(serverfd, &read_fd_set)) {
	int len = sizeof(cliaddr), n; 
        memset(arcnav_buffer, 0, MAXLINE);
	n = recvfrom(serverfd, (char *)arcnav_buffer, MAXLINE,  
		     MSG_DONTWAIT, ( struct sockaddr *) &cliaddr, 
		     &len); 
	printf("Received message from client %d\n", n);fflush(stdout);

	if (!strncmp(arcnav_buffer, "HEADER", 6)) {
	  printf("sending header to client\n"); fflush(stdout);
	  sendto(serverfd, headerString, strlen(headerString), 0,
		 (const struct sockaddr *) &cliaddr, 
		 len); 
	} else {
	  memset(dataString, 0, MAXLINE);
	  char *ptr = dataString;
	  ptr += sprintf(ptr,"%d,", time(NULL)); //LOGHOST.SYSTEM.UTC
	  ptr += sprintf(ptr,"%d,", ship.zdaTime);
	  ptr += sprintf(ptr,"NO_ITEM,");
	  ptr += sprintf(ptr,"NO_ITEM,");
	  ptr += sprintf(ptr,"%f,",ship.lat);
	  ptr += sprintf(ptr,"%f,",ship.lon);
	  ptr += sprintf(ptr,"%d,", ship.qual);
	  ptr += sprintf(ptr,"%f,", ship.sog);
	  ptr += sprintf(ptr,"%f,", ship.hdop);
	  ptr += sprintf(ptr,"%d,", ship.zdaTime);
	  ptr += sprintf(ptr,"%f,", ship.heading);
	  ptr += sprintf(ptr,"NO_ITEM,");
	  ptr += sprintf(ptr,"NO_ITEM,");
	  ptr += sprintf(ptr,"NO_ITEM,");
	  ptr += sprintf(ptr,"%f,",rov.fixTime);
	  ptr += sprintf(ptr,"%f,",rov.lat);
	  ptr += sprintf(ptr,"%f,",rov.lon);
	  ptr += sprintf(ptr,"%f,",rov.depth);
	  ptr += sprintf(ptr,"NO_ITEM,");
	  ptr += sprintf(ptr,"NO_ITEM,");
	  ptr += sprintf(ptr,"NO_ITEM,");
	  ptr += sprintf(ptr,"NO_ITEM,");
          time_t now_t = time(NULL);
	  struct tm *now = gmtime(&now_t);
	  ptr += sprintf(ptr,"%d,",now->tm_year+1900);
	  ptr += sprintf(ptr,"%d,",now->tm_yday);
	  char *timestr = asctime(now);
	  timestr[strlen(timestr)-1] = 0x00; //remove \n
	  ptr += sprintf(ptr,"%s,+0\0", timestr);
	  printf("%s\n", dataString);
	  sendto(serverfd, dataString, strlen(dataString), 0,
		 (const struct sockaddr *) &cliaddr, 
		 len); 
	}
      }

      //handle ship data
      if (FD_ISSET(shipfd, &read_fd_set)) {
	int n;
        memset(ship_buffer, 0, MAXLINE);
	n = recvfrom(shipfd, (char *)ship_buffer,MAXLINE,
		     MSG_DONTWAIT, NULL, NULL);
	//	write(0, ship_buffer, n); fflush(stdout);
        parseShipNMEA(ship_buffer, &ship);
      }

      //handle ROV data
      if (FD_ISSET(rovfd, &read_fd_set)) {
	int n;
        memset(rov_buffer, 0, MAXLINE);
	n = recvfrom(rovfd, (char *)rov_buffer,MAXLINE,
		     MSG_DONTWAIT, NULL, NULL);
	write(0, rov_buffer, n); fflush(stdout);
        parseROVData(rov_buffer, &rov);
      }

    }
      
    return 0;
}