#include "Waypoint.h"

#include <OgreSceneNode.h>
#include <BulletDynamics/Dynamics/btRigidBody.h>
#include "util/OrxAssert.h"
#include "core/CoreIncludes.h"

namespace orxonox
{
    RegisterClass(Waypoint);

    Waypoint::Waypoint(Context* context) : StaticEntity(context)
    {
        RegisterObject(Waypoint);

        this->setPriority(Priority::VeryLow);

        this->registerVariables();
    }

    Waypoint::~Waypoint()
    {
    }


    void Waypoint::XMLPort(Element& xmlelement, XMLPort::Mode mode){
        SUPER(Waypoint, XMLPort, xmlelement, mode); // From the SpaceShip.cc file


        //XMLPortObject(SpaceShip, Engine, "engines", addEngine, getEngine, xmlelement, mode); // TRY ADDING THE WAYPOINT ARROW LIKE AN ENGINE
 

    }



    void Waypoint::registerVariables()
    {
        // Ugly const casts, but are valid because position and orientation are not actually const
        registerVariable(const_cast<Vector3&>(this->getPosition()), \
            VariableDirection::ToClient, new NetworkCallback<StaticEntity>(this, &StaticEntity::positionChanged));
        registerVariable(const_cast<Quaternion&>(this->getOrientation()),
            VariableDirection::ToClient, new NetworkCallback<StaticEntity>(this, &StaticEntity::orientationChanged));
    }



    void Waypoint::setOrientation(const Quaternion& orientation)
    {
        if (this->addedToPhysicalWorld())
        {
            orxout(internal_warning) << "Attempting to change the orientation of a StaticEntity at physics run time. Ignoring change." << endl;
            return;
        }
        if (this->isStatic())
        {
            btTransform transf = this->physicalBody_->getWorldTransform();
            transf.setRotation(btQuaternion(orientation.x, orientation.y, orientation.z, orientation.w));
            this->physicalBody_->setWorldTransform(transf);
        }

        this->node_->setOrientation(orientation);
  
    }

    Vector3 Waypoint::toAimPosition(RadarViewable* target) const
    {
        Vector3 wePosition = HumanController::getLocalControllerSingleton()->getControllableEntity()->getWorldPosition();
        Vector3 targetPosition = target->getRVWorldPosition();
        Vector3 targetSpeed = target->getRVVelocity();

        return getPredictedPosition(wePosition, this->currentMunitionSpeed_, targetPosition, targetSpeed);
    }

/*
    bool StaticEntity::isCollisionTypeLegal(WorldEntity::CollisionType type) const
    {
        if (type == WorldEntity::CollisionType::Kinematic || type == WorldEntity::CollisionType::Dynamic)
        {
            orxout(internal_warning) << "Cannot tell a StaticEntity to have kinematic or dynamic collision type! Ignoring." << endl;
            assert(false); // Only in debug mode
            return false;
        }
        else
            return true;
    }
*/
    void Waypoint::setWorldTransform(const btTransform& worldTrans)
    {
        OrxAssert(false, "Setting world transform of a StaticEntity, which is CF_STATIC!");
    }

    void Waypoint::getWorldTransform(btTransform& worldTrans) const
    {
        worldTrans.setOrigin(btVector3(node_->getPosition().x, node_->getPosition().y, node_->getPosition().z));
        worldTrans.setRotation(btQuaternion(node_->getOrientation().x, node_->getOrientation().y, node_->getOrientation().z, node_->getOrientation().w));
    }
}


const Pawn* pawnPtr = orxonox_cast<const Pawn*>(it->first->getWorldEntity());

if (pawnPtr) {
   float position = pawnPtr->getPosition();