/*power.c - compute the power spectrum of a GnuroScan averaged EEG data file.
  Copyright (c) 1996 Matthew Belmonte

  This program is free software; you can redistribute it and/or
  modify it under the terms of the GNU General Public License
  as published by the Free Software Foundation; either version 2
  of the License, or (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

  If you find this program useful, please send mail to Matthew Belmonte.
  <mkb4@Cornell.edu>.  If you base a publication on data processed by this
  program, please notify Matthew Belmonte and include the following citation
  in your publication:

	Matthew Belmonte, `A Software System for Analysis of
	Steady-State Evoked Potentials', Association for Computing
	Machinery SIGBIO Newsletter 17:1:9-14 (April 1997).
*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#ifdef MSDOS
#  define strcasecmp strcmpi
#endif
#include <errno.h>
#include <math.h>
#ifdef NOFLOATS
#  define fsqrt(x) ((float)(sqrt((double)x)))
#endif
#include <nr.h>
#include "intel.h"
#include "gnuro.h"
#define PI 3.141592653589793

#define MAX_WINDOW_LENGTH 256
#define CH_MAX 16

int nearest_power_of_two(x)
int x;
  {
  register int result;
  for(result = 1; result < x; result <<= 1)
    ;
  return(result-x<=x-result/2? result: result/2);
  }

/*Stretch or squeeze the circular buffer s[offset%ns..(offset+ns-1)%ns] onto
  d[0..nd-1].*/
void resample(s, ns, d, nd, offset)
float *s;
int ns;
float *d;
int nd, offset;
  {
  register float acc;
  register int i, j;
  if(ns == nd)
    for(i = 0; i != nd; i++)
      d[i] = (float)(s[(i+offset)%nd]);
  else
    for(i = 0; i != nd; i++)
      {
      acc = 0;
      for(j = i*ns; j != (i+1)*ns; j++)
	acc += s[(j/nd+offset)%ns];
      d[i] = acc/ns;
      }
  }

/*Subtract any DC offset and linear trend (based upon the difference between
  the endpoints y[0] and y[n-1]) from the series y[0..n-1].*/
void detrend(y, n)
float *y;
int n;
  {
  register int i;
  register float decr, dy;
  dy = (y[n-1]-y[0])/(n-1);
  decr = y[0];
  for(i = 0; i != n; i++)
    {
    y[i] -= decr;
    decr += dy;
    }
  }

/*Convolve y[0..n-1] with x[0..n-1], and store the result in x[0..n-1].*/
void convolve(x, y, n)
float *x, *y;
int n;
  {
  register int i;
  for(i = 0; i != n; i++)
    x[i] *= y[i];
  }

