/* Do stty -F /dev/ttyS0 speed 19200 -crtscts timer -1  to restore.
   To compile: gcc pskmeter.c -lm -o pskmeter
   To run: ./pskmeter /dev/ttyS0 > data (for COM1, etc.)
   To process data: gnuplot plot "data"
*/

#include <stdio.h>
#include <sys/file.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <math.h>

extern double round(double);	/* math.h is missing these on some systems */
extern double sqrt(double);
extern double log10(double);
extern double sin(double);
extern double cos(double);
extern int abs(int);

double sqr(double);
void initTables();
void calculateMinMax(unsigned char *buf, int count, int *prmsvalue, int *pmin, int *pmin_pos, int *pmax, int *pzeroes, int *pminima);
void normalize(unsigned char *buf, double *dbuf, int min, int min_pos, int count, int max);
void calculateError(double *buf, int count, double *pmeanError, double *prmsError);
double calculateIMD(double *buf);
void getVersion(int fd);
int getData(int fd, unsigned char *buf);
int openPort(char *dev);
void closePort(int fd);
void dumpdbuf(double *dbuf, int count);
void dumpbuf(unsigned char *buf, int count);
void minidump(char *prompt, unsigned char *buf, int count);
void showError(double imd, double rmsError, double meanError);
void gnuplotDump(char *ctrlfile, char *datafile, double *buf, int count); 
void processSignal(unsigned char *buf, int count, int min, int min_pos, int max);

/* Constants */
#define FALSE (0)
#define TRUE (1)

/* Parameters */
#define RMS_MIN (1)
#define CARRIER_DELTA (2)

/* Debug flags */
#define FAKE (0)
#define DEBUG (1)
#define DEBUG_HARDWARE (0)
#define DEBUG_ZERO (0)
#define DEBUG_RMS (0)

#define MINIDUMP_CARRIER (0)
#define MINIDUMP_DATA (1)
#define MINIDUMP_SPLATTER (1)
#define MINIDUMP_SIGNAL (1)

#define DO_GNUPLOT (1)

const char *versionRequest="v";
const char *dataRequest="s";

int total = 0;
struct termios oldtio;

double theta[128];

int main(int argc, char **argv) {
  initTables();
  
  char *dev="/dev/ttyS0";
  unsigned char buf[256];
  if (argc > 0) {
    dev = argv[1];
  }
  if (DEBUG_HARDWARE)
    fprintf(stderr, "Using device %s\n", dev);

  int fd = openPort(dev);
  if (fd < 0) {
    return (-1);
  }

  if (!FAKE)
    getVersion(fd);

  int count;
  while ((count = getData(fd, buf)) >= 0) {
    int rmsvalue;
    int min;
    int min_pos;
    int max;
    int zeroes;
    int minima;
    calculateMinMax(buf, count, &rmsvalue, &min, &min_pos, &max, &zeroes, &minima);
    if (rmsvalue >= RMS_MIN) {
      if (DEBUG_RMS) fprintf(stderr, "rmsvalue %d\n", rmsvalue);
      if ((rmsvalue - min) < CARRIER_DELTA || ((max - rmsvalue) < CARRIER_DELTA)) {
	if (MINIDUMP_CARRIER) minidump("CARRIER", buf, count);
	fprintf(stderr, "CARRIER: min %d max %d rms %d\n", min, max, rmsvalue);
      } else if (minima < 2) {
	fprintf(stderr, "DATA: %d zeroes %d minima of %d\n", zeroes, minima, min);
	if (MINIDUMP_DATA) minidump("DATA", buf, count);
	processSignal(buf, count, min, min_pos, max);
      } else if (minima > 2) {
	fprintf(stderr, "SPLATTER: %d zeroes %d minima of %d\n", zeroes, minima, min);
	if (MINIDUMP_SPLATTER) minidump("SPLATTER", buf, count);
	processSignal(buf, count, min, min_pos, max);
      } else {
	processSignal(buf, count, min, min_pos, max);
	if (MINIDUMP_SIGNAL) minidump("SIGNAL", buf, count);
	// dumpbuf(buf, count);
      }
    } else {
      if (DEBUG_RMS)
	fprintf(stderr, "Skipping sample: rmsvalue=%d\n", rmsvalue);
    }
    sleep(1);
  }
  closePort(fd);
  return 0;
}

void processSignal(unsigned char *buf, int count, int min, int min_pos, int max) {
  double dbuf[256];
  double meanError, rmsError;
  normalize(buf, dbuf, min, min_pos, count, max);
  calculateError(dbuf, count, &meanError, &rmsError);
  double imd = calculateIMD(dbuf);
  gnuplotDump("d1", "d1.dat", dbuf, count);
  showError(imd, rmsError, meanError);
}

