#include "viewer/CameraControlManipulator.h"

#include <osg/MatrixTransform>
#include <osgViewer/View>

#include <osg/ComputeBoundsVisitor>

double g_currMouseX = 0.0;
double g_currMouseY = 0.0;

CameraControlManipulator::CameraControlManipulator() {
	_eye.set(0.0, 0.0, 0.0);

	_trackerMode = NODE_CENTER;
	_manipulatorMode = Tracker_ManipulatorMode;
	_projMode = Proj_Perspective;

	_bKeyDownUp = false;
	_bKeyDownDown = false;
	_bKeyDownLeft = false;
	_bKeyDownRight = false;

	_distance = 1.0;
	_minimumDistance = 0.001;

	_adjustValue = 1.0;

	_cameraHPR.set(0.0, 0.0, 0.0);

	_yawDegTemp = 0.0;
	_pitchDegTemp = 0.0;
	_rollDegTemp = 0.0;

	_yawDegIni = 0.0;
	_pitchDegIni = 0.0;
	_rollDegIni = 0.0;

	_allowThrow = true;
	_thrown = false;
	_zoomDelta = 0.1;

	_isMove = false;
	_moveFactor = 0.0;
	_rotFactor = 0.0;

	_delta_frame_time = 0.01;
	_last_frame_time = 0.0;

	_isEyeAnimation = false;

	_time_Animation = 0.0;
	_curr_time_Animation = 0.0;
}

CameraControlManipulator::~CameraControlManipulator(void) {

}

bool CameraControlManipulator::getVerticalAxisFixed() const {
	return _verticalAxisFixed;
}

void CameraControlManipulator::setVerticalAxisFixed(bool value) {
	_verticalAxisFixed = value;
}

void CameraControlManipulator::fixVerticalAxis(osg::Vec3d& eye, osg::Quat& rotation, bool disallowFlipOver) {
	osg::CoordinateFrame coordinateFrame = getCoordinateFrame(eye);
	osg::Vec3d localUp = getUpVector(coordinateFrame);

	fixVerticalAxis(rotation, localUp, disallowFlipOver);
}

void CameraControlManipulator::fixVerticalAxis(osg::Quat& rotation, const osg::Vec3d& localUp, bool disallowFlipOver) {
	// camera direction vectors
	osg::Vec3d cameraUp = rotation * osg::Vec3d(0., 1., 0.);
	osg::Vec3d cameraRight = rotation * osg::Vec3d(1., 0., 0.);
	osg::Vec3d cameraForward = rotation * osg::Vec3d(0., 0., -1.);

	// computed directions
	osg::Vec3d newCameraRight1 = cameraForward ^ localUp;
	osg::Vec3d newCameraRight2 = cameraUp ^ localUp;
	osg::Vec3d newCameraRight = (newCameraRight1.length2() > newCameraRight2.length2()) ?
		newCameraRight1 : newCameraRight2;

	if (newCameraRight * cameraRight < 0.)
		newCameraRight = -newCameraRight;

	// vertical axis correction
	osg::Quat rotationVerticalAxisCorrection;
	rotationVerticalAxisCorrection.makeRotate(cameraRight, newCameraRight);

	// rotate camera
	rotation *= rotationVerticalAxisCorrection;

	if (disallowFlipOver) {

		// make viewer's up vector to be always less than 90 degrees from "up" axis
		osg::Vec3d newCameraUp = newCameraRight ^ cameraForward;
		if (newCameraUp * localUp < 0.)
			rotation = osg::Quat(osg::PI, osg::Vec3d(0., 0., 1.)) * rotation;
	}
}

void CameraControlManipulator::setByMatrix(const osg::Matrixd& matrix) {
	switch (_manipulatorMode) {
	case Camera_ManipulatorMode:
	{
		_eye = matrix.getTrans();

		osg::Quat quat = matrix.getRotate();

		_rotation = quat;
	}
	break;

	case Tracker_ManipulatorMode:
	{
		osg::Vec3d eye, center, up;
		matrix.getLookAt(eye, center, up, _distance);

		_eye = eye;

		// compute rotation matrix
		osg::Vec3 lv(center - eye);
		_distance = lv.length();

		osg::Matrixd lookat;
		lookat.makeLookAt(eye, center, up);

		_rotation = lookat.getRotate().inverse();
	}
	break;

	case Roam_ManipulatorMode:
	case Model_ManipulatorMode:
	{
		_center = osg::Vec3d(0., 0., -_distance) * matrix;
		_rotation = matrix.getRotate();

		// fix current rotation
		if (getVerticalAxisFixed())
			fixVerticalAxis(_center, _rotation, true);
	}
	break;

	default:
		break;
	}
}

