/* osgVisual test. distortionNG, experimental.
*
*  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 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.
*/

#include "DistortionManipulator.h"
#include "extViewer.h"

#include <osgViewer/Viewer>
#include <osg/Point>
#include <osg/PolygonMode>
#include <osg/TextureRectangle>

#include <osgDB/Registry>
#include <osgDB/ReaderWriter>
#include <osgDB/WriteFile>

#include "DistortionSetupStrategy.h"

using namespace osg;
using namespace osgViewer;


DistortionManipulator::DistortionManipulator(extViewer* viewer, DistortionSet* ds)
 : _highlightColor( osg::Vec4(1.0f, 1.0f, 0.0f, 1.0f) ), _distortionSet( ds ), _viewer(viewer)
{
	activeSetupMode = DISABLED;
	activeDistortionMode = MESH;
	activeManualSetupMode = DISTORTION;
	activeVisualizationMode = NONE;

	_delegatedDistortionSetupStrategy = NULL;

	_highlighter = NULL;
	
	createVertexHighlighter();
	createHUD();

	_camera = 0;
	//_distortionMesh(distortionMesh)

	// Create Shader to visualize intensitymap during blending setup.
	osg::Shader* sh = osg::Shader::readShaderFile( osg::Shader::FRAGMENT, "shaderIntensityMapVis.frag" ); 
	sh->setName("shaderIntensityMapVis");
	ds->setShaderIntensityMapVis( sh );

	//LF Get Screen Resolution for Cursor Position Scaling
	osg::GraphicsContext::WindowingSystemInterface* wsi = osg::GraphicsContext::getWindowingSystemInterface();
	if (!wsi) { osg::notify(osg::NOTICE)<<"Error, no WindowSystemInterface available, cannot create windows."<<std::endl; }
	wsi->getScreenResolution(osg::GraphicsContext::ScreenIdentifier(0), _screenPixWidth, _screenPixHeight);


}

DistortionManipulator::~DistortionManipulator()
{
}

void DistortionManipulator::getUsage(osg::ApplicationUsage& usage) const
{
	usage.addKeyboardMouseBinding("Keypad 0","Show/Hide distortion HUD.");
	usage.addKeyboardMouseBinding("Keypad 1","Reset distortion.");
	usage.addKeyboardMouseBinding("Keypad 2","Reset intensity blending.");
	usage.addKeyboardMouseBinding("Keypad 4","Toggles Setup Mode between DISABLED, MANUAL & DELEGATED.");
 	usage.addKeyboardMouseBinding("Keypad 5","MANUAL Mode: Toggle between blending & distortion setup.");
    usage.addKeyboardMouseBinding("Keypad 6","MANUAL Mode: Toggle if distortion drags affect mesh or rtt texture coordinates.");	// Defaults to Mesh
	usage.addKeyboardMouseBinding("Keypad 7","Show distortion mesh / intensity map / none.");
	usage.addKeyboardMouseBinding("Keypad 8","Save distortion set.");	// via plugin
}

