///////////////////////////////////////////////////////////////////////////////////
//                        A small demo of sonar.
// Program allow distance measuring.
// Uses cross-correlation algorithm to find echos
//
// Author: kaklik  (kaklik@mlab.cz)
//$Id:$
///////////////////////////////////////////////////////////////////////////////////

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <SDL.h>
#include <alsa/asoundlib.h>

#define SOUND_SPEED			340.0	// sound speed in air in metrs per second
#define MAX_RANGE			5.0	// maximal working radius in meters
#define Xl				-0.1	// microphones position
#define Xr				0.1

// Alsa settings
static char *alsa_device = "plughw:0,0";
static snd_pcm_format_t alsa_format = SND_PCM_FORMAT_S16;			/* sample format */
static unsigned int alsa_rate = 96000;						/* stream rate */
static unsigned int alsa_buffer_time = 2 * (MAX_RANGE / SOUND_SPEED * 1e6);	/* ring buffer length in us */
static unsigned int alsa_period_time = MAX_RANGE / SOUND_SPEED * 1e6;		/* period time in us */
static int alsa_resample = 1;

#define V2D				SOUND_SPEED/alsa_rate	// count of values to distance

// Alsa driver

// Hasn't so pretty error messages, but can really cut off count of the lines
#define ALSA_ERR(a)	{ \
				int err; \
				err = a; \
				if (err < 0) { \
					printf("alsa: " #a /* stringify a */ ": %s\n", snd_strerror(err)); \
					return err; \
				} \
			}
			
static int set_hwparams(snd_pcm_t *handle, snd_pcm_hw_params_t *params, unsigned int channels, unsigned int rate,
	int resample, unsigned int buffer_time, snd_pcm_sframes_t *buffer_size,
	unsigned int period_time, snd_pcm_sframes_t *period_size, snd_pcm_format_t format)
{
	unsigned int rrate;
	snd_pcm_uframes_t size;
	int dir;
	
	/* choose all parameters */
	ALSA_ERR(snd_pcm_hw_params_any(handle, params));
	/* set hardware resampling */
	ALSA_ERR(snd_pcm_hw_params_set_rate_resample(handle, params, resample));
	/* set the interleaved read/write format */
	ALSA_ERR(snd_pcm_hw_params_set_access(handle, params, SND_PCM_ACCESS_RW_INTERLEAVED));
	/* set the sample format */
	ALSA_ERR(snd_pcm_hw_params_set_format(handle, params, format));
	/* set the count of channels */
	ALSA_ERR(snd_pcm_hw_params_set_channels(handle, params, channels));
	
	/* set the stream rate */
	rrate = rate;
	ALSA_ERR(snd_pcm_hw_params_set_rate_near(handle, params, &rrate, 0));
	if (rrate != rate) {
		printf("alsa: rate doesn't match (requested %iHz, get %iHz)\n", rate, rrate);
		return -EINVAL;
	} else {
		printf("alsa: rate set to %i Hz\n", rate);
	}
	
	/* set the buffer time */
	ALSA_ERR(snd_pcm_hw_params_set_buffer_time_near(handle, params, &buffer_time, &dir));
	ALSA_ERR(snd_pcm_hw_params_get_buffer_size(params, &size));
	*buffer_size = size;
	printf("alsa: bufffer size set to: %d requested buffer time: %ld \n", (int) *buffer_size,
		(long) buffer_time);
	
	// set the period time
	ALSA_ERR(snd_pcm_hw_params_set_period_time_near(handle, params, &period_time, &dir));
	ALSA_ERR(snd_pcm_hw_params_get_period_size(params, &size, &dir));
	*period_size = size;
	printf("alsa: period size set to: %d requested period time: %ld \n", (int) *period_size,
		(long) period_time);
	
	/* write the parameters to device */
	ALSA_ERR(snd_pcm_hw_params(handle, params));
	
	return 0;
}

static int set_swparams(snd_pcm_t *handle, snd_pcm_sw_params_t *swparams, unsigned int buffer_size)
{
	/* get the current swparams */
	ALSA_ERR(snd_pcm_sw_params_current(handle, swparams));
	
	// start the transfer when the buffer is almost full: never for our case
	ALSA_ERR(snd_pcm_sw_params_set_start_threshold(handle, swparams, 2 * buffer_size));
	ALSA_ERR(snd_pcm_sw_params_set_period_event(handle, swparams, 1));
	
	/* write the parameters to the playback device */
	ALSA_ERR(snd_pcm_sw_params(handle, swparams));
	
	return 0;
}

