/*======================================================================================

	Program:		Range Imaging Toolkit (RITK)
	Module:			OpenGLSkeletonEntity.cxx
	Version:		$Revision: 1467 $
	Date:			$Date: 2011-08-22 23:36:46 +0200 (Mon, 22 Aug 2011) $

	Copyright (c) 2010-2011
	Pattern Recognition Lab, 
	University Erlangen-Nuremberg, Germany
	All rights reserved.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 
	See RITKCopyright.txt for more information.

	http://www5.informatik.uni-erlangen.de/research/software/ritk

========================================================================================*/

#include "ritkDebugManager.h"

#include "OpenGLSkeletonEntity.h"
#include "ritkOpenGLExtensions.h"
#include "ritkTimer.h"
#include <stdio.h>
#include <time.h>
#include <string.h>
#include <direct.h>

#define CHECK_GL_ERROR()												\
{																		\
	GLenum errCode = glGetError();										\
	if ( errCode != GL_NO_ERROR )										\
	{																	\
	std::cout << "An OpenGL error occured: " << errCode << std::endl;	\
	std::cout << gluErrorString(errCode) << std::endl;					\
	std::cout << __FILE__ << " Line " << __LINE__  << std::endl;		\
	}																	\
}


//#define PERFORMANCE_CHECK
#define USE_SUB_TEX
//#define USE_INDEX_VBO

#pragma comment(lib, "opengl32.lib")
#pragma comment(lib, "glu32.lib")