bool DistortionManipulator::handle(const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& aa, osg::Object* obj, osg::NodeVisitor* nv)
{
	switch(ea.getEventType())
    {
        case(osgGA::GUIEventAdapter::MOVE): break;
        case(osgGA::GUIEventAdapter::DRAG):
		{
			if ( activeSetupMode == MANUAL)
			{
				OSG_ALWAYS<<std::endl<<"Drag:"<<std::endl<<"ea.getGraphicsContext()="<<ea.getGraphicsContext()<<std::endl;
				OSG_ALWAYS<<"ea.getXnormalized()="<<ea.getXnormalized()<<std::endl;
				OSG_ALWAYS<<"ea.getYnormalized()="<<ea.getYnormalized()<<std::endl;
				OSG_ALWAYS<<"ea.getX()="<<ea.getX()<<std::endl;
				OSG_ALWAYS<<"ea.getXin()="<<ea.getXmin()<<std::endl;
				OSG_ALWAYS<<"ea.getXmax()="<<ea.getXmax()<<std::endl;
				OSG_ALWAYS<<"ea.getY()="<<ea.getY()<<std::endl;
				OSG_ALWAYS<<"ea.getYin()="<<ea.getYmin()<<std::endl;
				OSG_ALWAYS<<"ea.getYmax()="<<ea.getYmax()<<std::endl;
				
				//LF get X,Y normalized to lowerleftcorner (0,0) / upperrightcorner (1,1)
				OSG_ALWAYS<<"getXnormalizedLF()="<<(ea.getXnormalized()+1.0)/2.0<<std::endl;
				OSG_ALWAYS<<"getYnormalizedLF()="<<(ea.getYnormalized()+1.0)/2.0<<std::endl;

				//LF get X,Y in pixels (lowerleftcorner (0,0) / upperrightcorner (screenwidth,screenheigth)
				OSG_ALWAYS<<std::endl<<"Screen Resolution: "<<_screenPixWidth<<" x "<<_screenPixHeight<<std::endl;
				OSG_ALWAYS<<"getXPixelspaceLF()="<<(int)((float)_screenPixWidth*(ea.getXnormalized()+1.0)/2.0)<<std::endl;
				OSG_ALWAYS<<"getYPixelspaceLF()="<<(int)((float)_screenPixHeight*(ea.getYnormalized()+1.0)/2.0)<<std::endl;

				if(activeDistortionMode == MESH)
				{
					OSG_ALWAYS<<"MESH!"<<std::endl;
				}
				else if (activeDistortionMode == TEXCOORDINATES)
				{
					OSG_ALWAYS<<"TEXCOORDINATES!"<<std::endl;
				}

				return true;	// true means event handled: not forwarded to the camera manipulator;
			}
			break;

		}
        case(osgGA::GUIEventAdapter::PUSH):
		{
			OSG_ALWAYS<<"mouse click!"<<std::endl;
 			if ( activeSetupMode == MANUAL /*&& ea.getModKeyMask()&osgGA::GUIEventAdapter::MODKEY_CTRL*/ && ea.getButton() == osgGA::GUIEventAdapter::LEFT_MOUSE_BUTTON )
			{
				osgViewer::View* viewer = dynamic_cast<osgViewer::View*>(&aa);
				if ( viewer )
				{
					osg::notify(osg::ALWAYS)<<std::endl<<"Intersection:"<<std::endl<<"ea.getX()="<<ea.getX()<<std::endl;
					osg::notify(osg::ALWAYS)<<"ea.getY()="<<ea.getY()<<std::endl;
					osg::ref_ptr<osgUtil::LineSegmentIntersector> intersector = new osgUtil::LineSegmentIntersector(osgUtil::Intersector::PROJECTION, ea.getX(), ea.getY());
					osgUtil::IntersectionVisitor iv( intersector.get() );
					_distortionSet->getDistortionCamera()->accept( iv );
			        
					if ( intersector->containsIntersections() )
					{
						//osgUtil::LineSegmentIntersector::Intersection& result = *(intersector->getIntersections().begin());
						osgUtil::LineSegmentIntersector::Intersection *result;
						result=&intersector->getFirstIntersection();
						computeSelectedVertex( *result );
						//computeSelectedVertex( result );
					}
				}
			}
			break;
		}
        case(osgGA::GUIEventAdapter::RELEASE): break;
        case(osgGA::GUIEventAdapter::KEYDOWN):
		{
			if (ea.getKey()==osgGA::GUIEventAdapter::KEY_KP_Insert)	// KP 0: Show/Hide HUD
			{
				bool oldValue = _distortionSet->getDistortionInternals()->getValue(DistortionSet::HUD);
				_distortionSet->getDistortionInternals()->setValue(DistortionSet::HUD, !oldValue);
				return(true);
			}
			if (ea.getKey()==osgGA::GUIEventAdapter::KEY_KP_End)	// KP 1: reset distortion
			{
				resetDistortion();
				return(true);
			}
			if (ea.getKey()==osgGA::GUIEventAdapter::KEY_KP_Down)	// KP 2: reset intensity map
			{
				resetIntensityMap();
				return(true);
			}
			if (ea.getKey()==osgGA::GUIEventAdapter::KEY_KP_Left)	// KP 4: Toggles Setup Mode between DISABLED, MANUAL & DELEGATED.
			{
				switch(activeSetupMode)
				{
					case DISABLED : 
					{
						activeSetupMode = MANUAL;
						OSG_NOTICE<<"SetupMode MANUAL activated"<<std::endl;
						_distortionSet->getDistortionInternals()->setValue(DistortionSet::HIGHLIGHTER, true);
						break;
					}
					case MANUAL :
					{
						activeSetupMode = DELEGATED;
						_distortionSet->getDistortionInternals()->setValue(DistortionSet::HIGHLIGHTER, false);
						OSG_NOTICE<<"SetupMode DELEGATED activated"<<std::endl;
						break;
					}
					case DELEGATED :
					{
						activeSetupMode = DISABLED;
						_distortionSet->getDistortionInternals()->setValue(DistortionSet::HIGHLIGHTER, false); 
						OSG_NOTICE<<"SetupMode DISABLED activated"<<std::endl;
						break;
					}
				}
				updateHUD();
				return(true);
			}
			if (ea.getKey()==osgGA::GUIEventAdapter::KEY_KP_Begin)	// KP 5: MANUAL Mode: Toggle between blending & distortion setup.
			{

				activeManualSetupMode = (activeManualSetupMode==DISTORTION?BLENDING:DISTORTION);
				updateHUD();
				return(true);
			}
			if (ea.getKey()==osgGA::GUIEventAdapter::KEY_KP_Right)	// KP 6: MANUAL Mode: Toggle if distortion drags affect mesh or rtt texture coordinates -> defaults to Mesh
			{
				activeDistortionMode = (activeDistortionMode==MESH?TEXCOORDINATES:MESH);
				updateHUD();
				return(true);
			}
			if (ea.getKey()==osgGA::GUIEventAdapter::KEY_KP_Home)	// KP 7: Show distortion mesh / intensity map / none.
			{
				switch(activeVisualizationMode)
				{	
					case DISTORTION_MESH : 
					{
						activeVisualizationMode = INTENSITY_MAP;
						showDistortionMesh(false);
						showIntensityMap(true);
						break;
					}
					case INTENSITY_MAP :
					{
						activeVisualizationMode = NONE;
						showDistortionMesh(false);
						showIntensityMap(false);
						break;
					}
					case NONE : 
					{
						activeVisualizationMode = DISTORTION_MESH;
						showDistortionMesh(true);
						showIntensityMap(false);
						break;
					}
				}

				updateHUD();
				return(true);
			}
			if (ea.getKey()==osgGA::GUIEventAdapter::KEY_KP_Up)	// KP 8: Save distortion set	 via plugin
			{
				OSG_ALWAYS<<"KEY_KP_8 : todo: Save DistortionContainer"<<std::endl;

				osgDB::writeObjectFile( *_distortionSet, "distortionset.dist" );
				return(true);
			}
			
			return false;	// Event ignored
		}
        case(osgGA::GUIEventAdapter::KEYUP): break;
		case(osgGA::GUIEventAdapter::FRAME):
		{
			if ( activeSetupMode == DELEGATED && _delegatedDistortionSetupStrategy.valid())
			{
				//OSG_ALWAYS<<"Calling delegated class.."<<std::endl;
				_delegatedDistortionSetupStrategy->delegateDistortionSetup(_distortionSet);
				if( _distortionSet->isDirty() && _viewer.valid())
				{
					updateDistortionSetup();
				}
				//OSG_ALWAYS<<"..done"<<std::endl;
			}
			break;
		}
        case (osgGA::GUIEventAdapter::RESIZE):	// todo: adapt distortion mesh to new screen size
		{
			OSG_ALWAYS<<"RESIZE!"<<std::endl;
			break;
		}

        default:
            return false;
    }
    return false;
}

