#include "VTKDataProcessingPlugin.h"
#include "ritkDebugManager.h"
#include "ritkManager.h"



#include <vtkPointPicker.h>
#include <vtkSmartPointer.h>
#include <vtkRendererCollection.h>
#include <vtkSphereSource.h>
#include <vtkLine.h>
#include <vtkProperty.h>
#include <vtkCellData.h>
#include <vtkPoints.h>
#include <vtkUnsignedCharArray.h>
#include <vtkPointData.h>
#include <vtkDecimatePro.h>
#include <vtkQuadricDecimation.h>
#include <vtkQuadricClustering.h>
#include <vtkFloatArray.h>
#include <vtkTransform.h>
#include <vtkCleanPolyData.h>
#include <vtkXMLPolyDataWriter.h>
#include <vtkSTLWriter.h>
#include <vtkXMLUnstructuredGridWriter.h>

#include <itkImageRegionConstIterator.h>
#include <itkRescaleIntensityImageFilter.h>

#include <QFileDialog.h>

#include <algorithm>

typedef vtkSmartPointer<vtkPoints> vtkPointsRef;
typedef vtkSmartPointer<vtkUnsignedCharArray> vtkUnsignedCharArrayRef;
typedef vtkSmartPointer<vtkCellArray> vtkCellArrayRef;
typedef vtkSmartPointer<vtkFloatArray> vtkFloatArrayRef;


VTKDataProcessingPlugin::VTKDataProcessingPlugin()
{
	// Create the widget
	m_Widget = new VTKDataProcessingWidget();
	connect(this, SIGNAL(UpdateGUI()), m_Widget, SLOT(UpdateGUI()));

	// signal/slot connections
	connect(m_Widget, SIGNAL(PushButtonLoadVTKData()), this, SLOT(LoadVTKData()));
	connect(m_Widget, SIGNAL(PushButtonLoadRangeData()), this, SLOT(LoadRangeData()));
	connect(m_Widget, SIGNAL(PushButtonSaveData()), this, SLOT(SaveData()));
	connect(m_Widget, SIGNAL(PushButtonPerformSegmentation()), this, SLOT(PerformSegmentation()));
	connect(m_Widget, SIGNAL(PushButtonPerformTriangulation()), this, SLOT(PerformTriangulation()));
	connect(m_Widget, SIGNAL(PushButtonPerformDecimation()), this, SLOT(PerformDecimation()));

	connect(m_Widget, SIGNAL(PushButtonSaveRangeData()), this, SLOT(SaveRangeData()));
	connect(m_Widget, SIGNAL(PushButtonSaveFlagData()), this, SLOT(SaveFlagData()));

	connect(m_Widget->m_PushButtonSaveMarkerCoordinates, SIGNAL(clicked()), this, SLOT(SaveMarkerCoordinates()));

	// add data actor
	m_DataActor3D = RImageActorPipelineRef::New();	
	m_DataActor3D->SetVisualizationMode(ritk::RImageActorPipeline::RGB);
	m_Widget->m_VisualizationWidget3D->AddActor(m_DataActor3D);

	// initialize objects
	m_VTKReader = vtkPolyDataReaderRef::New();
	m_VTKWriter = vtkPolyDataWriterRef::New();
	m_Data = vtkPolyDataRef::New();
	m_PolyData = vtkPolyDataRef::New();	
	m_ProcessedData = vtkPolyDataRef::New();

	// Point picker
	m_QtSlotConnector = vtkSmartPointer<vtkEventQtSlotConnect>::New();
	m_QtSlotConnector->Connect(
		m_Widget->m_VisualizationWidget3D->GetRenderWindow()->GetInteractor(),
		vtkCommand::RightButtonPressEvent,
        this,
        SLOT(RightButtonPressEventHandling(vtkObject*, unsigned long, void*, void*))
		);

	m_PickPoint1 = new double[3];
	m_PickPoint2 = new double[3];
	m_PickPoint3 = new double[3];
	m_PickPoint4 = new double[3];
	
	m_PickPoint1[0] = 0.0;
	m_PickPoint1[1] = 0.0;
	m_PickPoint1[2] = 0.0;

	m_PickPoint2[0] = 0.0;
	m_PickPoint2[1] = 0.0;
	m_PickPoint2[2] = 0.0;

	m_PickPoint3[0] = 0.0;
	m_PickPoint3[1] = 0.0;
	m_PickPoint3[2] = 0.0;

	m_PickPoint4[0] = 0.0;
	m_PickPoint4[1] = 0.0;
	m_PickPoint4[2] = 0.0;

	m_PickPoint1Actor = NULL;
	m_PickPoint2Actor = NULL;
	m_PickPoint3Actor = NULL;
	m_PickPoint4Actor = NULL;
	m_LineActor = NULL;

	UpdatePointPickerGUI();


	m_RImageVTKData = vtkSmartPointer<ritk::RImageVTKData>::New();
	m_RImageVTKData->SetPolyDataType(ritk::RImageVTKData::POINTCLOUD);
}

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


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


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