void getVersion(int fd) {
  char buf[256];
  if (DEBUG_HARDWARE)
    fprintf(stderr, "Sending %s\n", versionRequest);
  write(fd, versionRequest, 1);
  int done=FALSE;

  int newlinecount = 0;
  while (! done) {
    int res = read(fd,buf,255); /* returns after 255 chars -input */
    buf[res]=0;
    fprintf(stderr, "%s", buf);
    {
      int i = 0;
      for (i = 0; i < res; i++) {
	if (buf[i] == '\n') {
	  newlinecount++;
	  //printf("saw a %u at %d: newlinecount now %d\n", buf[i], i, newlinecount);
	}
      }
    }
    if (newlinecount >= 4) done=TRUE;
  }
}

#define FAKE_DATA_COUNT (4)
#define FAKE_DATA_SAMPLE_SIZE (64)
unsigned char sample_data[FAKE_DATA_COUNT][FAKE_DATA_SAMPLE_SIZE] 
#if FAKE
= { 
  {
    31, 30, 29, 28, 26, 24, 22, 19, 18, 15, 11, 9, 5, 2, 0, 1, 4, 7, 10, 14, 
    16, 19, 21, 23, 25, 27, 28, 29, 30, 31, 31, 31, 30, 30, 29, 27, 26, 24, 
    22, 19, 17, 14, 11, 8, 4, 1, 0, 2, 5, 8, 12, 14, 17, 20, 22, 24, 26, 27, 
    28, 30, 30, 31, 31, 31
  },
  {
    48, 41, 36, 29, 22, 15, 7, 1, 4, 11, 18, 25, 31, 37, 43, 50, 55,
    61, 65, 69, 72, 74, 76, 77, 76, 75, 73, 71, 68, 64, 59, 53, 46,
    40, 34, 27, 21, 14, 6, 0, 5, 12, 19, 26, 32, 39, 45, 51, 57,
    61, 66, 70, 73, 74, 75, 76, 76, 75, 73, 70, 66, 61, 57, 51 
  },
  {
    49, 55, 61, 66, 69, 73, 75, 77, 78, 78, 77, 75, 73, 71, 67, 63,
    58, 52, 45, 39, 32, 25, 18, 10, 2, 2, 9, 17, 24, 30, 37, 44,
    50, 56, 61, 67, 70, 73, 75, 77, 77, 77, 77, 75, 73, 71, 67, 63,
    57, 51, 44, 37, 31, 24, 17, 9, 1, 3, 11, 19, 25, 32, 38, 45
  },
  {
    80, 84, 87, 90, 90, 90, 90, 89, 86, 84, 80, 74, 69, 62, 54, 47,
    39, 32, 22, 15, 6, 1, 10, 18, 26, 34, 43, 49, 57, 65, 71, 76,
    80, 84, 88, 90, 91, 90, 90, 89, 85, 82, 79, 74, 68, 62, 55, 46,
    38, 30, 22, 10, 4, 3, 11, 19, 27, 34, 43, 51, 57, 65, 72, 76
  }
}
#endif
;

int fake_data_counter = 0;

int getData(int fd, unsigned char *buf) {
  if (FAKE) {
    int i;

    if (fake_data_counter >= FAKE_DATA_COUNT) {
      fprintf(stderr, "END OF SAMPLE DATA\n");
      exit(-1);
    }
    for (i = 0; i < FAKE_DATA_SAMPLE_SIZE; i++) {
      buf[i] = sample_data[fake_data_counter][i];
    }
    fake_data_counter++;
    return FAKE_DATA_SAMPLE_SIZE;
  }

  write(fd, dataRequest, 1);

  int done=FALSE;
  int count = 0;
  while (done==FALSE) {
    int res = read(fd,buf+count,64); /* returns after 64 chars -input */
    if (res < 0) {
      perror("getData error");
      return -1;
    }
    count += res;
    if (count >= 64) {
      done=TRUE;
    }
  }
  return count;
}

/**
 * Returns fd or -1 on error.
 */
