//
//	HearBr
//
//        Copyright (c) 2002 Jim Peters <http://uazu.net/>.  Released
//        under the GNU GPL version 2 as published by the Free
//        Software Foundation.  See the file COPYING for details, or
//        visit <http://www.gnu.org/copyleft/gpl.html>.
//

#define VERSION "0.1.0"
#define PROG_NAME "hearbr3"

#define ST_SIZ 16384		// Size of sine-table for generating output (power of 2)
#define ST_AMP 0x7FFFF          // Amplitude of wave in sine-table

#define AMP_FACT 50000		// Adjust this until output amplitude is about right
#define MULT_FACT 2400		// This adjusts the size of pitch changes in response to input

//
//	Includes
//

#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <math.h>
#include <sys/soundcard.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <unistd.h>

#define PI 3.14159265358979

#define ALLOC(type) ((type*)Alloc(sizeof(type)))
#define ALLOC_ARR(cnt, type) ((type*)Alloc((cnt) * sizeof(type)))
void error(char *fmt, ...) ;
void *Alloc(int siz) ;

//
//	Bring in filter designer code
//

#include "filt_designer.c"


//
//	Filter channel
//

typedef struct Chan Chan;
struct Chan {
   double freq0, freq1;	// Frequency range this band
   int base;		// Base frequency in octaves * 1200 relative to 256Hz
   BandPass filt;	// Band-pass filter
   double out;		// Output of filter

   double max;		// Maximum output recently encountered
   double max_fact;	// Factor to multiply 'max' by each sample, so that it decays

   double amp;		// Previous output amplitude
   double inc;		// Previous output offset increment
   double off;		// Current output offset into global sin[] table
};


//
//	Globals
//

float *sin_table;	// sine table for output generation
double rate_in;		// input sample rate
double rate_out;	// output sample rate
double global_amp;	// global amplitude
double global_mult;	// global multiplier for signal->pitch mapping
int out_fd;		// /dev/dsp file descriptor
int spf_in;		// Samples per frame on input
double spf_out;		// Samples per frame on output
float *tmp_out;		// Output workspace
short *buf_out;		// Output buffer
float *exp_table;	// 1200-entry one-octave exponential table

Chan chan[]= {
   { 0.2, 0.9, -500 },	// Sub-delta
   { 0.9, 4.0, 0 },	// Delta	
   { 4.0, 8.0, 700 },	// Theta
   { 8.0, 13.0, 1200 },	// Alpha
   { 13.0, 40.0, 1900 },	// Beta
   //   { 40.0, 999.0, 2400 },	// High-beta
};

#define N_CHAN (sizeof(chan)/sizeof(chan[0]))
   

//
//	Prototypes
//

void init_all() ;
void chan_init(Chan *cc) ;
void chan_in(Chan *cc, double val) ;
inline void chan_out(Chan *cc, float *dst, int cnt) ;
void bank_in(double val) ;
void bank_out(float *dst, int cnt) ;
void usage() ;
int main(int ac, char **av) ;
void audio_init(int ) ;
void audio_write(char *buf, int siz) ;


//
//	Util functions
//

void 
error(char *fmt, ...) {
   va_list ap;
   va_start(ap, fmt);
   fprintf(stderr, PROG_NAME ": ");
   vfprintf(stderr, fmt, ap);
   fprintf(stderr, "\n");
   exit(1);
}

void *
Alloc(int siz) {
   void *vp= calloc(siz, 1);
   if (!vp) error("Out of memory");
   return vp;
}


//
//	Initialise tables
//

void 
init_all() {
   int a;
   float *arr= ALLOC_ARR(ST_SIZ, float);
   float *farr= ALLOC_ARR(1200, float);

   for (a= 0; a<ST_SIZ; a++)
      arr[a]= sin((a * 3.14159265358979323846 * 2) / ST_SIZ);

   sin_table= arr;

   for (a= 0; a<1200; a++) 
      farr[a]= pow(2, a / 1200.0);

   exp_table= farr;

   for (a= 0; a<N_CHAN; a++)
      chan_init(&chan[a]);
}

//
//	Calculate a Hz frequency value from a pitch (in 1200/oct form)
//

double inline 
pitch_freq(int pit) {
   pit += 8*1200;		// Make relative to 1Hz, so we don't have to worry about -ves

   return ldexp(exp_table[pit % 1200], pit/1200);
}

//
//	Initialise a channel
//

void 
chan_init(Chan *cc) {
   bandpass_init(&cc->filt, cc->freq0 / rate_in, cc->freq1 / rate_in, 3);
   bandpass_dump(&cc->filt, stdout);

   // Drop 50% each waveform
   cc->max_fact= exp(log(0.5) * (0.5 * (cc->freq0+cc->freq1)/rate_in));	
}

//
//	Handle input of a sample to a channel
//

void 
chan_in(Chan *cc, double in) {
   cc->out= bandpass_process(&cc->filt, in);
}