/** Get the position of the manipulator as 4x4 Matrix.*/
osg::Matrixd CameraControlManipulator::getMatrix() const {
	osg::Matrix matrix;

	switch (_manipulatorMode) {
	case Camera_ManipulatorMode:
	{
		osg::Vec3d nodeCenter;
		osg::Quat nodeRotation;

		computeNodeCenterAndRotation(nodeCenter, nodeRotation);

		matrix = osg::Matrixd::rotate(_rotation) *
			osg::Matrix::translate(_eye) *
			osg::Matrixd::rotate(nodeRotation) *
			osg::Matrix::translate(nodeCenter);
	}
	break;

	case Tracker_ManipulatorMode:
	{
		osg::Vec3d nodeCenter;
		osg::Quat nodeRotation;
		computeNodeCenterAndRotation(nodeCenter, nodeRotation);

		matrix = osg::Matrixd::translate(0.0, 0.0, _distance) *
			osg::Matrixd::rotate(_rotation) *
			osg::Matrixd::rotate(nodeRotation) *
			osg::Matrix::translate(nodeCenter);
	}
	break;

	case Roam_ManipulatorMode:
	{
		matrix = osg::Matrixd::rotate(_rotation) *
			osg::Matrix::translate(_eye);
	}
	break;

	case Model_ManipulatorMode:
	{
		matrix = osg::Matrixd::translate(0.0, 0.0, _distance) *
			osg::Matrixd::rotate(_rotation) *
			osg::Matrixd::translate(_center);
	}
	break;

	default:
		break;
	}

	return matrix;

}

/** Get the position of the manipulator as a inverse matrix of the manipulator, typically used as a model view matrix.*/
osg::Matrixd CameraControlManipulator::getInverseMatrix() const {
	osg::Matrixd matrix;

	if (_manipulatorMode == Model_ManipulatorMode) {
		matrix = osg::Matrixd::translate(-_center) *
			osg::Matrixd::rotate(_rotation.inverse()) *
			osg::Matrixd::translate(0.0, 0.0, -_distance);
	} else {
		matrix = osg::Matrixd::inverse(getMatrix());
	}

	return matrix;
}

void CameraControlManipulator::init(const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& us) {
	flushMouseEventStack();
}

void CameraControlManipulator::setTrackerMode(TrackerMode mode) {
	_trackerMode = mode;
}

void CameraControlManipulator::setManipulatorMode(ManipulatorMode mode) {
	_manipulatorMode = mode;
}

void CameraControlManipulator::setDistance(float distance) {
	_distance = distance;
}

void CameraControlManipulator::setCameraPos(osg::Vec3d pos) {
	_eye = pos;
	_center = pos;
}

void CameraControlManipulator::setCameraDeltaPos(osg::Vec3d pos) {
	_cameraDelta = pos;
}

void CameraControlManipulator::setCameraIniRot(osg::Quat quat) {
	_rotation = quat;
}

void CameraControlManipulator::setCameraIniRot(double yaw, double pitch, double roll, bool bOgl) {
	_yawDegIni = yaw;
	_pitchDegIni = pitch;
	_rollDegIni = roll;

	_cameraHPR.set(yaw, pitch, roll);

	if (bOgl) {
		_rotation = osg::Quat(
			osg::DegreesToRadians(yaw), osg::Vec3(0.0f, 0.0f, 1.0f),
			osg::DegreesToRadians(pitch), osg::Vec3(1.0f, 0.0f, 0.0f),
			osg::DegreesToRadians(roll), osg::Vec3(0.0f, 1.0f, 0.0f));
	} else {
		_rotation = osg::Quat(
			osg::DegreesToRadians(roll), osg::Vec3(0.0f, 1.0f, 0.0f),
			osg::DegreesToRadians(pitch), osg::Vec3(1.0f, 0.0f, 0.0f),
			osg::DegreesToRadians(yaw), osg::Vec3(0.0f, 0.0f, 1.0f)
		);
	}
}

void CameraControlManipulator::setRoamCamera(const osg::Vec3& eye, const osg::Vec3& center, const osg::Vec3& up) {
	osg::Vec3 lv(center - eye);

	osg::Vec3 f(lv);
	f.normalize();
	osg::Vec3 s(f ^ up);
	s.normalize();
	osg::Vec3 u(s ^ f);
	u.normalize();

	osg::Matrix rotation_matrix(s[0], u[0], -f[0], 0.0f,
		s[1], u[1], -f[1], 0.0f,
		s[2], u[2], -f[2], 0.0f,
		0.0f, 0.0f, 0.0f, 1.0f);

	_eye = eye;
	_center = center;
	_distance = lv.length();
	_rotation = rotation_matrix.getRotate().inverse();
}

void CameraControlManipulator::flushMouseEventStack() {
	_ga_t1 = NULL;
	_ga_t0 = NULL;
}

