#include "Picker.h"
#include "MouseCursor.h"

#include <OgreManualObject.h>

template<> Picker* Ogre::Singleton<Picker>::ms_Singleton = 0;

Picker::Picker()
{
    active                      = false;
	dragging                    = false;

	sceneManager                = NULL;
	camera                      = NULL;
	mouse                       = NULL;
	keyboard                    = NULL;
	dragBody                    = NULL;
	previousForceTorqueCallback = NULL;
	dragLineNode                = NULL;
	dragLineObject              = NULL;
}

Picker::~Picker()
{
    deInit();
}

void Picker::init(Ogre::SceneManager* sceneManager, OgreNewt::World* world, OIS::Keyboard* keyboard, OIS::Mouse* mouse, int maxDistance, int forceMultiplier)
{
	this->sceneManager = sceneManager;
	this->world = world;
	this->keyboard = keyboard;
	this->mouse = mouse;
    this->maxDistance = maxDistance;
    this->forceMultiplier = forceMultiplier;

    dragLineNode = sceneManager->getRootSceneNode()->createChildSceneNode();
	dragLineObject = new Ogre::ManualObject("__DRAG_LINES__");

    active = true;
}

void Picker::deInit()
{
    if(active)
    {
        if(dragLineNode != NULL)
        {
            dragLineNode->detachAllObjects();

            if(dragLineObject != NULL)
            {
                sceneManager->destroyManualObject(dragLineObject);
                dragLineObject = NULL;
            }

            dragLineNode->getParentSceneNode()->removeAndDestroyChild(dragLineNode->getName());
            dragLineNode = NULL;
        }

        active = false;
    }
}

void Picker::update(Ogre::Camera* camera)
{
    if(!active || sceneManager == NULL)
    {
        return;
    }

    if(!dragging)
    {
        if(mouse->getMouseState().buttonDown(OIS::MB_Left))
        {
            Ogre::Vector3 start, end;

			MouseCursor::Position mousePos = MouseCursor::getSingleton().getPosition();

            // Calculate normalised mouse position [0..1]
			Ogre::Real mx = mousePos.normalisedX;
            Ogre::Real my = mousePos.normalisedY;

            // Get a world space ray as cast from the camera through a viewport position
            Ogre::Ray camray = camera->getCameraToViewportRay(mx, my);

            // Get ray origing and end coordinates
            start = camray.getOrigin();
            end = camray.getPoint(maxDistance);

            // Cast a ray between these points and check for first hit
            OgreNewt::BasicRaycast* ray = new OgreNewt::BasicRaycast(world, start, end, true);
            OgreNewt::BasicRaycast::BasicRaycastInfo info = ray->getFirstHit();

            // Found a body in the ray's path
            if(info.mBody)
            {
                Ogre::Vector3 bodyPos;
                Ogre::Quaternion bodyOrientation;

                // Store the body position and orientation
                info.mBody->getPositionOrientation(bodyPos, bodyOrientation);

                // Calculate hit global and local position (info.mDistance is in the range [0,1])
                Ogre::Vector3 globalPos = camray.getPoint(maxDistance * info.mDistance);
                Ogre::Vector3 localPos = bodyOrientation.Inverse() * (globalPos - bodyPos);

				// Store current force/torque callback
				previousForceTorqueCallback = info.mBody->getForceTorqueCallback();

                // Change the force callback from the standard one to the one that applies the spring picking force
                info.mBody->setCustomForceAndTorqueCallback<Picker>(&Picker::dragCallback, this);

                // Store the relevant picking information
                dragBody = info.mBody;
                dragDist = (maxDistance * info.mDistance);
                dragPoint = localPos;

                // We need the camera too, mark picking active
                this->camera = camera;
                dragging = true;
            }

            delete ray;
        }

        if(dragLineNode)
        {
            removeLine();
        }
    }
    else
    {
        // Check whether user has released the mouse button
        if(!mouse->getMouseState().buttonDown(OIS::MB_Left))
        {
            // Restore body force callback (FIXME: doesnt work it it was set to anything else but the default one)
            //dragBody->setStandardForceCallback();
			dragBody->setCustomForceAndTorqueCallback(previousForceTorqueCallback);

            // Reinitialise dragging information
            dragBody = NULL;
            dragPoint = Ogre::Vector3::ZERO;
            dragDist = 0.0;
            dragging = false;
        }
    }
}

void Picker::dragCallback(OgreNewt::Body* body, Ogre::Real timeStep, int threadIndex)
{
    // Deinitialise if scenemanager has disappeared
    if(sceneManager == NULL)
    {
        deInit();

        return;
    }

    if(keyboard->isKeyDown(OIS::KC_DOWN))
    {
        dragDist -= 0.1f;
    }
    else if(keyboard->isKeyDown(OIS::KC_UP))
    {
        dragDist += 0.1f;
    }

    // Again, get absolute mouse position on screen [0 - resulution] and renderer for window size
    MouseCursor::Position mousePos = MouseCursor::getSingleton().getPosition();;

    // Calculate normalised mouse position [0..1]
    Ogre::Real mx = mousePos.normalisedX;
    Ogre::Real my = mousePos.normalisedY;

    // Get a world space ray as cast from the camera through a viewport position
    Ogre::Ray camray = camera->getCameraToViewportRay(mx, my);

    // Get the global position our cursor is at
    Ogre::Vector3 cursorPos = camray.getPoint(dragDist);

    Ogre::Quaternion bodyOrientation;
    Ogre::Vector3 bodyPos;

    // Now find the global point on the body
    body->getPositionOrientation(bodyPos, bodyOrientation);

    // Fint the handle position we are holding the body from
    Ogre::Vector3 dragPos = (bodyOrientation * dragPoint) + bodyPos;

    Ogre::Vector3 inertia;
    Ogre::Real mass;

    body->getMassMatrix(mass, inertia);

    // Calculate picking spring force
    Ogre::Vector3 dragForce = ((cursorPos - dragPos) * mass * forceMultiplier) - body->getVelocity();

    Ogre::Real distance = cursorPos.distance(dragPos);

    // Draw a 3D line between these points for visual effect
    removeLine();
    dragLineObject->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST);
    dragLineObject->position(cursorPos);
    dragLineObject->position(dragPos);
    dragLineObject->end();
    dragLineNode->attachObject(dragLineObject);

    // Add the picking spring force at the handle
    body->addGlobalForce(dragForce, dragPos);

    // Add gravity too
    Ogre::Vector3 gravity = Ogre::Vector3(0,-9.8,0) * mass;
    body->addForce(gravity);
}

void Picker::removeLine()
{
    // Clear the line between handle and cursor
    dragLineNode->detachAllObjects();
    dragLineObject->clear();
}

Picker* Picker::getSingletonPtr(void)
{
    return ms_Singleton;
}

Picker& Picker::getSingleton(void)
{  
    assert(ms_Singleton);
	return (*ms_Singleton);
}