#include "Waypoint.h"

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


namespace orxonox
{
    RegisterClass(WaypointGroup);

    WaypointGroup::WaypointGroup(Context* context) : StaticEntity(context)
    {
        RegisterObject(WaypointGroup);
        activeWaypoint = nullptr;
        current = 0;

        //model = new Model(this->getContext());
        //model->setMeshSource("cube.mesh");  // Name of the arrow file for now bottle
        //is->attach(model);
        //model->setScale(3);
        //model->setOrientation(Vector3(0,0,-1));
        //model->setPosition(Vector3(0.0,0.0,0.0));        // this is wrong, it has to be triggered
        //waypoint_active = false;
        //distancetrigger = new DistanceTrigger(this->getContext());
        //distancetrigger->setDistance(100);
        //this->attach(distancetrigger);

    }

    WaypointGroup::~WaypointGroup()
    {
    }


     //WorldEntity::setDirection
     //WorldEntity::getPosition()
     //setOrientation()




    Waypoint* WaypointGroup::getWaypoint(unsigned int index)
    {
        unsigned int i = 0;
        for (Waypoint* child : this->waypoints_)
        {
            if (i == index)
                return child;
            ++i;
        }
        return nullptr;
    }

    void WaypointGroup::setWaypoint(Waypoint* object)
    {
        object->setWaypointGroup(this);
        this->waypoints_.insert(object);
    }

    void WaypointGroup::activateNext(){
        if (std::next(waypoints_.begin(), current) != std::next(waypoints_.end(), 0)){
          activeWaypoint = *std::next(waypoints_.begin(), current);
          ++current;
        }
    }

    Waypoint* WaypointGroup::getActive(){
          if (activeWaypoint == nullptr){
            orxout() << "Null Pointer" << endl;
            activateNext();

          }
          return activeWaypoint;
    }

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

 //   Waypoint* WaypointGroup::getFirst(){
 //       std::set<Waypoint*>::iterator it = waypoints_.begin();
 //       return *it;
 //   }



    void WaypointGroup::XMLPort(Element& xmlelement, XMLPort::Mode mode){
        SUPER(WaypointGroup, XMLPort, xmlelement, mode); // From the SpaceShip.cc file
        XMLPortObject(WaypointGroup, Waypoint, "waypoints", setWaypoint, getWaypoint, xmlelement, mode);

    }



}