//
//	Handle output for a channel.  Calculates resultant amplitude
//	and frequency from current ->out value.  Adds output sample
//	values to the given destination sample array.  Smoothly varies
//	amplitude and frequency from previous output value to new
//	output value.
//

inline void 
chan_out(Chan *cc, float *dst, int cnt) {
   double amp0, amp1, ampi, aa;
   double inc0, inc1, inci, ii;
   double amp, freq;
   int pit;
   double off;
   double dmy;

   amp0= cc->amp;
   inc0= cc->inc;

   cc->max *= cc->max_fact;
   if (fabs(cc->out) > cc->max) cc->max= fabs(cc->out);

   amp= cc->max;
   pit= (int)(cc->out * global_mult + cc->base);
   freq= pitch_freq(pit);

   printf("%10d", (int)(cc->max * global_amp));

   amp1= amp * global_amp;
   inc1= freq / rate_out;
   cc->amp= amp1;
   cc->inc= inc1;

   ampi= (amp1-amp0) / cnt;
   inci= (inc1-inc0) / cnt;
   
   off= cc->off;
   aa= amp0;
   ii= inc0;

   while (cnt-- > 0) {
      int idx= (ST_SIZ-1) & (int)(off*ST_SIZ);
      double val= aa * sin_table[idx];
      *dst++ += val;

      //OLD int shift= val >> 24;
      //OLD if (shift >= 0) 
      //OLD val= ((((val | 0xFF000000) ^ 0x01000000) >> shift) & 0x00FFFFFF) + 0x01000000;
      //OLD else 
      //OLD val= -((((((-val) | 0xFF000000) ^ 0x01000000) >> ~shift) & 0x00FFFFFF) + 0x01000000);
      //OLD *dst++ += val << 4;

      //OLD if (val < 0) 
      //OLD val= -((-val) & 0x00FFFFFF);
      //OLD else 
      //OLD val= val & 0x00FFFFFF;
      //OLD *dst++ += val << 4;

      off += ii;
      off= modf(off, &dmy);
      aa += ampi;
      ii += inci;
   }
   
   cc->off= off;
}
   
//
//	Add an input sample to the whole bank
//

void 
bank_in(double val) {
   int a;
   for (a= 0; a<N_CHAN; a++)
      chan_in(&chan[a], val);
}

//
//	Generate output into the given buffer for the whole bank
//

void 
bank_out(float *dst, int cnt) {
   int a;

   memset(dst, 0, cnt * sizeof(*dst));

   for (a= 0; a<N_CHAN; a++)
      chan_out(&chan[a], dst, cnt);
   
   printf("    \r"); fflush(stdout);
}

//
//	Usage
//

#define NL "\n"

void 
usage() {
   error("brainwave audition tool, version " VERSION NL
         NL "Usage: hearbr3 [options] <input-file> <sample-rate>"
         NL "Options:"
         NL " -j <off>/<len>    Input from a JM file, using byte <off> out of blocks of <len>"
         NL " -q <num>          Run at <num> times normal speed"
         NL " -d                Dump audio to file 'out.raw' instead of /dev/dsp"
         );
}

//
//	Main routine has to read in input and at the same time output
//	audio data.  If this is going to work in realtime, then the
//	two things would have to go on concurrently somehow.
//
//	For now I'm going to emulate the whole thing, reading from a
//	file.
//

int o_jm= 0;            // Jim Meissner file
int jm_off;             // Offset of interesting byte
int jm_len;             // Length of block

