#include "functions.h"


char *getPortName(int part){
	switch(part){
		case PART_HEAD: return "/head";
		case PART_TORSO: return "/torso";
		case PART_LEFT_ARM: return "/left_arm";
		case PART_RIGHT_ARM: return "/right_arm";
	}

	printf("ERROR getPort");
	return "";
}

gsl_matrix *pinv(gsl_matrix *m){
	/* matica musi mat vacsi pocet riadkov ako stlpcov */
	if(m->size1 < m->size2){
		printf("WARNING: Zly rozmer matice. \n");
		return NULL;
	}

	gsl_matrix * U = gsl_matrix_calloc (m->size1, m->size2);
	gsl_matrix_memcpy(U, m);

	gsl_vector * S = gsl_vector_calloc (m->size2);
	gsl_matrix * V= gsl_matrix_calloc (m->size2, m->size2);

	// Computing the SVD of A
	gsl_vector * work = gsl_vector_calloc (m->size2);
	gsl_linalg_SV_decomp (U, V, S, work);
	gsl_vector_free(work);

	//Inverting S
	// Compute the inverse like in the MATLAB script

	gsl_matrix * SI = gsl_matrix_calloc(m->size2, m->size2);

	double tolerance = pow((double)2.0, - (double)53.0) * m->size1 * gsl_vector_get(S, 0);
	//tolerance = 0.00001;
	for (unsigned int i = 0; i < S->size; i++) {
		// printf("S [ %d ] = %f \n", i, gsl_vector_get (S, i));0.0000000001
		if (gsl_vector_get (S, i) > tolerance){
			gsl_matrix_set (SI, i, i, 1.0 / gsl_vector_get (S, i));
		}else{
			gsl_matrix_set (SI, i, i, 0.0);
		}
	}		

	//THE PSEUDOINVERSE
	// Moore–Penrose pseudoinverse
	gsl_matrix * SIpUT = gsl_matrix_calloc (m->size2, m->size1);
	gsl_blas_dgemm(CblasNoTrans, CblasTrans,
		1.0, SI, U, 0.0, SIpUT);

	gsl_matrix * pinv = gsl_matrix_calloc (m->size2, m->size1);	
	gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
		1.0, V, SIpUT, 0.0, pinv);

	gsl_matrix_free(SI);
	gsl_matrix_free(SIpUT);
	gsl_matrix_free(U);
	gsl_matrix_free(V);
	gsl_vector_free(S);
	return pinv;
}


gsl_matrix *generate_sine(int fvz, int freq, double amplitude, int length){
	gsl_matrix *m = gsl_matrix_calloc(1, length);
	for(int i = 0; i < length; i++){
		gsl_matrix_set(m, 0, i, amplitude * sin(2.0 * PI * freq * i / (double)fvz));
	}
	return m;
}

void print_vector(gsl_vector *v){
	if(v == NULL){
		printf("ERROR: NULL POINTER(print_vector)");
		return;
	}
	for(unsigned int i = 0; i < v->size; i++){
		printf("%f \n", gsl_vector_get(v, i));
	}
}
void print_vector(gsl_vector_complex *v){
	if(v == NULL){
		printf("ERROR: NULL POINTER(print_vector)");
		return;
	}
	for(unsigned int i = 0; i < v->size; i++){
		printf("%f \n", gsl_complex_abs(gsl_vector_complex_get(v, i)));		
	}
}

double getAbsMaxEig(gsl_matrix *m){
	if(m->size1 != m->size2){
		printf("WARNING: Non-square matrix in getAbsMaxEig");
		return 0.0;
	}

	gsl_matrix *mcopy = gsl_matrix_alloc(m->size1, m->size1);
	gsl_matrix_memcpy(mcopy, m);

	gsl_vector_complex *eval = gsl_vector_complex_alloc (m->size1);
	gsl_matrix_complex *evec = gsl_matrix_complex_alloc (m->size1, m->size1);
	gsl_eigen_nonsymmv_workspace * w = gsl_eigen_nonsymmv_alloc (m->size1);
	gsl_eigen_nonsymmv(mcopy, eval, evec, w);
	gsl_eigen_nonsymmv_free(w);
	gsl_matrix_free(mcopy);
	gsl_eigen_nonsymmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_DESC);

	gsl_complex eigmax = gsl_vector_complex_get(eval, 0);
	return gsl_complex_abs(eigmax); 
}
gsl_matrix *create_matrix(int rows, int cols, double scaling){
	gsl_matrix *result = gsl_matrix_alloc(rows, cols);
	for(int i = 0; i < rows; i++){
		for(int j = 0; j < cols; j++){
			gsl_matrix_set(result, i, j, ((rand() / (double)RAND_MAX) - 0.5) * scaling * 2);
		}
	}
	return result;
}

