/* gm.c
 *
 * $Id: gm.c 117 2011-09-08 13:00:54Z csherwood $
 *
 * cleaned up version of gm.c with shared memory use
 *   must be started before ac-mx is, since the shared memory area is 
 *   established here.
 *
 * version of get_modtr_sbgwait_crs.c
 * a server, gets datagram packets from modtronics, 
 * writes IG-20 accellerometer data to shared memory for use in arm control
 * Operates continuously, so all raw data is logged
 * has the beginnings of a crc check implemented
 * uses sbg-systems functions with CRS mods for Moxa to parse the buffer
 *  tested on both Intel and Moxa systems
 * 
 *  The UDP data is currently formatted as:
 *  60 bytes of IG20 data
 *    (subtract 1 for C zero-based index into buf)
 *    bytes 1-5 IG20 syncx, stx, cmd, len
 *    bytes 6-17 IG20 Roll, Pitch, Yaw real 32, 4 bytes each
 *    bytes 18-29 IG20 Ax,Ay,Az real 32, 4 bytes each
 *    bytes 30-37 IG20 T0, T1 real 32, 4 bytes each
 *    bytes 38-41 IG20 time since reset, unit32, 4 bytes total
 *    bytes 42-43 IG20 CRC
 *    byte 44 IG20 ETX 0x03 
 *    byte 45 is the status of the breaker + power
 *    bytes 46-47 is the crc of the IG20 calculated by the Modtronix
 *      They should match bytes 42-43, if not, the IG20 data came corrupted
 *    bytes 48-49 is the A/D channel that will indicate Technadyne current
 *    bytes 50-51 is the A/D channel that will indicate the Technadyne Tachometer output
 *    bytes 52-53 is the control voltage sent to the Technadyne
 *    byte 54 is the status of RC5 (trip indicator): 0 =(not tripped) 1=(tripped) 
 *
 *  replaced tsout() with my_exit() for consistency with ac.c
 *    display_data hard_wired to 1 for the moment.
 *  uses gettimeofday() to have finer time resolution.
 *
 * csherwood@usgs.gov, emontgomery@usgs.gov
 * 16 August 2011
 *
 * $Id: gm.c 117 2011-09-08 13:00:54Z csherwood $
 */

//Define TARGET_MOXA if cross-compiling for the Moxa
#define TARGET_MOXA


#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <unistd.h>
#include <errno.h>
#include <limits.h>
#include <signal.h>
#include <string.h>
#include <time.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>

// Make sure this points to the correct spot
#if defined TARGET_MOXA
// this should point to the xscale version when Moxa is target
#include "/usr/local/xscale_sbgCom/src/sbgCom.h"
#else
#include "/usr/local/include/sbgCom/sbgCom.h"
#endif

// TODO can some of the globals be moved into main?

/* 
 * Must define one method for getting data from Modtronix:
 * DO_FIFO uses a file-based fifo pipe, and works with gmf.c
 * DO_PSHM uses a POSIX shared memory approach and works with gmsm.c
 */
#undef DO_FIFO
#define DO_PSHM

#if defined DO_FIFO
  // TODO - Where is best place to write this file?
  // see caution in TLPI about using a file in /tmp
  /* Well-known name for server's FIFO */
  #include "fifo.h"
  struct modx_data modx_data;
#endif

#if defined DO_PSHM
  #include "pshm.h"
  struct modx_data modx_data;
#endif

static void my_exit(int sig);
int my_output( FILE *fd, char *buf, int display_data );

void parse_modtr(void);
// //uint16 calcCRC_swab( char *pBuffer, unsigned int bufferSize);
// //uint16 calcCRC( char *pBuffer, unsigned int bufferSize);

#define MAXBUFLEN 200
#define MAX_FMT_OUT 255
#define MYPORT 54128
#define MAX_FNAME_SIZE 127


// Global variables
FILE *fdp_log, *fdp_data;
pid_t my_pid;
int display_data=0;