void CameraControlManipulator::addMouseEvent(const osgGA::GUIEventAdapter& ea) {
	_ga_t1 = _ga_t0;
	_ga_t0 = &ea;
}

void CameraControlManipulator::rotateYawPitch(osg::Quat& rotation, const double yaw, const double pitch,
	const osg::Vec3d& localUp) {
	bool verticalAxisFixed = (localUp != osg::Vec3d(0., 0., 0.));

	// fix current rotation
	if (verticalAxisFixed)
		fixVerticalAxis(rotation, localUp, true);

	// rotations
	osg::Quat rotateYaw(-yaw, verticalAxisFixed ? localUp : rotation * osg::Vec3d(0., 1., 0.));
	osg::Quat rotatePitch;
	osg::Quat newRotation;
	osg::Vec3d cameraRight(rotation * osg::Vec3d(1., 0., 0.));

	double my_dy = pitch;
	int i = 0;

	do {

		// rotations
		rotatePitch.makeRotate(my_dy, cameraRight);
		newRotation = rotation * rotateYaw * rotatePitch;

		// update vertical axis
		if (verticalAxisFixed)
			fixVerticalAxis(newRotation, localUp, false);

		// check for viewer's up vector to be more than 90 degrees from "up" axis
		osg::Vec3d newCameraUp = newRotation * osg::Vec3d(0., 1., 0.);
		if (newCameraUp * localUp > 0.) {

			// apply new rotation
			rotation = newRotation;
			return;

		}

		my_dy /= 2.;
		if (++i == 20) {
			rotation = rotation * rotateYaw;
			return;
		}

	} while (true);
}
bool CameraControlManipulator::calcMovement() {
	if (_ga_t0.get() == NULL || _ga_t1.get() == NULL)
		return false;

	double dx = _ga_t0->getXnormalized() - _ga_t1->getXnormalized();
	double dy = _ga_t0->getYnormalized() - _ga_t1->getYnormalized();

	if (dx == 0. && dy == 0.)
		return false;

	// get delta time
	double eventTimeDelta = _ga_t0->getTime() - _ga_t1->getTime();
	if (eventTimeDelta < 0.) {
		OSG_WARN << "Manipulator warning: eventTimeDelta = " << eventTimeDelta << std::endl;
		eventTimeDelta = 0.;
	}


	unsigned int buttonMask = _ga_t1->getButtonMask();

	if (buttonMask == osgGA::GUIEventAdapter::RIGHT_MOUSE_BUTTON) {
		if (_manipulatorMode == Roam_ManipulatorMode) {
			osg::CoordinateFrame coordinateFrame = getCoordinateFrame(_eye);
			osg::Vec3d localUp = getUpVector(coordinateFrame);

			rotateYawPitch(_rotation, dx, dy, localUp);

			return true;
		} else if (_manipulatorMode == Model_ManipulatorMode) {
			//if (_projMode == Proj_Perspective)
			//{
				osg::Vec3 axis;
				double angle;

				trackball(axis, angle, 
					_ga_t1->getXnormalized(), _ga_t1->getYnormalized(),
					_ga_t0->getXnormalized(), _ga_t0->getYnormalized() );

				osg::Quat new_rotate;
				new_rotate.makeRotate(angle,axis);

				_rotation = _rotation * new_rotate;

				return true;
			//}
		} else if (_manipulatorMode == Tracker_ManipulatorMode) {
			osg::Vec3 axis;
			double angle;

			double px0 = _ga_t0->getXnormalized();
			double py0 = _ga_t0->getYnormalized();

			double px1 = _ga_t1->getXnormalized();
			double py1 = _ga_t1->getYnormalized();

			trackball(axis, angle, px1, py1, px0, py0);

			osg::Quat new_rotate;
			new_rotate.makeRotate(angle, axis);

			_rotation = _rotation * new_rotate;
		}
	} else if (buttonMask == osgGA::GUIEventAdapter::LEFT_MOUSE_BUTTON) {
		if (_manipulatorMode == Roam_ManipulatorMode) {
			// zoom model.

			//float fd = _distance;
			//float scale = 1.0f+dy;

			//{
			//	osg::Matrix rotation_matrix(_rotation);

			//	osg::Vec3 dv = (osg::Vec3(0.0f,0.0f,-1.0f)*rotation_matrix)*(dy*-fd);

			//	{
			//		_eye += dv;
			//	}
			//}
			//
			//return true;
		} else if (_manipulatorMode == Model_ManipulatorMode) {
			// pan model
			float scale = -0.3f * _distance;

			panModel(dx * scale, dy * scale);

			return true;
		}
	}

	return false;
}

