/**
 * @file
 * @version     1.0.0
 *
 * \cond HIDDEN_SYMBOLS
 * 
 * Copyright (c) 2013
 * VoXel Interaction Design GmbH
 *
 * @author VoXel Interaction Design <office@voxel.at>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *
 * \endcond
 *
 * @section DESCRIPTION
 *
 * Example application for the sentis-ToF-m100 camera API
 *
 * This application shows an OpenGl view which displays 3D images based in distance values.
 * It uses the distances + amplitudes data format.
 * The view displays a grid of 160cmX120cm and the X axis shows the depth ~300cm.
 *
 * The amplitude values are used to set the color (gray) intensity of the image.
 *
 */

#define TICKS_PER_SECOND 20
#define LEN 8192

#ifdef __APPLE__
#include <GLUT/glut.h>
#define VSNPRINTF vsnprintf
#else
#if defined _WIN32 || defined _WIN64
#define _USE_MATH_DEFINES // VS losses some math macros as M_PI
#define VSNPRINTF vsnprintf_s
#include <windows.h>
#else
#define VSNPRINTF vsnprintf
#endif
#include <GL/glut.h>
#include <GL/gl.h>
#include <GL/glu.h>
#endif

#include <stdlib.h>
#include <m100api.h>

#include <stdio.h>
#include <stdarg.h>
#include <math.h>
#include <iostream>
using namespace std;

const int TIMER_MILLISECONDS = 100 / TICKS_PER_SECOND;

//angle of rotation
float xpos = 0, ypos = 0, zpos = 0, xrot = 0, yrot = 0, angle=0.0;
float lastx, lasty;
double dim=300.0;

int windowWidth=512;
int windowHeight=512;
int moving = 0;
int toggleAxes = 1;   /* toggle axes on and off */

//----- Camera control functions -----//
T_SENTIS_HANDLE handler;
T_ERROR_CODE error;
static int size = 76800;
static unsigned short buffer[120*2][160];

/**
 *
 * @brief Helper method that starts the connection with the camera and set default 
 * image_data format.
 * 
 * @param [in,out] T_SENTIS_HANDLE * Camera handler. 
 *
 */
int initCamera(T_SENTIS_HANDLE *handle) {

	T_SENTIS_CONFIG config;	
	// Default network configuration
	config.tcp_ip = "192.168.0.10"; 
	config.udp_ip = "224.0.0.1";
	config.udp_port = 10002;
	config.tcp_port = 10001;
	config.flags = HOLD_CONTROL_ALIVE;

	// Connect with the device
	*handle = STSopen(&config,&error);
	if (error != 0) {
		printf("Not possible to open the connection. Error: %d\n", error);
		return -1;
	}
	
	// Set image_data format to distances+amplitudes
	error = STSwriteRegister(*handle, ImageDataFormat, DEPTH_AMP_DATA );
	if (error != 0) {
		printf("Error when writing register. Error: %d\n", error);
		return -1;
	}

	return 0;
}

/**
 *
 * @brief Get a fame from the camera and save it in the global buffer variable.
 * 
 * @param [in] T_SENTIS_HANDLE Camera handler. 
 *
 */
int getFrame(T_SENTIS_HANDLE handle) {
	
	T_SENTIS_DATA_HEADER header;
	// size is 76800bytes; (120*2)*160*2bytes (short)
	size = (int)sizeof(buffer);
	error = STSgetData(handle, &header,  (char *)buffer, &size, 0, 0);
	if (error != 0) {
		printf("Error getting frame. Error: %d\n", error);
		return -1;
	}
	return 0;
}

/**
 *
 * @brief Close the connections with the camera and set free data structures
 * 
 * @param [in] T_SENTIS_HANDLE Camera handler. 
 *
 */
int stopCamera(T_SENTIS_HANDLE handle) {
	STSclose(handle);
	return 0;
}

//----- END Camera control functions END-----//

/**
 *
 * @brief Called before main loop to set up the program
 *
 */
void init() {
    glClearColor(0.0, 0.0, 0.0, 0.0);
    glEnable(GL_DEPTH_TEST);
    glShadeModel(GL_SMOOTH);
}

/**
 *
 * @brief Called every time a window is resized to resize the projection matrix
 *
 */
