#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <time.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/param.h>
#include <sys/stat.h>
#include <signal.h>
#include "adc.h"


/* Test Program Defaults */
#define DEF_PORT_STR		"/dev/ttyS0"
#define DEF_PORT_SPEED		57600  /* 57600, 38400, 19200 or 9600 */
#define DEF_TIME_REF		TIME_REF_GARMIN	/* Time reference type */
#define DEF_SAMPLE_RATE		100
#define DEF_NUMBER_CHANNELS	3
#define DEF_HIGH_TO_LOW_PPS	FALSE	/* GPS PPS direction */
#define DEF_NO_LED_STATUS	FALSE	/* Blink LED On */
#define DEF_12BIT_FLAGS		0	/* Bitmap of ADC channels that should be sending 12 bit data */
#define DEF_TIME_OFFSET		0	/* ~30 when using WWV time reference */

/* Configuration information sent to the DLL and ADC board */
AdcBoardConfig config;

/* Incoming ADC sample data is demuxed into this array. */
int32_t demuxData[MAX_ADC_CHANNELS][MAX_SPS_RATE];

HANDLE hBoard = 0;		/* Handle to the DLL/ADC board */
int displayBoardType = TRUE;	/* Used to display the board type */
char commPortStr[32];		/* Linux Comm Port string */

#define FREQUENCY DEF_SAMPLE_RATE

void sighandler(int x)
{
}

#define FILTERSIZE 16

static float x[FILTERSIZE+1];
static float y[FILTERSIZE+1];

/* Designed with http://www-users.cs.york.ac.uk/~fisher/mkfilter/trad.html */
float filterloop(float input)
{
  float *xv = x + FILTERSIZE;
  float *yv = y + FILTERSIZE;

  memmove(x, x+1, sizeof(float)*FILTERSIZE);
  memmove(y, y+1, sizeof(float)*FILTERSIZE);

  /* 10 Hz */
  xv -= 6;
  yv -= 6;
  xv[6] = input / 2936.532839F;
  yv[6] = (xv[0] + xv[6]) + 6 * (xv[1] + xv[5]) + 15 * (xv[2] + xv[4])
    + 20 * xv[3]
    + ( -0.0837564796F * yv[0]) + (  0.7052741145F * yv[1])
    + ( -2.5294949058F * yv[2]) + (  4.9654152288F * yv[3])
    + ( -5.6586671659F * yv[4]) + (  3.5794347983F * yv[5]);
  return yv[6];
}

void lowpass(int16_t *input, int16_t *output, int n)
{
  int i;
  for (i = 0; i < n; i++) {
    float f = filterloop((float)input[i]);
    if (f > 32767) f = 32767;
    if (f < -32767) f = -32767;
    output[i] = (int16_t)f;
  }
}

void usage(const char **argv)
{
  printf("Usage: %s [-d] [log interval] [output file] [input device] [raw output file]\n", argv[0]);
  printf("       -d = deamon mode\n");
  printf("       Default values:\n");
  printf("           log interval: 60 (seconds)\n");
  printf("           output file:  stdout\n");
  printf("           input device: stdin\n");
}

void printlog(FILE *output, time_t ts, int16_t maxN, int16_t minN,
	      int16_t maxE, int16_t minE, int16_t maxZ, int16_t minZ)
{
  char timestamp[15];
  struct tm* scratch_time = gmtime(&ts);

  strftime(timestamp, sizeof(timestamp), "%Y%m%d%H%M%S", scratch_time);
  fprintf(output, "%s\t%d\t%d\t%d\t%d\t%d\t%d\n", timestamp,
	  (int)maxN, (int)minN, (int)maxE, (int)minE, (int)maxZ, (int)minZ);
}

/* Log raw data: 16 bit big endian.  0x8000 marks that a 32 bit big endian
   timestamp (seconds since 1970-01-01 00:00:00 UTC) follows */