int openPort(char *dev) {
  if (FAKE) {
    fprintf(stderr, "Using built-in sample data\n");
    return 1;
  }
  struct termios options, oldtio;

  int fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
  fcntl(fd, F_SETFL, 0);

  /* save current port settings */
  tcgetattr(fd,&oldtio);

  /* get the current options and change them*/
  tcgetattr(fd, &options);

  cfsetispeed(&options, B19200);
  cfsetospeed(&options, B19200);

  /* set raw input, 1 second timeout */
  options.c_cflag |= (CLOCAL | CREAD);
  options.c_oflag &= ~OPOST;
  options.c_cc[VMIN] = 0;
  options.c_cc[VTIME] = 10;

  options.c_cflag &= ~PARENB; /* Clear parity enable */
  options.c_iflag &= ~INPCK; /* Enable parity checking */
  options.c_cflag &= ~CSTOPB;
  options.c_cflag &= ~CSIZE;
  options.c_cflag |= CS8;

  options.c_cflag &= ~CRTSCTS;

  options.c_iflag &= ~(IXON | IXOFF | IXANY); /* no flow control */

  options.c_oflag &= ~(IXON | IXOFF | IXANY); /* no flow control */

  options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
  options.c_oflag &= ~OPOST; /* No output processing */
  options.c_oflag &= ~ONLCR; /* Don't convert linefeeds */

  /* Miscellaneous stuff */
  options.c_cflag |= (CLOCAL | CREAD); /* Enable receiver, set local */
  //  /* Linux seems to have problem with the following ??!! */
  //  options.c_cflag |= (IXON | IXOFF); /* Software flow control */
  options.c_lflag = 0; /* no local flags */
  options.c_cflag |= HUPCL; /* Drop DTR on close */

  /* Setup non blocking, return on character */
  options.c_cc[VMIN] = 0;
  options.c_cc[VTIME] = 200;        /* 20 second timeout */

  /* Clear line */
  tcflush(fd,TCIFLUSH);

  /* Update the options */
  if (tcsetattr(fd,TCSANOW,&options) != 0) {
    perror("Could not set serial port options.");
    return(-1);
  }
  return fd;
}

void closePort(int fd) {
  tcsetattr(fd,TCSANOW,&oldtio); /* crock */
  close(fd);
}


void calculateMinMax(unsigned char *buf, int count, 
		     int *prmsvalue, int *pmin, int *pmin_pos, int *pmax, int *pzeroes, int *pminima) {
  int sumsquare = 0;
  int min = 257;
  int min_pos = -1;
  int max = 0;
  int zeroes = 0;
  int minima = 0;
  int i;
  for (i = 0; i < count; i++) {
    int value = buf[i];
    if (value < min) {
      min = value;
      min_pos = i;
    }
    // count zeroes; successive zeroes count as one
    // BUG: This may double-count a double zero at positions 0 and 63.
    if ((value == 0) && ((i == 0) || (buf[i-1] != 0)))
      zeroes++;
    if (value > max) {
      max = value;
    }
    sumsquare += value*value;
  }
  for (i = 0; i < count; i++) {
    // look for minima of min or min+2
    int local_minimum_threshold = min + 1;
    // BUG: This may double-count a double minimum at positions 0 and 63.
    if ((buf[i] <= local_minimum_threshold) && ((i == 0) || (buf[i-1] > local_minimum_threshold)))
      minima++;
  }
  *prmsvalue = sqrt(((double)sumsquare)) / 32;
  *pmin = min;
  *pmin_pos = min_pos;
  *pmax = max;
  *pzeroes = zeroes;
  *pminima = minima;
}

void calculateError(double *buf, int count, double *pmeanError, double *prmsError) {
  double rms = 0.0;
  double mean = 0.0;
  int i;
  for (i = 0; i < 32; i++) {
    double value = buf[i];
    double ideal = sin(theta[i]);
    double err = (value-ideal);
    rms += err*err;
    mean += err;
    if (FALSE && DEBUG) fprintf(stderr, "calculateError: %d %f %f\n", total++, value, ideal);
  }
  if (FALSE && DEBUG) printf("\n");
  rms = sqrt(rms) / 31.0;
  mean = mean / 32.0;
  *pmeanError = mean;
  *prmsError = rms;
}


