#ifndef M_PI 
#define M_PI 3.1415926535897932384626433832795
#endif

#include "VisualizationKinectPlugin.h"
#include "ritkDebugManager.h"
#include "ritkManager.h"
#include "KinectImage.h"
#include <GL/glu.h>
#include <GL/gl.h>

#include <stdio.h>
#include <string.h>
#include <iostream>
#include <fstream>


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

VisualizationKinectPlugin::VisualizationKinectPlugin()
{
	// Create the widget
	m_Widget = new VisualizationKinectWidget();
	connect(this, SIGNAL(UpdateGUI()), m_Widget, SLOT(UpdateGUI()));
	connect(m_Widget, SIGNAL(ConfigButtonPressed()), this, SLOT(LoadConfigFromFile()));
	m_Distance = 0;
	m_OffsetInnerCylHeight = 0;
	m_OffsetOuterCylHeight = 0;
}

VisualizationKinectPlugin::~VisualizationKinectPlugin()
{
	delete m_Widget;
}


//----------------------------------------------------------------------------
QString
VisualizationKinectPlugin::GetName()
{
	return tr("VisualizationKinectPlugin");
}


//----------------------------------------------------------------------------
QWidget*
VisualizationKinectPlugin::GetPluginGUI()
{
	return m_Widget;
}


//----------------------------------------------------------------------------
void
VisualizationKinectPlugin::ProcessEvent(ritk::Event::Pointer EventP)
{

	// New frame event
	if ( EventP->type() == ritk::NewFrameEvent::EventType )
	{
		// Cast the event
		ritk::NewFrameEvent::Pointer NewFrameEventP = qSharedPointerDynamicCast<ritk::NewFrameEvent,ritk::Event>(EventP);
		if ( !NewFrameEventP )
		{
			LOG_DEB("Event mismatch detected: Type=" << EventP->type());
			return;
		}
		
		// Try to get a kinect image
		KinectImageF2 * CurrentFrame = dynamic_cast<KinectImageF2*>((ritk::RImageF2*) NewFrameEventP->RImage.GetPointer());
		if ( !CurrentFrame )
		{
			std::cout << "Invalid image type detected: not a Kinect image..." << std::endl;
			return;
		}

		// Lock
		LockPlugin();
		
	
		// Update the range data
		m_Widget->SetRangeData(CurrentFrame);

		// Update the skeleton data
		if ( CurrentFrame->GetSkeleton() )
		{
			//std::cout << "Displaying skeleton data..." << std::endl;
			m_Widget->SetSkeletonData(CurrentFrame->GetSkeleton().GetPointer());
		}
		else
			std::cout << "Found no valid skeleton data..." << std::endl;


		// Unlock
		UnlockPlugin();

#if 0
		// if (m_Distance) then an image is shown of all points within the sphere
		// if not -> simple Visualization of the scene		
		if (!m_Distance) {
			LockPlugin();
			m_Widget->SetRangeData(CurrentFrameP);
			UnlockPlugin();
		}
		else
		{
			LockPlugin();
			m_CurrentFrame = CurrentFrameP;
			UnlockPlugin();


			//IntraSphereCheck();

			//// test
			//LockPlugin();
			//m_Widget->SetRangeData(RImageP);
			//UnlockPlugin();
			//UpdateCylinder();
		}
#endif
	}
	else
	{
		LOG_DEB("Unknown event");
	}
}