int
main(int ac, char **av) {
   FILE *in, *out= 0;
   char dmy;
   int quick= 1;
   int len;
   int dump= 0;

   // Handle arguments
   ac--; av++;
   
   while (ac > 0 && av[0][0] == '-' && av[0][1]) {
      char ch, *p= *av++ + 1; 
      ac--;
      while (ch= *p++) switch (ch) {
       case 'j':
          if (--ac < 0) usage();
          o_jm= 1;
          if (2 != sscanf(*av++, "%d/%d %c", &jm_off, &jm_len, &ch)) 
             error("Bad -j option data: %s", av[-1]);
          if (jm_len > 5) error("-j option length too large");
          jm_len--;
          jm_off--;
          if (jm_off >= jm_len) error("-j option offset invalid");
          break;
       case 'q':
          if (--ac < 0) usage();
          if (1 != sscanf(*av++, "%d %c", &quick, &ch) ||
              quick < 1)
             error("Bad -q option data: %s", av[-1]);
          break;
       case 'd':
	  dump= 1;
          break;
       default:
          error("Unknown option: %c", ch);
      }
   }

   if (ac != 2) usage();
   
   if (0 == strcmp("-", av[0])) {
      in= stdin;
   } else {
      in= fopen(av[0], "r");
      if (!in) error("Can't open input file: %s", av[0]);
   }
   av++;
   
   //out= fopen("tmp.raw", "w");

   if (1 != sscanf(*av++, "%lg %c", &rate_in, &dmy) ||
       rate_in < 10 || rate_in > 16384)
      error("Bad data sample-rate: %s", av[-1]);

   init_all();
   audio_init(dump);

   //spf_in= rate_in / 10;	// Input samples per frame
   spf_in= 1;		// Might as well update every frame
   if (spf_in < 1) spf_in= 1;
   spf_out= rate_out / rate_in * spf_in / quick;

   len= floor(spf_out + 2.0);		// Just to be safe
   tmp_out= ALLOC_ARR(len, float);
   buf_out= ALLOC_ARR(2*len, short);

   global_amp= AMP_FACT;
   global_mult= MULT_FACT;

   {
      double cpos= 0;	// Counter for judging output frame length

      while (in) {
	 double opos;
	 short *sp;
	 float *fp;
	 int cnt, a;

	 for (a= 0; a<spf_in; a++) {
	    int ival;
	    double val= 0;
	    
	    if (in && feof(in)) in= 0;
	    if (in) {
	       if (o_jm) {
		  char buf[4];
		  int a;
		  if (fgetc(in) != 3) {
		     a= 1;
		     while (a < 10 && 3 != fgetc(in)) a++;
		     if (a >= 10) error("Lost sync, new sync byte not found");
		     printf("WARNING: loss of sync in file, skipped %d bytes\n", a);
		  }
		  for (a= 0; a<jm_len; a++) buf[a]= fgetc(in);
		  ival= (buf[jm_off] & 255) - 128;
		  val= ival / 128.0;
	       } else {
		  ival= (fgetc(in) & 255);
		  ival += (fgetc(in) & 255) << 8;
		  ival= (ival ^ 32768) - 32768;
		  val= ival/32768.0;
	       }
	    }
	    
	    if (out) {
	       ival= (int)(val * 32767);
	       fputc(ival & 255, out);
	       fputc(ival >> 8, out);
	    }
	    
	    bank_in(val);
	 }

	 opos= cpos;
	 cpos += spf_out;
	 cnt= (int)cpos - (int)opos;
	 bank_out(tmp_out, cnt);

	 // Convert floating point to stereo 16-bit
	 fp= tmp_out;
	 sp= buf_out;
	 for (a= 0; a<cnt; a++) {
	    int ival;
	    float val= *fp++;
	    if (val <= -32768) val= -32768;
	    if (val >= 32767) val= 32767;
	   
	    ival= (int)val;
	    *sp++= ival;
	    *sp++= ival;
	 }
      
	 audio_write((char*)buf_out, cnt * 2 * sizeof(short));
      }
   }
   
   return 0;
}

//
//	Initialise audio device
//

void 
audio_init(int dump) {
   int stereo, rate, fragsize, numfrags, enc;
   int afmt_req, afmt;
   int test= 1;
   int out_buf_ms;
   audio_buf_info info;

   if (dump) {
      if (0 > (out_fd= open("out.raw", O_WRONLY | O_CREAT, 0666)))
	 error("Can't open out.raw, errno %d", errno);
      rate_out= 44100;
      return;
   }

   if (0 > (out_fd= open("/dev/dsp", O_WRONLY)))
      error("Can't open /dev/dsp, errno %d", errno);
    
   afmt= afmt_req= ((char*)&test)[0] ? AFMT_S16_LE : AFMT_S16_BE;
   stereo= 1;
   rate= 44100;
   fragsize= 14;
   numfrags= 4;        
    
   enc= (numfrags<<16) | fragsize;
    
   if (0 > ioctl(out_fd, SNDCTL_DSP_SETFRAGMENT, &enc) ||
       0 > ioctl(out_fd, SNDCTL_DSP_SAMPLESIZE, &afmt) ||
       0 > ioctl(out_fd, SNDCTL_DSP_STEREO, &stereo) ||
       0 > ioctl(out_fd, SNDCTL_DSP_SPEED, &rate))
      error("Can't configure /dev/dsp, errno %d", errno);
    
   if (afmt != afmt_req) 
      error("Can't open device in 16-bit mode");
   if (!stereo)
      error("Can't open device in stereo");
    
   rate_out= rate;
    
   if (-1 == ioctl(out_fd, SNDCTL_DSP_GETOSPACE, &info))
      error("Can't get audio buffer info, errno %d", errno);

   out_buf_ms= (int)(1000.0 * 0.25 * info.fragsize / rate_out);
    
   fprintf(stderr, 
	   "Outputting 16-bit audio at %d Hz with %d %d-sample fragments, %d ms per fragment\n",
	   rate, info.fragstotal, info.fragsize/4, out_buf_ms);
}

//
//	Write audio data to output
//

void 
audio_write(char *buf, int siz) {
   int rv;
   while (-1 != (rv= write(out_fd, buf, siz))) {
      if (0 == (siz -= rv)) return;
      buf += rv;
   }
   error("Output error");
}

// END //