void print_matrix(gsl_matrix *m){
	for(unsigned int i = 0; i < m->size1; i++){
		for(unsigned int j = 0; j < m->size2; j++){
			printf("%+.4f ", gsl_matrix_get(m, i, j));	
		}
		printf("\n");
	}
	printf("\n");
}
void print_matrix_to_file(gsl_matrix *m, const char *filename){
	FILE *f;
	fopen_s(&f, filename, "w");
	for(unsigned int i = 0; i < m->size1; i++){
		for(unsigned int j = 0; j < m->size2; j++){
			fprintf(f, "%+.25f ", gsl_matrix_get(m, i, j));	
		}
		fprintf(f, "\n");
	}
	fprintf(f, "\n");
	fclose(f);
}
void print_vector_to_file(gsl_vector *v, const char *filename){
	FILE *f;
	fopen_s(&f, filename, "w");

	for(unsigned int j = 0; j < v->size; j++){
		fprintf(f, "%+.25f ", gsl_vector_get(v, j));	
	}
	fprintf(f, "\n");	

	fclose(f);
}

void print_vector_to_file_append(gsl_vector *v, const char *filename){
	FILE *f;
	fopen_s(&f, filename, "a");

	for(unsigned int j = 0; j < v->size; j++){
		fprintf(f, "%+.15f \t", gsl_vector_get(v, j));	
	}
	fprintf(f, "\n");	

	fclose(f);
}

void print_vector_to_file(gsl_vector *v, FILE *f){
	for(unsigned int j = 0; j < v->size; j++){
		fprintf(f, "%+.15f \t", gsl_vector_get(v, j));	
	}
	fprintf(f, "\n");	
}

int factorial(int n) {
	if(n <= 1) 
		return 1;
	return  n * factorial(n - 1);
}

void shuffleCols(gsl_matrix *m_in, gsl_matrix *m_des){

	int *p = (int*)malloc(sizeof(int) * m_in->size2);

	for (unsigned int i = 0; i < m_in->size2; i++)
	{
		p[i] = i;
	}

	gsl_rng *r = gsl_rng_alloc(gsl_rng_default);

	gsl_ran_shuffle(r, p, m_in->size2, sizeof (int));
	
	for (unsigned int i = 0; i < m_in->size2; i++)
	{	
		gsl_matrix_swap_columns(m_in, i, p[i]);
		gsl_matrix_swap_columns(m_des, i, p[i]);
	}
	gsl_rng_free(r);
}

void splitDataset(gsl_matrix *m_in, gsl_matrix *m_des, gsl_matrix **train_in, gsl_matrix **train_des, gsl_matrix **test_in, gsl_matrix **test_des, double trainSize){
	int trainCount = (int)(m_in->size2 * trainSize);
	
	*train_in = gsl_matrix_alloc(m_in->size1, trainCount);
	*train_des = gsl_matrix_alloc(m_des->size1, trainCount);
	*test_in = gsl_matrix_alloc(m_in->size1, m_in->size2 - trainCount);
	*test_des = gsl_matrix_alloc(m_des->size1, m_des->size2 - trainCount);

	gsl_matrix_view train_in_view = gsl_matrix_submatrix(m_in, 0, 0, m_in->size1, trainCount);
	gsl_matrix_view train_des_view = gsl_matrix_submatrix(m_des, 0, 0, m_des->size1, trainCount);
	gsl_matrix_view test_in_view = gsl_matrix_submatrix(m_in, 0, trainCount, m_in->size1, m_in->size2 - trainCount);
	gsl_matrix_view test_des_view = gsl_matrix_submatrix(m_des, 0, trainCount, m_des->size1, m_des->size2 - trainCount);

	gsl_matrix_memcpy(*train_in, &train_in_view.matrix);
	gsl_matrix_memcpy(*train_des, &train_des_view.matrix);
	gsl_matrix_memcpy(*test_in, &test_in_view.matrix);
	gsl_matrix_memcpy(*test_des, &test_des_view.matrix);
}