//----------------------------------------------------------------------------
void
VTKDataProcessingPlugin::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;
		}
		m_CurrentFrame = NewFrameEventP->RImage;

		// Here comes your code. Access range data with CurrentFrame.
		// ...

		m_WorldCoordImage = m_CurrentFrame->GetWorldCoordImage();
		m_RangeImage = m_CurrentFrame->GetRangeImage();

		ritk::RGBRImageUCF2::ConstPointer CurrentFrameRGBP = dynamic_cast<const ritk::RGBRImageUCF2*>((const ritk::RImageF2*) m_CurrentFrame);
		if ( !CurrentFrameRGBP )
		{
			LOG_DEB("Failed to cast.");
			return;
		}
		m_RGBImage = CurrentFrameRGBP->GetRGBImage();
		
		// Try to cast the image
		/*m_CamCubeImage = dynamic_cast<const CamCubeImageType*>((const RImageType*)CurrentFrame);
		if ( !m_CamCubeImage )
		{
			LOG_DEB("Could not cast to CamCube image...");
			return;
		}*/

		m_RImageVTKData->GeneratePolyData(m_CurrentFrame);
		m_PolyData =  m_RImageVTKData->GetPolyData();
		
		emit UpdateGUI();
	}
	else
	{
		LOG_DEB("Unknown event");
	}
}


//----------------------------------------------------------------------------
void
VTKDataProcessingPlugin::LoadVTKData()
{	
	m_InputDataFilePath = QFileDialog::getOpenFileName(m_Widget, "Open VTK File", "E:/ProjectedLight/daten_puppen", "VTK files (*.vtk)");		
	//m_InputDataFilePath = "E:/ProjectedLight/daten_puppen/f1.vtk";

	if ( !m_InputDataFilePath.isEmpty() ) 
	{		
		m_VTKReader->SetFileName(m_InputDataFilePath.toStdString().c_str());
		m_VTKReader->Update();
		m_Data = m_VTKReader->GetOutput();		
	}	

	//// Flip
	//vtkPointsRef NewPoints = vtkPointsRef::New();
	//double * Point = new double[3];
	//for(vtkIdType i = 0; i < m_Data->GetNumberOfPoints(); i++)
	//{
	//	Point = m_Data->GetPoint(i);
	//	Point[2] *= -1;
	//	NewPoints->InsertNextPoint(Point);
	//}
	//m_Data->SetPoints(NewPoints);
	

	m_Data->BuildLinks();
	m_DataActor3D->SetData(m_Data,false);

	m_ProcessedData = m_Data;

	// update segmentation bounds w.r.t. the bounds of the loaded object
	m_Data->ComputeBounds();
	m_Data->GetBounds(m_DataBounds);
	m_Data->GetBounds(m_SegmentationBounds);

	m_Widget->m_SpinBoxMinX->setValue(m_SegmentationBounds[0]);
	m_Widget->m_SpinBoxMaxX->setValue(m_SegmentationBounds[1]);
	m_Widget->m_SpinBoxMinY->setValue(m_SegmentationBounds[2]);
	m_Widget->m_SpinBoxMaxY->setValue(m_SegmentationBounds[3]);
	m_Widget->m_SpinBoxMinZ->setValue(m_SegmentationBounds[4]);
	m_Widget->m_SpinBoxMaxZ->setValue(m_SegmentationBounds[5]);
	
	emit UpdateGUI();
}


//----------------------------------------------------------------------------
float ComputeMedian(std::vector<float>& vec) 
{ 
  std::nth_element( vec.begin(), vec.begin() + vec.size() / 2, vec.end() ); 
  return *( vec.begin() + vec.size() / 2 ); 
}


