/* Simple SONAR Routines.
The basic idea is:
1.) Play a sound out the speaker.
2.) Let the sound bounce off of objects in front of the speaker.
3.) Use the microphone to record the echos.
4.) Based on the time delay, determine the distance to each object.

Sadly, it's not quite so simple in the real world-- real speakers
send out a long, waving signal instead of a perfect pop, so the
echos from close objects tend to blend together.  Also, with shorter
signals, it gets tough to even hear the echo from small objects
(the speaker can't send out much power in a tiny fraction of a 
second).

That is, for a high-resolution output image, we want to play
a very short pop; but speakers *can't* do that, and a 
longer signal would have more power anyway.

We can use a trick to solve this problem-- we can send out
a long signal, which will result in one object's echo bleeding
over into the surrounding objects' echos, but then use signal
processing to bring the image back into focus.


I send out a *very* long pulse-- thousands of samples (about 1/20 second).
The "chirp" pulse must satisfy these constraints:

1.) It must come out of my speaker. I use a Radio Shack
Piezo buzzer, which has excellent freqency response compared
with the typical magnet/coil paper-cone speaker.  There is
a severe resonance effect around 2 kHz, however, so the chirp
has to start somewhere above 3-4 kHz.
2.) It must be recorded on my microphone and sound input 
hardware.  I have 44 kHz/16 bit (stereo) sound (like everyone 
else in the world), so the upper limit is 22 kHz.
3.) It must have very little power when multiplied by a shifted
version of itself.  That is, the autocorrelation function must
drop off quickly (within a few pixels).  This lets us separate
the echos from nearby objects.

Thus I chose a pulse that starts out at 4 kHz, and slowly 
ramps up (linearly) to 22 kHz.  I play this out my piezo speaker,
let it echo, and record the mixed-together echos of the objects 
in front of the speaker.  Property 3.) lets me separate out the
mixed echos of all the objects by multiplying the signal by
shifted versions of the chirp I sent out-- the echos of all 
the other objects will cancel out (have little power), and
I'll get just the echo from the object at the given shift.

That is, I calculate:

	echo[lag] := sum (signal * (chirp shifted by lag))

Since the chirp is thousands of samples, and we must sum it
for thousands of lags, computing this sum in a straightforward 
fashion is very slow.  Thus we use the famous Fast Fourier 
Transform (FFT) and the equally famous FFT Convolution theorem:

	echo := IFFT [  FFT[signal] * FFT[chirp]~  ]


Orion Sky Lawlor, olawlor@acm.org, 10/11/1999
*/
#include <stdio.h>
#include <math.h>
#include "dsp.h"
#include "fcomplex.h"
#include "fftext.h"
#include "dsp_sonar.h"

//********** Utility routines *******************/
int dsp_sonar::bufferLength(void)//Get preferred buffer length
{
	return 8*512;
}

//Render the given colors starting at startY, repeating
// each line vertRep times, and ending at line[len].
void dsp_sonar::renderLine(unsigned char *line,int len,int vertRep,int startY)
{
	int x,y;
	int wid=off->wid;
	int maxChunk=len/wid,chunkNo;
	for (chunkNo=0;chunkNo<maxChunk;chunkNo++)
		for (y=0;y<vertRep;y++)
		{
			unsigned char *in=&(line[chunkNo*wid]);
			unsigned char *out=&(off->baseAddr[(startY+chunkNo*vertRep+y)*off->rowBytes]);
			for (x=0;x<wid;x++)
				out[x]=in[x];
		}
}

static const double shrt2flt=1.0/65536.0,flt2shrt=65536.0;

//Convert a floating-point value to unsigned char dB
unsigned char cvt2dB(float v,float scalePostLog,float scalePreLog);
unsigned char cvt2dB(float v,float scalePostLog,float scalePreLog)
{
	if (v<0) v=-v;
	float dB=log10(v*scalePreLog)*scalePostLog;
	if (dB>255.0) dB=255.0;
	if (dB<0.0) dB=0.0;
	return (unsigned char)dB;
}

template <class shrt>
void writeBuf(char *name,shrt *buf,int len)
{
	FILE *f=fopen(name,"wb");
	int i;
	for (i=0;i<len;i++)
		fprintf(f,"%d\t%.3f\n",i,(float)buf[i]);
	fclose(f);
}
template <class shrt>
void readBuf(char *name,shrt *buf,int len)
{
	FILE *f=fopen(name,"rb");
	if (f==NULL) {fprintf(stderr,"ERROR! Can't open file '%s'!\n",name);exit(1);}
	int i;
	for (i=0;i<len;i++)
	{
		int ignored;float val;
		if (2!=fscanf(f,"%d%f",&ignored,&val))
			{fprintf(stderr,"ERROR! Can't read element %d from file '%s'!\n",i,name);exit(1);}
		buf[i]=val;
	}
	fclose(f);
}