void maxOnehot(gsl_matrix *m){
	double max;
	for(unsigned int n = 0; n < m->size2; n++)
	{
		max = 0.0;
		bool first = true;
		for(unsigned int i = 0; i < m->size1; i++)
		{
			max = ((gsl_matrix_get(m, i, n) > max) ? gsl_matrix_get(m, i, n) : max);
		}

		for(unsigned int i = 0; i < m->size1; i++)
		{
			if((gsl_matrix_get(m, i, n) == max) && first){
				first = !first;
				gsl_matrix_set(m, i, n, 1.0);
			}else{
				gsl_matrix_set(m, i, n, 0.0);
			}
		}
	}
}

double successfulness(gsl_matrix *desired, gsl_matrix *output){
	double result = 0.0;
	for(unsigned int n = 0; n < output->size2; n++)
	{
		bool hit = true;

		for(unsigned int i = 0; i < output->size1; i++)
		{
			if((hit && gsl_matrix_get(desired, i, n) != gsl_matrix_get(output, i, n))){
				hit = !hit;
			}
		}

		if(hit){
			result += 1.0;
		}else{
			result += 0.0;
		}

	}

	return (result / (double)output->size2);
}



// Backprojekciou vyrobim masku s oznacenym stolom
IplImage *getTable(IplImage *image){

	IplImage* img_table = 0;
	if((img_table = cvLoadImage("table.png", 1)) == 0)
		return 0;

	IplImage* hsv_table = cvCreateImage(cvGetSize(img_table), IPL_DEPTH_8U, 3);
	IplImage* back_img_table = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);

	cvCvtColor(img_table, hsv_table, CV_BGR2HSV);

	IplImage* h_plane = cvCreateImage(cvGetSize(img_table), 8, 1);
	IplImage* planes[] = {h_plane};
	cvCvtPixToPlane(hsv_table, h_plane, 0, 0, 0);

	// Build and fill the histogram
	int h_bins = 255;
	CvHistogram* hist;
	{
		int hist_size[] = { h_bins };
		float h_ranges[] = { 0, 255 };
		float* ranges[] = { h_ranges };
		hist = cvCreateHist(1, hist_size, CV_HIST_ARRAY, ranges, 1);
	}
	cvCalcHist(planes, hist, 0, 0); // Compute histogram
	cvNormalizeHist( hist, 20*255 ); // Normalize it


	IplImage* image_hsv = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 3);
	cvCvtColor(image, image_hsv, CV_BGR2HSV);

	IplImage* hue = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
	cvCvtPixToPlane(image_hsv, hue, 0,0,0);

	cvCalcBackProject(&hue , back_img_table, hist);// Calculate back projection

	cvThreshold(back_img_table, back_img_table, 100, 255, CV_THRESH_BINARY);
	cvDilate(back_img_table, back_img_table);
	cvErode(back_img_table, back_img_table, 0, 5);

	cvReleaseImage(&img_table);
	cvReleaseImage(&hsv_table);
	cvReleaseImage(&h_plane);
	cvReleaseImage(&image_hsv);
	cvReleaseImage(&hue);

	return back_img_table;
}


