// SONARBackEnd.cpp : Defines the entry point for the console application.
//



//#include "opencv2/core/core.hpp"
//#include "opencv2/highgui/highgui.hpp"
//#include "opencv2/imgproc/imgproc.hpp"
#include <librealsense/rs.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>

#include <iostream>
#include <fstream>
#include "AL/al.h"
#include "AL/alc.h"
#include "math.h"

using namespace rs;
using namespace std;

#define verticalsources 16
//#define AspectRatio 0.75
//#define fovDeg 58.5
#define PI 3.1415926535

#define CASE_RETURN(err) case (err): return "##err"

int xSize = 320;
int ySize = 240;

//User parameters for audio stuff
float freq = 200.f;
int horizontal_steps = 20;
float topfreq = 1200.f;
float freqInc = pow(topfreq/freq,1.f/verticalsources);
int stepDelay = 3;
float distance_decay = 2;
float amplitude = 0.05;
float dist_cut = 1.4;
 //cout << freqInc;

const char* al_err_str(ALenum err) {
	switch (err) {
		CASE_RETURN(AL_NO_ERROR);
		CASE_RETURN(AL_INVALID_NAME);
		CASE_RETURN(AL_INVALID_ENUM);
		CASE_RETURN(AL_INVALID_VALUE);
		CASE_RETURN(AL_INVALID_OPERATION);
		CASE_RETURN(AL_OUT_OF_MEMORY);
	}
	return "unknown";
}
#undef CASE_RETURN

#define __al_check_error(file,line) \
    do { \
        ALenum err = alGetError(); \
        for(; err!=AL_NO_ERROR; err=alGetError()) { \
            std::cerr << "AL Error " << al_err_str(err) << " at " << file << ":" << line << std::endl; \
        } \
    }while(0)

#define al_check_error() \
    __al_check_error(__FILE__, __LINE__)


void init_al() {
	ALCdevice *dev = NULL;
	ALCcontext *ctx = NULL;

	const char *defname = alcGetString(NULL, ALC_DEFAULT_DEVICE_SPECIFIER);
	std::cout << "Default device: " << defname << std::endl;

	dev = alcOpenDevice(defname);
	ctx = alcCreateContext(dev, NULL);
	alcMakeContextCurrent(ctx);
}

void exit_al() {
	ALCdevice *dev = NULL;
	ALCcontext *ctx = NULL;
	ctx = alcGetCurrentContext();
	dev = alcGetContextsDevice(ctx);

	alcMakeContextCurrent(NULL);
	alcDestroyContext(ctx);
	alcCloseDevice(dev);
}

void readUserParamFile() {

	ifstream readFile;

	readFile.open("UserParameters.txt");

	/*if(!readFile.good()){
		while(!readFile.good()){
			char buff[100];
			std::string current_working_dir(buff);
			cout << "loc is: " << current_working_dir << endl;
			cout << "Enter file location" << endl;
			string loc;
			cin >> loc;
			readFile.open(loc);
		}
	}*/

	if (readFile.good())
	{
		readFile >> freq;
		readFile >> freqInc;
		readFile >> horizontal_steps;
		readFile >> stepDelay;
	}

	printf("freq is %3.2f, freq increment is %2.2f\n horizontal steps is %d, step delay is %d ms\n", freq, freqInc, horizontal_steps, stepDelay);
	readFile.close();
}

int const INPUT_WIDTH      = 320;
int const INPUT_HEIGHT     = 240;
int const FRAMERATE        = 0;

context      _rs_ctx;
device&      _rs_camera = *_rs_ctx.get_device( 0 );
intrinsics   _depth_intrin;
intrinsics  _color_intrin;
bool         _loop = true;


// Initialize the application state. Upon success will return the static app_state vars address

bool initialize_streaming( )
{
       bool success = false;
       if( _rs_ctx.get_device_count( ) > 0 )
       {
             //_rs_camera.enable_stream( rs::stream::color, INPUT_WIDTH, INPUT_HEIGHT, rs::format::rgb8, FRAMERATE );
             _rs_camera.enable_stream( rs::stream::depth, INPUT_WIDTH, INPUT_HEIGHT, rs::format::z16, FRAMERATE );
             _rs_camera.start( );

             success = true;
       }
       return success;
}