int main(int argc, char *argv[])
{
  int sockfd;
  char datafname[255], logfname[255];
  const char svnId[80]="$Id: gm.c 117 2011-09-08 13:00:54Z csherwood $";
  struct timeval tv;  
  time_t t;
  struct tm gm, *gmp;
  struct sockaddr_in my_addr;
  struct sockaddr_in their_addr;
  socklen_t addr_len, numbytes;  // changed from next line per TLPI p. 1241
  // int addr_len, numbytes;
  int ier=0;
  int cvolts=0;		//remove this when this is in modtr data!

#if defined DO_FIFO 
 // Needed for fifo data exchange
  int serverFd;
#endif
#if defined DO_PSHM
  // Needed for posix shared memory exchange
  int fd_pshm;
  int oflags;
  mode_t perms;
  size_t modx_data_size;
  void *addr;
  int sval;
  sem_t *sem;
  struct timespec abs_timeout;
#endif

  //should this be char or unsigned char?
  char buf[ MAXBUFLEN ];
  char *pbuf, *pBuffer;
  char fmt_out[MAX_FMT_OUT];

  // these are needed by the sbg function
  uint8 targetOutputMode;
  uint32 outputMask; 
  uint16 bufferSize;
  uint16 ibreaker;
  uint16 istatus;
  uint16 ad_current, ad_tach;
  SbgOutput outputStructure, *outPtr;
  unsigned int usecs=100;
  int s;

  signal(SIGINT,my_exit);  /* terminal interuppt (e.g., ctrl-C) */
  signal(SIGTERM,my_exit); /* terminate process */
  
  t=time(NULL);
  gmp = gmtime(&t);
  gm = *gmp;
  my_pid = getpid();
 
  // open a log file with a name that includes the PID
  snprintf(logfname,MAX_FNAME_SIZE,
	  "/var/sda/logs/modtr_%04d%02d%02d%02d%02d%02d_%06d.log",
	  1900+gm.tm_year,gm.tm_mon+1,gm.tm_mday,gm.tm_hour,gm.tm_min,gm.tm_sec,(int)my_pid);
  fdp_log = fopen(logfname,"wt");
  if(fdp_log == NULL)
    {
      // Send this message to a better place? stdout and stderr?
      snprintf(buf,sizeof(buf),"Problem opening log file");
      ier = my_output( fdp_log, buf, display_data);
      my_exit(SIGUSR1);
    }
  // repeat for the data file with a name that includes the PID
  snprintf(datafname,MAX_FNAME_SIZE,
	   "/var/sda/data/modtr/modtr_%04d%02d%02d%02d%02d%02d_%06d.dat",
	   1900+gm.tm_year,gm.tm_mon+1,gm.tm_mday,gm.tm_hour,gm.tm_min,gm.tm_sec,(int)my_pid);
  fdp_data = fopen(datafname,"wt");
  if(fdp_data == NULL)
    {
      // Send this message to a better place? stdout and stderr?
      snprintf(buf,sizeof(buf),"Problem opening data file");
      ier = my_output( fdp_log, buf, display_data);
      my_exit(SIGUSR1);
    }
  snprintf(buf,sizeof(buf), "%s",svnId);
  ier = my_output( fdp_log, buf, display_data);
   
  //be sure buf stay under 200 chars
  snprintf(buf,sizeof(buf), "Using port : %d",MYPORT);
   ier = my_output( fdp_log, buf, display_data);
  snprintf(buf,sizeof(buf),"usecs to sleep= %d",usecs);
   ier = my_output( fdp_log, buf, display_data);
  snprintf(buf,sizeof(buf),"sizeof int: %d",sizeof(int));
   ier = my_output( fdp_log, buf, display_data);
  snprintf(buf,sizeof(buf),"sizeof short: %d",sizeof(short));
   ier = my_output( fdp_log, buf, display_data);
  snprintf(buf,sizeof(buf),"sizeof uint8 targetOutputMode: %d",sizeof( targetOutputMode ));
   ier = my_output( fdp_log, buf, display_data);
  snprintf(buf,sizeof(buf),"sizeof uint16 bufferSize:      %d",sizeof( bufferSize ));
   ier = my_output( fdp_log, buf, display_data);
  snprintf(buf,sizeof(buf),"sizeof uint32 outputMask:      %d",sizeof( outputMask ));
   ier = my_output( fdp_log, buf, display_data);
  snprintf(buf,sizeof(buf),"sizeof outputStructure:        %d",sizeof( outputStructure ));
   ier = my_output( fdp_log, buf, display_data);
  snprintf(buf,sizeof(buf),"sizeof pbuf:                   %d",sizeof( pbuf ));
   ier = my_output( fdp_log, buf, display_data);
  snprintf(buf,sizeof(buf),"AF_INET: %d",AF_INET);
   ier = my_output( fdp_log, buf, display_data);
 
  /* note SOCK_DGRAM is used for UDP */
  if((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) == -1)
    {
      perror("Server-socket() sockfd error.");
      exit(1);
    }
  else
    snprintf(buf,sizeof(buf),"Server-socket() sockfd is OK...");
    ier = my_output( fdp_log, buf, display_data);

  memset( &my_addr,0,sizeof(struct sockaddr)); //TLPI p. 1208
  /* host byte order */
  my_addr.sin_family = AF_INET;
  /* short, network byte order */
  my_addr.sin_port = htons(MYPORT);
  /* INADDR_ANY should accept connections on my_port from any client */
  my_addr.sin_addr.s_addr = INADDR_ANY;

  /* zero the rest of the struct */ 
  memset(&(my_addr.sin_zero), '\0', 8);

  if(bind(sockfd, (struct sockaddr *)&my_addr, sizeof(struct sockaddr)) == -1)
    {
      perror("Server-bind() error.");
      exit(1);
    }
   snprintf(buf,sizeof(buf),"Server-bind() is OK...");
    ier = my_output( fdp_log, buf, display_data);
   snprintf(buf,sizeof(buf),"My address is %s",inet_ntoa(my_addr.sin_addr));     
    ier = my_output( fdp_log, buf, display_data);
  
  addr_len = sizeof(struct sockaddr);
    
  //use sbg's functions here to create the pOutput structure
  //targetOutputMode says whether bigendian or not
  // Intel = LittleEndian
  // Moxa = BigEndian
 
  //..but this works on both Ubuntu and Moxa w/ CRS changes to sbgFillOutputFromBuffer
  targetOutputMode= SBG_OUTPUT_MODE_BIG_ENDIAN | SBG_OUTPUT_MODE_FLOAT;
  //targetOutputMode=SBG_OUTPUT_MODE_LITTLE_ENDIAN | SBG_OUTPUT_MODE_FLOAT;
      
  /* output mask says which data is included names and hex values below
     SBG_OUTPUT_TIME_SINCE_RESET 00000800
     SBG_OUTPUT_QUATERNION       00000001 (not output)
     SBG_OUTPUT_EULER 		 00000002
     SBG_OUTPUT_ACCELEROMETERS   00000010 			  
     SBG_OUTPUT_TEMPERATURES     00000040
     mask		       0x00000852
     */
  outputMask = SBG_OUTPUT_TIME_SINCE_RESET |
    SBG_OUTPUT_EULER |
    SBG_OUTPUT_ACCELEROMETERS |
    SBG_OUTPUT_TEMPERATURES;

  //  outputMask = SBG_OUTPUT_TIME_SINCE_RESET;
  snprintf(buf,sizeof(buf),"outputMask is %x",outputMask);
    ier = my_output( fdp_log, buf, display_data);

  // pOutput is the output structure we want to print from
  // allocate space for a pointer to it
  outPtr=malloc(sizeof(SbgOutput));
  outPtr=&outputStructure;

#if defined DO_PSHM
  // Initialize semaphore and shared memory
  // because these are named semaphores, you DON'T use sem_init()
  oflags = (O_RDWR | O_CREAT);
  perms = (S_IRUSR | S_IWUSR);
  sem = sem_open(SEM_NAME,oflags,perms,1);
  if(sem==SEM_FAILED){
    snprintf(buf,sizeof(buf),"sem_open error");
     ier = my_output( fdp_log, buf, display_data);
    exit(-1);
  }
  sem_getvalue(sem, &sval);
  snprintf(buf,sizeof(buf),"Initial value of sval: %d",sval);
    ier = my_output( fdp_log, buf, display_data);
   /* Make sure it is >0 so sem_timedwait below will not block */
  // TODO handle values other than 0 and 1?
  if(sval==0){
    sem_post( sem );
  }
  
  /* Create shared memory object and set its size */
  fd_pshm = shm_open(SHMEM_NAME, oflags, perms );
  if (fd_pshm == -1){
    snprintf(buf,sizeof(buf),"shm_open error");
      ier = my_output( fdp_log, buf, display_data);
    exit(-1);
  }
  modx_data_size = sizeof( modx_data );
  if (ftruncate(fd_pshm, modx_data_size) == -1) {
    snprintf(buf,sizeof(buf),"ftruncate error");
      ier = my_output( fdp_log, buf, display_data);
    exit(-1);
  }
  
  /* Map shared memory object */    
  addr = mmap(NULL, modx_data_size, 
	      PROT_READ|PROT_WRITE, MAP_SHARED, fd_pshm, 0);
  if (addr == MAP_FAILED) {
    snprintf(buf,sizeof(buf),"mmap error");
      ier = my_output( fdp_log, buf, display_data);
   exit(-1);
  }
#endif // End of DO_PSHM code

 
   snprintf(buf,sizeof(buf),"Waiting for data...");
    ier = my_output( fdp_log, buf, display_data);
   while (1)
    {
      ier=gettimeofday(&tv,NULL);
      numbytes = recvfrom(sockfd, buf, MAXBUFLEN, 0, (struct sockaddr *)&their_addr, &addr_len );
      //printf("got numbytes= %d\n",numbytes);
      if(numbytes<0)
	{
	  perror("Server-recvfrom() error.");
	  /*If something wrong, just exit...*/
	  exit(1);
	}
     if(numbytes<MAXBUFLEN)
	{
	  buf[numbytes] = '\0';
	}else
	{
	  snprintf(buf,sizeof(buf),"numbytes too big: %d",numbytes);
	    ier = my_output( fdp_log, buf, display_data);
      exit(-1);
	}
    if(numbytes>20){ //kludge to keep only good data     
      // *pBuffer is the data from ig20 via Modtronics
      pBuffer=&buf[5];
      //pbuf=&buf[2];	//for CRC use- trying to point to the same area modtronics compute over

      // bufferSize is 5 bytes shorter than UDP datagram
      bufferSize=((uint16)numbytes) - 5;
   
      sbgFillOutputFromBuffer(targetOutputMode,  outputMask, (void*) pBuffer,
			      bufferSize, (SbgOutput*) &outputStructure);
      //debug Marinna's extra bytes
      // printf(" %d %d %d %d \n",buf[47],buf[48],buf[49],buf[50]);
      // printf(" %d %d %d %d \n",(char)buf[47],(char)buf[48],(char)buf[49],(char)buf[50]);
      // printf(" %d %d %d %d \n",(uint8)buf[47],(uint8)buf[48],(uint8)buf[49],(uint8)buf[50]);
      // printf(" %02x %02x    %02x %02x    %02x\n",
      //     (unsigned char)buf[47],(unsigned char)buf[48],
      //     (unsigned char)buf[49],(unsigned char)buf[50],
      //     (unsigned char)buf[51]);

      // parse Marinna's extra bytes
      istatus = (uint16)(uint8)(buf[44]);
      ad_current = 256*(uint16)(uint8)(buf[47])+(uint16)(uint8)(buf[48]);
      ad_tach = 256*(uint16)(uint8)(buf[49])+(uint16)(uint8)(buf[50]);
      cvolts = (uint16)(uint8)(buf[51]);
      ibreaker = (uint16)(uint8)(buf[52]);
      //printf("status, current, tach, breaker: %d %d %d %d\n",
      //  istatus,ad_current,ad_tach,ibreaker);

      // compare CRCs from IG-20 and Modtronix
      if( (buf[41]==buf[45]) & (buf[42]==buf[46]) )
	{
	  // only output if the time is a positive number (redundant?)
	  if ((outPtr->timeSinceReset) > 0)
	    {
	      // Fill data structure
	      modx_data.tv = tv;
	      modx_data.reset_time = outPtr->timeSinceReset;
	      modx_data.accel[0]=(outPtr->accelerometers[0]);
	      modx_data.accel[1]=(outPtr->accelerometers[1]);
	      modx_data.accel[2]=(outPtr->accelerometers[2]);
	      modx_data.angle[0]=SBG_RAD_TO_DEG(outPtr->stateEuler[0]);
	      modx_data.angle[1]=SBG_RAD_TO_DEG(outPtr->stateEuler[1]);
	      modx_data.istatus = istatus;
	      modx_data.ad_current = ad_current;
	      modx_data.ad_tach = ad_tach;
	      modx_data.cvolts = cvolts;
	      modx_data.ibreaker = ibreaker; 

#if defined DO_FIFO
	      // Put in FIFO pipe
	      serverFd = open(SERVER_FIFO, O_WRONLY);
	      if (serverFd == -1){
		//TODO put in proper error handling
		printf("Can't open %s\n", SERVER_FIFO);
		exit(-1);
	      }
	      if (write(serverFd, &modx_data, sizeof(struct modx_data)) !=
		  sizeof(struct modx_data))
		     printf("Can't write to server\n");

	      close(serverFd);
#endif // end of FIFO code

#if defined DO_PSHM
	      // Only mess with memory if semaphore is zero
	      //TODO - change this to sem_timedwait(); TLPI p. 1096
	        clock_gettime( CLOCK_REALTIME, &abs_timeout );
            abs_timeout.tv_sec+=0;                  // seconds
            abs_timeout.tv_nsec+=100000;            // about 1/10 sec
            ier=sem_timedwait( sem, &abs_timeout ); // blocks until sem>0, then decrements to zero

	      //sem_wait( sem ); // blocks until sem>0, then decrements to zero
	      // update shared memory here
	      memcpy(addr, &modx_data, modx_data_size);
	      sem_post( sem ); // unblocks
#endif // end of PHSH code


       // format data string here	     
	     snprintf(fmt_out,MAX_FMT_OUT," %08d %6.2f %6.2f %10.6f %10.6f %10.6f %5.2f %5.2f %02d %04d %04d %04d %02d",
		       (outPtr->timeSinceReset),	     
		       SBG_RAD_TO_DEG(outPtr->stateEuler[0]),
		       SBG_RAD_TO_DEG(outPtr->stateEuler[1]),
		       (outPtr->accelerometers[0]),
		       (outPtr->accelerometers[1]),
		       (outPtr->accelerometers[2]),
		       (outPtr->temperatures[0]),
		       (outPtr->temperatures[1]),
		       istatus,
		       (unsigned int)ad_current,
		       (unsigned int)ad_tach,
		       (unsigned int)cvolts,
		       ibreaker);
	      
	      ier = my_output( fdp_data, fmt_out, display_data );

	      s=usleep(usecs);
	      if (s==-1 && errno != EINTR)
		{
		  perror("usleep failed\n");
		  exit(1);
		}        
	  }
	  else {
	    snprintf(buf,sizeof(buf),"time since reset: %d",outPtr->timeSinceReset);
	    ier = my_output( fdp_log, buf, display_data);
      }
	}
      //else
      //	{
      //	  printf("CRCs dont match\n");
      //	}
      }
    }    //end while or for
    
  if(close(sockfd) != 0) {
    snprintf(buf,sizeof(buf),"Server-sockfd closing failed!");
    ier = my_output( fdp_log, buf, display_data);
  } else {
    snprintf(buf,sizeof(buf),"Server-sockfd successfully closed!");
    ier = my_output( fdp_log, buf, display_data);
   } 

  // stop logging
  fclose(fdp_log);
  fclose(fdp_data);

  return 0;
}

