/* aplay.c a looping .wav player with time and pitch shifting */

#include <stdio.h>
#include <linux/soundcard.h>
#include <sys/ioctl.h>
#include <math.h>

#include <signal.h> /* for signal handler */ 

/* for open() call */
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>

#include <stdlib.h>   /* for atof */
#define MAXWIN	65536


char    *progname;
int begin = 0;
int end = 0;
double interp1(); 
double poly2();
double w();		/* pitch interpolator window function */
double look();

static char id[]="@(#)richard_walker@omnisterra.com $Header: aplay13.c$";
int done=0;
void cleanup();
int resolution = 16;	/* can be 8 or 16 */
int rate = 44100;	/* sample rate */
int stereoflag = 1;	/* 0=mono, 1=stereo */
int debug=0;
int rsize;		/* size of basic audio read unit in bytes */
double balance = -99.0;

int agc=0;
#define EPS 0.0001

main(argc, argv)    /* looping audio player */
int argc;
char    *argv[];
{

    extern int  optind;  /* argv index of next option */
    extern int  opterr;
    extern char *optarg;
    int gval;
    int errflag;
    int temp, returncode;
    int ret;

    int infd;
    int outfd=0;	/* output file descriptor, zero means use /dev/dsp */
    unsigned char buf[4];
    int i=0;
    int s=0;
    int start, stop;
    double offset;
    int old_offset;
    int loop = 1;
    int loopcount = 1;
    double begin = 0.0;
    double end = 0.0;
    double delta = 1.0;
    double speed = 1.0;
    double pitch = 1.0;
    double transpose = 0.0;
    int wsize = 0;
    int verbose = 0;
    double samp_l = 0.0;
    double samp_r = 0.0;
    unsigned char c;

    /* pitch shift variables */
    double d_l[MAXWIN];
    double d_r[MAXWIN];
    int dp;
    double x,out,w1,w2;
    double rp;
    double scale;

    errflag = 0;
    opterr = 0;     /* disables getopt's error msg's */
    progname = argv[0];

    signal(SIGINT, cleanup);
    signal(SIGQUIT, cleanup);

    /* void (*signal(int signum, void (*handler)(int)))(int);  */

    while ((gval = getopt(argc, argv, "Ab:de:k:l:n:o:p:r:s:t:MSvw:")) != EOF)
        switch (gval) {
        case 'A':
            agc++;
            break;
        case 'b':
            begin = (atof(optarg));
            break;
        case 'd':
            debug++;
            break;
        case 'e':
            end = (atof(optarg));
            break;
        case 'k':
            balance = (atof(optarg));
	    if (balance < -1.0 || balance > 1.0) {
		fprintf(stderr,"karaoke balance must be between -1.0 and 1.0\n");
		exit(1);
	    }
	    break;
        case 'l':
	    if(sscanf(optarg, "%d", &loop) == 0) {
		fprintf(stderr,"invalid loop count\n");
		exit(1);
	    }
	    if (loop<=0) {
		loop=0;
	    }
	    loopcount=loop;
            break;
        case 'n':
            resolution = (int) (atof(optarg));
	    if (resolution != 8 && resolution != 16) {
		fprintf(stderr,"error resolution must be 8 or 16\n");
		exit(1);
	    }
            break;
        case 'o':
	    if( (outfd=open(optarg,O_RDWR|O_CREAT,0777)) == -1) {
		fprintf(stderr,"error opening %s\n",optarg);
		exit(1);
	    }
            break;
        case 'r':
            rate = (int) (atof(optarg));
            break;
        case 'p':
            pitch = (atof(optarg));
            break;
        case 's':
            speed = (atof(optarg));
            break;
        case 't':
            transpose = (double) ((atof(optarg)));
	    if (transpose > 12.0 || transpose < -12.0) {
		fprintf(stderr,"transpose limited to +/- 12, got %g\n", transpose);
		exit(1);
	    }
            break;
        case 'v':
            verbose++;
            break;
        case 'M':
            stereoflag=0;
            break;
        case 'S':
            stereoflag=1;
            break;
        case 'w':
            wsize = (int) (atof(optarg));
	    if (wsize >= MAXWIN || wsize < 10) {
		/* complain */
		fprintf(stderr,"warning: wsize must be less than %d\n", MAXWIN);
		/* and unset it so it will get default value */
		wsize = 0;
	    }
            break;
        case '?':
            errflag++;
            break;
        }

    /* set pitch to track speed, such that musical pitch stays */
    /* constant otherwise, set the actual pitch taking speed and */
    /* desired transpose steps into account */

    pitch = pow(2.0,transpose/12.0)*pitch*1.0/speed;

    /* if window size is not yet set, then set it to be equal to */
    /* 1500/speed .  This keeps the window time constant at the */
    /* minimum noticeable frame size */

    if (wsize == 0) {
	wsize = (int) (((double) 1500)/speed);
	if (wsize > MAXWIN) {
	    fprintf(stderr,"warning: wsize must be less than %d\n", MAXWIN);
	    wsize == MAXWIN;
	}
    }

    if (errflag) {
        fprintf(stderr, "usage: %s [<options>] <filename>\n",progname);
        fprintf(stderr, "\t[-A \"enable fast attack slow decay AGC\"\n");
        fprintf(stderr, "\t[-b <begintime(seconds)>]\n");
        fprintf(stderr, "\t[-e <endtime(seconds)>]\n");
        fprintf(stderr, "\t[-k <right channel gain>] \"karaoke mode\"\n");
        fprintf(stderr, "\t[-l <count> \"loop the song\" (0=infinite)\n");
        fprintf(stderr, "\t[-n <resolution>] \"8, or 16 bits\"\n");
        fprintf(stderr, "\t[-o <output_file>]\n");
        fprintf(stderr, "\t[-p <factor>] \"shift pitch by factor\"\n");
        fprintf(stderr, "\t[-r <rate>] \"sample rate\"\n");
        fprintf(stderr, "\t[-s <factor>] \"play at speed\"\n");
        fprintf(stderr, "\t[-t <n>] \"transpose +/- n half steps\"\n");
        fprintf(stderr, "\t[-M] \"play mono (stereo is default)\"\n");
        fprintf(stderr, "\t[-S] \"play stereo (stereo is default)\"\n");
        fprintf(stderr, "\t[-v] \"verbose mode\"\n");
        fprintf(stderr, "\t[-w <size>] \"pitch shift window size\"\n");
        exit(2);
    } 

    /* set read size for converting between seconds and file offset */
    rsize = resolution/8;
    if (stereoflag) {
	rsize *= 2;
    } 

    if (optind == argc) { /* no file specified */
	infd=(int) 0; 	 /* ... 0 is stdin */
    } else {
	/* notice that this call must only use O_RD rather
	   than O_RDWR, because this would otherwise result
	   in a situation where the program wouldn't be allowed
	   to do something that was otherwise quite reasonable */

	   /* like trying to play a file that someone gives you
	   read, but NOT write permission - in fact this bars you
	   from nothing and is quite fair because you can always
	   make your own copy and simulate everything that you
	   could ever do to HIS without actually doing so in HIS
	   world... */

	if( (infd=open(argv[optind],O_RDWR)) == -1) {
	    fprintf(stderr,"error opening %s\n",argv[optind]);
	    exit(1);
	}
    }

    if (!outfd) {
	if( (outfd=open("/dev/dsp",O_WRONLY|O_SYNC)) == -1) {
	    fprintf(stderr,"error opening /dev/dsp\n");
	    exit(1);
	}

	/* ioctls should always be set in order of */
	/* format, channels, sample rate */

	if( ioctl(outfd,SNDCTL_DSP_RESET, (char *) NULL) != 0) {
	    fprintf(stderr,"error on IOCTL RESET\n");
	}

	if (resolution == 8) {
	    ret = AFMT_U8;
	} else {
	    ret = AFMT_S16_LE;
	}
	if( ioctl(outfd,SNDCTL_DSP_SETFMT, &ret) != 0) {
	    fprintf(stderr,"error on IOCTL SETFMT\n");
	}

	ret = stereoflag; 	/* 0=mono, 1=stereo */
	if( ioctl(outfd,SNDCTL_DSP_STEREO, &ret) == -1) {
	    fprintf(stderr,"error on IOCTL STEREO\n");
	}

	ret = rate;
	if( ioctl(outfd,SNDCTL_DSP_SPEED, &ret) == -1) {
	    fprintf(stderr,"error on IOCTL SPEED\n");
	}
	if (ret != rate) {
	    fprintf(stderr,"warning: sample rate set to %d\n", ret);
	}
    } 

    if (begin > 0.0) {
	start = (off_t) (((double) rate)*begin*rsize);
	/* round off to multiple of 4 bytes for safety */
	start = 4*(start/4);
    } else {
	start = 0;
    }
    if (end > begin) {
	stop = (off_t) (((double) rate)*end*rsize);
	stop = 4*(stop/4);
    } else {
	stop = 0;
    }

    if (speed != 1.0) {
	delta = speed;
    }

    if (verbose) {
	printf("begin=%g, start=%d, end=%g, stop=%d",begin,start,end,stop);
    }

    lseek(infd, (off_t) start, SEEK_SET);
    offset=(double) start;
    old_offset = (int) (offset/((double) (rate*rsize)));

    if (verbose) {
	if (loop <= 0) {
	    printf("\nlooping forever: count=%d: ", 1-loop);
	} else {
	    printf("\nloop %d of %d: ", loopcount-loop+1, loopcount);
	}
    }

    dp=0; rp=0.0;
    while (!done) {
	if (getval(infd, &samp_l, &samp_r, delta) == 0) {
	    if (--loop) {	/* loop on end of file */
		if (verbose) {
		    if (loop <= 0) {
			printf("\nlooping forever: count=%d: ", 1-loop);
		    } else {
			printf("\nloop %d of %d: ", loopcount-loop+1, loopcount);
		    }
		}
		lseek(infd, (off_t) start, SEEK_SET);
		offset= (double) start;
		old_offset = (int) (offset/((double) (rate*rsize)));
	    } else {
		done++;
	    }
	}
	/* loop when hitting stop */
	if ((stop != 0) && (int) offset >= stop) {
	    if (--loop) {
		if (verbose) {
		    if (loop <= 0) {
			printf("\nlooping forever: count=%d: ", 1-loop);
		    } else {
			printf("\nloop %d of %d: ", loopcount-loop+1, loopcount);
		    }
		}
		lseek(infd, (off_t) start, SEEK_SET);
		offset= (double) start;
		old_offset = (int) (offset/((double) (rate*rsize)));
	    } else {
		done++; 	/* not looping, but should stop */
	    }
	}

	if (fabs(pitch-1.0) >= 1e-3) {
	    d_l[dp] = samp_l; 
	    d_r[dp] = samp_r; 
	    dp = (++dp)%wsize;

	    rp = fmod(rp+pitch, (double) wsize);

	    /* create gain blending factors for read pointers */
	    w1 = w((double)((((int)rp)-dp+wsize)%wsize),(double)wsize);
	    w2 = w((double)((((int)rp)-dp+wsize+wsize/4)%wsize),(double)wsize);

	    /* force w1,w2 to sum to unity power */
	    /* or sum to unity gain ... (pick one) */
	    scale = (w1+w2)*1.5;
	    scale = sqrt(w1*w1+w2*w2)*1.5;

	    w1 /= scale;
	    w2 /= scale;

	    /* sample = (w1)*d[(int)rp] + (w2)*d[((int)rp+wsize/2)%wsize]; */

	    samp_l = (w1)*look(d_l, rp, wsize) + 
		        (w2)*look(d_l, rp+(double) wsize/4.0, wsize);
	    samp_r = (w1)*look(d_r, rp, wsize) + 
		        (w2)*look(d_r, rp+(double) wsize/4.0, wsize);
	} 

	if (!debug) {
	    if (resolution == 8) {
		/* only output the high byte */
		c=(unsigned char) (((int) samp_l >> 8) & 0xff);
		bwrite(outfd, &c, (size_t) 1); 
		if (stereoflag) {	/* write second channel */
		    c=(unsigned char) (((int) samp_r >> 8) & 0xff);
		    bwrite(outfd, &c, (size_t) 1); 
		}
	    } else {
		/* output 16 bit sample, low byte, then high byte */
		c=(unsigned char) ((int) samp_l & 0xff);
		bwrite(outfd, &c, (size_t) 1);
		c=(unsigned char) (((int) samp_l >> 8) & 0xff);
		bwrite(outfd, &c, (size_t) 1); 
		if (stereoflag) {	/* write second channel */
		    c=(unsigned char) ((int) samp_r & 0xff);
		    bwrite(outfd, &c, (size_t) 1);
		    c=(unsigned char) (((int) samp_r >> 8) & 0xff);
		    bwrite(outfd, &c, (size_t) 1); 
		}
	    }
	} else {	/* in debug mode, output ascii values */
	    if (stereoflag) {	/* write second channel */
		printf("%d %g\n", (int)(offset/(double) (rsize)), (samp_l+samp_r)/2.0);
	    } else {
		printf("%d %g\n", (int)(offset/(double) (rsize)), samp_l);
	    }
	}
	offset+=delta*rsize;

	if (verbose && 
	    (((int)(offset/((double) (rate*rsize))) >
	    old_offset))) {

	    old_offset = (int) (offset/((double) (rate*rsize)));
	    printf("%-3d ",old_offset);
	    if (old_offset%20 == 0) {
		printf("\n");
	    }
	    fflush(stdout);
	}

    } 

    /* done is set only on interrupt, so clear buffer immediately */
    if (done) {
	if (verbose) {
	    printf("\ncalling DSP RESET\n");
	}
	if( ioctl(outfd,SNDCTL_DSP_RESET, (char *) NULL) != 0) {
	    fprintf(stderr,"error on IOCTL RESET\n");
	}
	/* printf("finished DSP RESET\n"); */
    }

    /* printf("closing infd %d\n", close(infd)); */
    /* printf("closing outfd %d\n", close(outfd)); */

    if (done) {
	exit(1);		/* signal error */
    } else {
	exit(0);		/* normal exit */
    }
}