void DistortionManipulator::resetIntensityMap()
{
	if(!_distortionSet.valid())
		return;

	osg::ref_ptr<osg::Image> image = _distortionSet->getIntensityMap();

	OSG_ALWAYS<<"Reseting IntensityMap Blending."<<std::endl;

	//isDataContiguous is unknown to MSVS2010 and GCC
	/*if (!image->isDataContiguous())
	{
		OSG_WARN<<"Warning: DistortionManipulator does not support working with non contiguous imagery as blendmaps!"<<std::endl;
		return;
	}*/

	// Fill intensity map with 255 
	unsigned char* dataPtr = image->data(); 
	for(unsigned int i=0;i<image->getTotalSizeInBytes();i++)
	{
		*dataPtr++ = 255; 
	}
	image->dirty();
}

void DistortionManipulator::resetDistortion()
{
	if(!_distortionSet.valid())
		return;

	
	_distortionSet->setDistortionMeshDimensions(2, 2, true);	// Reset dimensions and coordinates/texcoords & rebuild mesh data
	_distortionSet->dirtyMesh();
	
	// Restore ProjectionMatrix
	// Set up basic projection Matrix
	double fovy = 30., aspectRatio = 1., zNear = 1., zFar = 1000.;
	_distortionSet->getProjectionOffset().makePerspective( fovy, aspectRatio, zNear, zFar );

	double newAspectRatio = double(_distortionSet->getIntensityMap()->s()) / double(_distortionSet->getIntensityMap()->t());
	double aspectRatioChange = newAspectRatio / aspectRatio;
	if (aspectRatioChange != 1.0)
	{
		_distortionSet->getProjectionOffset() *= osg::Matrix::scale(1.0/aspectRatioChange,1.0,1.0);
	}

	_distortionSet->dirtyMatrix();
}