void VisualizationKinectPlugin::IntraSphereCheck() {
	
	// Remove m_IntraSphereActor actors !!?
	m_CopyOfCurrentFrame = dynamic_cast<RImageType*>(m_CurrentFrame->CreateAnother().GetPointer());

	if (!m_CopyOfCurrentFrame)
		return;

	m_CopyOfCurrentFrame->DeepCopy(m_CurrentFrame);


	typedef itk::ImageRegionConstIterator<ritk::NewFrameEvent::RImageType::WorldCoordImageType> RangeIteratorType;
	typedef itk::ImageRegionIterator<RImageType::RGBImageType> RGBIteratorType;

	RangeIteratorType itRange(m_CopyOfCurrentFrame->GetWorldCoordImage(),m_CopyOfCurrentFrame->GetWorldCoordImage()->GetRequestedRegion());
	RGBIteratorType     itRGB(m_CopyOfCurrentFrame->GetRGBImage(),       m_CopyOfCurrentFrame->GetRGBImage()->GetRequestedRegion());	
    
	double boundsCylinder1[6];
	double delta[3] = {0.0, 0.0, 0.0};
	VF3 tmp; // = {0.0, 0.0, 0.0};
	double green, red, distLineInner, distLineOuter, distPlaneVertical;

	VF3 p1Inner, p2Inner, p1Outer, p2Outer;
	//double *p1Outer = new double[3];
	//double *p2Outer = new double[3];


	// Apply custom Cylinder offset to Ground
	p1Inner[0] = m_PickPoint1[0] - m_OffsetInnerCylHeight * m_planeNormalVector[0];
	p1Inner[1] = m_PickPoint1[1] - m_OffsetInnerCylHeight * m_planeNormalVector[1];
	p1Inner[2] = m_PickPoint1[2] - m_OffsetInnerCylHeight * m_planeNormalVector[2];

	p2Inner[0] = m_PickPoint2[0] - m_OffsetInnerCylHeight * m_planeNormalVector[0];
	p2Inner[1] = m_PickPoint2[1] - m_OffsetInnerCylHeight * m_planeNormalVector[1];
	p2Inner[2] = m_PickPoint2[2] - m_OffsetInnerCylHeight * m_planeNormalVector[2];


	// Apply custom Cylinder offset to Ground
	p1Outer[0] = m_PickPoint1[0] - m_OffsetOuterCylHeight * m_planeNormalVector[0];
	p1Outer[1] = m_PickPoint1[1] - m_OffsetOuterCylHeight * m_planeNormalVector[1];
	p1Outer[2] = m_PickPoint1[2] - m_OffsetOuterCylHeight * m_planeNormalVector[2];

	p1Outer[0] = m_PickPoint2[0] - m_OffsetOuterCylHeight * m_planeNormalVector[0];
	p1Outer[1] = m_PickPoint2[1] - m_OffsetOuterCylHeight * m_planeNormalVector[1];
	p1Outer[2] = m_PickPoint2[2] - m_OffsetOuterCylHeight * m_planeNormalVector[2];

	//m_CylinderActor1->GetBounds(boundsCylinder1);
	
	for ( itRange.GoToBegin(), itRGB.GoToBegin(); !itRange.IsAtEnd(); ++itRange, ++itRGB)
	{	
		//
		tmp[0] = itRange.Value()[0];
		tmp[1] = itRange.Value()[1];
		tmp[2] = itRange.Value()[2];

		VF3 tmpVec = VF3Minus(m_planePoint1,tmp);


		// Check if a Point is below or above the table plane
		double distPlaneH = VF3ScalarProduct(tmpVec, m_planeNormalVector);

		// Check if depth coordinate is within the bounding box of the outer cylinder
		if(distPlaneH > 0)
		{
			distLineInner = VF3Abs(CrossProduct(VF3Minus(p1Inner, tmp), VF3Minus(p2Inner, tmp)))
				/ VF3Abs(VF3Minus(p1Inner, p2Inner));

			//std::cout << distLineInner << std::endl;

			distLineOuter = VF3Abs(CrossProduct(VF3Minus(p1Outer, tmp), VF3Minus(p2Outer, tmp)))
				/ VF3Abs(VF3Minus(p1Outer, p2Outer));

			if (distLineInner < m_RadiusOuter && distLineInner < m_RadiusOuter)
			{
				// this pixel lies not within the tolerance threshold
				if (distLineInner > m_RadiusInner) {

				red = (distLineInner - m_RadiusInner) / (m_RadiusOuter-m_RadiusInner) * 255;
				//red = (distance) / (m_RadiusInner) * 255;
				green = 255 - red;
				

				//setting RGB Values
				double tmpR = itRGB.Value()[0] + red;
				itRGB.Value()[0] = (int) tmpR; //(int) tmpR;
				if (tmpR > 255)
					itRGB.Value()[0] = 255;

				double tmpG = itRGB.Value()[1] + green;
				itRGB.Value()[1] = (int) tmpG; //(int) tmpG;
				if (tmpG > 255)
					itRGB.Value()[1] = 255;

				double tmpB = itRGB.Value()[2] / 50;
				itRGB.Value()[2] = 0; //(int) (itRGB.Value()[2] / 5) ;
				}

			}
			else if (0) //distLine < m_RadiusOuter + 400)
			{
				itRGB.Value()[0] = 255;
				itRGB.Value()[1] = 0;
				itRGB.Value()[2] = 0;
			}
		}
	}
		
	m_CopyOfCurrentFrame->Update();

	//// cast to const pointer and set data
	//RImagePointer tmpFrame = m_CopyOfCurrentFrame;
	//tmpFrame->DeepCopy(m_CopyOfCurrentFrame);

	m_Widget->SetRangeData((const ritk::RImageF2 *)m_CopyOfCurrentFrame);

}

