1/* Sonic library
2   Copyright 2010
3   Bill Cox
4   This file is part of the Sonic Library.
5
6   This file is licensed under the Apache 2.0 license.
7*/
8
9#include <stdio.h>
10#include <stdlib.h>
11#include <string.h>
12#include <stdarg.h>
13#ifdef SONIC_USE_SIN
14#include <math.h>
15#ifndef M_PI
16#define M_PI 3.14159265358979323846
17#endif
18#endif
19#include "sonic.h"
20
21struct sonicStreamStruct {
22    short *inputBuffer;
23    short *outputBuffer;
24    short *pitchBuffer;
25    short *downSampleBuffer;
26    float speed;
27    float volume;
28    float pitch;
29    float rate;
30    int oldRatePosition;
31    int newRatePosition;
32    int useChordPitch;
33    int quality;
34    int numChannels;
35    int inputBufferSize;
36    int pitchBufferSize;
37    int outputBufferSize;
38    int numInputSamples;
39    int numOutputSamples;
40    int numPitchSamples;
41    int minPeriod;
42    int maxPeriod;
43    int maxRequired;
44    int remainingInputToCopy;
45    int sampleRate;
46    int prevPeriod;
47    int prevMinDiff;
48};
49
50/* Just used for debugging */
51/*
52void sonicMSG(char *format, ...)
53{
54    char buffer[4096];
55    va_list ap;
56    FILE *file;
57
58    va_start(ap, format);
59    vsprintf((char *)buffer, (char *)format, ap);
60    va_end(ap);
61    file=fopen("/tmp/sonic.log", "a");
62    fprintf(file, "%s", buffer);
63    fclose(file);
64}
65*/
66
67/* Scale the samples by the factor. */
68static void scaleSamples(
69    short *samples,
70    int numSamples,
71    float volume)
72{
73    int fixedPointVolume = volume*4096.0f;
74    int value;
75
76    while(numSamples--) {
77	value = (*samples*fixedPointVolume) >> 12;
78	if(value > 32767) {
79	    value = 32767;
80	} else if(value < -32767) {
81	    value = -32767;
82	}
83	*samples++ = value;
84    }
85}
86
87/* Get the speed of the stream. */
88float sonicGetSpeed(
89    sonicStream stream)
90{
91    return stream->speed;
92}
93
94/* Set the speed of the stream. */
95void sonicSetSpeed(
96    sonicStream stream,
97    float speed)
98{
99    stream->speed = speed;
100}
101
102/* Get the pitch of the stream. */
103float sonicGetPitch(
104    sonicStream stream)
105{
106    return stream->pitch;
107}
108
109/* Set the pitch of the stream. */
110void sonicSetPitch(
111    sonicStream stream,
112    float pitch)
113{
114    stream->pitch = pitch;
115}
116
117/* Get the rate of the stream. */
118float sonicGetRate(
119    sonicStream stream)
120{
121    return stream->rate;
122}
123
124/* Set the playback rate of the stream. This scales pitch and speed at the same time. */
125void sonicSetRate(
126    sonicStream stream,
127    float rate)
128{
129    stream->rate = rate;
130
131    stream->oldRatePosition = 0;
132    stream->newRatePosition = 0;
133}
134
135/* Get the vocal chord pitch setting. */
136int sonicGetChordPitch(
137    sonicStream stream)
138{
139    return stream->useChordPitch;
140}
141
142/* Set the vocal chord mode for pitch computation.  Default is off. */
143void sonicSetChordPitch(
144    sonicStream stream,
145    int useChordPitch)
146{
147    stream->useChordPitch = useChordPitch;
148}
149
150/* Get the quality setting. */
151int sonicGetQuality(
152    sonicStream stream)
153{
154    return stream->quality;
155}
156
157/* Set the "quality".  Default 0 is virtually as good as 1, but very much faster. */
158void sonicSetQuality(
159    sonicStream stream,
160    int quality)
161{
162    stream->quality = quality;
163}
164
165/* Get the scaling factor of the stream. */
166float sonicGetVolume(
167    sonicStream stream)
168{
169    return stream->volume;
170}
171
172/* Set the scaling factor of the stream. */
173void sonicSetVolume(
174    sonicStream stream,
175    float volume)
176{
177    stream->volume = volume;
178}
179
180/* Free stream buffers. */
181static void freeStreamBuffers(
182    sonicStream stream)
183{
184    if(stream->inputBuffer != NULL) {
185	free(stream->inputBuffer);
186    }
187    if(stream->outputBuffer != NULL) {
188	free(stream->outputBuffer);
189    }
190    if(stream->pitchBuffer != NULL) {
191	free(stream->pitchBuffer);
192    }
193    if(stream->downSampleBuffer != NULL) {
194	free(stream->downSampleBuffer);
195    }
196}
197
198/* Destroy the sonic stream. */
199void sonicDestroyStream(
200    sonicStream stream)
201{
202    freeStreamBuffers(stream);
203    free(stream);
204}
205
206/* Allocate stream buffers. */
207static int allocateStreamBuffers(
208    sonicStream stream,
209    int sampleRate,
210    int numChannels)
211{
212    int minPeriod = sampleRate/SONIC_MAX_PITCH;
213    int maxPeriod = sampleRate/SONIC_MIN_PITCH;
214    int maxRequired = 2*maxPeriod;
215
216    stream->inputBufferSize = maxRequired;
217    stream->inputBuffer = (short *)calloc(maxRequired, sizeof(short)*numChannels);
218    if(stream->inputBuffer == NULL) {
219	sonicDestroyStream(stream);
220	return 0;
221    }
222    stream->outputBufferSize = maxRequired;
223    stream->outputBuffer = (short *)calloc(maxRequired, sizeof(short)*numChannels);
224    if(stream->outputBuffer == NULL) {
225	sonicDestroyStream(stream);
226	return 0;
227    }
228    stream->pitchBufferSize = maxRequired;
229    stream->pitchBuffer = (short *)calloc(maxRequired, sizeof(short)*numChannels);
230    if(stream->pitchBuffer == NULL) {
231	sonicDestroyStream(stream);
232	return 0;
233    }
234    stream->downSampleBuffer = (short *)calloc(maxRequired, sizeof(short));
235    if(stream->downSampleBuffer == NULL) {
236	sonicDestroyStream(stream);
237	return 0;
238    }
239    stream->sampleRate = sampleRate;
240    stream->numChannels = numChannels;
241    stream->oldRatePosition = 0;
242    stream->newRatePosition = 0;
243    stream->minPeriod = minPeriod;
244    stream->maxPeriod = maxPeriod;
245    stream->maxRequired = maxRequired;
246    stream->prevPeriod = 0;
247    return 1;
248}
249
250/* Create a sonic stream.  Return NULL only if we are out of memory and cannot
251   allocate the stream. */
252sonicStream sonicCreateStream(
253    int sampleRate,
254    int numChannels)
255{
256    sonicStream stream = (sonicStream)calloc(1, sizeof(struct sonicStreamStruct));
257
258    if(stream == NULL) {
259	return NULL;
260    }
261    if(!allocateStreamBuffers(stream, sampleRate, numChannels)) {
262        return NULL;
263    }
264    stream->speed = 1.0f;
265    stream->pitch = 1.0f;
266    stream->volume = 1.0f;
267    stream->rate = 1.0f;
268    stream->oldRatePosition = 0;
269    stream->newRatePosition = 0;
270    stream->useChordPitch = 0;
271    stream->quality = 0;
272    return stream;
273}
274
275/* Get the sample rate of the stream. */
276int sonicGetSampleRate(
277    sonicStream stream)
278{
279    return stream->sampleRate;
280}
281
282/* Set the sample rate of the stream.  This will cause samples buffered in the stream to
283   be lost. */
284void sonicSetSampleRate(
285    sonicStream stream,
286    int sampleRate)
287{
288    freeStreamBuffers(stream);
289    allocateStreamBuffers(stream, sampleRate, stream->numChannels);
290}
291
292/* Get the number of channels. */
293int sonicGetNumChannels(
294    sonicStream stream)
295{
296    return stream->numChannels;
297}
298
299/* Set the num channels of the stream.  This will cause samples buffered in the stream to
300   be lost. */
301void sonicSetNumChannels(
302    sonicStream stream,
303    int numChannels)
304{
305    freeStreamBuffers(stream);
306    allocateStreamBuffers(stream, stream->sampleRate, numChannels);
307}
308
309/* Enlarge the output buffer if needed. */
310static int enlargeOutputBufferIfNeeded(
311    sonicStream stream,
312    int numSamples)
313{
314    if(stream->numOutputSamples + numSamples > stream->outputBufferSize) {
315	stream->outputBufferSize += (stream->outputBufferSize >> 1) + numSamples;
316	stream->outputBuffer = (short *)realloc(stream->outputBuffer,
317	    stream->outputBufferSize*sizeof(short)*stream->numChannels);
318	if(stream->outputBuffer == NULL) {
319	    return 0;
320	}
321    }
322    return 1;
323}
324
325/* Enlarge the input buffer if needed. */
326static int enlargeInputBufferIfNeeded(
327    sonicStream stream,
328    int numSamples)
329{
330    if(stream->numInputSamples + numSamples > stream->inputBufferSize) {
331	stream->inputBufferSize += (stream->inputBufferSize >> 1) + numSamples;
332	stream->inputBuffer = (short *)realloc(stream->inputBuffer,
333	    stream->inputBufferSize*sizeof(short)*stream->numChannels);
334	if(stream->inputBuffer == NULL) {
335	    return 0;
336	}
337    }
338    return 1;
339}
340
341/* Add the input samples to the input buffer. */
342static int addFloatSamplesToInputBuffer(
343    sonicStream stream,
344    float *samples,
345    int numSamples)
346{
347    short *buffer;
348    int count = numSamples*stream->numChannels;
349
350    if(numSamples == 0) {
351	return 1;
352    }
353    if(!enlargeInputBufferIfNeeded(stream, numSamples)) {
354	return 0;
355    }
356    buffer = stream->inputBuffer + stream->numInputSamples*stream->numChannels;
357    while(count--) {
358        *buffer++ = (*samples++)*32767.0f;
359    }
360    stream->numInputSamples += numSamples;
361    return 1;
362}
363
364/* Add the input samples to the input buffer. */
365static int addShortSamplesToInputBuffer(
366    sonicStream stream,
367    short *samples,
368    int numSamples)
369{
370    if(numSamples == 0) {
371	return 1;
372    }
373    if(!enlargeInputBufferIfNeeded(stream, numSamples)) {
374	return 0;
375    }
376    memcpy(stream->inputBuffer + stream->numInputSamples*stream->numChannels, samples,
377        numSamples*sizeof(short)*stream->numChannels);
378    stream->numInputSamples += numSamples;
379    return 1;
380}
381
382/* Add the input samples to the input buffer. */
383static int addUnsignedCharSamplesToInputBuffer(
384    sonicStream stream,
385    unsigned char *samples,
386    int numSamples)
387{
388    short *buffer;
389    int count = numSamples*stream->numChannels;
390
391    if(numSamples == 0) {
392	return 1;
393    }
394    if(!enlargeInputBufferIfNeeded(stream, numSamples)) {
395	return 0;
396    }
397    buffer = stream->inputBuffer + stream->numInputSamples*stream->numChannels;
398    while(count--) {
399        *buffer++ = (*samples++ - 128) << 8;
400    }
401    stream->numInputSamples += numSamples;
402    return 1;
403}
404
405/* Remove input samples that we have already processed. */
406static void removeInputSamples(
407    sonicStream stream,
408    int position)
409{
410    int remainingSamples = stream->numInputSamples - position;
411
412    if(remainingSamples > 0) {
413	memmove(stream->inputBuffer, stream->inputBuffer + position*stream->numChannels,
414	    remainingSamples*sizeof(short)*stream->numChannels);
415    }
416    stream->numInputSamples = remainingSamples;
417}
418
419/* Just copy from the array to the output buffer */
420static int copyToOutput(
421    sonicStream stream,
422    short *samples,
423    int numSamples)
424{
425    if(!enlargeOutputBufferIfNeeded(stream, numSamples)) {
426	return 0;
427    }
428    memcpy(stream->outputBuffer + stream->numOutputSamples*stream->numChannels,
429	samples, numSamples*sizeof(short)*stream->numChannels);
430    stream->numOutputSamples += numSamples;
431    return 1;
432}
433
434/* Just copy from the input buffer to the output buffer.  Return 0 if we fail to
435   resize the output buffer.  Otherwise, return numSamples */
436static int copyInputToOutput(
437    sonicStream stream,
438    int position)
439{
440    int numSamples = stream->remainingInputToCopy;
441
442    if(numSamples > stream->maxRequired) {
443	numSamples = stream->maxRequired;
444    }
445    if(!copyToOutput(stream, stream->inputBuffer + position*stream->numChannels,
446	    numSamples)) {
447	return 0;
448    }
449    stream->remainingInputToCopy -= numSamples;
450    return numSamples;
451}
452
453/* Read data out of the stream.  Sometimes no data will be available, and zero
454   is returned, which is not an error condition. */
455int sonicReadFloatFromStream(
456    sonicStream stream,
457    float *samples,
458    int maxSamples)
459{
460    int numSamples = stream->numOutputSamples;
461    int remainingSamples = 0;
462    short *buffer;
463    int count;
464
465    if(numSamples == 0) {
466	return 0;
467    }
468    if(numSamples > maxSamples) {
469	remainingSamples = numSamples - maxSamples;
470	numSamples = maxSamples;
471    }
472    buffer = stream->outputBuffer;
473    count = numSamples*stream->numChannels;
474    while(count--) {
475	*samples++ = (*buffer++)/32767.0f;
476    }
477    if(remainingSamples > 0) {
478	memmove(stream->outputBuffer, stream->outputBuffer + numSamples*stream->numChannels,
479	    remainingSamples*sizeof(short)*stream->numChannels);
480    }
481    stream->numOutputSamples = remainingSamples;
482    return numSamples;
483}
484
485/* Read short data out of the stream.  Sometimes no data will be available, and zero
486   is returned, which is not an error condition. */
487int sonicReadShortFromStream(
488    sonicStream stream,
489    short *samples,
490    int maxSamples)
491{
492    int numSamples = stream->numOutputSamples;
493    int remainingSamples = 0;
494
495    if(numSamples == 0) {
496	return 0;
497    }
498    if(numSamples > maxSamples) {
499	remainingSamples = numSamples - maxSamples;
500	numSamples = maxSamples;
501    }
502    memcpy(samples, stream->outputBuffer, numSamples*sizeof(short)*stream->numChannels);
503    if(remainingSamples > 0) {
504	memmove(stream->outputBuffer, stream->outputBuffer + numSamples*stream->numChannels,
505	    remainingSamples*sizeof(short)*stream->numChannels);
506    }
507    stream->numOutputSamples = remainingSamples;
508    return numSamples;
509}
510
511/* Read unsigned char data out of the stream.  Sometimes no data will be available, and zero
512   is returned, which is not an error condition. */
513int sonicReadUnsignedCharFromStream(
514    sonicStream stream,
515    unsigned char *samples,
516    int maxSamples)
517{
518    int numSamples = stream->numOutputSamples;
519    int remainingSamples = 0;
520    short *buffer;
521    int count;
522
523    if(numSamples == 0) {
524	return 0;
525    }
526    if(numSamples > maxSamples) {
527	remainingSamples = numSamples - maxSamples;
528	numSamples = maxSamples;
529    }
530    buffer = stream->outputBuffer;
531    count = numSamples*stream->numChannels;
532    while(count--) {
533	*samples++ = (char)((*buffer++) >> 8) + 128;
534    }
535    if(remainingSamples > 0) {
536	memmove(stream->outputBuffer, stream->outputBuffer + numSamples*stream->numChannels,
537	    remainingSamples*sizeof(short)*stream->numChannels);
538    }
539    stream->numOutputSamples = remainingSamples;
540    return numSamples;
541}
542
543/* Force the sonic stream to generate output using whatever data it currently
544   has.  No extra delay will be added to the output, but flushing in the middle of
545   words could introduce distortion. */
546int sonicFlushStream(
547    sonicStream stream)
548{
549    int maxRequired = stream->maxRequired;
550    int remainingSamples = stream->numInputSamples;
551    float speed = stream->speed/stream->pitch;
552    float rate = stream->rate*stream->pitch;
553    int expectedOutputSamples = stream->numOutputSamples +
554	(int)((remainingSamples/speed + stream->numPitchSamples)/rate + 0.5f);
555
556    /* Add enough silence to flush both input and pitch buffers. */
557    if(!enlargeInputBufferIfNeeded(stream, remainingSamples + 2*maxRequired)) {
558        return 0;
559    }
560    memset(stream->inputBuffer + remainingSamples*stream->numChannels, 0,
561	2*maxRequired*sizeof(short)*stream->numChannels);
562    stream->numInputSamples += 2*maxRequired;
563    if(!sonicWriteShortToStream(stream, NULL, 0)) {
564	return 0;
565    }
566    /* Throw away any extra samples we generated due to the silence we added */
567    if(stream->numOutputSamples > expectedOutputSamples) {
568	stream->numOutputSamples = expectedOutputSamples;
569    }
570    /* Empty input and pitch buffers */
571    stream->numInputSamples = 0;
572    stream->remainingInputToCopy = 0;
573    stream->numPitchSamples = 0;
574    return 1;
575}
576
577/* Return the number of samples in the output buffer */
578int sonicSamplesAvailable(
579   sonicStream stream)
580{
581    return stream->numOutputSamples;
582}
583
584/* If skip is greater than one, average skip samples together and write them to
585   the down-sample buffer.  If numChannels is greater than one, mix the channels
586   together as we down sample. */
587static void downSampleInput(
588    sonicStream stream,
589    short *samples,
590    int skip)
591{
592    int numSamples = stream->maxRequired/skip;
593    int samplesPerValue = stream->numChannels*skip;
594    int i, j;
595    int value;
596    short *downSamples = stream->downSampleBuffer;
597
598    for(i = 0; i < numSamples; i++) {
599	value = 0;
600        for(j = 0; j < samplesPerValue; j++) {
601	    value += *samples++;
602	}
603	value /= samplesPerValue;
604        *downSamples++ = value;
605    }
606}
607
608/* Find the best frequency match in the range, and given a sample skip multiple.
609   For now, just find the pitch of the first channel. */
610static int findPitchPeriodInRange(
611    short *samples,
612    int minPeriod,
613    int maxPeriod,
614    int *retMinDiff,
615    int *retMaxDiff)
616{
617    int period, bestPeriod = 0, worstPeriod = 255;
618    short *s, *p, sVal, pVal;
619    unsigned long diff, minDiff = 1, maxDiff = 0;
620    int i;
621
622    for(period = minPeriod; period <= maxPeriod; period++) {
623	diff = 0;
624	s = samples;
625	p = samples + period;
626	for(i = 0; i < period; i++) {
627	    sVal = *s++;
628	    pVal = *p++;
629	    diff += sVal >= pVal? (unsigned short)(sVal - pVal) :
630	        (unsigned short)(pVal - sVal);
631	}
632	/* Note that the highest number of samples we add into diff will be less
633	   than 256, since we skip samples.  Thus, diff is a 24 bit number, and
634	   we can safely multiply by numSamples without overflow */
635	if(diff*bestPeriod < minDiff*period) {
636	    minDiff = diff;
637	    bestPeriod = period;
638	}
639	if(diff*worstPeriod > maxDiff*period) {
640	    maxDiff = diff;
641	    worstPeriod = period;
642	}
643    }
644    *retMinDiff = minDiff/bestPeriod;
645    *retMaxDiff = maxDiff/worstPeriod;
646    return bestPeriod;
647}
648
649/* At abrupt ends of voiced words, we can have pitch periods that are better
650   approximated by the previous pitch period estimate.  Try to detect this case. */
651static int prevPeriodBetter(
652    sonicStream stream,
653    int period,
654    int minDiff,
655    int maxDiff,
656    int preferNewPeriod)
657{
658    if(minDiff == 0 || stream->prevPeriod == 0) {
659	return 0;
660    }
661    if(preferNewPeriod) {
662	if(maxDiff > minDiff*3) {
663	    /* Got a reasonable match this period */
664	    return 0;
665	}
666	if(minDiff*2 <= stream->prevMinDiff*3) {
667	    /* Mismatch is not that much greater this period */
668	    return 0;
669	}
670    } else {
671	if(minDiff <= stream->prevMinDiff) {
672	    return 0;
673	}
674    }
675    return 1;
676}
677
678/* Find the pitch period.  This is a critical step, and we may have to try
679   multiple ways to get a good answer.  This version uses AMDF.  To improve
680   speed, we down sample by an integer factor get in the 11KHz range, and then
681   do it again with a narrower frequency range without down sampling */
682static int findPitchPeriod(
683    sonicStream stream,
684    short *samples,
685    int preferNewPeriod)
686{
687    int minPeriod = stream->minPeriod;
688    int maxPeriod = stream->maxPeriod;
689    int sampleRate = stream->sampleRate;
690    int minDiff, maxDiff, retPeriod;
691    int skip = 1;
692    int period;
693
694    if(sampleRate > SONIC_AMDF_FREQ && stream->quality == 0) {
695	skip = sampleRate/SONIC_AMDF_FREQ;
696    }
697    if(stream->numChannels == 1 && skip == 1) {
698	period = findPitchPeriodInRange(samples, minPeriod, maxPeriod, &minDiff, &maxDiff);
699    } else {
700	downSampleInput(stream, samples, skip);
701	period = findPitchPeriodInRange(stream->downSampleBuffer, minPeriod/skip,
702	    maxPeriod/skip, &minDiff, &maxDiff);
703	if(skip != 1) {
704	    period *= skip;
705	    minPeriod = period - (skip << 2);
706	    maxPeriod = period + (skip << 2);
707	    if(minPeriod < stream->minPeriod) {
708		minPeriod = stream->minPeriod;
709	    }
710	    if(maxPeriod > stream->maxPeriod) {
711		maxPeriod = stream->maxPeriod;
712	    }
713	    if(stream->numChannels == 1) {
714		period = findPitchPeriodInRange(samples, minPeriod, maxPeriod,
715		    &minDiff, &maxDiff);
716	    } else {
717		downSampleInput(stream, samples, 1);
718		period = findPitchPeriodInRange(stream->downSampleBuffer, minPeriod,
719		    maxPeriod, &minDiff, &maxDiff);
720	    }
721	}
722    }
723    if(prevPeriodBetter(stream, period, minDiff, maxDiff, preferNewPeriod)) {
724        retPeriod = stream->prevPeriod;
725    } else {
726	retPeriod = period;
727    }
728    stream->prevMinDiff = minDiff;
729    stream->prevPeriod = period;
730    return retPeriod;
731}
732
733/* Overlap two sound segments, ramp the volume of one down, while ramping the
734   other one from zero up, and add them, storing the result at the output. */
735static void overlapAdd(
736    int numSamples,
737    int numChannels,
738    short *out,
739    short *rampDown,
740    short *rampUp)
741{
742    short *o, *u, *d;
743    int i, t;
744
745    for(i = 0; i < numChannels; i++) {
746	o = out + i;
747	u = rampUp + i;
748	d = rampDown + i;
749	for(t = 0; t < numSamples; t++) {
750#ifdef SONIC_USE_SIN
751	    float ratio = sin(t*M_PI/(2*numSamples));
752	    *o = *d*(1.0f - ratio) + *u*ratio;
753#else
754	    *o = (*d*(numSamples - t) + *u*t)/numSamples;
755#endif
756	    o += numChannels;
757	    d += numChannels;
758	    u += numChannels;
759	}
760    }
761}
762
763/* Overlap two sound segments, ramp the volume of one down, while ramping the
764   other one from zero up, and add them, storing the result at the output. */
765static void overlapAddWithSeparation(
766    int numSamples,
767    int numChannels,
768    int separation,
769    short *out,
770    short *rampDown,
771    short *rampUp)
772{
773    short *o, *u, *d;
774    int i, t;
775
776    for(i = 0; i < numChannels; i++) {
777	o = out + i;
778	u = rampUp + i;
779	d = rampDown + i;
780	for(t = 0; t < numSamples + separation; t++) {
781	    if(t < separation) {
782		*o = *d*(numSamples - t)/numSamples;
783		d += numChannels;
784	    } else if(t < numSamples) {
785		*o = (*d*(numSamples - t) + *u*(t - separation))/numSamples;
786		d += numChannels;
787		u += numChannels;
788	    } else {
789		*o = *u*(t - separation)/numSamples;
790		u += numChannels;
791	    }
792	    o += numChannels;
793	}
794    }
795}
796
797/* Just move the new samples in the output buffer to the pitch buffer */
798static int moveNewSamplesToPitchBuffer(
799    sonicStream stream,
800    int originalNumOutputSamples)
801{
802    int numSamples = stream->numOutputSamples - originalNumOutputSamples;
803    int numChannels = stream->numChannels;
804
805    if(stream->numPitchSamples + numSamples > stream->pitchBufferSize) {
806	stream->pitchBufferSize += (stream->pitchBufferSize >> 1) + numSamples;
807	stream->pitchBuffer = (short *)realloc(stream->pitchBuffer,
808	    stream->pitchBufferSize*sizeof(short)*numChannels);
809	if(stream->pitchBuffer == NULL) {
810	    return 0;
811	}
812    }
813    memcpy(stream->pitchBuffer + stream->numPitchSamples*numChannels,
814        stream->outputBuffer + originalNumOutputSamples*numChannels,
815	numSamples*sizeof(short)*numChannels);
816    stream->numOutputSamples = originalNumOutputSamples;
817    stream->numPitchSamples += numSamples;
818    return 1;
819}
820
821/* Remove processed samples from the pitch buffer. */
822static void removePitchSamples(
823    sonicStream stream,
824    int numSamples)
825{
826    int numChannels = stream->numChannels;
827    short *source = stream->pitchBuffer + numSamples*numChannels;
828
829    if(numSamples == 0) {
830	return;
831    }
832    if(numSamples != stream->numPitchSamples) {
833	memmove(stream->pitchBuffer, source, (stream->numPitchSamples -
834	    numSamples)*sizeof(short)*numChannels);
835    }
836    stream->numPitchSamples -= numSamples;
837}
838
839/* Change the pitch.  The latency this introduces could be reduced by looking at
840   past samples to determine pitch, rather than future. */
841static int adjustPitch(
842    sonicStream stream,
843    int originalNumOutputSamples)
844{
845    float pitch = stream->pitch;
846    int numChannels = stream->numChannels;
847    int period, newPeriod, separation;
848    int position = 0;
849    short *out, *rampDown, *rampUp;
850
851    if(stream->numOutputSamples == originalNumOutputSamples) {
852	return 1;
853    }
854    if(!moveNewSamplesToPitchBuffer(stream, originalNumOutputSamples)) {
855	return 0;
856    }
857    while(stream->numPitchSamples - position >= stream->maxRequired) {
858	period = findPitchPeriod(stream, stream->pitchBuffer + position*numChannels, 0);
859	newPeriod = period/pitch;
860	if(!enlargeOutputBufferIfNeeded(stream, newPeriod)) {
861	    return 0;
862	}
863	out = stream->outputBuffer + stream->numOutputSamples*numChannels;
864	if(pitch >= 1.0f) {
865	    rampDown = stream->pitchBuffer + position*numChannels;
866	    rampUp = stream->pitchBuffer + (position + period - newPeriod)*numChannels;
867	    overlapAdd(newPeriod, numChannels, out, rampDown, rampUp);
868	} else {
869	    rampDown = stream->pitchBuffer + position*numChannels;
870	    rampUp = stream->pitchBuffer + position*numChannels;
871	    separation = newPeriod - period;
872	    overlapAddWithSeparation(period, numChannels, separation, out, rampDown, rampUp);
873	}
874	stream->numOutputSamples += newPeriod;
875	position += period;
876    }
877    removePitchSamples(stream, position);
878    return 1;
879}
880
881/* Interpolate the new output sample. */
882static short interpolate(
883    sonicStream stream,
884    short *in,
885    int oldSampleRate,
886    int newSampleRate)
887{
888    short left = *in;
889    short right = in[stream->numChannels];
890    int position = stream->newRatePosition*oldSampleRate;
891    int leftPosition = stream->oldRatePosition*newSampleRate;
892    int rightPosition = (stream->oldRatePosition + 1)*newSampleRate;
893    int ratio = rightPosition - position;
894    int width = rightPosition - leftPosition;
895
896    return (ratio*left + (width - ratio)*right)/width;
897}
898
899/* Change the rate. */
900static int adjustRate(
901    sonicStream stream,
902    float rate,
903    int originalNumOutputSamples)
904{
905    int newSampleRate = stream->sampleRate/rate;
906    int oldSampleRate = stream->sampleRate;
907    int numChannels = stream->numChannels;
908    int position = 0;
909    short *in, *out;
910    int i;
911
912    /* Set these values to help with the integer math */
913    while(newSampleRate > (1 << 14) || oldSampleRate > (1 << 14)) {
914	newSampleRate >>= 1;
915	oldSampleRate >>= 1;
916    }
917    if(stream->numOutputSamples == originalNumOutputSamples) {
918	return 1;
919    }
920    if(!moveNewSamplesToPitchBuffer(stream, originalNumOutputSamples)) {
921	return 0;
922    }
923    /* Leave at least one pitch sample in the buffer */
924    for(position = 0; position < stream->numPitchSamples - 1; position++) {
925	while((stream->oldRatePosition + 1)*newSampleRate >
926	        stream->newRatePosition*oldSampleRate) {
927	    if(!enlargeOutputBufferIfNeeded(stream, 1)) {
928		return 0;
929	    }
930	    out = stream->outputBuffer + stream->numOutputSamples*numChannels;
931	    in = stream->pitchBuffer + position;
932	    for(i = 0; i < numChannels; i++) {
933		*out++ = interpolate(stream, in, oldSampleRate, newSampleRate);
934		in++;
935	    }
936	    stream->newRatePosition++;
937	    stream->numOutputSamples++;
938	}
939	stream->oldRatePosition++;
940	if(stream->oldRatePosition == oldSampleRate) {
941	    stream->oldRatePosition = 0;
942	    if(stream->newRatePosition != newSampleRate) {
943		fprintf(stderr,
944		    "Assertion failed: stream->newRatePosition != newSampleRate\n");
945		exit(1);
946	    }
947	    stream->newRatePosition = 0;
948	}
949    }
950    removePitchSamples(stream, position);
951    return 1;
952}
953
954
955/* Skip over a pitch period, and copy period/speed samples to the output */
956static int skipPitchPeriod(
957    sonicStream stream,
958    short *samples,
959    float speed,
960    int period)
961{
962    long newSamples;
963    int numChannels = stream->numChannels;
964
965    if(speed >= 2.0f) {
966	newSamples = period/(speed - 1.0f);
967    } else {
968	newSamples = period;
969	stream->remainingInputToCopy = period*(2.0f - speed)/(speed - 1.0f);
970    }
971    if(!enlargeOutputBufferIfNeeded(stream, newSamples)) {
972	return 0;
973    }
974    overlapAdd(newSamples, numChannels, stream->outputBuffer +
975        stream->numOutputSamples*numChannels, samples, samples + period*numChannels);
976    stream->numOutputSamples += newSamples;
977    return newSamples;
978}
979
980/* Insert a pitch period, and determine how much input to copy directly. */
981static int insertPitchPeriod(
982    sonicStream stream,
983    short *samples,
984    float speed,
985    int period)
986{
987    long newSamples;
988    short *out;
989    int numChannels = stream->numChannels;
990
991    if(speed < 0.5f) {
992        newSamples = period*speed/(1.0f - speed);
993    } else {
994        newSamples = period;
995	stream->remainingInputToCopy = period*(2.0f*speed - 1.0f)/(1.0f - speed);
996    }
997    if(!enlargeOutputBufferIfNeeded(stream, period + newSamples)) {
998	return 0;
999    }
1000    out = stream->outputBuffer + stream->numOutputSamples*numChannels;
1001    memcpy(out, samples, period*sizeof(short)*numChannels);
1002    out = stream->outputBuffer + (stream->numOutputSamples + period)*numChannels;
1003    overlapAdd(newSamples, numChannels, out, samples + period*numChannels, samples);
1004    stream->numOutputSamples += period + newSamples;
1005    return newSamples;
1006}
1007
1008/* Resample as many pitch periods as we have buffered on the input.  Return 0 if
1009   we fail to resize an input or output buffer.  Also scale the output by the volume. */
1010static int changeSpeed(
1011    sonicStream stream,
1012    float speed)
1013{
1014    short *samples;
1015    int numSamples = stream->numInputSamples;
1016    int position = 0, period, newSamples;
1017    int maxRequired = stream->maxRequired;
1018
1019    if(stream->numInputSamples < maxRequired) {
1020	return 1;
1021    }
1022    do {
1023	if(stream->remainingInputToCopy > 0) {
1024            newSamples = copyInputToOutput(stream, position);
1025	    position += newSamples;
1026	} else {
1027	    samples = stream->inputBuffer + position*stream->numChannels;
1028	    period = findPitchPeriod(stream, samples, 1);
1029	    if(speed > 1.0) {
1030		newSamples = skipPitchPeriod(stream, samples, speed, period);
1031		position += period + newSamples;
1032	    } else {
1033		newSamples = insertPitchPeriod(stream, samples, speed, period);
1034		position += newSamples;
1035	    }
1036	}
1037	if(newSamples == 0) {
1038	    return 0; /* Failed to resize output buffer */
1039	}
1040    } while(position + maxRequired <= numSamples);
1041    removeInputSamples(stream, position);
1042    return 1;
1043}
1044
1045/* Resample as many pitch periods as we have buffered on the input.  Return 0 if
1046   we fail to resize an input or output buffer.  Also scale the output by the volume. */
1047static int processStreamInput(
1048    sonicStream stream)
1049{
1050    int originalNumOutputSamples = stream->numOutputSamples;
1051    float speed = stream->speed/stream->pitch;
1052    float rate = stream->rate;
1053
1054    if(!stream->useChordPitch) {
1055	rate *= stream->pitch;
1056    }
1057    if(speed > 1.00001 || speed < 0.99999) {
1058	changeSpeed(stream, speed);
1059    } else {
1060        if(!copyToOutput(stream, stream->inputBuffer, stream->numInputSamples)) {
1061	    return 0;
1062	}
1063	stream->numInputSamples = 0;
1064    }
1065    if(stream->useChordPitch) {
1066	if(stream->pitch != 1.0f) {
1067	    if(!adjustPitch(stream, originalNumOutputSamples)) {
1068		return 0;
1069	    }
1070	}
1071    } else if(rate != 1.0f) {
1072	if(!adjustRate(stream, rate, originalNumOutputSamples)) {
1073	    return 0;
1074	}
1075    }
1076    if(stream->volume != 1.0f) {
1077	/* Adjust output volume. */
1078        scaleSamples(stream->outputBuffer + originalNumOutputSamples*stream->numChannels,
1079	    (stream->numOutputSamples - originalNumOutputSamples)*stream->numChannels,
1080	    stream->volume);
1081    }
1082    return 1;
1083}
1084
1085/* Write floating point data to the input buffer and process it. */
1086int sonicWriteFloatToStream(
1087    sonicStream stream,
1088    float *samples,
1089    int numSamples)
1090{
1091    if(!addFloatSamplesToInputBuffer(stream, samples, numSamples)) {
1092	return 0;
1093    }
1094    return processStreamInput(stream);
1095}
1096
1097/* Simple wrapper around sonicWriteFloatToStream that does the short to float
1098   conversion for you. */
1099int sonicWriteShortToStream(
1100    sonicStream stream,
1101    short *samples,
1102    int numSamples)
1103{
1104    if(!addShortSamplesToInputBuffer(stream, samples, numSamples)) {
1105	return 0;
1106    }
1107    return processStreamInput(stream);
1108}
1109
1110/* Simple wrapper around sonicWriteFloatToStream that does the unsigned char to float
1111   conversion for you. */
1112int sonicWriteUnsignedCharToStream(
1113    sonicStream stream,
1114    unsigned char *samples,
1115    int numSamples)
1116{
1117    if(!addUnsignedCharSamplesToInputBuffer(stream, samples, numSamples)) {
1118	return 0;
1119    }
1120    return processStreamInput(stream);
1121}
1122
1123/* This is a non-stream oriented interface to just change the speed of a sound sample */
1124int sonicChangeFloatSpeed(
1125    float *samples,
1126    int numSamples,
1127    float speed,
1128    float pitch,
1129    float rate,
1130    float volume,
1131    int useChordPitch,
1132    int sampleRate,
1133    int numChannels)
1134{
1135    sonicStream stream = sonicCreateStream(sampleRate, numChannels);
1136
1137    sonicSetSpeed(stream, speed);
1138    sonicSetPitch(stream, pitch);
1139    sonicSetRate(stream, rate);
1140    sonicSetVolume(stream, volume);
1141    sonicSetChordPitch(stream, useChordPitch);
1142    sonicWriteFloatToStream(stream, samples, numSamples);
1143    sonicFlushStream(stream);
1144    numSamples = sonicSamplesAvailable(stream);
1145    sonicReadFloatFromStream(stream, samples, numSamples);
1146    sonicDestroyStream(stream);
1147    return numSamples;
1148}
1149
1150/* This is a non-stream oriented interface to just change the speed of a sound sample */
1151int sonicChangeShortSpeed(
1152    short *samples,
1153    int numSamples,
1154    float speed,
1155    float pitch,
1156    float rate,
1157    float volume,
1158    int useChordPitch,
1159    int sampleRate,
1160    int numChannels)
1161{
1162    sonicStream stream = sonicCreateStream(sampleRate, numChannels);
1163
1164    sonicSetSpeed(stream, speed);
1165    sonicSetPitch(stream, pitch);
1166    sonicSetRate(stream, rate);
1167    sonicSetVolume(stream, volume);
1168    sonicSetChordPitch(stream, useChordPitch);
1169    sonicWriteShortToStream(stream, samples, numSamples);
1170    sonicFlushStream(stream);
1171    numSamples = sonicSamplesAvailable(stream);
1172    sonicReadShortFromStream(stream, samples, numSamples);
1173    sonicDestroyStream(stream);
1174    return numSamples;
1175}
1176