//Nastavim ROI na cast obrazku so stolom
void setROIFromMask(IplImage *img, IplImage *mask){

	IplImage* dst = cvCreateImage(cvGetSize(img), 8, 1);
	IplImage* color_dst = cvCreateImage(cvGetSize(img), 8, 3);

	// OpenCV datova struktura - memory storage
	CvMemStorage* storage = cvCreateMemStorage(0);
	// OpenCV datova struktura - sequence na pracu s postupnostami
	CvSeq* lines = 0;

	// spocitame hrany
	cvCanny( mask, dst, 50, 200, 3 );
	cvCvtColor(dst, color_dst, CV_GRAY2BGR);

	// rho a theta rovnako
	// param1 - minimalna dlzka ciary
	// param2 - maximalna dlzka medzery medzi segmetami usecky
	lines = cvHoughLines2( dst, storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI/180, 50, 100, 25);

	int top = cvGetSize(img).height; 
	int bottom = 0;

	// vykreslenie useciek
	for(int i = 0; i < lines->total; i++ )
	{
		// pre CV_HOUGH_PROBABILISTIC je v "lines" ulozena postupost CvPoint-ov
		CvPoint* line = (CvPoint*)cvGetSeqElem(lines,i);
		if(line[0].y == line[1].y){
			if(line[0].y < top){
				top = line[0].y;
			}
			if(line[0].y > bottom){
				bottom = line[0].y;
			}
		}
		cvLine( color_dst, line[0], line[1], CV_RGB(255,0,0), 3, CV_AA, 0 );
	}

	//printf("%d - %d \n", top, bottom);

	if(top == bottom){
		if(top < 128){
			bottom = img->height;
		}else{
			top = 0;
		}
	}

	if((lines->total == 0) || (top >= bottom)){
		bottom = top;
		top = 0;
	}
	
	cvSetImageROI(img, cvRect(0, top, cvGetSize(img).width, bottom - top));

	//printf("1>x: %d, y: %d, w: %d, h: %d \n",  0, top, cvGetSize(img).width, bottom - top);

	/*
	cvNamedWindow( "Source", 1 );
	cvShowImage( "Source", img );
	cvNamedWindow( "Hough", 1 );
	cvShowImage( "Hough", color_dst );
	*/
	cvReleaseImage(&dst);
	cvReleaseImage(&color_dst);
	cvReleaseMemStorage(&storage);

}

// Backprojekcia
IplImage *backProject(IplImage *image, char *file){
	IplImage* img_table = 0;
	if((img_table = cvLoadImage(file, 1)) == 0)
		return 0;

	IplImage* hsv_table = cvCreateImage(cvGetSize(img_table), IPL_DEPTH_8U, 3);
	IplImage* back_img = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);

	cvCvtColor(img_table, hsv_table, CV_BGR2HSV);

	IplImage* h_plane = cvCreateImage(cvGetSize(img_table), 8, 1);
	IplImage* planes[] = {h_plane};
	cvCvtPixToPlane(hsv_table, h_plane, 0, 0, 0);

	// Build and fill the histogram
	int h_bins = 255;
	CvHistogram* hist;
	{
		int hist_size[] = { h_bins };
		float h_ranges[] = { 0, 255 };
		float* ranges[] = { h_ranges };
		hist = cvCreateHist(1, hist_size, CV_HIST_ARRAY, ranges, 1);
	}
	cvCalcHist(planes, hist, 0, 0); // Compute histogram
	cvNormalizeHist( hist, 20*255 ); // Normalize it


	IplImage* image_hsv = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 3);
	cvCvtColor(image, image_hsv, CV_BGR2HSV);

	IplImage* hue = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
	cvCvtPixToPlane(image_hsv, hue, 0,0,0);

	cvCalcBackProject(&hue , back_img, hist);// Calculate back projection

	cvReleaseImage(&img_table);
	cvReleaseImage(&hsv_table);
	cvReleaseImage(&h_plane);
	cvReleaseImage(&image_hsv);
	cvReleaseImage(&hue);

	return back_img;

}