// MY_EXIT - Clean up, close files, write to log, exit
static void my_exit(int sig)
{
  int ier;
  char tmp[100];
  // sig is assigned by signal or passed by user
  // UNSAFE use of printf() flush(), close(), and exit()
  // TODO - Do this with async-safe calls?
  // TODO - send these to the log
  
  // (1) identify cause of exit and close log file
  // seems like with the sem related variables being local not global 
  // we don't need to munmap here
  switch (sig){
  case SIGINT : {
    snprintf(tmp,sizeof(tmp), " gm-mx caught SIGINT, exiting.");
    ier = my_output( fdp_log, tmp, display_data );
    break;
  }
  case SIGTERM : {
    snprintf(tmp,sizeof(tmp), " gm-mx caught SIGTERM, exiting.");
    ier = my_output( fdp_log, tmp, display_data );
    break;
  }
  case SIGUSR1 : {
    snprintf(tmp,sizeof(tmp)," gm-mx caught SIGUSR1, exiting.");
    ier = my_output( fdp_log, tmp, display_data );
    break;
  }
  default : {
    snprintf(tmp,sizeof(tmp)," gm-mx: not sure why here, but exiting.");
    ier = my_output( fdp_log, tmp, display_data );
    break;
  }
  }
  fclose(fdp_log);
  fclose(fdp_data);
  fflush(NULL);
  exit(0);
}    


