#include <iostream>
using namespace std;

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <limits.h>
#include <alsa/asoundlib.h>
#include "/usr/include/alsa/asoundlib.h"

//#define SAMPLE_TYPE float
//#define SAMPLE_TYPE_ALSA SND_PCM_FORMAT_FLOAT_LE

#define SAMPLE_TYPE short
#define SAMPLE_TYPE_ALSA SND_PCM_FORMAT_S16_LE


class RunningAverage {
	int _nbValuesForAverage;
	int _nbValues;
	float _mean;

public:
	RunningAverage(int nbValuesForAverage) {
		_nbValuesForAverage = nbValuesForAverage;
		_mean = 0;
		_nbValues = 0;
	}

	void newValue(SAMPLE_TYPE v) {
		if (_nbValues < _nbValuesForAverage)
			_nbValues++;
		_mean = ((_mean * (_nbValues - 1)) + v) / (float)_nbValues;
	}

	SAMPLE_TYPE getMean() {
		return (SAMPLE_TYPE) _mean;
	}
};

class SoundSourceLoc {

	static const int _nbSamplesMaxDiff = 13;

	
	static const int _bufferSize = 4096;

	
	static constexpr float _minLevelFactorForValidLoc = 1.05f;

	
	static constexpr float _soundSpeed = 344;

	/**
	 * sound sampling rate in Hz
	 */
	unsigned int _soundSamplingRate;

	static constexpr float _distanceBetweenMicrophones = 0.1f;

	RunningAverage* _averageSoundLevel;

	
	snd_pcm_t* _capture_handle;


	SAMPLE_TYPE _rightBuffer[_bufferSize];
	SAMPLE_TYPE _leftBuffer[_bufferSize];

public:
	SoundSourceLoc() {
		_averageSoundLevel = new RunningAverage(50);
		_soundSamplingRate = 44100;

		int err;
		snd_pcm_hw_params_t* hw_params;

		// ideally use "hw:0,0" for embedded, to limit processing. But check if card support our needs...
		const char* device = "plughw:1,0";
		if ((err = snd_pcm_open(&_capture_handle, device,
				SND_PCM_STREAM_CAPTURE, 0)) < 0) {
			fprintf(stderr, "cannot open audio device %s (%s)\n", device,
					snd_strerror(err));
			exit(1);
		}

		if ((err = snd_pcm_hw_params_malloc(&hw_params)) < 0) {
			fprintf(stderr,
					"cannot allocate hardware parameter structure (%s)\n",
					snd_strerror(err));
			exit(1);
		}

		if ((err = snd_pcm_hw_params_any(_capture_handle, hw_params)) < 0) {
			fprintf(stderr,
					"cannot initialize hardware parameter structure (%s)\n",
					snd_strerror(err));
			exit(1);
		}

		if ((err = snd_pcm_hw_params_set_access(_capture_handle, hw_params,
				SND_PCM_ACCESS_RW_NONINTERLEAVED)) < 0) {
			fprintf(stderr, "cannot set access type (%s)\n", snd_strerror(err));
			exit(1);
		}

		if ((err = snd_pcm_hw_params_set_format(_capture_handle, hw_params,
				SAMPLE_TYPE_ALSA)) < 0) {
			fprintf(stderr, "cannot set sample format (%s)\n",
					snd_strerror(err));
			exit(1);
		}

		if ((err = snd_pcm_hw_params_set_rate_near(_capture_handle, hw_params,
				&_soundSamplingRate, 0)) < 0) {
			fprintf(stderr, "cannot set sample rate (%s)\n", snd_strerror(err));
			exit(1);
		}

		if ((err = snd_pcm_hw_params_set_channels(_capture_handle, hw_params, 2))
				< 0) {
			fprintf(stderr, "cannot set channel count (%s)\n",
					snd_strerror(err));
			exit(1);
		}

		if ((err = snd_pcm_hw_params(_capture_handle, hw_params)) < 0) {
			fprintf(stderr, "cannot set parameters (%s)\n", snd_strerror(err));
			exit(1);
		}

		snd_pcm_hw_params_free(hw_params);

		if ((err = snd_pcm_prepare(_capture_handle)) < 0) {
			fprintf(stderr, "cannot prepare audio interface for use (%s)\n",
					snd_strerror(err));
			exit(1);
		}
	}

	/** Clean exit */
	~SoundSourceLoc() {
		snd_pcm_close(_capture_handle);
		delete _averageSoundLevel;
	}

	
	void run() {
		while (true) {
			processNextSoundBlock();
		}
	}

private:
	
	void processNextSoundBlock() {
		SAMPLE_TYPE* bufs[2];
		bufs[0] = _rightBuffer;
		bufs[1] = _leftBuffer;
		int err;
		if ((err = snd_pcm_readn(_capture_handle, (void**) bufs, _bufferSize))
				!= _bufferSize) {
			fprintf(stderr, "read from audio interface failed (%s)\n",
					snd_strerror(err));
			exit(1);
		}

		
		SAMPLE_TYPE level = computeLevel(_rightBuffer, _leftBuffer);
		
		_averageSoundLevel->newValue(level);
		
		float relativeLevel = (float) level
				/ (float) _averageSoundLevel->getMean();
		

		int minDiff = INT_MAX;
		int minDiffTime = -1;
		
		for (int t = -_nbSamplesMaxDiff; t < _nbSamplesMaxDiff; t++) {
			
			int diff = 0;
			for (int i = _nbSamplesMaxDiff;
					i < _bufferSize - _nbSamplesMaxDiff - 1; i++) {
				diff += abs(_leftBuffer[i] - _rightBuffer[i + t]);
			}
			if (diff < minDiff) {
				minDiff = diff;
				minDiffTime = t;
			}
		}

		
		if ((relativeLevel > _minLevelFactorForValidLoc)
				&& (minDiffTime > -_nbSamplesMaxDiff)
				&& (minDiffTime < _nbSamplesMaxDiff)) {
			
			float angle =
					-(float) asin(
							(minDiffTime * _soundSpeed)
									/ (_soundSamplingRate
											* _distanceBetweenMicrophones));

			cout << angle << ";" << relativeLevel << endl;
		}
	}

	
	SAMPLE_TYPE computeLevel(SAMPLE_TYPE right[], SAMPLE_TYPE left[]) {
		float level = 0;
		for (int i = 0; i < _bufferSize; i++) {
			float s = (left[i] + right[i]) / 2;
			level += (s * s);
		}
		level /= _bufferSize;
		level = sqrt(level);
		return (SAMPLE_TYPE) level;
	}
};

int main(int argc, char *argv[]) {
	SoundSourceLoc soundLoc;
	soundLoc.run();
	exit(0);
}