//----------------------------------------------------------------------------
void
VTKDataProcessingPlugin::LoadRangeData()
{
	m_Data->DeepCopy(m_PolyData);	
	m_Data->BuildLinks();

	// Get the min and max value from the current frame
	typedef itk::ImageRegionConstIterator<ritk::NewFrameEvent::RImageType::RangeImageType> IteratorType;
	IteratorType it(m_RangeImage,m_RangeImage->GetRequestedRegion());
	double range[2];
	range[0] = 1e16;
	range[1] = -1e16;
	for ( it.GoToBegin(); !it.IsAtEnd(); ++it )
	{
		if ( it.Value() < range[0] )
			range[0] = it.Value();
		if ( it.Value() > range[1] )
			range[1] = it.Value();
	}

	m_DataActor3D->SetData(m_Data,m_CurrentFrame);
  m_DataActor3D->SetRGBInterval(range[0],range[1]);
	
/* SVEN nicht mehr da rgb
	// Rescale and cast the Grayscale image
	typedef itk::RescaleIntensityImageFilter<RImageType::GrayscaleImageType,itk::Image<unsigned char,2>> RescalerType;
	RescalerType::Pointer Rescaler = RescalerType::New();
	Rescaler->SetOutputMinimum(0);
	Rescaler->SetOutputMaximum(255);
	Rescaler->SetInput(m_CurrentFrame->GetGrayscaleImage());
	Rescaler->GetOutput()->SetRequestedRegion(m_CurrentFrame->GetGrayscaleImage()->GetRequestedRegion());
	Rescaler->Update();
	// Copy the scalars into an unsigned char array that defines the colors
	// This will hold the scalar values (colors)
	vtkSmartPointer<vtkDataArray> Colors;
	Colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
	Colors->SetNumberOfComponents(4);
	typedef itk::ImageRegionConstIterator<RescalerType::OutputImageType> RescalerOutputIteratorType;
	RescalerOutputIteratorType itRescalerOutput(Rescaler->GetOutput(),Rescaler->GetOutput()->GetRequestedRegion());
	for ( itRescalerOutput.GoToBegin(); !itRescalerOutput.IsAtEnd(); ++itRescalerOutput )
	{
		unsigned char c = itRescalerOutput.Value();
		Colors->InsertNextTuple4(c,c,c,255);
		//Colors->InsertNextTuple4(c,c,c,(Image->GetFlagImage()->GetBufferPointer()[i]&RImageType::FOREGROUND)*255);
	}
	*/
  // This will hold the scalar values (colors)
	vtkSmartPointer<vtkDataArray> Colors;
	Colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
	Colors->SetNumberOfComponents(4);


	typedef itk::ImageRegionConstIterator<ritk::RGBRImageUCF2::RGBImageType> RGBIteratorType;
	RGBIteratorType itRGB(m_RGBImage,m_RGBImage->GetRequestedRegion());
	for ( itRGB.GoToBegin(); !itRGB.IsAtEnd(); ++itRGB )
	{
		Colors->InsertNextTuple4(itRGB.Value()[0],itRGB.Value()[1],itRGB.Value()[2],255);
		//Colors->InsertNextTuple4(c,c,c,(Image->GetFlagImage()->GetBufferPointer()[i]&RImageType::FOREGROUND)*255);
	}

	m_Data->GetPointData()->SetScalars(Colors);	
	m_ProcessedData = m_Data;

	// update segmentation bounds w.r.t. the bounds of the loaded object
	m_Data->ComputeBounds();
	m_Data->GetBounds(m_DataBounds);
	m_Data->GetBounds(m_SegmentationBounds);


	m_Widget->m_SpinBoxMinX->setValue(m_SegmentationBounds[0]);
	m_Widget->m_SpinBoxMaxX->setValue(m_SegmentationBounds[1]);
	m_Widget->m_SpinBoxMinY->setValue(m_SegmentationBounds[2]);
	m_Widget->m_SpinBoxMaxY->setValue(m_SegmentationBounds[3]);
	m_Widget->m_SpinBoxMinZ->setValue(m_SegmentationBounds[4]);
	m_Widget->m_SpinBoxMaxZ->setValue(m_SegmentationBounds[5]);
	
	emit UpdateGUI();
}