//Najdenie objektov na stole
CvSeq* do_contours(IplImage *grey, float thresh, float min_obsah, int vypln)
{
	IplImage *cont_image = 0, *edge = 0;

	//! pomocne premenne pre kontury
	CvMemStorage* storage = 0;
	int levels = 3;
	CvSeq* contours = 0;
	CvSeq* contour_first = 0;
	
	storage = cvCreateMemStorage(0);
	// vysledny obrazok
	cont_image = cvCreateImage(cvGetSize(grey), grey->depth, grey->nChannels);

	edge = cvCreateImage(cvGetSize(grey), grey->depth, grey->nChannels);
	cvThreshold( grey, edge, thresh, 255, CV_THRESH_BINARY );
	// cvCanny(grey,edge,thresh,thresh*2);

	//! najdi kontury z binarneho obrazka a urob z nich zoznam
	cvFindContours( edge, storage, &contours, sizeof(CvContour), CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0) );
	//! vynuluj obrazok (black)
	cvZero( cont_image );

	int cvFilled = vypln==0 ? CV_FILLED : vypln;
	contour_first = contours;

	//! pre vsetky kontury vyrataj obsah a obvod a ak je obsah vacsie ako 3xmin_obsak tak ju vykresli nahodnou farbou
	for(int i=0 ; contours != 0; i++, contours = contours->h_next )
	{
		//! vypocitaj obsah a obvod kontury
		double obsah = fabs(cvContourArea(contours));
		double obvod = cvArcLength(contours, CV_WHOLE_SEQ, -1 );

		//! zobraz kontury, ktore maju obsah vacsi ako
		if (obsah > min_obsah)
		{
			
			CvScalar color = CV_RGB( rand()&255, rand()&255, rand()&255 );
			//! CV_FILLED = vyplnene kontury
			cvDrawContours( cont_image, contours, color, color, -1, cvFilled, 8 );

			//! vyrata a vykresli tazisko pre kazdu kontury cez momenty
			CvMoments* mom = new CvMoments();
			cvMoments(contours, mom);
			CvPoint cog = cvPoint((int)(mom->m10 / mom->m00), (int)(mom->m01 / mom->m00));
			//printf("(%d, %d): ", cog.x, cog.y);
			//printf("obsah %d -tej kontury je %.4f a obvod je %.4f\n",i,(float) obsah, (float) obvod);
			color.val[0] = 255 - color.val[0];
			color.val[1] = 255 - color.val[1];
			color.val[2] = 255 - color.val[2];
			cvCircle(cont_image, cog, 1, color);
			delete mom;
		}
	}
	//cvShowImage("kont", cont_image);
	
	//cvReleaseMemStorage(&storage);
	cvReleaseImage(&cont_image);
	cvReleaseImage(&edge);

	return contour_first;
}

// Spolocny minimalny BoundingBox
CvRect getBoundingBox(CvSeq* object_contours, float min_obsah){
	CvRect result = cvRect(0, 0, 0, 0);
	bool first = true;

	CvSeq *contours = object_contours;
	for(int i=0 ; contours != 0; i++, contours = contours->h_next )
	{
		//! vypocitaj obsah a obvod kontury
		double obsah = fabs(cvContourArea(contours));
		double obvod = cvArcLength(contours, CV_WHOLE_SEQ, -1 );

		if (obsah > min_obsah)
		{
			CvRect brect = cvContourBoundingRect(contours, 0);
			if(first){
				first = false;
				result.x = brect.x;
				result.y = brect.y;
				result.width = brect.width;
				result.height = brect.height;
			}else{
				if(brect.x < result.x){
					result.width += result.x - brect.x; 
					result.x = brect.x;
				}
				if(brect.x + brect.width > result.x + result.width)	{				
					result.width += brect.x + brect.width - result.x - result.width;
				}
				if(brect.y < result.y){
					result.height += result.y - brect.y; 
					result.y = brect.y;
				}
				if(brect.y + brect.height > result.y + result.height)	{				
					result.height += brect.y + brect.height - result.y - result.height;
				}
			}
		}
	}
	result.x -=3;
	result.width += 6;
	result.y -=3;
	result.height += 6;

	return result;
}

void getMoments(CvSeq* contours, float min_obsah){

	for(int i=0 ; contours != 0; i++, contours = contours->h_next )
	{
		// vypocitaj obsah kontury
		double obsah = fabs(cvContourArea(contours));
		
		if (obsah > min_obsah)
		{
			// vyrata momenty pre kazdu konturu cez momenty
			CvMoments* mom= new CvMoments();
			cvMoments(contours, mom);
			CvPoint cog = cvPoint((int)(mom->m10 / mom->m00), (int)(mom->m01 / mom->m00));
			
			//printf("\n");
			//printf("(%d, %d): ", cog.x, cog.y);
			//printf("obsah %d -tej kontury je %.4f \n",i,(float) obsah);			

			delete mom;
		}
	}

}