void VisualizationKinectPlugin::UpdateCylinder()
{

	// Remove old actors ??
	if (!m_Distance)
		return;

	VF3 p1Inner, p2Inner, p1Outer, p2Outer;

	// Apply custom Cylinder offset to Ground (Inner Cylinder)
	p1Inner[0] = m_PickPoint1[0] - m_OffsetInnerCylHeight * m_planeNormalVector[0];
	p1Inner[1] = m_PickPoint1[1] - m_OffsetInnerCylHeight * m_planeNormalVector[1];
	p1Inner[2] = m_PickPoint1[2] - m_OffsetInnerCylHeight * m_planeNormalVector[2];

	p2Inner[0] = m_PickPoint2[0] - m_OffsetInnerCylHeight * m_planeNormalVector[0];
	p2Inner[1] = m_PickPoint2[1] - m_OffsetInnerCylHeight * m_planeNormalVector[1];
	p2Inner[2] = m_PickPoint2[2] - m_OffsetInnerCylHeight * m_planeNormalVector[2];

	// Apply custom Cylinder offset to Ground (Outer Cylinder)
	p1Outer[0] = m_PickPoint1[0] - m_OffsetOuterCylHeight * m_planeNormalVector[0];
	p1Outer[1] = m_PickPoint1[1] - m_OffsetOuterCylHeight * m_planeNormalVector[1];
	p1Outer[2] = m_PickPoint1[2] - m_OffsetOuterCylHeight * m_planeNormalVector[2];

	p2Outer[0] = m_PickPoint2[0] - m_OffsetOuterCylHeight * m_planeNormalVector[0];
	p2Outer[1] = m_PickPoint2[1] - m_OffsetOuterCylHeight * m_planeNormalVector[1];
	p2Outer[2] = m_PickPoint2[2] - m_OffsetOuterCylHeight * m_planeNormalVector[2];


	// Setup cylinder data
	GLUquadricObj *cylinder = gluNewQuadric();
	glColorMaterial(GL_FRONT_AND_BACK, GL_DIFFUSE);
	glEnable(GL_COLOR_MATERIAL);

	// Draw cylinder
	RenderCylinder(p1Inner, p2Inner, cylinder, m_RadiusInner);
	RenderCylinder(p1Outer, p2Outer, cylinder, m_RadiusOuter);

	gluDeleteQuadric(cylinder);
	emit UpdateGUI();
}


//----------------------------------------------------------------------------
VF3 
VisualizationKinectPlugin::CrossProduct(VF3 vector1, VF3 vector2)
{
	VF3 res;
	res[0] = vector1[1] * vector2[2] - vector1[2] * vector2[1];
	res[1] = vector1[2] * vector2[0] - vector1[0] * vector2[2];
	res[2] = vector1[0] * vector2[1] - vector1[1] * vector2[0];

	return res;
}


//----------------------------------------------------------------------------
VF3 
VisualizationKinectPlugin::VF3Minus(VF3 vector1, VF3 vector2)
{
	vector1[0] = vector1[0] - vector2[0];
	vector1[1] = vector1[1] - vector2[1];
	vector1[2] = vector1[2] - vector2[2];

	return vector1;
}