//----------------------------------------------------------------------------
void
VTKDataProcessingPlugin::PerformSegmentation()
{
	vtkPolyDataRef NewPolyData = vtkPolyDataRef::New();
	vtkPointsRef NewPoints = vtkPointsRef::New();
	vtkCellArrayRef NewCells = vtkCellArrayRef::New();
	vtkFloatArrayRef NewScalars = vtkFloatArrayRef::New();

	if ( m_Data->GetPointData()->GetScalars() )
	{
		if(m_Data->GetPointData()->GetScalars()->GetNumberOfComponents() == 1)
		{
			NewScalars->SetNumberOfComponents(1);
		}
		else if(m_Data->GetPointData()->GetScalars()->GetNumberOfComponents() == 3)
		{
			NewScalars->SetNumberOfComponents(3);
		}
	}

	double * Point = new double[3];
	double * Scalar = new double[3];
	for(vtkIdType i = 0; i < m_Data->GetNumberOfPoints(); i++)
	{
		Point = m_Data->GetPoint(i);

		if(	(Point[0] > m_Widget->m_SpinBoxMinX->value()) &&
			(Point[0] < m_Widget->m_SpinBoxMaxX->value()) &&
			(Point[1] > m_Widget->m_SpinBoxMinY->value()) &&
			(Point[1] < m_Widget->m_SpinBoxMaxY->value()) &&
			(Point[2] > m_Widget->m_SpinBoxMinZ->value()) &&
			(Point[2] < m_Widget->m_SpinBoxMaxZ->value()) )
		{			
			NewPoints->InsertNextPoint(Point);

			if ( m_Data->GetPointData()->GetScalars() )
			{
			
				if(m_Data->GetPointData()->GetScalars()->GetNumberOfComponents() == 1)
				{
					double ScalarValue = m_Data->GetPointData()->GetScalars()->GetTuple1(i);
					NewScalars->InsertNextTuple1(ScalarValue);
				}
				else if(m_Data->GetPointData()->GetScalars()->GetNumberOfComponents() == 3)
				{
					Scalar = m_Data->GetPointData()->GetScalars()->GetTuple3(i);
					NewScalars->InsertNextTuple3(Scalar[0],Scalar[1],Scalar[2]);
				}
			}				
		}	
	}

	NewPolyData->SetPoints(NewPoints);

	for(vtkIdType i = 0; i < NewPolyData->GetNumberOfPoints(); i++)
	{
		NewCells->InsertNextCell(1, &i);
	}

	if ( m_Data->GetPointData()->GetScalars() )
		NewPolyData->GetPointData()->SetScalars(NewScalars);	
	NewPolyData->SetVerts(NewCells);
	m_ProcessedData->DeepCopy(NewPolyData);

	//m_ProcessedData->BuildLinks();

	m_DataActor3D->SetData(m_ProcessedData,false);
	emit UpdateGUI();
}

//----------------------------------------------------------------------------
void
VTKDataProcessingPlugin::PerformTriangulation()
{	
	vtkSmartPointer<vtkTransform> Transform = vtkSmartPointer<vtkTransform>::New();
	//Transform->RotateX(90);

	vtkSmartPointer<vtkDelaunay2D> Delaunay2D = vtkSmartPointer<vtkDelaunay2D>::New();
	Delaunay2D->SetInput(m_ProcessedData);
	Delaunay2D->SetTransform(Transform);
	Delaunay2D->Update();

	// @todo as an alternative, provide vtkDelaunay3D

	m_ProcessedData = Delaunay2D->GetOutput();

	m_DataActor3D->SetData(m_ProcessedData,m_CurrentFrame);
	emit UpdateGUI();
}

//----------------------------------------------------------------------------
void
VTKDataProcessingPlugin::PerformDecimation()
{
	//vtkSmartPointer<vtkDecimatePro> Decimator = vtkSmartPointer<vtkDecimatePro>::New();
	//Decimator->SetInput(m_ProcessedData);
	//Decimator->SetTargetReduction(((float) m_Widget->m_SpinBoxDecimationPercentage->value())/100.0f); //99% reduction (if there was 100 triangles, now there will be 1)
	//Decimator->Update();

	vtkSmartPointer<vtkQuadricDecimation> QuadricDecimation = vtkSmartPointer<vtkQuadricDecimation>::New();
	QuadricDecimation->SetTargetReduction(((float) m_Widget->m_SpinBoxDecimationPercentage->value())/100.0f); //99% reduction (if there was 100 triangles, now there will be 1)
	QuadricDecimation->SetInput(m_ProcessedData);
	QuadricDecimation->Update();

	//vtkSmartPointer<vtkQuadricClustering> QuadricClustering = vtkSmartPointer<vtkQuadricClustering>::New();
	//QuadricClustering->SetNumberOfXDivisions(16);
	//QuadricClustering->SetNumberOfYDivisions(16);
	//QuadricClustering->SetNumberOfZDivisions(16);
	////QuadricClustering->AutoAdjustNumberOfDivisionsOn();
	//QuadricClustering->SetInput(m_ProcessedData);
	//QuadricClustering->Update();

	m_ProcessedData = QuadricDecimation->GetOutput();

	m_DataActor3D->SetData(m_ProcessedData,false);
	emit UpdateGUI();
}