void reshape(int w, int h) {
	double w2h = (h>0) ? (double)w/h : 1;
	glViewport(0,0, w,h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	/* orthogonal projection */
	glOrtho(-dim*w2h,dim*w2h, -dim,dim, -dim,+dim);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void camera (void) {
    glRotatef(xrot,1.0,0.0,0.0);  //rotate our camera on the x-axis (left and right).
    glRotatef(yrot,0.0,1.0,0.0);  //rotate our camera on the y-axis (up and down).
	glTranslated(-xpos,-ypos,-zpos); //translate the screen to the position of our camera.
}


/**
 *
 * @brief Print axis names
 *
 */
void printv(va_list args, const char* format)
{
  char buf[LEN];
  char* ch=buf;
  VSNPRINTF(buf,LEN,format,args);
  while (*ch)
    glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18,*ch++);
}

/**
 *
 * @brief Loop for print axis names
 *
 */
void print(const char* format, ...) {
  va_list args;
  va_start(args,format);
  	printv(args,format);
  va_end(args);
}

/**
 *
 * @brief draw axes at defined position 
 *
 * We have a grid of 160cm*120cm. The depth of the X axis is set to 250cm
 *
 */
void drawAxes() {
  if (toggleAxes) {
    /*  Length of axes */
    //double len = 250.0f;
    glColor3f(0,1.0,0);
    glBegin(GL_LINES);
    glVertex3d(0,0,0);
    glVertex3d(160.f,0,0);
    glVertex3d(0,0,0);
    glVertex3d(0,120.f,0);
    glVertex3d(0,0,0);
    glVertex3d(0,0,250.f);
    glEnd();
    /*  Label axes */
    glRasterPos3d(160.f,0,0);
    print("Y");
    glRasterPos3d(0,120.f,0);
    print("Z");
    glRasterPos3d(0,0,250.f);
    print("X");
  }
}

// Static values for image displaying
static float lastMax=4000.f;
static float intens, x,y,z;	
static float min, max, mid;

/**
 *
 * @brief Called at the start of the program, after a glutPostRedisplay() and during idle
 * to display a frame
 *
 */
void display(int value) {
		
	if(getFrame(handler) == -1)
		cout << "*Frame fail." << endl;
	
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glEnable(GL_DEPTH_TEST);
	glLoadIdentity();
	camera();
	drawAxes();
    	
	glPushMatrix();
	glColor3f(1,1,1);
    	glBegin(GL_POINTS);
	
	float min=65535.f, max=0.f, mid=4000.f;
	
	int i,j,n=0;
	// We read all channel in one loop 160*120
	for (i= 0 ; i < 120; i++) {
		for(j=0; j<160; j++) {
			y = j*1.f-80.f;
			z = i*1.f-60.f;
			// We have a range of ~ 2000mm for 1500us Integration time (See chapter 8 of the camera documentation).
			// Here we are working in centimeters.
			x = (buffer[i][j])*0.1f+2;
			intens = (buffer[i+(120)][j])*1.f;

			if ( ( x==0.f && y==0.f && z==0.f ) || intens==0.f) {
				n++;
				continue;
			}
			mid += intens;
			if (min>intens) { min = intens; };
			if (max<intens) { max = intens; };
		
			glColor3f(intens/lastMax,intens/lastMax,intens/lastMax);
			glVertex3f(y, z, x);
		}
	}
	
	// Uncoment to display intensity values evolution
	/*cout << n <<  '\t' ;
	cout << min <<  '\t';
	cout << mid/(19200-n) <<  '\t';
	cout << max <<  '\t';
	cout << lastMax <<  '\t' << endl << flush;*/
	lastMax = (float)(0.8 * lastMax + 0.1 *max) / 2.f;


    	glEnd();
	glPopMatrix();
	glFlush();
	glutSwapBuffers();
	
	glutTimerFunc(TIMER_MILLISECONDS, display, 0);

	return;
}

/**
 *
 * @brief Called at the start of the program.
 *
 */
void firstDisplay() {
	display(0);
}

/**
 *
 * @brief Called after mouse click. It calculates the movement of the 
 * mouse and redisplay the scene
 *
 */
void mouseMovement(int x, int y) {
   if (moving) { 
	    float diffx=x-lastx; //check the difference between the    current x and the last x position
	    float diffy=y-lasty; //check the difference between the    current y and the last y position
	    lastx= (float) x; //set lastx to the current x position
	    lasty= (float) y; //set lasty to the current y position
	    xrot += diffy; //set the xrot to xrot with the addition    of the difference in the y position
	    yrot += diffx;    //set the xrot to yrot with the addition    of the difference in the x position
	    glutPostRedisplay();
	}
}

/**
 *
 * @brief Callback for mouse clicks
 *
 */
void mouse(int button, int state, int x, int y) {
	if (button == GLUT_LEFT_BUTTON && state == GLUT_DOWN) {
	moving = 1;
	lastx = (float) x;
	lasty = (float) y;
	}
 if (button == GLUT_LEFT_BUTTON && state == GLUT_UP) {
	moving = 0;
	}

}

/**
 *
 * @brief Callback for normal key pressed
 *
 */
void keyboard (unsigned char key, int x, int y) {
    float xrotrad, yrotrad;
    switch (key) {
        case 'w':
            yrotrad = float(yrot / 180 * M_PI);
            xrotrad = float(xrot / 180 * M_PI);
            xpos += float(sin(yrotrad)) ;
            zpos -= float(cos(yrotrad)) ;
            ypos -= float(sin(xrotrad)) ;
            break;
        case 's':
            yrotrad = float(yrot / 180 * M_PI);
            xrotrad = float(xrot / 180 * M_PI);
            xpos -= float(sin(yrotrad));
            zpos += float(cos(yrotrad)) ;
            ypos += float(sin(xrotrad));
            break;
        case 'd':
            yrotrad = float(yrot / 180 * M_PI);
            xpos += float(cos(yrotrad) * 0.2);
            zpos += float(sin(yrotrad) * 0.2);
            break;
        case 'a':
            yrotrad = float(yrot / 180 * M_PI);
            xpos -= float(cos(yrotrad) * 0.2);
            zpos -= float(sin(yrotrad) * 0.2);
            break;
	case 'x':
		toggleAxes = 1-toggleAxes;
	break;
        case 27:
		stopCamera(handler);
            exit(0);
        default:
        	break;
    }
}

/**
 *
 * @brief Callback for arrow keys
 *
 */
void windowSpecial(int key,int x,int y)
{
  /*  Right arrow key - increase azimuth by 5 degrees */
  if (key == GLUT_KEY_RIGHT) xrot += 5;
  /*  Left arrow key - decrease azimuth by 5 degrees */
  else if (key == GLUT_KEY_LEFT) xrot -= 5;
  /*  Up arrow key - increase elevation by 5 degrees */
  else if (key == GLUT_KEY_UP) yrot += 5;
  /*  Down arrow key - decrease elevation by 5 degrees */
  else if (key == GLUT_KEY_DOWN) yrot -= 5;

  /*  Keep angles to +/-360 degrees */
  //xrot = remainderf(xrot,360.0);
  //yrot = remainderf(yrot,360.0);
  xrot = (float)fmod(xrot,360.0);
  yrot = (float)fmod(yrot,360.0);

  glutPostRedisplay();
}

int main(int argc, char **argv)
{
    glutInit(&argc, argv); // Initializes glut
    
    // Sets up a double buffer with RGBA components and a depth component
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_DEPTH | GLUT_RGBA);
    
    // Sets the window size to 512*512 square pixels
    glutInitWindowSize(windowWidth, windowHeight);
    
    // Sets the window position to the upper left
    glutInitWindowPosition(0, 0);
    
    // Creates a window using internal glut functionality
    glutCreateWindow("3d");
    
    // passes reshape and display functions to the OpenGL machine for callback
    glutReshapeFunc(reshape);
    glutDisplayFunc(firstDisplay);
    //glutIdleFunc(display);

    glutMouseFunc(mouse);
    glutMotionFunc(mouseMovement); //check for mouse movement
    
    glutKeyboardFunc (keyboard);
    glutSpecialFunc(windowSpecial);

    if(initCamera(&handler) == -1)
		exit(1);

    init();

    // Starts the program.
    glutMainLoop();

    return 0;
}