/* buffered write */
bwrite(outfd, c, n)
int outfd;
unsigned char *c;
int n;
{
    static int size=4;
    static int count=0;
    int ret;
    unsigned char bwbuf[size];

    /* write (outfd, c, (size_t) size); */
    /* return(1); */

    if (n!=1) { 
	fprintf(stderr,"error in bwrite: can only write 1 byte at a time\n");
	exit(1);
    }

    bwbuf[count]=*c;
    count++;
    if (count == size) {
	ret=write(outfd, bwbuf, (size_t) size);
	if (ret != size) {
	    fprintf(stderr,"error in bwrite: outfd=%d, ret=%d\n",outfd, ret);
	    exit(1);
	}
	count=0;
    }
}

/* do linear interpolation on circular ring buffer d, with num */
/* elements = wsize and index = (double) rp */
double look(d, rp, wsize) 
double *d;
double rp;
int wsize;
{
    double val1, val2, del;

    val1 = d[((int)rp)%wsize];
    val2 = d[((int) rp+1)%wsize];
    del = rp - (double) ((int) rp);
    
    return val1*(1.0-del) + val2*del;
}

/* program that does linear interpolation between
** quadratic approximants to input x,y pairs
** (original awk program) RCW 10/28/93 converted to c 10/01/96
**
** For four points (x0,y0),(x1,y1)...(x3,y3) compute 2nd order
** approximations for the pair-triples: {0,1,2} and {1,2,3}.  The output
** points from x=x1 to x=x2 are then linearly interpolated from the
** values of the polynomials for the two regions. 
**
** This results in a continuous first derivative of the interpolated
** curve.  The shape of the curve between any two points is controlled by
** the two points on either side of the interval.  Perfect interpolation
** is achieved for linear and quadratic inputs. 
*/