void 
VTKDataProcessingPlugin::UpdatePointPickerGUI()
{
	m_Widget->m_LineEditP1x->setText(QString::number(m_PickPoint1[0]));
	m_Widget->m_LineEditP1y->setText(QString::number(m_PickPoint1[1]));
	m_Widget->m_LineEditP1z->setText(QString::number(m_PickPoint1[2]));

	m_Widget->m_LineEditP2x->setText(QString::number(m_PickPoint2[0]));
	m_Widget->m_LineEditP2y->setText(QString::number(m_PickPoint2[1]));
	m_Widget->m_LineEditP2z->setText(QString::number(m_PickPoint2[2]));	

	m_Widget->m_LineEditDistance->setText(QString::number(m_Distance));
}


void 
VTKDataProcessingPlugin::RightButtonPressEventHandling(vtkObject* caller, unsigned long vtk_event, void* client_data, void* call_data)
{
	vtkRenderWindowInteractor * iren		=	vtkRenderWindowInteractor::SafeDownCast(caller);
	vtkSmartPointer<vtkPointPicker> picker	=	vtkSmartPointer<vtkPointPicker>::New();
	vtkSmartPointer<vtkPoints> points		=	vtkSmartPointer<vtkPoints>::New();
	int event_pos[2];
	double * Point = new double[3];

	if( vtk_event == vtkCommand::RightButtonPressEvent )
	{		
		iren->GetEventPosition(event_pos);

		if(m_PickPoint1Actor)	m_PickPoint1Actor->VisibilityOff();
		if(m_PickPoint2Actor)	m_PickPoint2Actor->VisibilityOff();
		if(m_PickPoint3Actor)	m_PickPoint3Actor->VisibilityOff();
		if(m_PickPoint4Actor)	m_PickPoint4Actor->VisibilityOff();
		if(m_LineActor)			m_LineActor->VisibilityOff();

		picker->SetTolerance(0.5);
		if(picker->Pick(event_pos[0], event_pos[1], 0, m_Widget->m_VisualizationWidget3D->GetRenderWindow()->GetRenderers()->GetFirstRenderer() ) != 0)
		{			
			if(*(iren->GetKeySym()) == '1')
			{
				Point = picker->GetPickPosition();
				m_PickPoint1[0] = Point[0];
				m_PickPoint1[1] = Point[1];
				m_PickPoint1[2] = Point[2];

				// Remove old actor
				if(m_PickPoint1Actor) m_Widget->m_VisualizationWidget3D->RemoveActor(m_PickPoint1Actor);
	
				vtkSmartPointer<vtkSphereSource> SphereSource = vtkSmartPointer<vtkSphereSource>::New();
				SphereSource->SetCenter(m_PickPoint1);
				SphereSource->SetRadius(5);
 
				vtkSmartPointer<vtkPolyDataMapper> SphereMapper = vtkSmartPointer<vtkPolyDataMapper>::New();
				SphereMapper->SetInput(SphereSource->GetOutput());
 
				m_PickPoint1Actor = vtkSmartPointer<vtkActor>::New();
				m_PickPoint1Actor->SetMapper(SphereMapper);
				m_PickPoint1Actor->GetProperty()->SetOpacity(0.8);
				m_PickPoint1Actor->GetProperty()->SetColor(0.0,0.8,0.0);

				// Add sphere actor to renderer
				m_Widget->m_VisualizationWidget3D->AddActor(m_PickPoint1Actor);				
			}
			else if(*(iren->GetKeySym()) == '2')
			{
				Point = picker->GetPickPosition();
				m_PickPoint2[0] = Point[0];
				m_PickPoint2[1] = Point[1];
				m_PickPoint2[2] = Point[2];

				// Remove old actor
				if(m_PickPoint2Actor) m_Widget->m_VisualizationWidget3D->RemoveActor(m_PickPoint2Actor);
	
				vtkSmartPointer<vtkSphereSource> SphereSource = vtkSmartPointer<vtkSphereSource>::New();
				SphereSource->SetCenter(m_PickPoint2);
				SphereSource->SetRadius(5);
 
				vtkSmartPointer<vtkPolyDataMapper> SphereMapper = vtkSmartPointer<vtkPolyDataMapper>::New();
				SphereMapper->SetInput(SphereSource->GetOutput());
 
				m_PickPoint2Actor = vtkSmartPointer<vtkActor>::New();
				m_PickPoint2Actor->SetMapper(SphereMapper);
				m_PickPoint2Actor->GetProperty()->SetOpacity(0.8);
				m_PickPoint2Actor->GetProperty()->SetColor(0.0,0.0,0.8);

				// Add sphere actor to renderer
				m_Widget->m_VisualizationWidget3D->AddActor(m_PickPoint2Actor);				
			}
			else if(*(iren->GetKeySym()) == '4')
			{
				Point = picker->GetPickPosition();
				m_PickPoint3[0] = Point[0];
				m_PickPoint3[1] = Point[1];
				m_PickPoint3[2] = Point[2];

				// Remove old actor
				if(m_PickPoint3Actor) m_Widget->m_VisualizationWidget3D->RemoveActor(m_PickPoint3Actor);
	
				vtkSmartPointer<vtkSphereSource> SphereSource = vtkSmartPointer<vtkSphereSource>::New();
				SphereSource->SetCenter(m_PickPoint3);
				SphereSource->SetRadius(5);
 
				vtkSmartPointer<vtkPolyDataMapper> SphereMapper = vtkSmartPointer<vtkPolyDataMapper>::New();
				SphereMapper->SetInput(SphereSource->GetOutput());
 
				m_PickPoint3Actor = vtkSmartPointer<vtkActor>::New();
				m_PickPoint3Actor->SetMapper(SphereMapper);
				m_PickPoint3Actor->GetProperty()->SetOpacity(0.8);
				m_PickPoint3Actor->GetProperty()->SetColor(0.8,0.0,0.0);

				// Add sphere actor to renderer
				m_Widget->m_VisualizationWidget3D->AddActor(m_PickPoint3Actor);				
			}
			else if(*(iren->GetKeySym()) == '5')
			{
				Point = picker->GetPickPosition();
				m_PickPoint4[0] = Point[0];
				m_PickPoint4[1] = Point[1];
				m_PickPoint4[2] = Point[2];

				// Remove old actor
				if(m_PickPoint4Actor) m_Widget->m_VisualizationWidget3D->RemoveActor(m_PickPoint4Actor);
	
				vtkSmartPointer<vtkSphereSource> SphereSource = vtkSmartPointer<vtkSphereSource>::New();
				SphereSource->SetCenter(m_PickPoint4);
				SphereSource->SetRadius(5);
 
				vtkSmartPointer<vtkPolyDataMapper> SphereMapper = vtkSmartPointer<vtkPolyDataMapper>::New();
				SphereMapper->SetInput(SphereSource->GetOutput());
 
				m_PickPoint4Actor = vtkSmartPointer<vtkActor>::New();
				m_PickPoint4Actor->SetMapper(SphereMapper);
				m_PickPoint4Actor->GetProperty()->SetOpacity(0.8);
				m_PickPoint4Actor->GetProperty()->SetColor(0.0,0.8,0.8);

				// Add sphere actor to renderer
				m_Widget->m_VisualizationWidget3D->AddActor(m_PickPoint4Actor);				
			}
				

			// Plot line in between points
				 
			if( !( ((m_PickPoint1[0] == 0.0) && (m_PickPoint1[1] == 0.0) && (m_PickPoint1[2] == 0.0)) ||
				   ((m_PickPoint2[0] == 0.0) && (m_PickPoint2[1] == 0.0) && (m_PickPoint2[2] == 0.0)) ))
			{
				// Remove old actor
				if(m_LineActor) m_Widget->m_VisualizationWidget3D->RemoveActor(m_LineActor);

				//Create a vtkPoints object and store the points in it
				vtkSmartPointer<vtkPoints> pts = vtkSmartPointer<vtkPoints>::New();
				pts->InsertNextPoint(m_PickPoint1);
				pts->InsertNextPoint(m_PickPoint2);				

				//Setup the colors array
				vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
				colors->SetNumberOfComponents(3);
				colors->SetName("Colors");
				//Add the colors we created to the colors array				
				unsigned char black[3] = {0, 0, 0};
				colors->InsertNextTupleValue(black);

				//Create the first line (between Origin and P0)
				vtkSmartPointer<vtkLine> line0 = vtkSmartPointer<vtkLine>::New();
				line0->GetPointIds()->SetId(0,0); //the second 0 is the index of the Origin in the vtkPoints
				line0->GetPointIds()->SetId(1,1); //the second 1 is the index of P0 in the vtkPoints

				//Create a cell array to store the lines in and add the lines to it
				vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
				lines->InsertNextCell(line0);				

				//Create a polydata to store everything in
				vtkSmartPointer<vtkPolyData> linesPolyData = vtkSmartPointer<vtkPolyData>::New();
				//Add the points to the dataset
				linesPolyData->SetPoints(pts);
				//Add the lines to the dataset
				linesPolyData->SetLines(lines);

				//Color the lines - associate the first component (red) of the
				//colors array with the first component of the cell array (line 0)
				//and the second component (green) of the colors array with the
				//second component of the cell array (line 1)
				linesPolyData->GetCellData()->SetScalars(colors);

				//Setup actor and mapper
				vtkSmartPointer<vtkPolyDataMapper> mapper =	vtkSmartPointer<vtkPolyDataMapper>::New();
				mapper->SetInput(linesPolyData);

				m_LineActor = vtkSmartPointer<vtkActor>::New();
				m_LineActor->SetMapper(mapper);
				m_Widget->m_VisualizationWidget3D->AddActor(m_LineActor);
			}

			if(m_PickPoint1Actor)	m_PickPoint1Actor->VisibilityOn();
			if(m_PickPoint2Actor)	m_PickPoint2Actor->VisibilityOn();
			if(m_PickPoint3Actor)	m_PickPoint3Actor->VisibilityOn();
			if(m_PickPoint4Actor)	m_PickPoint4Actor->VisibilityOn();
			if(m_LineActor)			m_LineActor->VisibilityOn();

			m_Distance = sqrt(pow((m_PickPoint1[0]-m_PickPoint2[0]),2)+pow((m_PickPoint1[1]-m_PickPoint2[1]),2)+pow((m_PickPoint1[2]-m_PickPoint2[2]),2));

			UpdatePointPickerGUI();
		}
	}
}