int main()
{
	readUserParamFile();

	//OpenAL stuff----------------------------------
	unsigned short pointdist;
	float pointdistnorm;
	float angle;
	float zdist;
	float xdist;

	init_al();

	ALuint buf[verticalsources];
	alGenBuffers(verticalsources, buf);
	al_check_error();

	/* Fill buffer with Sine-Wave */
	float seconds = 1;
	float rolloff = 0.85f;

	unsigned sample_rate = 22050;
	size_t buf_size = seconds * sample_rate;

	short *samples;
	samples = new short[buf_size];

	//Set up the frequencied buffers
	//

	int square_cntr = 0;
	int period = 0;
	for (int q = 0; q < verticalsources; q++){
		period = sample_rate/(2.f*float(PI)*freq);
		for (int i = 0; i<buf_size; ++i) {
			//if (square_cntr > period) {
				//square_cntr = 0;
				//samples[i] = -16380*amplitude;
			//} else if (square_cntr > period/2) {
				//samples[i] = 16380*amplitude;
			//} else {
				//samples[i] = -16380*amplitude;
			//}
			//square_cntr ++;
			samples[i] = 16380 * sin((2.f*float(PI)*freq) / sample_rate * i) * amplitude;
		}

		amplitude = amplitude * rolloff;

		freq = freq*freqInc;
		/* Download buffer to OpenAL */
		alBufferData(buf[q], AL_FORMAT_MONO16, samples, buf_size * 2, sample_rate);
		al_check_error();
		cout << freq;
		cout << "\n";

	}

	//Generate 16 sources
	ALuint *srclist;
	srclist = new ALuint[verticalsources];

	alGenSources(verticalsources, srclist);
	int x;
	int xPix;

	for (int i = 0; i < verticalsources; ++i) {
		//Vertical position
		x = i;
		xPix = (ySize/(verticalsources *2))+(ySize/ verticalsources)*x;

		alSourcei(srclist[i], AL_BUFFER, buf[i]);
		alSourcef(srclist[i], AL_REFERENCE_DISTANCE, 1.0f);
		alSourcei(srclist[i], AL_SOURCE_RELATIVE, AL_TRUE);
		alSourcei(srclist[i], AL_LOOPING, AL_TRUE);
		alSource3f(srclist[i], AL_POSITION, -2, 0, 1);
		alSourcePlay(srclist[i]);

	}

	//Second set of sources
	//Generate 16 sources
	ALuint *srclist2;
	srclist2 = new ALuint[verticalsources];

	alGenSources(verticalsources, srclist2);

	for (int i = 0; i < verticalsources; ++i) {
			//Vertical position
			x = i;
			xPix = (ySize/(verticalsources *2))+(ySize/ verticalsources)*x;

			alSourcei(srclist2[i], AL_BUFFER, buf[i]);
			alSourcef(srclist2[i], AL_REFERENCE_DISTANCE, 1.0f);
			alSourcei(srclist2[i], AL_SOURCE_RELATIVE, AL_TRUE);
			alSourcei(srclist2[i], AL_LOOPING, AL_TRUE);
			alSource3f(srclist2[i], AL_POSITION, -2, 0, 1);
			alSourcePlay(srclist2[i]);

		}

	//End second set of sources

	al_check_error();

	//RealSense stuff

	if( !initialize_streaming( ) )
	   {
			 std::cout << "Unable to locate a camera" << std::endl;
			 rs::log_to_console( rs::log_severity::fatal );
			 return EXIT_FAILURE;
	   }

	//-------------------------------Back to openCV stuff
	cv::Mat depth8u;
	cv::Mat flipped;

	cv::namedWindow("Display window", cv::WINDOW_AUTOSIZE);// Create a window for display.

	int64_t delay;
	int64_t starttick;

	int sweepdirection = -1;
	int horizpos = 0;
	bool cPressed = false;
	int keyCode = 255;
	// 255: 'no key' press, < 0: 'no key registered', 99: 'c' pressed
	while(keyCode == 255 || keyCode < 0 || keyCode == 99){
		starttick = cv::getTickCount();
		keyCode = cv::waitKey(1);
		// Pressed c, hide/show window
		if (keyCode == 99)
		{
			cPressed = !cPressed;
			printf("cpressed is: %d\n", cPressed);
		}
		//printf("key code is: %d\n", keyCode);
		//if (horizpos == horizontal_steps/2) {
			//printf("new thing \n");
		//	horizpos = 0;

		//}

		if ((horizpos <= 0) or (horizpos >= (horizontal_steps/2 - 1))) {
			sweepdirection = sweepdirection * -1;

			if( _rs_camera.is_streaming( ) )
			                    _rs_camera.wait_for_frames( );

			//show new image
			_depth_intrin       = _rs_camera.get_stream_intrinsics( rs::stream::depth );

		   // Create depth image
		   cv::Mat depth16( _depth_intrin.height,
									  _depth_intrin.width,
									  CV_16U,
									  (uchar *)_rs_camera.get_frame_data( rs::stream::depth ) );

		   depth8u = depth16/dist_cut;

		   depth8u.convertTo( depth8u, CV_8UC1, 255.0/5000 );

		   cv::bitwise_not(depth8u, depth8u);
		   cv::threshold( depth8u, depth8u, 254, 255, 4 );
		   cv::bitwise_not(depth8u, depth8u);

		   cv::flip(depth8u, flipped, 0);

		   imshow("Display window", depth8u);

		   //printf("showing stuff\n");
		}

		//OpenAL Stuff-----------------------------------
		//Right sources, sweep left to right
		for (int i = 0; i < verticalsources; ++i) {
			//defines roi
			cv::Rect roi((xSize/horizontal_steps)*(horizpos + (horizontal_steps/2)), (ySize/ verticalsources)*i, (xSize/horizontal_steps), (ySize/ verticalsources));

			//copies input image in roi
			cv::Mat image_roi = flipped(roi);

			//computes mean over roi
			cv::Scalar avgPixelIntensity = cv::mean(image_roi);

			//prints out only .val[0] since image was grayscale
			int pointdist = avgPixelIntensity.val[0];

			pointdistnorm = float(pointdist) / 255;
			//rectangle(planes[0], Point(horizpos*(xSize / horizontal_steps), sourceMatCoords[i]), Point(horizpos*(xSize / horizontal_steps) + 3, sourceMatCoords[i] + 3), Scalar(255));

			angle = PI * (0.5 + float(horizpos) / float(horizontal_steps-1));

			//cout << "\nAngle1:";
			//cout << angle;

			xdist = -cos(angle);
			zdist = sin(angle);

			alSource3f(srclist[i], AL_POSITION, xdist, 0, zdist);
			alSourcef(srclist[i], AL_GAIN, pow((1-pointdistnorm),distance_decay)); //should be 6.908 for normal rolloff
		}
		//End right sources

		//Left sources, sweep right to left
		for (int i = 0; i < verticalsources; ++i) {
			//defines roi
			cv::Rect roi((xSize/horizontal_steps)*((horizontal_steps/2) - 1 - horizpos), (ySize/ verticalsources)*i, (xSize/horizontal_steps), (ySize/ verticalsources));

			//copies input image in roi
			cv::Mat image_roi = flipped(roi);

			//computes mean over roi
			cv::Scalar avgPixelIntensity = cv::mean(image_roi);

			//prints out only .val[0] since image was grayscale
			int pointdist = avgPixelIntensity.val[0];

			pointdistnorm = float(pointdist) / 255;
			//rectangle(planes[0], Point(horizpos*(xSize / horizontal_steps), sourceMatCoords[i]), Point(horizpos*(xSize / horizontal_steps) + 3, sourceMatCoords[i] + 3), Scalar(255));

			angle = PI * (0.5 - (float(horizpos) / float(horizontal_steps-1)));

			//cout << "\nAngle2:";
			//cout << angle;

			xdist = -cos(angle);
			zdist = sin(angle);

			alSource3f(srclist2[i], AL_POSITION, xdist, 0, zdist);
			alSourcef(srclist2[i], AL_GAIN, pow((1-pointdistnorm),distance_decay)); //should be 6.908 for normal rolloff
		}
		//end left sources

		//End openAL stuff-------------------------------------------

		delay = ((cv::getTickCount() - starttick) * 1000) / cv::getTickFrequency();
		while (delay < stepDelay) { delay = ((cv::getTickCount() - starttick) * 1000) / cv::getTickFrequency(); }
		//printf("%d delay", delay);

		horizpos += sweepdirection;
	}

	//Realsense stuff
	cv::destroyAllWindows( );
	_rs_camera.stop( );

	//OpenAL stuff-----------------
	alDeleteSources(verticalsources, srclist);
	alDeleteSources(verticalsources, srclist2);
	al_check_error();
	exit_al();
	//Stop OpenAL stuff-------------------

	return 0;
}