bool CameraControlManipulator::isMouseMoving() {
	if (_ga_t0.get() == NULL || _ga_t1.get() == NULL)
		return false;

	static const float velocity = 0.1f;

	float dx = _ga_t0->getXnormalized() - _ga_t1->getXnormalized();
	float dy = _ga_t0->getYnormalized() - _ga_t1->getYnormalized();
	float len = sqrtf(dx * dx + dy * dy);
	float dt = _ga_t0->getTime() - _ga_t1->getTime();

	return (len > dt * velocity);
}

void CameraControlManipulator::SetCameraSenario(EyeViewport eyeViewport) {
	if (eyeViewport.time > 0.0) {
		osg::Matrixd matrix = getInverseMatrix();

		osg::Vec3d eye, center, up;
		float distance = 100.0;
		matrix.getLookAt(eye, center, up, distance);

		setTrackNode(NULL);
		setManipulatorMode(CameraControlManipulator::Roam_ManipulatorMode);
		setVerticalAxisFixed(true);
		setRoamCamera(eye, center, up);

		_isEyeAnimation = true;
		_time_Animation = eyeViewport.time;

		_firstCP_Animation = osg::AnimationPath::ControlPoint(_eye, _rotation);
		_secondCP_Animation = osg::AnimationPath::ControlPoint(eyeViewport.eye, eyeViewport.rotation);
	} else {
		setTrackNode(NULL);
		setManipulatorMode(CameraControlManipulator::Roam_ManipulatorMode);
		setVerticalAxisFixed(true);

		_eye = eyeViewport.eye;
		_rotation = eyeViewport.rotation;
	}
}

bool CameraControlManipulator::handle(const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& us) {
	switch (ea.getEventType()) {
	case osgGA::GUIEventAdapter::FRAME:
		return handleFrame(ea, us);

	case osgGA::GUIEventAdapter::RESIZE:
		return handleResize(ea, us);

	default:
		break;
	}

	if (ea.getHandled())
		return false;

	switch (ea.getEventType()) {
	case osgGA::GUIEventAdapter::MOVE:
		return handleMouseMove(ea, us);

	case osgGA::GUIEventAdapter::DRAG:
		return handleMouseDrag(ea, us);

	case osgGA::GUIEventAdapter::PUSH:
		return handleMousePush(ea, us);

	case osgGA::GUIEventAdapter::RELEASE:
		return handleMouseRelease(ea, us);

	case osgGA::GUIEventAdapter::KEYDOWN:
		return handleKeyDown(ea, us);

	case osgGA::GUIEventAdapter::KEYUP:
		return handleKeyUp(ea, us);

	case osgGA::GUIEventAdapter::SCROLL:
		return handleMouseWheel(ea, us);

	default:
		return false;
	}

	return false;
}

void CameraControlManipulator::computeNodeWorldToLocal(osg::Matrixd& worldToLocal) const {
	osg::NodePath nodePath;
	if (_trackNodePath.getNodePath(nodePath)) {
		worldToLocal = osg::computeWorldToLocal(nodePath);
	}
}

void CameraControlManipulator::computeNodeLocalToWorld(osg::Matrixd& localToWorld) const {
	osg::NodePath nodePath;
	if (_trackNodePath.getNodePath(nodePath)) {
		localToWorld = osg::computeLocalToWorld(nodePath);
	}
}

void CameraControlManipulator::computeNodeCenterAndRotation(osg::Vec3d& nodeCenter, osg::Quat& nodeRotation) const {
	osg::Matrixd localToWorld, worldToLocal;
	computeNodeLocalToWorld(localToWorld);
	computeNodeWorldToLocal(worldToLocal);

	osg::NodePath nodePath;
	//if (_trackNodePath.getNodePath(nodePath) && !nodePath.empty())
	//	nodeCenter = osg::Vec3d(nodePath.back()->getBound().center())*localToWorld;
	//else
	nodeCenter = _cameraDelta * localToWorld;

	switch (_trackerMode) {
	case(NODE_CENTER_AND_AZIM):
	{
		osg::CoordinateFrame coordinateFrame = getCoordinateFrame(nodeCenter);
		osg::Matrixd localToFrame(localToWorld * osg::Matrixd::inverse(coordinateFrame));

		double azim = atan2(-localToFrame(0, 1), localToFrame(0, 0));
		osg::Quat nodeRotationRelToFrame, rotationOfFrame;
		nodeRotationRelToFrame.makeRotate(-azim, 0.0, 0.0, 1.0);
		rotationOfFrame = coordinateFrame.getRotate();
		nodeRotation = nodeRotationRelToFrame * rotationOfFrame;
	}
	break;

	case(NODE_CENTER_AND_ROTATION):
	{
		// scale the matrix to get rid of any scales before we extract the rotation.
		double sx = 1.0 / sqrt(localToWorld(0, 0) * localToWorld(0, 0) + localToWorld(1, 0) * localToWorld(1, 0) + localToWorld(2, 0) * localToWorld(2, 0));
		double sy = 1.0 / sqrt(localToWorld(0, 1) * localToWorld(0, 1) + localToWorld(1, 1) * localToWorld(1, 1) + localToWorld(2, 1) * localToWorld(2, 1));
		double sz = 1.0 / sqrt(localToWorld(0, 2) * localToWorld(0, 2) + localToWorld(1, 2) * localToWorld(1, 2) + localToWorld(2, 2) * localToWorld(2, 2));
		localToWorld = localToWorld * osg::Matrixd::scale(sx, sy, sz);

		nodeRotation = localToWorld.getRotate();
	}
	break;

	case(NODE_CENTER):
	default:
	{
		osg::CoordinateFrame coordinateFrame = getCoordinateFrame(nodeCenter);
		nodeRotation = coordinateFrame.getRotate();
	}
	break;
	}
}