namespace ritk
{

//----------------------------------------------------------------------------
OpenGLSkeletonEntity::OpenGLSkeletonEntity()
{
	// Not initialized by default
	m_InitFlag = false;

	// Create our control unit
	m_ControlUnit = new OpenGLSkeletonControlUnit();

	// do not draw inferred joint positions
	m_drawInferredJoints = false;
	m_drawSkeleton = true;

	m_ControlUnit->m_checkBoxWriteJointData->setVisible(false);
	m_ControlUnit->m_AlphaSlider->setVisible(false);
	m_ControlUnit->m_checkBoxShowSkeleton->setChecked(true);
	m_ControlUnit->m_checkBoxInferredJoints->setChecked(false);

	connect(m_ControlUnit, SIGNAL(changeInferredJointVisualization(bool)), this, SLOT(SetInferredJointsVisible(bool)));
	connect(m_ControlUnit, SIGNAL(changeJointOutput(bool)), this, SLOT(SetJointOutputStream(bool)));
	connect(m_ControlUnit, SIGNAL(showSkeletonSignal(bool)), this, SLOT(showSkeleton(bool)));
}

//----------------------------------------------------------------------------
OpenGLSkeletonEntity::~OpenGLSkeletonEntity()
{
	// Clean up
	CloseGL();

	if ( m_ControlUnit )
		delete m_ControlUnit;
}


//----------------------------------------------------------------------------
void
OpenGLSkeletonEntity::SetInferredJointsVisible(bool enable)
{
	m_drawInferredJoints = enable;
}

//----------------------------------------------------------------------------
void
OpenGLSkeletonEntity::SetSkeletonData(Skeleton::ConstPointer Data)
{
	m_Skeleton = Data;
	m_InitFlag = true;
}


//----------------------------------------------------------------------------
OpenGLSkeletonEntity::BoundingBoxType
OpenGLSkeletonEntity::GetBoundingBox() const
{

	OpenGLSkeletonEntity::BoundingBoxType SkeletonBB;

	if (!m_InitFlag)
	{
		for (int i = 0; i < 6; i ++)
			SkeletonBB[i] = 0;
		return SkeletonBB;
	}

	// do for all skeletons (at the moment there are 2 in the microsoft SDK)
	for (int j = 0; j < 2; j++)
	{
		for (Skeleton::JointTypes i = Skeleton::SKEL_HIP_Center; i != Skeleton::SKEL_COUNT; i = static_cast<Skeleton::JointTypes> (i + 1))
		{
			VF3 joint = m_Skeleton->GetJoint(i,j);

			// max X,Y, and Z value
			for (int j = 0; j < 3; j++)
			{
				SkeletonBB[j] = std::max(joint[j],SkeletonBB[j]);	
			}

			// min X,Y, and Z value
			for (int j = 3; j < 6; j++)
			{
				SkeletonBB[j] = std::min(joint[j],SkeletonBB[j]);	
			}
		}
	}
	
	return SkeletonBB;
}


//----------------------------------------------------------------------------
void
OpenGLSkeletonEntity::InitGL()
{

}


//----------------------------------------------------------------------------
void
OpenGLSkeletonEntity::CloseGL()
{
	m_InitFlag = false;
}

void
OpenGLSkeletonEntity::SetJointOutputStream(bool enable)
{
	m_WriteOutputTXT = enable;

	if (enable)
	{
		time_t now = time (NULL);
		struct tm * timeinfo = localtime ( &now );
		m_timestream.clear();
		m_timestream <<  timeinfo->tm_mon << '-';
		m_timestream <<  timeinfo->tm_mday << '_';
		m_timestream <<  timeinfo->tm_hour << '-';
		m_timestream <<  timeinfo->tm_min << '-';
		m_timestream <<  timeinfo->tm_sec;
	}

}

// Draws a sphere for a given joint -------------------------------------------
void
OpenGLSkeletonEntity::RenderJointSphere(Skeleton::JointTypes joint, int SkelIDX, GLUquadricObj *JointSphere, VF3 color, int radius)
{
	
	VF3 joint3D = m_Skeleton->GetJoint(joint,SkelIDX);
	glColor3f(color[0],color[1],color[2]);

	if (m_Skeleton->GetTrackingState(joint,SkelIDX) == Skeleton::INFERRED)
	{
		if (!m_drawInferredJoints)
			return;

		glColor3f(0,0,0);
	}
	
	glTranslatef(joint3D[0],joint3D[1],joint3D[2]);
	gluSphere( JointSphere , radius , 36 , 18 );
	glTranslatef(-joint3D[0],-joint3D[1],-joint3D[2]);
}

// connects two given joints with a cylinder ----------------------------------------------------------------------------
void
OpenGLSkeletonEntity::RenderCylinder(Skeleton::JointTypes joint1, Skeleton::JointTypes joint2, int SkelIDX, GLUquadricObj *cylinder, VF3 color, int radius)
{
	double distance = 0, rotationAngle = 0;
	VF3 distanceVec, point1, point2;

	// joint positions in 3DWC
	point1 = m_Skeleton->GetJoint(joint1, SkelIDX);
	point2 = m_Skeleton->GetJoint(joint2, SkelIDX);

	// Set the color of the cylinder
	glColor3f(color[0],color[1],color[2]);
	
	if (m_Skeleton->GetTrackingState(joint1, SkelIDX) == Skeleton::INFERRED ||
		m_Skeleton->GetTrackingState(joint2, SkelIDX) == Skeleton::INFERRED)
	{
		if (!m_drawInferredJoints)
			return;

		// set the color of a connection between an inferred joints to white
		glColor3f(0,0,0);
	}

	// Calculate the distance between 2 given points
	distanceVec[0] = point1[0] - point2[0];
	distanceVec[1] = point1[1] - point2[1];
	distanceVec[2] = point1[2] - point2[2];

	distance = std::sqrt(distanceVec[0] * distanceVec[0] +
						 distanceVec[1] * distanceVec[1] +
						 distanceVec[2] * distanceVec[2]);

	rotationAngle = std::acos(distanceVec[2] / std::sqrt((distanceVec[0] * distanceVec[0] +
														 distanceVec[1] * distanceVec[1] +
														 distanceVec[2] * distanceVec[2])));
	rotationAngle = 2 * M_PI - rotationAngle;

	// convert from radians in degree
	rotationAngle *= 180 / M_PI;

	
	glPushMatrix();
	glTranslatef(point1[0] - distanceVec[0], point1[1] - distanceVec[1], point1[2] - distanceVec[2]);

	// calculate Rotation axis and angle from the 2 Point distance vector
	// crossproduct of (x,y,z) x (0,1,0) is (-z,0,x)
	glRotatef(rotationAngle, distanceVec[1], -distanceVec[0], 0);

	// draw Cylinder
	gluCylinder(cylinder, radius, radius, distance, 36, 50);
	glTranslatef(-point1[0] + distanceVec[0],-point1[1] + distanceVec[1],-point1[2] + distanceVec[2]);
	glPopMatrix();
}

//----------------------------------------------------------------------------
void
	OpenGLSkeletonEntity::Render()
{

	if (!m_InitFlag || !m_drawSkeleton)
		return;

	// debug
	if (m_WriteOutputTXT)
	{
		VF3 jointcoords[9];
		int trackingstate[9];

		jointcoords[0] = m_Skeleton->GetJoint(Skeleton::SKEL_HEAD, 0);
		jointcoords[1] = m_Skeleton->GetJoint(Skeleton::SKEL_RIGHT_ELBOW, 0);
		jointcoords[2] = m_Skeleton->GetJoint(Skeleton::SKEL_LEFT_ELBOW, 0);
		jointcoords[3] = m_Skeleton->GetJoint(Skeleton::SKEL_RIGHT_HAND, 0);
		jointcoords[4] = m_Skeleton->GetJoint(Skeleton::SKEL_LEFT_HAND, 0);
		jointcoords[5] = m_Skeleton->GetJoint(Skeleton::SKEL_RIGHT_KNEE, 0);
		jointcoords[6] = m_Skeleton->GetJoint(Skeleton::SKEL_LEFT_KNEE, 0);
		jointcoords[7] = m_Skeleton->GetJoint(Skeleton::SKEL_RIGHT_FOOT, 0);
		jointcoords[8] = m_Skeleton->GetJoint(Skeleton::SKEL_LEFT_FOOT, 0);

		trackingstate[0] = m_Skeleton->GetTrackingState(Skeleton::SKEL_HEAD, 0);
		trackingstate[1] = m_Skeleton->GetTrackingState(Skeleton::SKEL_RIGHT_ELBOW, 0);
		trackingstate[2] = m_Skeleton->GetTrackingState(Skeleton::SKEL_LEFT_ELBOW, 0);
		trackingstate[3] = m_Skeleton->GetTrackingState(Skeleton::SKEL_RIGHT_HAND, 0);
		trackingstate[4] = m_Skeleton->GetTrackingState(Skeleton::SKEL_LEFT_HAND, 0);
		trackingstate[5] = m_Skeleton->GetTrackingState(Skeleton::SKEL_RIGHT_KNEE, 0);
		trackingstate[6] = m_Skeleton->GetTrackingState(Skeleton::SKEL_LEFT_KNEE, 0);
		trackingstate[7] = m_Skeleton->GetTrackingState(Skeleton::SKEL_RIGHT_FOOT, 0);
		trackingstate[8] = m_Skeleton->GetTrackingState(Skeleton::SKEL_LEFT_FOOT, 0);
 

		std::fstream xStream;
		std::fstream yStream;
		std::fstream zStream;
		std::fstream fStream;

		std::string dir = "D:\\records\\txtdata\\" + m_timestream.str() + "\\";
		_mkdir(dir.c_str());

		std::string x = dir + "X" + ".txt";
		std::string y = dir + "Y"  + ".txt";;
		std::string z = dir + "Z"  + ".txt";;
		std::string flag = dir + "Flag" + ".txt";;

		xStream.open(x, std::ios::app);
		yStream.open(y, std::ios::app);
		zStream.open(z, std::ios::app);
		fStream.open(flag, std::ios::app);

		for (int i = 0; i < 9; i++)
		{
			xStream << jointcoords[i][0];
			yStream << jointcoords[i][1];
			zStream << jointcoords[i][2];
			fStream << trackingstate[i];

			if (i != 8)
			{
				xStream <<  " ";
				yStream << " ";
				zStream <<  " ";
				fStream <<  " ";
			}
		}


		xStream << "\n";
		yStream << "\n";
		zStream << "\n";
		fStream << "\n";

		xStream.close();
		yStream.close();
		zStream.close();
		fStream.close();


	}

	// Setup joint sphere data
	VF3 joint;
	VF3 color;
	color[0] = color[1] = color[2] = 0;
	GLUquadricObj *JointSphere;
	JointSphere = gluNewQuadric();
	gluQuadricDrawStyle(JointSphere, GLU_FILL );
	
	// Setup cylinder data (to connect the joints properly)
	GLUquadricObj *cylinder = gluNewQuadric();
	
	glColorMaterial(GL_FRONT_AND_BACK, GL_DIFFUSE);
	glEnable(GL_COLOR_MATERIAL);
	
	// draw a sphere for each joint and connect properly with cylinder	

	// blue
	color[2] = 1;

	for (int i = 0; i < NUMBEROFSKELETONS; i ++)
	{
		// Valid skeleton data check
		if(m_Skeleton->GetJoint(Skeleton::SKEL_HIP_Center,i)[0] == 0)
			continue;

		RenderJointSphere(Skeleton::SKEL_HEAD, i, JointSphere, color, 100);
		RenderJointSphere(Skeleton::SKEL_NECK, i, JointSphere, color, 60);
		RenderJointSphere(Skeleton::SKEL_HIP_Center, i, JointSphere, color, 40);
		RenderJointSphere(Skeleton::SKEL_TORSO, i, JointSphere, color, 40);
		RenderJointSphere(Skeleton::SKEL_RIGHT_HIP, i, JointSphere, color, 40);
		RenderJointSphere(Skeleton::SKEL_LEFT_HIP, i, JointSphere, color, 40);
		RenderJointSphere(Skeleton::SKEL_RIGHT_SHOULDER, i, JointSphere, color, 40);
		RenderJointSphere(Skeleton::SKEL_LEFT_SHOULDER, i, JointSphere, color, 40);

		// yellow
		color[2] = 0;
		color[0] = color[1] = 1;
		
	
		short trackingState = m_Skeleton->GetTrackingState(Skeleton::SKEL_LEFT_HAND, i);
		if (trackingState == Skeleton::INFERRED)
			color[1] = 0;

		RenderJointSphere(Skeleton::SKEL_LEFT_HAND, i, JointSphere, color, 35);
		
		color[1] = 1;
		RenderJointSphere(Skeleton::SKEL_RIGHT_HAND, i, JointSphere, color, 35);	
		RenderJointSphere(Skeleton::SKEL_RIGHT_FOOT, i, JointSphere, color, 35);
		RenderJointSphere(Skeleton::SKEL_LEFT_FOOT, i, JointSphere, color, 35);

		// red
		color[1] = 0;
	
		RenderJointSphere(Skeleton::SKEL_RIGHT_ELBOW, i, JointSphere, color, 40);
		RenderJointSphere(Skeleton::SKEL_LEFT_ELBOW, i, JointSphere, color, 40);
		RenderJointSphere(Skeleton::SKEL_RIGHT_KNEE, i, JointSphere, color, 40);
		RenderJointSphere(Skeleton::SKEL_LEFT_KNEE, i, JointSphere, color, 40);

		// green
		color[0] = 0;
		color[1] = 1;

		// connect the spheres with cylinders
		RenderCylinder(Skeleton::SKEL_HEAD, 
					   Skeleton::SKEL_NECK,  i, cylinder, color, 40);
		RenderCylinder(Skeleton::SKEL_NECK,  
					   Skeleton::SKEL_RIGHT_SHOULDER, i, cylinder, color, 25);
		RenderCylinder(Skeleton::SKEL_NECK, 
					   Skeleton::SKEL_LEFT_SHOULDER,  i, cylinder, color, 25);
		RenderCylinder(Skeleton::SKEL_RIGHT_SHOULDER,  
					   Skeleton::SKEL_RIGHT_ELBOW, i,cylinder, color, 25);
		RenderCylinder(Skeleton::SKEL_LEFT_SHOULDER,   
					   Skeleton::SKEL_LEFT_ELBOW, i, cylinder, color, 25);
		RenderCylinder(Skeleton::SKEL_RIGHT_ELBOW,  
					   Skeleton::SKEL_RIGHT_HAND, i, cylinder, color, 20);
		RenderCylinder(Skeleton::SKEL_LEFT_ELBOW, 
					   Skeleton::SKEL_LEFT_HAND, i, cylinder, color, 20);
		RenderCylinder(Skeleton::SKEL_NECK, 
					   Skeleton::SKEL_TORSO, i, cylinder, color, 30);
		RenderCylinder(Skeleton::SKEL_TORSO, 
					   Skeleton::SKEL_HIP_Center, i, cylinder, color, 25);
		RenderCylinder(Skeleton::SKEL_HIP_Center, 
					   Skeleton::SKEL_RIGHT_HIP, i, cylinder, color, 25);
		RenderCylinder(Skeleton::SKEL_HIP_Center, 
					   Skeleton::SKEL_LEFT_HIP, i, cylinder, color, 25);
		RenderCylinder(Skeleton::SKEL_RIGHT_HIP, 
					   Skeleton::SKEL_RIGHT_KNEE, i, cylinder, color, 25);
		RenderCylinder(Skeleton::SKEL_LEFT_HIP, 
					   Skeleton::SKEL_LEFT_KNEE, i, cylinder, color, 25);
		RenderCylinder(Skeleton::SKEL_RIGHT_KNEE, 
					   Skeleton::SKEL_RIGHT_FOOT, i, cylinder, color, 20);
		RenderCylinder(Skeleton::SKEL_LEFT_KNEE, 
					   Skeleton::SKEL_LEFT_FOOT, i, cylinder, color, 20);


	}

	gluDeleteQuadric(JointSphere);

}


//----------------------------------------------------------------------------
QDockWidget*
OpenGLSkeletonEntity::GetControlUnit() const
{
	return m_ControlUnit;	
}


//----------------------------------------------------------------------------
void
OpenGLSkeletonEntity::PrintSelf(std::ostream& os, itk::Indent indent) const
{
	std::cout << "OpenGLSkeletonEntity" << std::endl;	
}


//----------------------------------------------------------------------------
void
OpenGLSkeletonEntity::showSkeleton(bool enable)
{
	m_drawSkeleton = enable;
}

} // namespace ritk