//----------------------------------------------------------------------------
void
VTKDataProcessingPlugin::SaveData()
{	
	m_OutputDataFilePath = QFileDialog::getSaveFileName(m_Widget, "Save VTK File", "E:/ProjectedLight/daten_puppen", "VTK files (*.vtk)");		
	//m_OutputDataFilePath = "E:/ProjectedLight/daten_puppen/f1_modified.vtk";


	vtkSmartPointer<vtkPolyData>			CleanedPolyData = vtkSmartPointer<vtkPolyData>::New();
	vtkSmartPointer<vtkPoints>				CleanedPoints	= vtkSmartPointer<vtkPoints>::New();	
	vtkSmartPointer<vtkCellArray>			CleanedCells	= vtkSmartPointer<vtkCellArray>::New();
	vtkSmartPointer<vtkUnsignedCharArray>	TextureData		= vtkSmartPointer<vtkUnsignedCharArray>::New();
	TextureData->SetNumberOfComponents(4);

	std::cout << m_ProcessedData->GetPointData()->GetScalars()->GetNumberOfComponents() << std::endl;

	double * Scalar = new double[4];
	for(vtkIdType i = 0; i < m_ProcessedData->GetNumberOfPoints(); i++)
	{
		double * CurrentPoint = new double[3];
		CurrentPoint = m_ProcessedData->GetPoints()->GetPoint(i);
		if(CurrentPoint[0]==CurrentPoint[0])
		{
			CleanedPoints->InsertNextPoint(&CurrentPoint[0]);
			
			Scalar = m_ProcessedData->GetPointData()->GetScalars()->GetTuple4(i);
			TextureData->InsertNextTuple4(Scalar[0],Scalar[1],Scalar[2],Scalar[3]);
		}
		else
		{
			//std::cout << "false" << std::endl;
		}
	}

	CleanedPolyData->SetPoints(CleanedPoints);
	CleanedPolyData->GetPointData()->SetScalars(TextureData);	

	for(vtkIdType i = 0; i < CleanedPolyData->GetNumberOfPoints(); i++)
	{
		CleanedCells->InsertNextCell(1, &i);
	}
	CleanedPolyData->SetVerts(CleanedCells);
	std::cout << CleanedPolyData->GetNumberOfPoints() << " " << CleanedPolyData->GetNumberOfCells() << std::endl;

	CleanedPolyData->BuildCells();
	CleanedPolyData->BuildLinks();

	if ( !m_OutputDataFilePath.isEmpty() ) 
	{	
		//vtkSmartPointer<vtkCleanPolyData> CleanFilter = vtkSmartPointer<vtkCleanPolyData>::New();
		//CleanFilter->SetInput(m_ProcessedData);
		//m_VTKWriter->SetFileName(m_OutputDataFilePath.toStdString().c_str());
		//m_VTKWriter->SetInput(CleanFilter->GetOutput());
		//m_VTKWriter->Update();

		m_VTKWriter->SetFileName(m_OutputDataFilePath.toStdString().c_str());
		m_VTKWriter->SetInput(CleanedPolyData);
		m_VTKWriter->Update();

		//vtkSmartPointer<vtkXMLPolyDataWriter> writer = vtkSmartPointer<vtkXMLPolyDataWriter>::New();
		//writer->SetFileName("C:/test.vtp");
		//writer->SetInput(m_ProcessedData);
		// Optional - set the mode. The default is binary.
		//writer->SetDataModeToBinary();
		//writer->SetDataModeToAscii();
		//writer->Write();

		vtkSmartPointer<vtkSTLWriter> stlWriter = vtkSmartPointer<vtkSTLWriter>::New();
		stlWriter->SetFileName("C:/test.stl");
		stlWriter->SetInput(CleanedPolyData);
		stlWriter->Write();

		//vtkSmartPointer<vtkXMLUnstructuredGridWriter> writer2 = vtkSmartPointer<vtkXMLUnstructuredGridWriter>::New();
		//writer2->SetFileName("C:/test.vtu");
		//writer2->SetInput(m_ProcessedData);
		//writer2->Write();
		
	}	
}