void CameraControlManipulator::setTrackNodePath(const osg::NodePath& nodePath) {
	_trackNodePath.setNodePath(nodePath);
}

void CameraControlManipulator::setTrackNode(osg::Node* node) {
	if (!node) {
		OSG_NOTICE << "NodeTrackerManipulator::setTrackNode(Node*):  Unable to set tracked node due to null Node*" << std::endl;

		return;
	}

	osg::NodePathList nodePaths = node->getParentalNodePaths();
	if (!nodePaths.empty()) {
		if (nodePaths.size() > 1) {
			OSG_NOTICE << "osgGA::NodeTrackerManipualtor::setTrackNode(..) taking first parent path, ignoring others." << std::endl;
		}

		for (unsigned int i = 0; i < nodePaths.size(); ++i) {
			OSG_NOTICE << "NodePath " << i << std::endl;

			for (osg::NodePath::iterator itr = nodePaths[i].begin();
				itr != nodePaths[i].end();
				++itr) {
				OSG_NOTICE << "     " << (*itr)->className() << std::endl;
			}
		}


		OSG_INFO << "NodeTrackerManipulator::setTrackNode(Node*" << node << " " << node->getName() << "): Path set" << std::endl;

		setTrackNodePath(nodePaths[0]);
	} else {
		OSG_NOTICE << "NodeTrackerManipulator::setTrackNode(Node*): Unable to set tracked node due to empty parental path." << std::endl;
	}
}

/*
* This size should really be based on the distance from the center of
* rotation to the point on the object underneath the mouse.  That
* point would then track the mouse as closely as possible.  This is a
* simple example, though, so that is left as an Exercise for the
* Programmer.
*/
const float TRACKBALLSIZE = 0.8f;

/*
* Ok, simulate a track-ball.  Project the points onto the virtual
* trackball, then figure out the axis of rotation, which is the cross
* product of P1 P2 and O P1 (O is the center of the ball, 0,0,0)
* Note:  This is a deformed trackball-- is a trackball in the center,
* but is deformed into a hyperbolic sheet of rotation away from the
* center.  This particular function was chosen after trying out
* several variations.
*
* It is assumed that the arguments to this routine are in the range
* (-1.0 ... 1.0)
*/
void CameraControlManipulator::trackball(osg::Vec3& axis, double& angle, double  p1x, double  p1y, double  p2x, double  p2y) {
	/*
	* First, figure out z-coordinates for projection of P1 and P2 to
	* deformed sphere
	*/

	osg::Matrix rotation_matrix(_rotation);


	osg::Vec3d uv = osg::Vec3d(0.0, 1.0, 0.0) * rotation_matrix;
	osg::Vec3d sv = osg::Vec3d(1.0, 0.0, 0.0) * rotation_matrix;
	osg::Vec3d lv = osg::Vec3d(0.0, 0.0, -1.0) * rotation_matrix;

	osg::Vec3d p1 = sv * p1x + uv * p1y - lv * tb_project_to_sphere(TRACKBALLSIZE, p1x, p1y);
	osg::Vec3d p2 = sv * p2x + uv * p2y - lv * tb_project_to_sphere(TRACKBALLSIZE, p2x, p2y);

	/*
	*  Now, we want the cross product of P1 and P2
	*/
	// This was the quick 'n' dirty  fix to get the trackball doing the right 
	// thing after fixing the Quat rotations to be right-handed.  You may want
	// to do something more elegant.
	//   axis = p1^p2;
	axis = p2 ^ p1;
	axis.normalize();

	/*
	*  Figure out how much to rotate around that axis.
	*/
	double t = (p2 - p1).length() / (2.0 * TRACKBALLSIZE);

	/*
	* Avoid problems with out-of-control values...
	*/
	if (t > 1.0) t = 1.0;
	if (t < -1.0) t = -1.0;
	angle = osg::inRadians(asin(t));
}