//----------------------------------------------------------------------------
VF3 
VisualizationKinectPlugin::VF3Plus(VF3 vector1, VF3 vector2)
{
	vector1[0] = vector1[0] + vector2[0];
	vector1[1] = vector1[1] + vector2[1];
	vector1[2] = vector1[2] + vector2[2];

	return vector1;
}

//----------------------------------------------------------------------------
double 
VisualizationKinectPlugin::VF3ScalarProduct(VF3 vector1, VF3 vector2)
{
	double res;
	res = vector1[0] * vector2[0];
	res += vector1[1] * vector2[1];
	res += vector1[2] * vector2[2];
	return res;
}

//----------------------------------------------------------------------------
double 
VisualizationKinectPlugin::VF3Abs(VF3 vector)
{
	return std::sqrt(vector[0] * vector[0] + vector[1] * vector[1] + vector[2] * vector[2]);
}

void
VisualizationKinectPlugin::RenderCylinder(VF3 point1, VF3 point2, GLUquadricObj *cylinder, int radius)
{
	double distance = 0, rotationAngle = 0;
	VF3 distanceVec;


	// Calculate the distance between 2 given points
	distanceVec = VF3Minus(point1,point2);
	distance = VF3Abs(distanceVec);

	rotationAngle = std::acos(distanceVec[2] / distance);
	

	// normalize and convert from radians in degree
	rotationAngle = 2 * M_PI - rotationAngle;
	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
VisualizationKinectPlugin::LoadConfigFromFile()
{
	std::ifstream myfile;
	std::string file = Openfilename();
	std::cout << file << std::endl;
	double values[19];
	myfile.open (file.c_str());

	for (int i = 0; i < 19; i++)
		myfile >> values[i];
	myfile.close();

	int idx = 0;
	m_PickPoint1[0] = values[idx++];
	m_PickPoint1[1] = values[idx++];
	m_PickPoint1[2] = values[idx++];

	m_PickPoint2[0] = values[idx++];
	m_PickPoint2[1] = values[idx++];
	m_PickPoint2[2] = values[idx++];

	m_planePoint1[0] = values[idx++];
	m_planePoint1[1] = values[idx++];
	m_planePoint1[2] = values[idx++];

	m_planePoint2[0] = values[idx++];
	m_planePoint2[1] = values[idx++];
	m_planePoint2[2] = values[idx++];

	m_planePoint3[0] = values[idx++];
	m_planePoint3[1] = values[idx++];
	m_planePoint3[2] = values[idx++];

	m_RadiusInner = values[idx++];
	m_RadiusOuter = values[idx++];

	std::cout << m_RadiusInner << std::endl;
	std::cout << m_RadiusOuter << std::endl;


	m_OffsetInnerCylHeight = values[idx++];
	m_OffsetOuterCylHeight = values[idx];

	m_Distance = VF3Abs(VF3Minus(m_PickPoint1, m_PickPoint2));

	UpdateCylinder();
}

std::string
VisualizationKinectPlugin::Openfilename() {
  /*OPENFILENAME ofn;
  char fileName[MAX_PATH] = "";
  ZeroMemory(&ofn, sizeof(ofn));
 
  ofn.lStructSize = sizeof(OPENFILENAME);
  ofn.hwndOwner = NULL;
  //ofn.lpstrFilter = "txt files (*.txt)|*.txt|All files (*.*)|*.*";
  ofn.lpstrFile = fileName;
  ofn.nMaxFile = MAX_PATH;
  ofn.Flags = OFN_EXPLORER | OFN_FILEMUSTEXIST | OFN_HIDEREADONLY;
  ofn.lpstrDefExt = "";
 
  std::string fileNameStr;
 
  if ( GetOpenFileName(&ofn) )
    fileNameStr = fileName;
 
  return fileNameStr;*/
	return "Blabla"; // @Johannes: This is NOT std C++
}
//
////----------------------------------------------------------------------------
//void 
//	VisualizationKinectPlugin::mousePressEvent(QMouseEvent *e)
//{
//	if ( e->buttons() & Qt::RightButton )
//	{
//		std::cout<< "Rightclick" << std::endl;
//		m_LastMousePos = e->pos();
//	}
//	// check Key and project
//}