/******************* Do Sonar Processing **********************/
void dsp_sonar::process(int buffer)
{
	int i;
	short *curBuf=bufferList[buffer];
	/*for(i=0;i<fftLen;i++)//Draw raw sound
		outLine[i]=cvt2dB(curBuf[i],30.0);
	renderLine(outLine,fftLen,4,0);*/
	
//Convert the curBuf echos into per-object echos
	for (i=0;i<fftLen;i++)
	{
		signalFFT[i].r=curBuf[i];
		signalFFT[i].i=0;
	}
	ffts((float *)signalFFT,M,1);
	//Take complex product of signal with conjugate of chirp--
	// this correlates the chirp against the signal.
	for (i=0;i<fftLen;i++)
		signalFFT[i]*=chirpFFT[i];
	//Inverse FFT product
	iffts((float *)signalFFT,M,1);
	
	//Now signalFFT contains echo coefficents for
	// each object.  Draw in funky colors
	for(i=0;i<fftLen;i++)
		outLine[i]=cvt2dB(signalFFT[i].r,100.0,8.0);
	renderLine(outLine,fftLen,16,0);

#define DO_WRITEOUT 0
#if DO_WRITEOUT
	static int time=0;	
	time++;
	if (time==20)
	{
		writeBuf("o_echo.txt",curBuf,fftLen);
		writeBuf("o_chirp.txt",chirp,chirpLen);
	}
#endif
	
//Draw the fft'd buffer
	win->copy(off,0,0);
//Finally, write our chirp to the new buffer
	for (i=0;i<bufLen;i++)
		curBuf[i]=0;
	for (i=0;i<chirpLen;i++)
		curBuf[0*512+i]=chirp[i];
}

//Initialization routine-- prepares screen buffers,
// chirp, etc.
void dsp_sonar::init0(void)
{
	//Prepare output buffer
	int offWid=512,offHt=128,x,y;
	off=new simpleOffscreen(offWid,offHt,8,GetCTable(1024));
	off->beginDrawing();
	for (y=0;y<off->ht;y++)
		for (x=0;x<off->wid;x++)
			off->baseAddr[y*off->rowBytes+x]=(unsigned char)(0);
	off->doneDrawing();
	
	win=new simpleWindow(10,45,offWid,offHt,"\pSonar Range");
	
	//Prepare FFT
	int bufLen=bufferLength();
	fftLen=bufLen;
	outLine=new unsigned char[fftLen];
	M=(int)(log(bufLen)/log(2));
	fftInit(M);
	chirpFFT=new fcomplex[fftLen];
	signalFFT=new fcomplex[fftLen];
	
	chirpLen=3000;
	inChirp=new float[chirpLen];
	
	//Start the microphone
	dsp::init0();
	
	//Prepare our linear-FM upchirp
	chirp=new short[chirpLen];
	double dt=1.0/mic->getSampleRate();//dt is sample interval, in seconds
	const double startFreq=4000.0;//Start chirp at this freqency (Hz)
	const double endFreq=22000.0;//End chirp at this frequency (Hz)
	double chirpDur=chirpLen*dt;//Chirp takes this long
	double chirpSlope=(endFreq-startFreq)/chirpDur;//Chirp slope, in Hz/sec
	const double pi=3.14159265358979323;
	int i;
	double chirpScale=32000.0,chirpInv=1.0/(chirpScale*chirpLen);
	for (i=0;i<chirpLen;i++)
	{
		double t=i*dt;//Time since chirp start, in seconds
		double phs=t*startFreq+t*t*0.5*chirpSlope;//Chirp phase, in revolutions
		double amp=cos(2*pi*phs);//Chirp amplitude at this point: [-1,1]
		chirp[i]=(short)(chirpScale*amp);
	}
	
	//Knurl down the ends of the chirp (stops high-freqency ringing)
	int knurlLen=50;
	for (i=0;i<knurlLen;i++)
	{
		double scaleFact=0.5*(1.0-cos(pi*(double)i/knurlLen));
		chirp[i]*=scaleFact;
		chirp[chirpLen-1-i]*=scaleFact;
	}
	
	//Create Complex chirp, FFT, & conjugate
	for (i=0;i<fftLen;i++)
		chirpFFT[i].r=chirpFFT[i].i=0.0;
	for (i=0;i<chirpLen;i++)
		chirpFFT[i].r=chirp[i]*chirpInv;
	ffts((float *)chirpFFT,M,1);
	for (i=0;i<fftLen;i++)
		chirpFFT[i].i=-chirpFFT[i].i;
}
void dsp_sonar::die(void)
{
	delete win;
	delete off;
	delete outLine;
	delete [] chirp;
	dsp::die();
}