/*
* Project an x,y pair onto a sphere of radius r OR a hyperbolic sheet
* if we are away from the center of the sphere.
*/
double CameraControlManipulator::tb_project_to_sphere(double  r, double  x, double  y) {
	float d, t, z;

	d = sqrt(x * x + y * y);
	/* Inside sphere */
	if (d < r * 0.70710678118654752440) {
		z = sqrt(r * r - d * d);
	}                            /* On hyperbola */
	else {
		t = r / 1.41421356237309504880;
		z = t * t / d;
	}

	return z;
}

bool CameraControlManipulator::handleFrame(const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& us) {
	double current_frame_time = ea.getTime();

	_delta_frame_time = current_frame_time - _last_frame_time;
	_last_frame_time = current_frame_time;

	if (_thrown &&
		((_manipulatorMode == Tracker_ManipulatorMode) ||
			((_manipulatorMode == Roam_ManipulatorMode) &&
				(_allowThrow)))) {
		if (calcMovement())
			us.requestRedraw();
	}

	if (_isMove && (_manipulatorMode == Tracker_ManipulatorMode)) {
		osg::Node* currNode = getTrackNode();
		if (currNode) {
			osg::MatrixTransform* mt = dynamic_cast<osg::MatrixTransform*>(currNode->getParent(0)->getParent(0));

			if (mt) {
				osg::Matrix matrix = mt->getMatrix();

				matrix = osg::Matrix::translate(0.0, _moveFactor, 0.0) *
					osg::Matrix::rotate(osg::DegreesToRadians(_rotFactor), osg::Vec3d(0.0, 0.0, 1.0)) *
					matrix;

				mt->setMatrix(matrix);
			}
		}

		_isMove = false;

		_moveFactor = 0.0;
		_rotFactor = 0.0;
	}

	if (_isEyeAnimation && (_manipulatorMode == Roam_ManipulatorMode)) {
		_curr_time_Animation += _delta_frame_time;

		double ratio = _curr_time_Animation / _time_Animation;

		double one_minus_ratio = 1.0 - ratio;

		osg::Vec3 position = _firstCP_Animation.getPosition() * one_minus_ratio + _secondCP_Animation.getPosition() * ratio;

		_eye = position;
		_rotation.slerp(ratio, _firstCP_Animation.getRotation(), _secondCP_Animation.getRotation());

		if (_curr_time_Animation > _time_Animation) {
			_isEyeAnimation = false;
			_curr_time_Animation = 0.0;
			_time_Animation = 0.0;
		}
	}

	if (_manipulatorMode == Roam_ManipulatorMode) {
		if (_bKeyDownUp) {
			float fd = 0.5;

			osg::Matrix rotation_matrix(_rotation);

			osg::Vec3 dv = osg::Vec3(0.0f, 0.0f, -fd) * rotation_matrix;

			_eye += dv;

			return true;
		} else if (_bKeyDownDown) {
			float fd = 0.5;

			osg::Matrix rotation_matrix(_rotation);

			osg::Vec3 dv = osg::Vec3(0.0f, 0.0f, fd) * rotation_matrix;

			_eye += dv;

			return true;
		} else if (_bKeyDownLeft) {
			float fd = 0.5;

			osg::Matrix rotation_matrix(_rotation);

			osg::Vec3 dv = osg::Vec3(-fd, 0.0f, 0.0f) * rotation_matrix;

			_eye += dv;

			return true;
		} else if (_bKeyDownRight) {
			float fd = 0.5;

			osg::Matrix rotation_matrix(_rotation);

			osg::Vec3 dv = osg::Vec3(fd, 0.0f, 0.0f) * rotation_matrix;

			_eye += dv;

			return true;
		}
	}
	//{
	//	osg::Matrixd matrix = getInverseMatrix();

	//	osg::Vec3d eye,center,up;
	//	float distance = 100.0;
	//	matrix.getLookAt(eye,center,up, distance);

	//	osg::Quat rotation;

	//	{
	//		osg::Vec3 lv(center-eye);

	//		osg::Vec3 f(lv);
	//		f.normalize();
	//		osg::Vec3 s(f^up);
	//		s.normalize();
	//		osg::Vec3 u(s^f);
	//		u.normalize();

	//		osg::Matrix rotation_matrix(s[0],     u[0],     -f[0],     0.0f,
	//			s[1],     u[1],     -f[1],     0.0f,
	//			s[2],     u[2],     -f[2],     0.0f,
	//			0.0f,     0.0f,     0.0f,      1.0f);

	//		rotation = rotation_matrix.getRotate().inverse();
	//	}

	return false;
}

float CameraControlManipulator::getThrowScale(const double eventTimeDelta) const {
	if (_thrown)
		return float(_delta_frame_time / eventTimeDelta);
	else
		return 1.f;
}

void CameraControlManipulator::panModel(const float dx, const float dy, const float dz) {
	osg::Matrix rotation_matrix;
	rotation_matrix.makeRotate(_rotation);

	osg::Vec3d dv(dx, dy, dz);

	_center += dv * rotation_matrix;
}