#if defined NOTDEF
// USGS crc calc for IG20 from IG20 docs
// try this cast as BYTE
// checked this by outputting the calculated CRC on UDP and it matched the crc in the IG20 packet
uint16 calcCRC(char *pBuffer, unsigned int bufferSize) 
{
	uint16 poly = 0x8408; // 16 bit int
	uint16 crc = 0;
	uint8 carry; // 8 bit
	uint8 i_bits;
	uint16 j;

    printf ("buffersize is %d\n",bufferSize);
	for (j=0; j<bufferSize; j++)
	{
	    printf("pbuf[%d]= %02x\n",j,(unsigned char)pBuffer[j]);
		crc = crc^pBuffer[j];
		for (i_bits=0; i_bits<8; i_bits++)
		{
			carry = crc & 1;
			crc = crc / 2;
			if (carry) 
			{
				crc = crc^poly;
			}
		}
	}
	return crc;
}
// USGS crc calc for IG20 from IG20 docs
// try this cast as BYTE
// checked this by outputting the calculated CRC on UDP and it matched the crc in the IG20 packet
uint16 calcCRC_swab(char *pBuffer, unsigned int bufferSize) 
{
	uint16 poly = 0x8408; // 16 bit int
	uint16 crc = 0;
	uint8 carry; // 8 bit
	uint8 i_bits;
	uint16 j;

    printf ("buffersize is %d\n",bufferSize);
	for (j=1; j<=bufferSize; j+=2)
	{
	    printf("pbuf[%d]= %02x\n",j,(unsigned char)pBuffer[j]);
		crc = crc^pBuffer[j];
		for (i_bits=0; i_bits<8; i_bits++)
		{
			carry = crc & 1;
			crc = crc / 2;
			if (carry) 
			{
				crc = crc^poly;
			}
		}
	   printf("pbuf[%d]= %02x\n",j-1,(unsigned char)pBuffer[j-1]);
	   crc = crc^pBuffer[j-1];
		for (i_bits=0; i_bits<8; i_bits++)
		{
			carry = crc & 1;
			crc = crc / 2;
			if (carry) 
			{
				crc = crc^poly;
			}
		}

	}
	return crc;
}
#endif