void DistortionManipulator::showDistortionMesh(bool show)
{
	osg::StateSet* stateset = _distortionSet->getDistortionInternals()->getChild(DistortionSet::MESH)->getOrCreateStateSet();

	// Append a PolygonMode stateset if required
    osg::PolygonMode* polyModeObj = dynamic_cast<osg::PolygonMode*>(stateset->getAttribute(osg::StateAttribute::POLYGONMODE));
    if (!polyModeObj)
    {
        polyModeObj = new osg::PolygonMode;
        stateset->setAttribute(polyModeObj, osg::StateAttribute::PROTECTED|osg::StateAttribute::ON);
    }
 
	// Set Polygonmode
	if(show)
		polyModeObj->setMode(osg::PolygonMode::FRONT_AND_BACK,osg::PolygonMode::LINE);
	else
		polyModeObj->setMode(osg::PolygonMode::FRONT_AND_BACK,osg::PolygonMode::FILL);
}

void DistortionManipulator::showIntensityMap(bool show)
{
	if(_distortionSet.valid())
	{
		osg::StateSet* stateset = _distortionSet->getDistortionInternals()->getChild(DistortionSet::MESH)->getOrCreateStateSet();
		osg::Program* program = static_cast<osg::Program*>(stateset->getAttribute(::osg::StateAttribute::PROGRAM));

		if(show)
		{
			program->removeShader( _distortionSet->getShaderIntensityMap() );
			program->addShader( _distortionSet->getShaderIntensityMapVis() );
		}
		else
		{
			program->removeShader( _distortionSet->getShaderIntensityMapVis() );
			program->addShader( _distortionSet->getShaderIntensityMap() );
		}
	}
}

void DistortionManipulator::createVertexHighlighter()
{
	osg::ref_ptr<osg::Vec4Array> colors = new osg::Vec4Array(1);
	(*colors)[0] = _highlightColor;

	_highlighter = new osg::Geometry;
	_highlighter->setDataVariance( osg::Object::DYNAMIC );
	_highlighter->setUseDisplayList( false );
	_highlighter->setUseVertexBufferObjects( true );
	_highlighter->setVertexArray( new osg::Vec3Array(1) );	// The highlighter vertex is updated by computeSelectedVertex(..)
	_highlighter->setColorArray( colors.get() );
	_highlighter->setColorBinding( osg::Geometry::BIND_OVERALL );
	_highlighter->addPrimitiveSet( new osg::DrawArrays(GL_POINTS, 0, 1) );

	
	osg::ref_ptr<osg::Geode> geode = new osg::Geode;
	geode->addDrawable( _highlighter.get() );
	geode->getOrCreateStateSet()->setAttributeAndModes( new osg::Point(8.0f) );
	geode->getOrCreateStateSet()->setMode( GL_LIGHTING, osg::StateAttribute::OFF );
	geode->setCullingActive(false);	// disable the culling for the selector, otherwise the selector is culled away on the edge.

	_distortionSet->getDistortionInternals()->addChild(geode, false);	//  = child #1  :definition: child #0 = mesh, #1 = highlighter, #2 HUD
}