void CameraControlManipulator::zoomModel(const float dy, bool pushForwardIfNeeded) {
	// scale
	float scale = 1.0f + dy;

	// minimum distance
	float minDist = _minimumDistance;
	//if( getRelativeFlag( _minimumDistanceFlagIndex ) )
	//	minDist *= _modelSize;

	if (_distance * scale > minDist) {
		_distance *= scale;
	} else {
		if (pushForwardIfNeeded) {
			// push the camera forward
			float scale = -_distance;
			osg::Matrixd rotation_matrix(_rotation);
			osg::Vec3d dv = (osg::Vec3d(0.0f, 0.0f, -1.0f) * rotation_matrix) * (dy * scale);
			_center += dv;
		} else {
			// set distance on its minimum value
			_distance = minDist;
		}
	}
}

bool CameraControlManipulator::handleResize(const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& us) {
	init(ea, us);
	us.requestRedraw();

	return true;
}

bool CameraControlManipulator::handleMouseMove(const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& us) {
	return false;
}

bool CameraControlManipulator::handleMouseDrag(const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& us) {
	if (_manipulatorMode == Camera_ManipulatorMode) {
		addMouseEvent(ea);

		if (_ga_t0.get() == NULL || _ga_t1.get() == NULL)
			return false;

		double dx = _ga_t0->getXnormalized() - _ga_t1->getXnormalized();
		double dy = _ga_t0->getYnormalized() - _ga_t1->getYnormalized();

		osg::CoordinateFrame coordinateFrame = getCoordinateFrame(_eye);
		osg::Vec3d localUp = getUpVector(coordinateFrame);

		rotateYawPitch(_rotation, dx, dy, localUp);

		return true;
	} else {
		addMouseEvent(ea);

		if (calcMovement())
			us.requestRedraw();

		us.requestContinuousUpdate(false);

		_thrown = false;

		if (ea.isMultiTouchEvent()) {
			osgGA::GUIEventAdapter::TouchData* data = ea.getTouchData();

			if (data->getNumTouchPoints() == 2) {
				if ((_lastTouchData.valid()) && (_lastTouchData->getNumTouchPoints() == 2)) {
					const float zoom = 1.0f;

					osg::Vec2 pt_1_now(data->get(0).x, data->get(0).y);
					osg::Vec2 pt_2_now(data->get(1).x, data->get(1).y);

					osg::Vec2 pt_1_last(_lastTouchData->get(0).x, _lastTouchData->get(0).y);
					osg::Vec2 pt_2_last(_lastTouchData->get(1).x, _lastTouchData->get(1).y);

					float gap_now((pt_1_now - pt_2_now).length());
					float gap_last((pt_1_last - pt_2_last).length());

					if (fabs(gap_last - gap_now) >= zoom) {
						zoomModel((gap_last - gap_now) * 0.1, true);
					}
				}
			}

			_lastTouchData = data;

			unsigned int num_touches_ended(0);
			for (osgGA::GUIEventAdapter::TouchData::iterator i = data->begin(); i != data->end(); ++i) {
				if ((*i).phase == osgGA::GUIEventAdapter::TOUCH_ENDED) {
					num_touches_ended++;
				}
			}

			if (num_touches_ended == data->getNumTouchPoints()) {
				_lastTouchData = NULL;
			}
		}

		return true;
	}

	return false;
}

bool CameraControlManipulator::handleMousePush(const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& us) {
	if ((_manipulatorMode == Tracker_ManipulatorMode) ||
		(_manipulatorMode == Roam_ManipulatorMode)) {
		flushMouseEventStack();

		addMouseEvent(ea);

		if (calcMovement())
			us.requestRedraw();

		us.requestContinuousUpdate(false);

		_thrown = false;
		return true;
	} else {
		flushMouseEventStack();

		addMouseEvent(ea);

		return true;
	}

	return false;
}

bool CameraControlManipulator::handleMouseRelease(const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& us) {
	g_currMouseX = ea.getX();

	g_currMouseY = ea.getY();

	if ((_manipulatorMode == Tracker_ManipulatorMode) ||
		(_manipulatorMode == Roam_ManipulatorMode)) {
		if (ea.getButtonMask() == 0) {

			double timeSinceLastRecordEvent =
				_ga_t0.valid() ? (ea.getTime() - _ga_t0->getTime()) : DBL_MAX;

			if (timeSinceLastRecordEvent > 0.02)
				flushMouseEventStack();

			if (isMouseMoving()) {
				if (calcMovement()) {
					us.requestRedraw();
					us.requestContinuousUpdate(true);

					//_thrown = true;
					_thrown = _allowThrow;
				}
			} else {
				flushMouseEventStack();
				addMouseEvent(ea);

				if (calcMovement())
					us.requestRedraw();

				us.requestContinuousUpdate(false);

				_thrown = false;
			}

		} else {
			flushMouseEventStack();
			addMouseEvent(ea);

			if (calcMovement())
				us.requestRedraw();

			us.requestContinuousUpdate(false);

			_thrown = false;
		}

		return true;
	} else {
		flushMouseEventStack();

		addMouseEvent(ea);

		return true;
	}

	return false;
}