//----------------------------------------------------------------------------
void
VTKDataProcessingPlugin::SaveRangeData()
{	
	QString FileName = QFileDialog::getSaveFileName(m_Widget, "Save range data TXT file", "E:/", "TXT files (*.txt)");		
	
	std::ofstream FileStream;
	FileStream.open(FileName.toStdString().c_str());	
	
  if(FileStream.good())
  {
    typedef itk::ImageRegionConstIterator<RImageType::RangeImageType> RangeIteratorType;
	  RangeIteratorType itRange(m_RangeImage,m_RangeImage->GetRequestedRegion());
	  for ( itRange.GoToBegin(); !itRange.IsAtEnd(); ++itRange )
	  {
		  FileStream << itRange.Value() << " ";		
	  }
  }
	FileStream.close();	
}

//----------------------------------------------------------------------------
void
VTKDataProcessingPlugin::SaveFlagData()
{	
#if 0
	QString FileName = QFileDialog::getSaveFileName(m_Widget, "Save flag data TXT file", "E:/", "TXT files (*.txt)");		
	
	std::ofstream FileStream;
	FileStream.open(FileName.toStdString().c_str());	
	
	typedef itk::ImageRegionConstIterator<CamCubeImageType::CCFlagImageType> FlagIteratorType;
	FlagIteratorType itFlag(m_CamCubeImage->GetCCFlagImage(),m_CamCubeImage->GetCCFlagImage()->GetRequestedRegion());
	for ( itFlag.GoToBegin(); !itFlag.IsAtEnd(); ++itFlag )
	{
		// Depending on the flag
		if ( itFlag.Value()&PMD_FLAG_INVALID )
		{
			FileStream << 1.0f << " ";
		}
		else
		{
			FileStream << 0.0f << " ";
		}
	}

	FileStream.close();	
#endif
}
	
	
//----------------------------------------------------------------------------
void
VTKDataProcessingPlugin::SaveMarkerCoordinates()
{	
	std::string MarkerCoordinatesFilePath = m_InputDataFilePath.toStdString() + "MarkerCoordinates.txt";
		
		// Write the reference features
	std::ofstream MarkerCoordinatesStream(MarkerCoordinatesFilePath.c_str());
	
	MarkerCoordinatesStream << m_PickPoint1[0] << " " << m_PickPoint1[1] << " " << m_PickPoint1[2] << std::endl;
	MarkerCoordinatesStream << m_PickPoint2[0] << " " << m_PickPoint2[1] << " " << m_PickPoint2[2] << std::endl;
	MarkerCoordinatesStream << m_PickPoint3[0] << " " << m_PickPoint3[1] << " " << m_PickPoint3[2] << std::endl;
	MarkerCoordinatesStream << m_PickPoint4[0] << " " << m_PickPoint4[1] << " " << m_PickPoint4[2] << std::endl;

	MarkerCoordinatesStream.close();
}