void DistortionManipulator::createHUD()
{
	osg::ref_ptr<osg::Geode> geode = new osg::Geode;
	
	osg::StateSet* stateSet = geode->getOrCreateStateSet();
	stateSet->setRenderBinDetails( 99, "RenderBin");
	// disable depth test to ensure that it is always drawn.
	stateSet->setMode(GL_DEPTH_TEST,osg::StateAttribute::OFF);
	stateSet->setMode(GL_LIGHTING,osg::StateAttribute::OFF);

	// Modify StateSet to protect it against state changes by the stateset Manipulator
	osg::PolygonMode* polyModeObj = new osg::PolygonMode;
    stateSet->setAttribute(polyModeObj, osg::StateAttribute::PROTECTED|osg::StateAttribute::ON);

	// Add Text: 
	osg::Vec3 position(20.0f,120.0f,0.0f);
	osg::Vec3 delta(0.0f,-25.0f,0.0f);
	std::string timesFont("fonts/arial.ttf");
	{
		osg::ref_ptr<osgText::Text> textHeader = new osgText::Text;
		textHeader->setText("Distortion Setup:");
		textHeader->setFont(timesFont);
		textHeader->setPosition(position);
		textHeader->setCharacterSize(21);
		textHeader->setDataVariance(osg::Object::STATIC); 
		geode->addDrawable( textHeader );

		position += delta;

		hudSetupMode = new osgText::Text;
		hudSetupMode->setFont(timesFont);
		hudSetupMode->setPosition(position);
		hudSetupMode->setCharacterSize(21);
		hudSetupMode->setDataVariance(osg::Object::DYNAMIC); 
		geode->addDrawable( hudSetupMode );

		position += delta;

		hudDistortionMode = new osgText::Text;
		hudDistortionMode->setFont(timesFont);
		hudDistortionMode->setPosition(position);
		hudDistortionMode->setCharacterSize(21);
		hudDistortionMode->setDataVariance(osg::Object::DYNAMIC); 
		geode->addDrawable( hudDistortionMode );

		position += delta;

		hudManualSetupMode = new osgText::Text;
		hudManualSetupMode->setFont(timesFont);
		hudManualSetupMode->setPosition(position);
		hudManualSetupMode->setCharacterSize(21);
		hudManualSetupMode->setDataVariance(osg::Object::DYNAMIC); 
		geode->addDrawable( hudManualSetupMode );

		position += delta;

		hudVisualizationMode = new osgText::Text;
		hudVisualizationMode->setFont(timesFont);
		hudVisualizationMode->setPosition(position);
		hudVisualizationMode->setCharacterSize(21);
		hudVisualizationMode->setDataVariance(osg::Object::DYNAMIC); 
		geode->addDrawable( hudVisualizationMode );
	}
	
	updateHUD();
	_distortionSet->getDistortionInternals()->addChild(geode, false);	//  = child #2. The definition is: child #0 = mesh, #1 = highlighter, #2 HUD
}

void DistortionManipulator::updateHUD()
{
	switch(activeSetupMode)
	{
		case DISABLED : hudSetupMode->setText("Setup Mode : DISABLED"); break;
		case MANUAL : hudSetupMode->setText("Setup Mode : MANUAL"); break;
		case DELEGATED : hudSetupMode->setText("Setup Mode : DELEGATED"); break;
		default: hudSetupMode->setText("");
	};
	switch(activeDistortionMode)
	{
		case MESH : hudDistortionMode->setText("Distortion Mode : MESH"); break;
		case TEXCOORDINATES : hudDistortionMode->setText("Distortion Mode : TEXCOORDINATES"); break;
		default: hudDistortionMode->setText("");
	};
	switch(activeManualSetupMode)
	{
		case DISTORTION : hudManualSetupMode->setText("Manual Setup Mode : DISTORTION"); break;
		case BLENDING : hudManualSetupMode->setText("Manual Setup Mode : BLENDING"); break;
		default: hudManualSetupMode->setText("");
	};
	switch(activeVisualizationMode)
	{
		case DISTORTION_MESH : hudVisualizationMode->setText("Visualization Mode : DISTORTION_MESH"); break;
		case INTENSITY_MAP : hudVisualizationMode->setText("Visualization Mode : INTENSITY_MAP"); break;
		case NONE : hudVisualizationMode->setText("Visualization Mode : NONE"); break;
		default: hudVisualizationMode->setText("");
	};
}