getval(fd, left, right, delta) /* read vals from disk on t=1,2,3... boundaries */
int fd;                  /* return vals at monotonic floating point times */
double *left;            /* using cubic spline interpolation */
double *right;           /* using cubic spline interpolation */
double delta;
{
    static double x0, tmp;
    static double ly0, ly1, ly2, ly3;
    static double ry0, ry1, ry2, ry3;
    static double index = 0.0;
    static double time = 0.0;
    double mono;
    double l,r;
    double t1, t2;
    static int was_called = 0;
    static double peak = 0.0;
    double max,gain;
    int i;

    if (!was_called) {   /* initialize buffers */
	for (i=0; i<=3; i++) {
	    ly3 = ly2; ly2 = ly1; ly1 = ly0;
	    ry3 = ry2; ry2 = ry1; ry1 = ry0;
	    x0 = (double) i;
	    if (readval(fd, &ry0, &ly0) == 0) {
		return(0);
	    }
	}
	time = 3.0;
	was_called++;
    } 

    time+=delta;

    while (x0-1.0 < time) {
	ly3 = ly2; ly2 = ly1; ly1 = ly0;
        ry3 = ry2; ry2 = ry1; ry1 = ry0;
	x0++;
	if (readval(fd, &ry0, &ly0) == 0) {
	    return(0);
	}
    }

    /* interpolate samples for l,r channels */
    t1 = poly2(time,x0,     x0-1.0, x0-2.0, ly0,ly1,ly2);
    t2 = poly2(time,x0-1.0, x0-2.0, x0-3.0, ly1,ly2,ly3);
    l = interp1(x0-2.0, x0-1.0,time,t1,t2);

    t1 = poly2(time,x0,     x0-1.0, x0-2.0, ry0,ry1,ry2);
    t2 = poly2(time,x0-1.0, x0-2.0, x0-3.0, ry1,ry2,ry3);
    r = interp1(x0-2.0, x0-1.0,time,t1,t2);

    if (sqrt(l*l) >  sqrt(r*r)) {
	max = sqrt(l*l);
    } else {
	max = sqrt(r*r);
    }

    if (max > peak) {
	peak = max;
    } else {
	peak = (1.0-EPS)*peak + EPS*max;
    }

    gain = pow(32700.0/peak,0.8);

    if (agc) {
	*left = l*gain;
	*right= r*gain;
    } else {
	*left = l;
	*right= r;
    }

    /* karaoke mode, turn into mono using the either the sum or */
    /* the difference of two channels with the gain of right */
    /* channel being "balance" and the left being 1-mag(balance) */

    if (balance <= 1.0 && balance >= -1.0) {
	mono = *right*balance;
	mono += *left*(1.0 - sqrt(balance*balance));
	*left = mono;
	*right = mono;
    }

    return(1);
}