void lograw(FILE *output, int16_t *log, unsigned int n)
{
  unsigned int i = 0;
  uint32_t t = (uint32_t)time(0) - n / FREQUENCY;

  fputc(0x80, output);
  fputc(0x00, output);
  fputc((t >> 24) & 0xff, output);
  fputc((t >> 16) & 0xff, output);
  fputc((t >>  8) & 0xff, output);
  fputc((t >>  0) & 0xff, output);
  while (i < n) {
    int16_t v = log[i] == -32768 ? -32767 : log[i];
    fputc((v >> 8) & 0xff, output);
    fputc((v >> 0) & 0xff, output);
    i++;
  }
}

void process(const int16_t *log, unsigned int i, int16_t *max, int16_t *min)
{
  *max = -32768;
  *min = 32767;

  while (i--) {
    if (*max < log[i]) *max = log[i];
    if (*min > log[i]) *min = log[i];
  }
}

static int16_t *logN, *filteredN;
static int16_t *logE, *filteredE;
static int16_t *logZ, *filteredZ;
static FILE *output;
static FILE *rawoutputN = 0;
static FILE *rawoutputE = 0;
static FILE *rawoutputZ = 0;
static int interval;
static int gargc;
static const char** gargv;
static time_t logtime;
static int logindex;



int main(int argc, const char** argv)
{
  const char **cmdline = argv;
  int forked = 0;
  int i = 0;
  uint32_t version, sts;

  for (i = 0; i <= FILTERSIZE; i++)
    x[i] = y[i] = 0.0;

  while (argc > 1 && argv[1] && *argv[1] == '-') {
    switch (argv[1][1]) {
    case 'd':
      if (!forked) {
	forked = 1;
	umask(0);
	setpgrp();
	if (fork())
	  return 0;
      }
      break;
    default:
      printf("%s: Unknown option -%c\n", cmdline[0], argv[1][1]);
      usage(cmdline);
      return -1;
    }
    argc--;
    argv++;
  }

  interval = argc > 1 && argv[1] ? atoi(argv[1]) : 60;
  logtime = time(NULL);
  logindex = 0;

  logN = (int16_t*)malloc(FREQUENCY * interval * sizeof(int16_t) + sizeof(int16_t));
  logE = (int16_t*)malloc(FREQUENCY * interval * sizeof(int16_t) + sizeof(int16_t));
  logZ = (int16_t*)malloc(FREQUENCY * interval * sizeof(int16_t) + sizeof(int16_t));
  filteredN = (int16_t*)malloc(FREQUENCY * interval * sizeof(int16_t) + sizeof(int16_t));
  filteredE = (int16_t*)malloc(FREQUENCY * interval * sizeof(int16_t) + sizeof(int16_t));
  filteredZ = (int16_t*)malloc(FREQUENCY * interval * sizeof(int16_t) + sizeof(int16_t));

  if (!logN || !logE || !logZ || !filteredN || !filteredE || !filteredZ) {
    printf("%s: Could allocate memory.\n", cmdline[0]);
    return -1;
  }

  if (!(argc > 2 && argv[2]))
    output = stdout;

  strncpy(commPortStr, argc > 3 && argv[3] ? argv[3] : "/dev/ttyUSB0",
	  sizeof(commPortStr));

  /* Open the board.  Function will return 0 if an error has occurred */
  hBoard = PSNOpenBoard();
  if (!hBoard)  {
    printf("PSNOpenBoard error\n");
    return 0;
  }
  
  /* Get DLL Version number. Version info is returned a 16 bit unsigned word */
  if (!PSNGetBoardInfo( hBoard, ADC_GET_DLL_VERSION, &version))  {
    printf("PSNGetBoardInfo error\n");
    PSNCloseBoard(hBoard);
    return 0;
  }

  printf("PSNADBoard library version %d.%1d\n", version / 10, version % 10);
  
  /* Fill in the configuration structure with the default info above */
  MakeConfig(&config);

  /* Pass the configuration information to the DLL. */
  sts = PSNConfigBoard( hBoard, &config,  ProcessNewData );	 
 
  if (!sts)  {
    printf("PSNConfigBoard error\n");
    PSNCloseBoard(hBoard);
    return 0;
  }

  if (!PSNSendBoardCommand(hBoard, ADC_CMD_RESET_BOARD, 0))  {
    printf("PSNSendBoardCommand error: ADC_CMD_RESET_BOARD\n");
    PSNCloseBoard(hBoard);
    return 0;
  }

  gargc = argc;
  gargv = argv;

  /* Now start the data collection.  This will open the comm port and new data
     from the DLL or ADC board will be sent to the callback function */
  sts = PSNStartStopCollect(hBoard, TRUE);
  if (!sts)  {
    printf("PSNStartStopCollect error\n");
    PSNCloseBoard(hBoard);
    return 0;
  }

  /* Do nothing, mew data will be posted using the callback function. */
  signal(SIGHUP, &sighandler);
  signal(SIGTERM, &sighandler);
  signal(SIGINT, &sighandler);
  pause();
  
  /* Stop the data collection.  This will close the comm port */
  if (!PSNStartStopCollect( hBoard, FALSE))
    printf("PSNStartStopCollect Error\n");
  
  /* Sleep for a while so the DLL has some time to shut things down */
  usleep(250000);
  
  /* Now close the board */
  if (!PSNCloseBoard(hBoard))
    printf("PSNCloseBoard error\n");

  return 0;
}


