#include "Waypointarrow.h"

#include "util/Convert.h"

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


namespace orxonox
{
    RegisterClass(Waypointarrow);
    // SetConsoleCommand("Waypointarrow", "toggleArrow", &Waypointarrow::toggleArrow);


    Waypointarrow::Waypointarrow(Context* context) : StaticEntity(context)
    {
        RegisterObject(Waypointarrow);
        model = new Model(this->getContext());
        model->setMeshSource("arrow.mesh");  // Name of the arrow file for now bottle
        this->attach(model);
        model->setScale(3);
       // model->setOrientation(Vector3(0,0,-1));
        model->setPosition(Vector3(0,15, 0));
        waypoints_ = nullptr;
    }

    Waypointarrow::~Waypointarrow()
    {
    }

   /*
    Waypoint* WaypointGroup::getActive(){
          for (Waypoint* object : this->waypoints_){
            if(!(object->waypoint_actived)){
                object->enable_waypoint();
                return object;
            }
        }
    }


    void tick::getActive


*/

 /*   void Waypointarrow::toggleArrow(){
        if(model->getScale() == 3)
            model->setScale(0);
        else
            model->setScale(3);
    }
*/

    WaypointGroup* Waypointarrow::getWaypointgroup()
    {
        if (waypoints_ == nullptr){

            for (WaypointGroup* group : ObjectList<WaypointGroup>())
                waypoints_ = group;

        }

        return waypoints_;
    }




    void Waypointarrow::tick(float dt){

        if (getWaypointgroup() == nullptr){
                //orxout() << "Second" << endl;

            return;
        }

        waypoints_ = getWaypointgroup();
        waypoint = waypoints_->getActive();
        Vector3 waypoint_position = waypoint->getWorldPosition();


        // orxout() << model->scale_ << endl;
        //orxout() << "waypoint " << waypoint_position.x <<  "waypoint " << waypoint_position.y << "waypoint " << waypoint_position.z << endl;
        //orxout() << "model " << this->model->getWorldPosition().x <<  "model " << this->model->getWorldPosition().y << "model " << this->model->getWorldPosition().z << endl;
        //orxout() << "this " << this->getWorldPosition().x <<  "this " << this->getWorldPosition().y << "this " << this->getWorldPosition().z << endl;
        //orxout() << "trial " << trial.x <<  "trial " << trial.y << "trial " << trial.z << endl;


        Vector3 origin = model->getWorldPosition();;
        model->setDirection( origin - waypoint_position, TransformSpace::World);
     

    }

    void Waypointarrow::XMLPort(Element& xmlelement, XMLPort::Mode mode){
        SUPER(Waypointarrow, XMLPort, xmlelement, mode); // From the SpaceShip.cc file
        //XMLPortParamTemplate(WorldEntity, "orientation", setOrientation, getOrientation, xmlelement, mode, Vector3)
        //XMLPortObject(SpaceShip, Engine, "engines", addEngine, getEngine, xmlelement, mode); // TRY ADDING THE WAYPOINT ARROW LIKE AN ENGINE


    }


}