int my_output( FILE *fd, char *buf, int disp_data )
{
  // Prepend time stamp, print contents of buf, add CR
  struct tm gm, *gmp;
  struct timeval tv;
  int iw=1, tstat, nlen=0;
  char tbuf[255];
   
  tstat=gettimeofday(&tv,NULL);
  //gmp = gmtime(&t);
  // pointing at the tv seconds gets the time from gettimeofday, so usecs will be synched
  gmp = gmtime(&tv.tv_sec);   
  gm = *gmp; // save local copy...see TLPI p. 192
 
  snprintf(tbuf,29,"%4d %02d %02d %02d %02d %10.7f ",
 	  1900+gm.tm_year,gm.tm_mon+1,gm.tm_mday,gm.tm_hour,gm.tm_min,(gm.tm_sec+((double)tv.tv_usec/1000000)));

  //check that concatenated string isn't too big
  nlen=strlen(buf);
  if ((strlen(tbuf) + strlen(buf)) < sizeof(tbuf)-1)
  { 
   strncat(tbuf,buf,nlen);
   strncat(tbuf,"\n",1);
    if (disp_data ==1)				//display data and write to file
    {
     iw = fprintf(stdout,"%s",tbuf);
     iw = fprintf(fd,"%s",tbuf);
    } else {
     iw = fprintf(fd,"%s",tbuf);	//just write to the file
    }
   fflush(fd);
  }
  else
  {
    printf("string too long- look for error in data!\n");
    //email manager that something's wrong
     iw=-1;
  }
  return(iw);
}