/* In the Callback mode this function is called by the DLL when new data is 
   available to the application. It is also called by the function above in 
   the poll data mode.  */
void ProcessNewData( uint32_t type, void *data, void *data1, uint32_t dataLen )
{
  switch( type )  {
  case ADC_MSG:		/* Display various text messages sent by the DLL or ADC board */
  case ADC_ERROR:
  case ADC_AD_MSG:
    DisplayMsg( type, (char *)data );
    break;
    
  case ADC_AD_DATA:	/* New ADC sample data  */
    NewADData( type, (DataHeader *)data, data1, dataLen );
    break;
    
  case ADC_GPS_DATA:	/* New raw GPS data */
    NewGpsData( (uint8_t *)data, dataLen );
    break;
    
  case ADC_STATUS:	/* New Status information */
    DisplayStatus( (StatusInfo *)data );
    break;
    
  case ADC_SAVE_TIME_INFO:	/* Display Status Information */
    SaveTimeInfo( (TimeInfo *)data );
    break;
    
  default:		printf("Unknown message type = %d", type ); break;
  }
}

/* This function is called once per second with new ADC sample data. The header has the time
   of day of the first sample in the data array as well as the time reference lock status and
   packet ID. The number of samples in the adcData array is the number of channels
   being recorded times the sample rate. */