void normalize(unsigned char *buf, double *dbuf, int min, int min_pos, int count, int max) {
  if (count > 64) {
    fprintf(stderr, "Error: unable to normalize more than 64 samples: count = %d\n", count);
    exit(-1);
  }
  /* restore the second half of the waveform to the negative region to make the Fourier
   transform happy.  Look for a second zero or 1 within +/- 1 of (min_pos+32) % 64..  */
  double dmax = (double)max;
  int minval = buf[min_pos];
  int second_min_pos = (min_pos + 32) % 64;
  if (DEBUG_ZERO)
    fprintf(stderr, "Min %d at %d; looking for second min at .+32=%d\n",
	    minval, min_pos, second_min_pos);
  if (buf[second_min_pos] == minval) {
    if (DEBUG_ZERO)
      fprintf(stderr, "found %d at .+32\n", minval);
  } else if (buf[(second_min_pos + 1) % 64] == minval) {
    second_min_pos = (second_min_pos + 1) % 64;
    if (DEBUG_ZERO)
      fprintf(stderr, "found %d at .+33\n", minval);
  } else if (buf[(second_min_pos - 1) % 64] == minval) {
    second_min_pos = (second_min_pos - 1) % 64;
    if (DEBUG_ZERO)
      fprintf(stderr, "found %d at .+31\n", minval);
  } else if (buf[second_min_pos] == minval+1) {
    if (DEBUG_ZERO)
      fprintf(stderr, "found %d at .+32\n", minval+1);
  } else if (buf[(second_min_pos + 1) % 64] == minval+1) {
    second_min_pos = (second_min_pos + 1) % 64;
    if (DEBUG_ZERO)
      fprintf(stderr, "found %d at 33\n", minval+1);
  } else if (buf[(second_min_pos - 1) % 64] == minval+1) {
    second_min_pos = (second_min_pos - 1) % 64;
    if (DEBUG_ZERO)
      fprintf(stderr, "found %d at 31\n", minval+1);
  } else {
    fprintf(stderr, "Could not find second zero crossing.\n");
  }
  int half_wave_length = abs(second_min_pos - min_pos) % 64;
  if (DEBUG_ZERO) 
    fprintf(stderr, 
	    "min_pos = %d, second_min_pos = %d, half_wave_length = %d\n",
	    min_pos, second_min_pos, half_wave_length);
  int i;
  int j = 0;
  for (i = min_pos; i < count; i++) {
    dbuf[j] = ((double)(buf[i]-min)) / dmax;
    if (j > half_wave_length) dbuf[j] = -dbuf[j];
    if (FALSE) fprintf(stderr, "normalize: %d %f\n", j, dbuf[j]);
    j++;
  }
  for (i = 0; i < min_pos; i++) {
    dbuf[j] = ((double)(buf[i]-min)) / dmax;
    if (j > half_wave_length) dbuf[j] = -dbuf[j];
    if (FALSE) fprintf(stderr, "normalize: %d %f\n", j, dbuf[j]);
    j++;
  }
}


double calculateIMD(double *s) {
  double imd = 0.0;
  double a[11];
  double b[11];
  a[0] = 0.0;
  b[0] = 0.0;
  {
    int i, j;
    for (j = 1; j < 11; j++) {
      a[j] = 0.0;
      b[j] = 0.0;
      for (i = 0; i < 64; i++) {
	a[j] += s[i]*sin(j*theta[i]);
	b[j] += s[i]*cos(j*theta[i]);
      }
    }
  }
  {
    int j;
    double e = 0;
    for (j = 1; j < 11; j++) {
      e += sqr (a[j]) + sqr(b[j]);
    }
    double h = e - (sqr(a[1]) + sqr(b[1]));
    double f = h/e;
    if (FALSE && DEBUG) {
      fprintf(stderr, "a[1]=%f b[1]=%f\n", a[1], b[1]);
      fprintf(stderr, "(sqr(a[1]) + sqr(b[1]))=%f\n", (sqr(a[1]) + sqr(b[1])));
      fprintf(stderr, "h = %f e = %f h/e = %F\n", h, e, h/e);
    }
    imd = 10.0*log10(f);
  }
  return imd;
}

double sqr(double a) {
  return a*a;
}


void initTables() {
  int i;
  for (i = 0; i < 128; i++) {
    double x = (double)i;
    theta[i] = ((x/64.0)*(2*M_PI));
  }
}

void dumpdbuf(double *dbuf, int count) {
  int i;
  for (i = 0; i < count; i++) {
    fprintf(stderr, " %d %f\n", i, dbuf[i]);
  }
}
void dumpbuf(unsigned char *buf, int count) {
  int i;
  for (i = 0; i < count; i++) {
    fprintf(stderr, "%d %u\n", i, buf[i]);
  }
}
void minidump(char *prompt, unsigned char *buf, int count) {
  int i;
  fprintf(stderr, "%s ", prompt);
  for (i = 0; i < count; i++) {
    fprintf(stderr, "%u ", buf[i]);
  }
  fprintf(stderr,"\n");
}

void showError(double imd, double rmsError, double meanError) {
  fprintf(stderr, "IMD = %f RMS_ERROR = %f %s\n", imd, rmsError, meanError < 0 ? "under" : "over");
}

void gnuplotDump(char *ctrlfile, char *datafile, double *buf, int count) {
  {
    FILE *file = fopen(datafile, "w");
    int i;
    for (i = 0; i < count; i++) {
      fprintf(file, "%d %g\n", i, buf[i]);
    }
    fclose (file);
  }
  {
    FILE *file = fopen(ctrlfile, "w");
    fprintf(file, "plot \"%s\" with lines\npause 1\n", datafile);
    fclose (file);
  }
  {
    char buf[256];
    sprintf(buf, "gnuplot %s\n", ctrlfile);
    system(buf);
  }
}