bool CameraControlManipulator::handleKeyDown(const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& us) {
	osgViewer::View* view = dynamic_cast<osgViewer::View*>(&us);

    if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Up) {
        _bKeyDownUp = true;
    } else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Down) {
        _bKeyDownDown = true;
    } else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Left) {
        _bKeyDownLeft = true;
    } else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Right) {
        _bKeyDownRight = true;
    }

	return false;
}

bool CameraControlManipulator::handleKeyUp(const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& us) {
	osgViewer::View* view = dynamic_cast<osgViewer::View*>(&us);

	if (view->getName() == "MainView") {
		if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Up) {
			_bKeyDownUp = false;
		} else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Down) {
			_bKeyDownDown = false;
		} else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Left) {
			_bKeyDownLeft = false;
		} else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Right) {
			_bKeyDownRight = false;
		}
	}

	return false;
}

bool CameraControlManipulator::handleMouseWheel(const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& us) {
	if (_manipulatorMode == Camera_ManipulatorMode) {
		flushMouseEventStack();

		addMouseEvent(ea);

		if (_ga_t0.get() != NULL) {
			double dy = 0.0;

			switch (_ga_t0->getScrollingMotion()) {
			case osgGA::GUIEventAdapter::SCROLL_UP:
				dy = _zoomDelta;
				break;

			case osgGA::GUIEventAdapter::SCROLL_DOWN:
				dy = -_zoomDelta;
				break;

			default:
				break;
			}

			/*_eye.y() *= (1.0 + dy);*/
		}
	} else {
		flushMouseEventStack();

		addMouseEvent(ea);

		if (_ga_t0.get() != NULL) {
			double dy = 0.0;

			switch (_ga_t0->getScrollingMotion()) {
			case osgGA::GUIEventAdapter::SCROLL_UP:
				dy = -_zoomDelta;
				break;

			case osgGA::GUIEventAdapter::SCROLL_DOWN:
				dy = _zoomDelta;
				break;

			case osgGA::GUIEventAdapter::SCROLL_LEFT:
			case osgGA::GUIEventAdapter::SCROLL_RIGHT:
				// pass
				break;

			case osgGA::GUIEventAdapter::SCROLL_2D:
				break;

			default:
				break;
			}

			if (_manipulatorMode == Tracker_ManipulatorMode) {
				// zoom model.
				double fd = _distance;
				double scale = 1.0f + dy;
				if (fd * scale > _minimumDistance) {
					_distance *= scale;

					//if (_distance > 1500.0)
					//{
					//	_distance = 1500.0;
					//}
				} else {
					_distance = _minimumDistance;
				}

			} else if (_manipulatorMode == Roam_ManipulatorMode) {
				float fd = _distance;
				osg::Matrix rotation_matrix(_rotation);
				osg::Vec3 dv = (osg::Vec3(0.0f, 0.0f, -1.0f) * rotation_matrix) * (dy * -fd);
				//_eye += dv;

				osg::Vec3d currPos = _eye;

				currPos += dv;

				double maxHeight =
					sqrt(1900.0 * 1900.0 - currPos.x() * currPos.x() - currPos.y() * currPos.y());
				if (currPos.z() > maxHeight) {
				} else {
					_eye += dv;
				}
			} else if (_manipulatorMode == Model_ManipulatorMode) {
				if (_projMode == Proj_Perspective) {
					zoomModel(dy, true);
				} else if (_projMode == Proj_Ortho) {
					osgViewer::View* view = dynamic_cast<osgViewer::View*>(&us);
					if (view) {
						double left = 0.0;
						double right = 0.0;
						double bottom = 0.0;
						double top = 0.0;
						double zNear = 0.0;
						double zFar = 0.0;

						view->getCamera()->getProjectionMatrixAsOrtho(left, right, bottom, top, zNear, zFar);

						float scale = 1.0f + static_cast<float>(dy);

						_distance *= scale;

						left *= scale;
						right *= scale;
						bottom *= scale;
						top *= scale;

						view->getCamera()->setProjectionMatrixAsOrtho(left, right, bottom, top, zNear, zFar);
					}
				}
			}

			us.requestRedraw();
		}

		us.requestContinuousUpdate(false);

		_thrown = false;

		return true;
	}

	return false;
}

bool CameraControlManipulator::handleMouseDeltaMovement(const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& us) {
	return false;
}