void NewADData( uint32_t type, DataHeader *hdr, void *adcData, uint32_t dataLen )
{
  uint32_t boardType, samples, index, c;
  const char *boardStr = "???", *lockStr;
  short *spData;		// used to demux 16 bit data
  int32_t *lpData;		// used to demux 32 bit data from the VolksMeter sensor
  time_t now;
  int i;

  /* On the first time here display the ADC board type */
  if( displayBoardType )  {
    displayBoardType = 0;
    if( !PSNGetBoardInfo( hBoard, ADC_GET_BOARD_TYPE, &boardType ) )
      printf("\nPSNGetBoardInfo Error\n\n");
    else  {
      boardStr = "Unknown";
      if( boardType == BOARD_V1 )
	boardStr = "V1 Rabbit CPU";
      else if( boardType == BOARD_V2 )
	boardStr = "V2 PICC CPU";
      else if( boardType == BOARD_VM )
	boardStr = "VolksMeter Sensor";
      else if( boardType == BOARD_V3 )
	boardStr = "V3 dsPic CPU";
      printf("\nADC Board Type = %s\n\n", boardStr );
    }
  }
  if( hdr->timeRefStatus == TIME_REF_NOT_LOCKED )
    lockStr = "Not Locked";
  else if( hdr->timeRefStatus == TIME_REF_WAS_LOCKED )
    lockStr = "Was Locked";
  else if( hdr->timeRefStatus == TIME_REF_LOCKED )
    lockStr = "Locked";
  else
    lockStr = "????";
  /*
  printf( "ID=%d Time=%02d/%02d/%02d %02d:%02d:%02d.%03d  Time Ref Status=%s             ", 
	  hdr->packetID, st->wMonth, st->wDay, st->wYear % 100, st->wHour, st->wMinute, 
	  st->wSecond, st->wMilliseconds, lockStr );
  
  printf("\n");
  */
  
  /* Now demux the data */
  samples = config.sampleRate;
  if( boardType != BOARD_VM )  {
    spData = (short *)adcData;
    index = 0;
    while( samples-- )  {
      for( c = 0; c != config.numberChannels; c++ )
	demuxData[ c ][ index ] = *spData++;
      ++index;
    }
  }
  else  {
    lpData = (int32_t *)adcData;
    index = 0;
    while( samples-- )  {
      for( c = 0; c != config.numberChannels; c++ )
	demuxData[ c ][ index ] = *lpData++;
      ++index;
    }
  }	
  
  /* The user should add code here to save and or display the data */
  for (i = 0; i != (int)config.sampleRate; i++) {
    logN[logindex] = demuxData[1][i];
    logE[logindex] = demuxData[2][i];
    logZ[logindex] = demuxData[0][i];
    logindex += logindex <= FREQUENCY * interval;
    if (logindex > FREQUENCY * interval) {
      memmove(logN, logN + 1, FREQUENCY * interval * sizeof(int16_t));
      memmove(logE, logE + 1, FREQUENCY * interval * sizeof(int16_t));
      memmove(logZ, logZ + 1, FREQUENCY * interval * sizeof(int16_t));
    }
  }

  now = time(NULL);
  if (now - logtime >= interval) {
    int16_t maxN, minN, maxE, minE, maxZ, minZ;
    
    if (gargc > 4 && gargv[4]) {
      unsigned int i, l;
      static char raw[256];
      l = strlen(gargv[4]);
      if (l > sizeof(raw) - 2)
	l = sizeof(raw) - 2;
      i = l;
      memcpy(raw, gargv[4], i);
      while (i && gargv[4][i] != '/') i--;
      while (gargv[4][i] != '.' && gargv[4][i] != 0) i++;
      raw[i++] = '_';
      memcpy(raw + i + 1, gargv[4] + i - 1, l - i + 2);
      raw[i] = 'n';
      rawoutputN = fopen(raw, "a");
      raw[i] = 'e';
      rawoutputE = fopen(raw, "a");
      raw[i] = 'z';
      rawoutputZ = fopen(raw, "a");
    }
    else
      rawoutputN = rawoutputE = rawoutputZ = 0;
    
    if (rawoutputN) {
      lograw(rawoutputN, logN, logindex);
      fclose(rawoutputN);
    }
    if (rawoutputE) {
      lograw(rawoutputE, logE, logindex);
      fclose(rawoutputE);
    }
    if (rawoutputZ) {
      lograw(rawoutputZ, logZ, logindex);
      fclose(rawoutputZ);
    }
    
    lowpass(logN, filteredN, logindex);
    lowpass(logE, filteredE, logindex);
    lowpass(logZ, filteredZ, logindex);
    process(filteredN, logindex, &maxN, &minN);
    process(filteredE, logindex, &maxE, &minE);
    process(filteredZ, logindex, &maxZ, &minZ);
    
    if (output != stdout)
      output = fopen(gargv[2], "a");
    
    if (output) {
      printlog(output, now/2 + logtime/2, maxN, minN, maxE, minE, maxZ, minZ);
      if (output != stdout)
	fclose(output);
    }
    
    logindex = 0;
    logtime = now;
  }
}

/* Called when new raw GPS data is sent by the DLL. Use the ADC_CMD_GPS_DATA_ON and
   ADC_CMD_GPS_DATA_OFF commands to receive or stop raw GPS data. */
void NewGpsData( uint8_t *data,  uint32_t dataLen )
{
  data[ dataLen ] = 0;
  if( config.timeRefType != TIME_REF_MOT_BIN )
    printf("\n%s", data );
  else
    printf("\nNew GPS Data\n");
}