unsigned int linear_windowed_chirp(short *pole)  // generate the ping signal
{
	unsigned int maxval = (1 << (snd_pcm_format_width(alsa_format) - 1)) - 1;
	
	static const float f0 = 5000;		// starting frequency
	static const float fmax = 10000;	// ending frequency
	static const float Tw = 0.0015;		// time width of ping in seconds 
	static float k;
	
	unsigned int n=0;
	double t;
	unsigned int chirp_samples;		//number of samples per period
	
	k=2*(fmax-f0)/Tw;
	chirp_samples = ceil(alsa_rate*Tw);		// compute size of ping sinal in samples
	
	for (n=0;n<=chirp_samples;n++)
	{
		t = (double) n / (double)alsa_rate;
		pole[n] = (short) floor( (0.35875 - 0.48829*cos(2*M_PI*t*1/Tw) + 0.14128*cos(2*M_PI*2*t*1/Tw) - 0.01168*cos(2*M_PI*3*t*1/Tw))*maxval*sin(2*M_PI*(t)*(f0+(k/2)*(t))) ); // signal generation formula
	}
	return (chirp_samples);	// return count of samples in ping
}

int main (int argc, char *argv[]) {
	snd_pcm_t *playback_handle, *capture_handle;
	snd_pcm_hw_params_t *hwparams;
	snd_pcm_sw_params_t *swparams;
	snd_pcm_sframes_t buffer_size;	// size of buffer at sound card
	snd_pcm_sframes_t period_size;	// samples per frame
	
	int err, j, l, i, r;
	unsigned int n, m;
	
	printf("Simple PC sonar $Rev:$ starting work.. \n");
	
	printf("Initializing ALSA.. \r");
	fflush(stdout);
	
	snd_pcm_hw_params_alloca(&hwparams);	// allocation of soundcard parameters registers
	snd_pcm_sw_params_alloca(&swparams);
	
	ALSA_ERR(snd_pcm_open(&playback_handle, alsa_device, SND_PCM_STREAM_PLAYBACK, 0));
	ALSA_ERR(set_hwparams(playback_handle, hwparams, 1, alsa_rate, alsa_resample,
		alsa_buffer_time, &buffer_size, alsa_period_time, &period_size, alsa_format));
	ALSA_ERR(set_swparams(playback_handle, swparams, buffer_size));
	
	ALSA_ERR(snd_pcm_open(&capture_handle, alsa_device, SND_PCM_STREAM_CAPTURE, 0));
	ALSA_ERR(set_hwparams(capture_handle, hwparams, 2, alsa_rate, alsa_resample,
		alsa_buffer_time, &buffer_size, alsa_period_time, &period_size, alsa_format));
	ALSA_ERR(set_swparams(capture_handle, swparams, buffer_size));
	
	printf("Initializing ALSA.. done!\n");
	
	short *chirp = calloc(2*period_size, sizeof(short));
	short *signal = malloc(2*period_size * sizeof(short));
	short *signal_l = malloc(period_size * sizeof(short)); 
	short *signal_r = malloc(period_size * sizeof(short));
	long int *correlation_l = malloc(period_size * sizeof(long int)); // arrays to store correlation curve
	long int *correlation_r = malloc(period_size * sizeof(long int));
	float *echo_map = malloc(period_size * period_size * sizeof(float));
	unsigned char *echo_map_ = malloc(400 * 400 * sizeof(unsigned char));
	
	if (!chirp || !signal) {
		printf("Can't allocate buffers!\n");
	}
	
	printf("Generating ping signal.. \r");
	fflush(stdout);
	
	unsigned int chirp_size = linear_windowed_chirp(chirp);
	
	printf("Generating ping signal.. done!\n");
	
	SDL_Surface *sdl_screen;
	{
		SDL_Init(SDL_INIT_VIDEO);
		sdl_screen = SDL_SetVideoMode(400, 400, 32, SDL_SWSURFACE);
	}
	
	while (1) {
		ALSA_ERR(snd_pcm_writei(playback_handle, chirp, period_size));
		ALSA_ERR(snd_pcm_start(playback_handle));
		
		ALSA_ERR(snd_pcm_start(capture_handle));
		
		while (snd_pcm_avail_update(capture_handle) < period_size) {
			usleep(1000);
		}
		
		ALSA_ERR(snd_pcm_drop(playback_handle));
		ALSA_ERR(snd_pcm_drain(capture_handle));
		
		ALSA_ERR(snd_pcm_readi(capture_handle, signal, period_size));
		
		for (i = 0; i < period_size * 2; i += 2) {
			signal_l[i >> 1] = signal[i];
			signal_r[i >> 1] = signal[i + 1];
		}
		
		{
			printf("Chirp transmitted\nCorrelating\n");
			for (n = 0; n < (period_size - chirp_size - 1); n++) {
				l = 0;
				r = 0;
				
				for (m = 0; m < chirp_size; m++) {
					l += chirp[m] * signal_l[m + n];// correlate with left channel
					r += chirp[m] * signal_r[m + n];// correlate with right channel
				}
				
				correlation_l[n] = abs(l);
				correlation_r[n] = abs(r);
			}
			
			for (i = 0; i < period_size * period_size; i += 1)
				echo_map[i] = 0.0f;
			for (i = 0; i < 400 * 400; i += 1)
				echo_map_[i] = 0;
			
			for (i = 0; i < period_size; i += 1) {
				double f = V2D * i;
				
				for (j = 0; j < period_size; j += 1) {
					double g = V2D * j;
					double a, b;
					
					/*
					a=(2*f*g*Xl+f*f*Xr+Xl*(g*g+(Xl-Xr)*Xr))/(2*g*Xl+2*f*Xr);
					b=(g*g*Xl-2*f*g*Xr+Xr*(f*f+Xl*(-Xl+Xr)))/(2*g*Xl-2*f*Xr);
					*/
					
					a = f - sqrt(pow(g * g * Xl - Xr * (f * f + Xl * 
						(-Xl + Xr)), 2) / pow(g * Xl - f * Xr, 2)) / 2.0;
					b = g - sqrt(pow(g * g * Xl - Xr * (f * f + Xl *
						(-Xl + Xr)), 2) / pow(g * Xl - f * Xr, 2)) / 2.0;
					
					if (((Xr-Xl)<=a+b) && (b<=a+(Xr-Xl)) && (a<=b+(Xr-Xl))) {
						/*
						float x = (f*((f-g)*g + Xl*Xl)-b*Xr*Xr)/(2*f*Xl-2*g*Xr);
						float y = sqrt(((-g*g+Xl*Xl)*(f-Xr)*(f-g+Xl-Xr)*(f+Xr)
							*(f-g-Xl+Xr))/(4*(f*Xl-g*Xr)*(f*Xl-g*Xr)));	
						*/
						
						errno = 0;
						
						float x = (f * f * g - g * Xl * Xl + f
							 * (- g * g + Xr * Xr))
							 / (-2 * g * Xl + 2 * f * Xr);
						
						float y = sqrt(-((g * g * (f * f - Xl * Xl)
							 * (f * f - 2 * f * g + g * g
							 - (Xl - Xr) * (Xl - Xr)) * (g * g - Xr * Xr))
							 / pow(g * Xl - f * Xr, 2)))/(2.0 * g);
						
						/*
						if (x < 0)
							printf("%f\n", x);
						*/
						
						if ((x == x) && (y == y) && (errno != EDOM)) // check for NaN (NaN == NaN is false)
							echo_map[period_size * ((int) (y / ((float) V2D)))
								 + ((int) (x / ((float) V2D))
								 + (period_size/2))] = 
								sqrt(correlation_l[i]/2)*sqrt(correlation_r[j]/2);
					}
				}
			}
			
			for (i = 0; i < period_size; i += 1) {
				for (j = 0; j < period_size; j += 1) {
					int y = i * 400 / period_size;
					int x = j * 400 / period_size;
					
					Uint8 green = echo_map[i*period_size+j]/(2000000000/255);
					Uint8 *pointer = &echo_map_[y*400+x];
					
					if (green > *pointer)
						*pointer = green;
				}
			}
			
			
			for (i = 0; i < 400; i += 1) {
				for (j = 0; j < 400; j += 1) {
					Uint32* bufp = (Uint32 *)sdl_screen->pixels + i*sdl_screen->pitch/4 + j;
					*bufp = SDL_MapRGB(sdl_screen->format, 0, echo_map_[i*400+j], 0);
				}
			}
			SDL_UpdateRect(sdl_screen, 0, 0, 400, 400);
			
			SDL_Event sdl_event;
			while (SDL_PollEvent(&sdl_event)) {
				if (sdl_event.type == SDL_QUIT)
					return 0;
			}
			
			/*
			FILE *out = fopen("output", "w");
			for (i = 0; i <= (period_size - 1); i++) {
				fprintf(out, "%2.3f %6d %6d %9ld %9ld\n", (float)i*V2D, signal_l[i], signal_r[i], correlation_l[i], correlation_r[i]);
			}
			fclose(out);
			*/
		}
		
		ALSA_ERR(snd_pcm_prepare(playback_handle));
		ALSA_ERR(snd_pcm_prepare(capture_handle));
	}
	
	return 0;
}