void DistortionManipulator::computeSelectedVertex( osgUtil::LineSegmentIntersector::Intersection& result )
{
	osg::Geometry* geom = dynamic_cast<osg::Geometry*>( result.drawable.get() );
	if ( !geom || geom==_highlighter )
		return;

	osg::Vec3Array* vertices = dynamic_cast<osg::Vec3Array*>( geom->getVertexArray() );
	osg::Vec3Array* selVertices = dynamic_cast<osg::Vec3Array*>( _highlighter->getVertexArray() );
	if ( !vertices || !selVertices )
		return;
	
	OSG_NOTIFY(osg::ALWAYS)<<"size of vertices="<<vertices->size()<<std::endl;
	OSG_NOTIFY(osg::ALWAYS)<<"size of selVertices="<<selVertices->size()<<std::endl;

	osg::Vec3 point = result.getWorldIntersectPoint();
	osg::Matrix matrix = osg::computeLocalToWorld( result.nodePath );	// To compute the intersection vertices in world coordinates not in model coordinates
	OSG_NOTIFY(osg::ALWAYS) << "Intersection-indices: Size=" << result.indexList.size() << std::endl;
	const std::vector<unsigned int>& selIndices = result.indexList;
	{
		double maxRatio = 0.0;
		int closestVertexIndex = 0;
		for ( unsigned int i=0; i<3 && i<result.ratioList.size(); i++ )	//iterate through rations and search for maxRation=nearestVertex
		{
			if(result.ratioList[i] > maxRatio)
			{
				maxRatio = result.ratioList[i];
				closestVertexIndex = result.indexList[i];
			}
			OSG_NOTIFY(osg::ALWAYS)<<"maxRatio="<<maxRatio<<std::endl;
			OSG_NOTIFY(osg::ALWAYS)<<"closestVertexIndex="<<closestVertexIndex<<std::endl;
		}
		OSG_NOTIFY(osg::ALWAYS)<<"nearest vertex: X="<<(*vertices)[closestVertexIndex].x()<<" Y="<<(*vertices)[closestVertexIndex].y()<<" Z="<<(*vertices)[closestVertexIndex].z()<<std::endl;
		osg::Vec3 vertex = (*vertices)[closestVertexIndex] * matrix;

		selVertices->front() = vertex;		
		OSG_NOTIFY(osg::ALWAYS)<<"selected vertice: X="<<vertex.x()<<" Y="<<vertex.y()<<" Z="<<vertex.z()<<std::endl;
	}
	selVertices->dirty();
	_highlighter->dirtyBound(); 
}

void DistortionManipulator::updateDistortionSetup()
{
	updateDistortionViewProjectionMatrix();
	updateDistortionMesh();
	_distortionSet->clearDirtyStatus();
}

void DistortionManipulator::updateDistortionViewProjectionMatrix()
{
	if(!_distortionSet.valid())
	{
		osg::notify(osg::WARN) << "You cannot update the distortion without distortion set up." << std::endl;
		return;
	}

	if(_viewer.valid())
	{

#if 0
		/*******************************************/
		/***   ViewMatrix-Testcase  "Garching"   ***/
		/*******************************************/

		osg::Matrixd viewMatrix, transView, rotView;
		
		rotView.makeRotate(
			osg::DegreesToRadians(-130.0), osg::Vec3(0,1,0),		// heading
			osg::DegreesToRadians(0.0), osg::Vec3(-1,0,0), 	// pitch
			osg::DegreesToRadians(-80.0), osg::Vec3(0,0,1));	// roll

		transView.makeTranslate(-4169457.75*0.99975, -860009.06*0.99975, -4737192.00*0.99975);
		viewMatrix = transView * rotView;

		if(_viewer->getCameraManipulator()==NULL)
		{
			_viewer->getCamera()->setViewMatrix(viewMatrix);
		}
		else
		{
			_viewer->getCameraManipulator()->setByMatrix(osg::Matrixd::inverse(viewMatrix));
		}

		/**************************************/
		/***   End of Testcase "Garching"   ***/
		/**************************************/
#endif

		_viewer->getCamera()->setProjectionMatrix(osg::Matrix::identity());

		// Get slave struct to update the offset matrixes.
		//Slave Camera Settings
		osgViewer::View::Slave* sceneSlave = _viewer->findSlaveForCamera( _distortionSet->getSceneCamera() );
		sceneSlave->_projectionOffset = _distortionSet->getProjectionOffset();
		sceneSlave->_viewOffset = _distortionSet->getViewOffset();

#if 0
		// Print viewMatrixAsLookAt for Debug Purposes
		osg::Vec3 Eye,Focus,Vertical;
		_viewer->getCamera()->getViewMatrixAsLookAt(Eye,Focus,Vertical);
		OSG_ALWAYS<<"Camera Eyepoint: X:"<<Eye.x()<<" Y:"<<Eye.y()<<" Z:"<<Eye.z()<<std::endl;
		printf("Camera Eyepoint: %.2lf, %.2lf, %.2lf\n",Eye.x(),Eye.y(),Eye.z());
		printf("Camera Direction: %.2lf, %.2lf, %.2lf\n",Focus.x()-Eye.x(),Focus.y()-Eye.y(),Focus.z()-Eye.z());
		printf("Vertical after: %.2lf, %.2lf, %.2lf\n",Vertical.x()-Eye.x(),Vertical.y()-Eye.y(),Vertical.z()-Eye.z());
#endif

	}
	else
		OSG_NOTIFY(osg::ALWAYS)<<"Invalid viewer!"<<std::endl;
}