/* Display the various messages from the DLL or ADC board. */
void DisplayMsg( uint32_t type, char *string )
{
  const char *preStr;
  
  switch( type )  {
  case ADC_MSG:
    preStr = "DLLMsg";
    break;
  case ADC_ERROR:
    preStr = "DLLError";
    break;
  case ADC_AD_MSG:
    preStr = "AdcMsg";
    break;
  default:
    preStr = "???";
    break;
    
  }
  printf("\n%s=%s\n", preStr, string );
}

/* Displays DLL and ADC board information. Called when a ADC_STATUS message is sent 
   by the DLL. The DLL will send this message when a ADC_CMD_SEND_STATUS command 
   is sent to the DLL. */
void DisplayStatus( StatusInfo *sts )
{
  TimeInfo *ti = &sts->timeInfo;
  
  printf("\nStatus Information:\n");
  printf( "    BoardType=%d Version=%d.%d NumberChannels=%d SampleRate=%d LockStatus=%d\n", 
	  sts->boardType, sts->majorVersion, sts->minorVersion, sts->numChannels, sts->spsRate, sts->lockStatus );
  
  printf( "    CrcErrors=%d PacketsSent=%d RetranNumber=%d RetranError=%d IncomingPackets=%d\n", 
	  sts->crcErrors, sts->numProcessed, sts->numRetran, sts->numRetranErr, sts->packetsRcvd );
  
  printf( "    AddFlag=%d AddDropCount=%d WWVWidth=%d LockTime=%d AdjustNum=%d TimeDiff=%d Offset=%d\n", 
	  ti->addDropFlag, ti->addDropCount, ti->pulseWidth, ti->timeLocked, ti->adjustNumber, 
	  ti->averageTimeDiff, ti->timeOffset );	
}

/* Saves time adjustment information to a file. Called when a ADC_SAVE_TIME_INFO message
   is sent by the DLL */
void SaveTimeInfo( TimeInfo *info )
{
  FILE *fp;
  
  if( ! (fp = fopen( TIME_FILE_NAME, "w") ) )
    return;
  fprintf(fp, "%d %d %d\n", info->addDropFlag, info->addDropCount, info->pulseWidth );
  fclose( fp );	
}

/* Reads the time adjustment information from a file. This information is then sent
   to the DLL.*/
void ReadTimeInfo( TimeInfo *info )
{
  FILE *fp;
  char buff[256];
  int cnt, flag, addDrop, width;
  
  memset( info, 0, sizeof(TimeInfo) );
  if( !(fp = fopen( TIME_FILE_NAME, "r") ) )
    return;
  fgets( buff, 127, fp );
  fclose( fp );
  cnt = sscanf( buff, "%d %d %d", &flag, &addDrop, &width );
  if( cnt != 3 )
    return;
  info->addDropFlag = flag;
  info->addDropCount = addDrop;
  info->pulseWidth = width;
}

/* This function fills in the configuration structure with information needed 
   to run the DLL and ADC board */
void MakeConfig( AdcBoardConfig *cfg )
{
  TimeInfo timeInfo;
  
  memset( cfg, 0, sizeof( AdcBoardConfig ) );
  
  ReadTimeInfo( &timeInfo );
  
  strcpy( cfg->commPortStr, commPortStr );	// use port string under Linux
  
  cfg->commSpeed = DEF_PORT_SPEED;
  cfg->numberChannels = DEF_NUMBER_CHANNELS;
  cfg->sampleRate = DEF_SAMPLE_RATE;
  cfg->timeRefType = DEF_TIME_REF;
  cfg->highToLowPPS = DEF_HIGH_TO_LOW_PPS;
  cfg->noPPSLedStatus = DEF_NO_LED_STATUS;
  cfg->addDropTimer = timeInfo.addDropCount;
  cfg->addDropMode = timeInfo.addDropFlag;
  cfg->pulseWidth = timeInfo.pulseWidth;
  cfg->mode12BitFlags = DEF_12BIT_FLAGS;
  cfg->timeOffset = DEF_TIME_OFFSET;
}