void main(argc, argv)
int argc;
char **argv;
  {
  register int i, sample, channel, frequency_bin, window_length, fft_length, num_samples, num_channels;
  int c, srate, window_length_ms;
  FILE *in, *(out[MAX_WINDOW_LENGTH/2]);
  char buf[sizeof(float)];
  int transform_flag[CH_MAX];
  float cosine_bell[MAX_WINDOW_LENGTH];
  float window[MAX_WINDOW_LENGTH], transform[MAX_WINDOW_LENGTH];
  char erp[packed_sizeof_SETUP], ch_hdr[CH_MAX][packed_sizeof_ELECTLOC];
  char filename[21];
  printf("Copyright (c) 1996 Matthew Belmonte <mkb4@Cornell.edu>.  Please cite.\n");
  if(argc != 3)
    {
    fprintf(stderr, "usage: %s <file prefix> <window length (ms)>\n", *argv);
    exit(1);
    }
  sprintf(filename, "%s.avg", argv[1]);
  if((in = fopen(filename,
#ifdef MSDOS
    "rb"
#else
    "r"
#endif
    )) == NULL)
    {
    perror(argv[1]);
    exit(errno);
    }
  fread(erp, packed_sizeof_SETUP, 1, in);
  S_variance(erp) = 0; /*variance data not included in output*/
  num_channels = S_nchannels(erp);
  if(num_channels > CH_MAX)
    {
    fclose(in);
    fprintf(stderr, "Recompile %s with CH_MAX=%d\n", *argv, num_channels);
    exit(1);
    }
  num_samples = S_pnts(erp);
  srate = S_rate(erp);
  window_length_ms = atoi(argv[2]);
  window_length = (window_length_ms*srate+500)/1000;
  if(window_length > MAX_WINDOW_LENGTH)
    {
    fclose(in);
    fprintf(stderr, "Recompile %s with MAX_WINDOW_LENGTH=%d\n", *argv, window_length);
    exit(1);
    }
  printf("%d points at %dHz on %d channels - using a %d-point window\n", num_samples, srate, num_channels, window_length);
  if(window_length > num_samples)
    window_length = num_samples;
  fft_length = nearest_power_of_two(window_length);
  for(sample = 1; sample != fft_length-1; sample++)
    cosine_bell[sample] = (1.0-cos(2.0*PI*sample/(fft_length-1)))/2.0;
  cosine_bell[0] = cosine_bell[fft_length-1] = 0.0;
  for(channel = 0; channel != num_channels; channel++)
    {
    fread(ch_hdr[channel], packed_sizeof_ELECTLOC, 1, in);
    transform_flag[channel] = ((strcasecmp("heog", EL_lab(ch_hdr[channel])) != 0)
      &&(strcasecmp("veog", EL_lab(ch_hdr[channel])) != 0)
      &&(strcasecmp("blink", EL_lab(ch_hdr[channel])) != 0)
      &&(strcasecmp("upe", EL_lab(ch_hdr[channel])) != 0)
      &&(strcasecmp("loe", EL_lab(ch_hdr[channel])) != 0)
      &&(strcasecmp("lc", EL_lab(ch_hdr[channel])) != 0)
      &&(strcasecmp("rc", EL_lab(ch_hdr[channel])) != 0));
    }
  for(frequency_bin = 0; frequency_bin != fft_length/2; frequency_bin++)
    {
    sprintf(filename, "%s%03d.avg", argv[1], (1000*(1+frequency_bin)+(window_length_ms/2))/window_length_ms);
    out[frequency_bin] = fopen(filename,
#ifdef MSDOS
      "wb"
#else
      "w"
#endif
      );
  /*900-byte file header*/
    fwrite(erp, packed_sizeof_SETUP, 1, out[frequency_bin]);
  /*75-byte channel header*/
    for(channel = 0; channel != num_channels; channel++)
      fwrite(ch_hdr[channel], packed_sizeof_ELECTLOC, 1, out[frequency_bin]);
    }
  for(channel = 0; channel != num_channels; channel++)
    {
  /*5-byte sweep header*/
    for(sample = 0; sample != 5; sample++)
      {
      c = getc(in);
      for(i = 0; i != fft_length/2; i++)
	putc(c, out[i]);
      }
  /*if this is an eye channel, do not detrend it; rather, just copy it as is*/
    if(transform_flag[channel])
      {
      for(sample = 0; sample != window_length; sample++)
	{
	fread(buf, sizeof(float), 1, in);
	window[sample] = fetch_Intel_float(buf, 0);
	}
      resample(window, window_length, transform, fft_length, 0);
      detrend(transform, fft_length);
      convolve(transform, cosine_bell, fft_length);
      realft(transform-1, fft_length, 1);
    /*write the same value window_length/2 times to the left-hand boundary*/
      for(frequency_bin = 0; frequency_bin != fft_length/2-1; frequency_bin++)
	{
	store_Intel_float(buf, 0, fsqrt(2.0*(transform[2*frequency_bin+2]*transform[2*frequency_bin+2]+transform[2*frequency_bin+3]*transform[2*frequency_bin+3])/(fft_length/2)));
	for(i = 0; i != window_length/2; i++)
	  fwrite(buf, sizeof(float), 1, out[frequency_bin]);
	}
      store_Intel_float(buf, 0, fsqrt(transform[1]*transform[1]));
      for(i = 0; i != window_length/2; i++)
	fwrite(buf, sizeof(float), 1, out[fft_length/2-1]);
    /*step the FFT window through the body of the epoch*/
      for(sample = window_length; sample < num_samples-1; sample++)
	{
	fread(buf, sizeof(float), 1, in);
	window[sample%window_length] = fetch_Intel_float(buf, 0);
	resample(window, window_length, transform, fft_length, 1+sample);
	detrend(transform, fft_length);
	convolve(transform, cosine_bell, fft_length);
	realft(transform-1, fft_length, 1);
	for(frequency_bin = 0; frequency_bin != fft_length/2-1; frequency_bin++)
	  {
	  store_Intel_float(buf, 0, fsqrt(2.0*(transform[2*frequency_bin+2]*transform[2*frequency_bin+2]+transform[2*frequency_bin+3]*transform[2*frequency_bin+3])/(fft_length/2)));
	  fwrite(buf, sizeof(float), 1, out[frequency_bin]);
	  }
	store_Intel_float(buf, 0, fsqrt(transform[1]*transform[1]));
	fwrite(buf, sizeof(float), 1, out[fft_length/2-1]);
	}
    /*write same value (window_length+1)/2 times to the right-hand boundary*/
      fread(buf, sizeof(float), 1, in);
      window[sample%window_length] = fetch_Intel_float(buf, 0);
      resample(window, window_length, transform, fft_length, 1+sample);
      detrend(transform, fft_length);
      convolve(transform, cosine_bell, fft_length);
      realft(transform-1, fft_length, 1);
      for(frequency_bin = 0; frequency_bin != fft_length/2-1; frequency_bin++)
	{
	store_Intel_float(buf, 0, fsqrt(2.0*(transform[2*frequency_bin+2]*transform[2*frequency_bin+2]+transform[2*frequency_bin+3]*transform[2*frequency_bin+3])/(fft_length/2)));
	for(i = 0; i != 1+(window_length+1)/2; i++)
	  fwrite(buf, sizeof(float), 1, out[frequency_bin]);
	}
      store_Intel_float(buf, 0, fsqrt(transform[1]*transform[1]));
      for(i = 0; i != 1+(window_length+1)/2; i++)
	fwrite(buf, sizeof(float), 1, out[fft_length/2-1]);
      }
    else
      for(i = 0; i != num_samples*sizeof(float); i++)
	{
	c = getc(in);
	for(frequency_bin = 0; frequency_bin != fft_length/2-1; frequency_bin++)
	  putc(c, out[frequency_bin]);
	}
    }
  for(frequency_bin = 0; frequency_bin != fft_length/2; frequency_bin++)
    fclose(out[frequency_bin]);
  fclose(in);
  exit(0);
  }