/*  second order interpolating polynomial */
/*  given x1 <= x <= x3, and (x1,y1) (x2,y2) (x3,y3) */

double poly2(x,x1,x2,x3,y1,y2,y3)
double x,x1,x2,x3,y1,y2,y3;
{
    double temp;
    temp  = y1*(x-x2)*(x-x3)/((x1-x2)*(x1-x3));
    temp += y2*(x-x1)*(x-x3)/((x2-x1)*(x2-x3));
    temp += y3*(x-x1)*(x-x2)/((x3-x1)*(x3-x2));
    /* printf("poly: x=%g, x1=%g, x2=%g, x3=%g, y1=%g, y2=%g, y3=%g, out=%g\n",
	    x, x1, x2, x3, y1, y2, y3, temp); */
    return(temp);
}

/* given x0 < x < x1, and g(x) = g, and	h(x) = h */
/* do a linear interpolation between the values of  */
/* g(x) and h(x) as a function of x position between */
/* x0 and x1.  (gives continuous 1st-derivative) */

double interp1(x0,x1,x,g,h) 
double x0,x1,x,g,h;
{
    double k;
    k = (x-x0)/(x1-x0);
    return(g*k + h*(1-k));
}

/* TODO: needs to be written to work (interpolate) with doubles and 
 * to properly read/write either chars or doubles depending on
 * the soundfile resolution.
 */