IplImage *getObjectImage(IplImage *img){
	//na obrazku najde back projekciou stol 
	IplImage* table = getTable(img);

	cvShowImage("table", table);

	//nastavi ROI image podla pozicie stola (svetle pixely)
	setROIFromMask(img, table);

	cvReleaseImage(&table);

	table = backProject(img, "table.png"); // oznaci stol
	cvDilate(table, table, 0, 2);
	cvErode(table, table, 0, 2);

	cvNot(table, table); //zneguje - oznaci objekty
	//cvCanny(table, table, 50, 100);

	IplImage *table2 = cvCreateImage(cvGetSize(table), table->depth, table->nChannels);
	cvCopy(table, table2);

	CvSeq* contours = do_contours(table2);
	//cvSetImageROI(table2, getBoundingBox(contours));
	cvSetImageROI(table2, cvRect(58, 132, 270, 100));

	//printf("x: %d, y: %d, w: %d, h: %d \n",  getBoundingBox(contours).x, getBoundingBox(contours).y, getBoundingBox(contours).width, getBoundingBox(contours).height);

	IplImage *objects = cvCreateImage(cvSize(60, 20), IPL_DEPTH_8U, 1);
	cvResize(table2, objects, CV_INTER_AREA);
	cvCanny(objects, objects, 10, 20);

	cvShowImage("table2", table2);
	cvShowImage("objekty", objects);
	//cvSaveImage("obj.png", objects);
	cvReleaseMemStorage(&contours->storage);
	cvReleaseImage(&table);
	return objects;
}

gsl_vector *createNeuronVector(int in, int hidden, int out){

	gsl_vector *neurons = gsl_vector_alloc(3);
	gsl_vector_set(neurons, 0, in);
	gsl_vector_set(neurons, 1, hidden);
	gsl_vector_set(neurons, 2, out);
	return neurons;
}

double distance_euclidean(position x, position y){
	double xdif = (x.x - y.x);
	double ydif = (x.y - y.y);
	double zdif = (x.z - y.z);
	return sqrt(xdif * xdif + ydif * ydif + zdif * zdif);
}

// prida bias na koniec a uvolni v
gsl_vector *addBias(gsl_vector *v){
	gsl_vector *result = gsl_vector_alloc(v->size + 1);
	gsl_vector_set_all(result, -1.0);
	for(unsigned int i = 0; i < v->size; i++){
		gsl_vector_set(result, i, gsl_vector_get(v, i));
	}
	gsl_vector_free(v);
	return result;
}

gsl_vector *addBit(gsl_vector *v, double bit){
	gsl_vector *result = gsl_vector_alloc(v->size + 1);
	gsl_vector_set_all(result, -1.0);
	for(unsigned int i = 0; i < v->size; i++){
		gsl_vector_set(result, i, gsl_vector_get(v, i));
	}
	gsl_vector_free(v);
	gsl_vector_set(result, result->size - 1, bit);

	return result;
}

gsl_vector *createOneHotVector(int size, int active){
	gsl_vector *v = gsl_vector_calloc(size);
	gsl_vector_set(v, active, 1.0);
	return v;
}

int getOneHotActive(gsl_vector *onehot){
	int pos = 0;
	for(unsigned int i = 0; i < onehot->size; i++){
		if(onehot->data[i] == 1.0){
			pos = i;
		}
	}
	return pos;
}

gsl_vector *combineVectors(gsl_vector *v1, gsl_vector *v2){
	gsl_vector *result = gsl_vector_alloc(v1->size + v2->size);
	for(unsigned int i = 0; i < v1->size; i++){
		gsl_vector_set(result, i, gsl_vector_get(v1, i));
	}
	for(unsigned int i = 0; i < v2->size; i++){
		gsl_vector_set(result, i + v1->size, gsl_vector_get(v2, i));
	}

return result;
}

gsl_vector *combineVectorsFreeFirst(gsl_vector *v1, gsl_vector *v2){
	gsl_vector *result = combineVectors(v1, v2);
	gsl_vector_free(v1);
	return result;
}

gsl_vector *getPositionVector(position pos){
	gsl_vector *result = gsl_vector_alloc(3);
	gsl_vector_set(result, 0, pos.x);
	gsl_vector_set(result, 1, pos.y);
	gsl_vector_set(result, 2, pos.z);
	
	return result;
}

char *toStringPosition(int position){
	switch(position){
		case POSITION_LEFT: return "left";
		case POSITION_MIDDLE: return "middle";
		case POSITION_RIGHT: return "right";
	}
}

char *toStringAction(int action){
	switch(action){
		case ACTION_POINT: return "point";
		case ACTION_TOUCH: return "touch";
		case ACTION_PUSH: return "push";
	}
}