void DistortionManipulator::updateDistortionMesh()
{
	// Create mesh vertex array & textCoord array
	osg::Vec3Array* vertices = new osg::Vec3Array;
	osg::Vec2Array* texCoords = new osg::Vec2Array;
	osg::Vec4Array* colors = new osg::Vec4Array;

	for(unsigned int i=0;i<_distortionSet->getDistortionMesh()->size();i++)
	{
		osg::Vec4 tmpSrc = _distortionSet->getDistortionMesh()->at(i);

		// Scale vector with the screen resolution
		osg::Vec3 tmpVec = osg::Vec3(	tmpSrc.x()*_screenPixWidth,
										tmpSrc.y()*_screenPixHeight,
										0.0	);
		vertices->push_back( tmpVec );
		//OSG_ALWAYS<<"Transfering mesh vertex : X:"<<tmpVec.x()<<" Y:"<<tmpVec.y()<<" Z:"<<tmpVec.z()<<std::endl;

		// Note texCoords
		osg::Vec2 tmpTexCoords = osg::Vec2(	tmpSrc.z()*_screenPixWidth,
											tmpSrc.w()*_screenPixHeight );

		texCoords->push_back(tmpTexCoords);
		//OSG_ALWAYS<<"Transfering texcoord vertex : X:"<<tmpTexCoords.x()<<" Y:"<<tmpTexCoords.y()<<std::endl;

		colors->push_back(osg::Vec4(1.0f,1.0f,1.0f,1.0f));
	}

	// --------- Apply Arrays -----------------
	osg::Geometry* geom = _distortionSet->getDistortionInternals()->getChild(osgViewer::DistortionSet::MESH)->asGeode()->getDrawable(0)->asGeometry();

	// If arraysize differs: Replace primite sets
	bool regeneratePrimitiveSets = true;
	osg::Vec3Array* prevArray = dynamic_cast<osg::Vec3Array*>(geom->getVertexArray());
	if(prevArray) 
		regeneratePrimitiveSets = prevArray->size()!=vertices->size()?true:false;
	
	// Set new Arrays
	geom->setVertexArray(vertices);
	geom->setTexCoordArray(0, texCoords);	// todo: 0 ist hardcoded und müsste durch getTexUnitScene() ersetzt werden
	geom->setColorArray(colors);

	// If required: regenerate primitiveset:
	OSG_ALWAYS<<"PrimitiveSet regeneration required!"<<std::endl;
	geom->removePrimitiveSet(0, geom->getNumPrimitiveSets());	
	//// Create new primitives : Quads grid
	unsigned int rows = _distortionSet->getDistortionMeshRows();
	unsigned int columns = _distortionSet->getDistortionMeshColumns();
	for ( unsigned int row=0; row<rows-1; row++ )	// each strip consists of two affected vertex rows, so we need only row-1 strips.
	{
		osg::ref_ptr<osg::DrawElementsUInt> de = new osg::DrawElementsUInt(osg::PrimitiveSet::QUAD_STRIP, columns*2);	// columns*2 = number of involved vertices for this strip.
		for ( unsigned int col=0; col<columns; col++ )
		{
			(*de)[col*2 + 0] = row*columns + col;
			(*de)[col*2 + 1] = (row+1)*columns + col;
		}
		geom->addPrimitiveSet( de.get() );
	}
}