int readval(fd, left, right)
int fd;
double *left;
double *right;
{
    unsigned char ibuf[4];

    if (resolution == 8) {
	if (stereoflag) {
	    if (read(fd, &ibuf, (size_t) 2) == 0) {
		return(0);
	    }
	    *right = 256.0*(double) (ibuf[0]-128);
	    *left  = 256.0*(double) (ibuf[1]-128);
	} else {
	    if (read(fd, &ibuf, (size_t) 1) == 0) {
		return(0);
	    }
	    *right = 256.0*(double) (ibuf[0]-128);
	    *left  = *right;
	}
    } else {
	if (stereoflag) {
	    /* read full stereo sample */
	    if (read(fd, &ibuf, (size_t) 4) == 0) {
		return(0);
	    }

	    if (ibuf[1] > 128) { /* right channel */
		*right=((((double) ibuf[1]) * 256.0 + ((double) ibuf[0])) -65536.0);
	    } else {
		*right=(((double) ibuf[1]) * 256.0 + ((double) ibuf[0]));
	    }

	    if (ibuf[3] > 128) { /* left channel */
		*left=((((double) ibuf[3]) * 256.0 + ((double) ibuf[2])) -65536.0);
	    } else {
		*left=(((double) ibuf[3]) * 256.0 + ((double) ibuf[2]));
	    }

	} else {
	    if (read(fd, &ibuf, (size_t) 2) == 0) {
		return(0);
	    }

	    if (ibuf[1] > 128) { /* right channel */
		*right=((((double) ibuf[1]) * 256.0 + ((double) ibuf[0])) -65536.0);
	    } else {
		*right=(((double) ibuf[1]) * 256.0 + ((double) ibuf[0]));
	    }
	    *left=*right;
	}
    }
    return(1);
}

void cleanup(x)
int x;
{
    /* set a global variable to stop the main loop */
    done++;
    printf("done(%d)!\n",x);
}


double w(x,n) 	/* compute windowing function */
double x;
double n;
{
    return sqrt(1.0-cos(2.0*3.1415926*x/n));
    return sqrt(((n/2.0)-fabs(x-(n/2.0)))/(n/2.0)); 
}