/**
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as
* published by the Free Software Foundation; either version 2 of
* the License, or (at your option) any later version.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package    RobotComponents::applications::AStarPathPlannerTestApp
* @author     Raphael Grimm ( ufdrv at student dot kit dot edu )
* @date       2015 Humanoids Group, H2T, KIT
* @license    http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/

#include "AStarPathPlannerTestComponent.h"
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>

#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/updater/ObjectLocalization/MemoryXUpdaterObjectFactories.h>

void armarx::AStarPathPlannerTestComponent::onInitComponent()
{
    usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
    usingProxy(getProperty<std::string>("DebugDrawerName").getValue());
    usingProxy(getProperty<std::string>("AStarPathPlannerName").getValue());
}

void armarx::AStarPathPlannerTestComponent::onConnectComponent()
{
    workingMemoryPrx = getProxy<memoryx::WorkingMemoryInterfacePrx>(getProperty<std::string>("WorkingMemoryName").getValue());
    debugDrawerPrx = getProxy<armarx::DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerName").getValue());
    aStarPathPlannerPrx = getProxy<armarx::AStarPathPlannerBasePrx>(getProperty<std::string>("AStarPathPlannerName").getValue());

    //pass all objects from the scene to the planner
    auto objInstPrx = workingMemoryPrx->getObjectInstancesSegment();
    const auto objIds = objInstPrx->getAllEntityIds();

    armarx::ObjectPositionBaseList objects{};

    for(const auto& id :objIds)
    {
        const memoryx::EntityBasePtr entityBase = objInstPrx->getEntityById(id);
        const memoryx::ObjectInstanceBasePtr object = memoryx::ObjectInstancePtr::dynamicCast(entityBase);

        assert(object);

        const std::string className = object->getMostProbableClass();

        memoryx::ObjectClassList classes = workingMemoryPrx->getPriorKnowledge()->getObjectClassesSegment()->getClassWithSubclasses(className);

        if(!classes.size())
        {
            ARMARX_INFO << "No classes for most probable class '" << className << "' of object '" << object->getName() << "' with id " << id;
            continue;
        }

        armarx::ObjectPositionBase obj;

        obj.objectClassBase = classes.at(0);

        obj.objectPose = ::armarx::PoseBasePtr{new Pose{object->getPositionBase(), object->getOrientationBase()}};

        objects.push_back(obj);

        ARMARX_VERBOSE << "Added class '" << className << "' of object '" << object->getName() << "' with id " << id;
    }

    ARMARX_INFO << "using " << objects.size() << "objects";

    aStarPathPlannerPrx->setCollisionObjects(objects);

    memoryx::AgentInstancesSegmentBasePrx agSegPrx = workingMemoryPrx->getAgentInstancesSegment();

    //set agent
    const auto agentIds = agSegPrx->getAllEntityIds();
    if(!agentIds.size())
    {
        ARMARX_ERROR << "No agent found in memory";
        return;
    }
    const auto agent0Id = agentIds.at(0);

    ARMARX_INFO << "using agent " << agent0Id;

    memoryx::AgentInstanceBasePtr agent = agSegPrx->getAgentInstanceById(agent0Id);

    std::string agentCollisionModelName{"Platform"};

    aStarPathPlannerPrx->setAgent(agent, agentCollisionModelName);


    //plan the path
    armarx::Vector3BasePtr from{new armarx::Vector3{1900.f, 2300.f, 0.f}};
    armarx::Vector3BasePtr to{new armarx::Vector3{2500.f, 7000.f, 1.f}};

    ARMARX_INFO << "Starting path planning";
    const auto path = aStarPathPlannerPrx->getPath(from, to);
    ARMARX_INFO << "Path planning done.";

    if(!path.size())
    {
        ARMARX_INFO << "Found no path!";
        return;
    }

    //draw the path
    std::string layerName{"AStarPathPlannerTestLayer"};

    std::stringstream lineName{};
    DrawColor color{1.f, 0.f, 0.f, 1.f};

    for(std::size_t i = 0; i < path.size() - 1; ++i)
    {
        lineName.str(layerName);
        lineName << "from_" << i << "_to_" << i + 1;
        debugDrawerPrx->setLineVisu(layerName, lineName.str(), path.at(i), path.at(i + 1), 5.f, color);
        //change color to make the first edge distinct
        if(!i)
        {
            color.r = 0.f;
            color.g = 1.f;
        }
    }
    ARMARX_INFO << "Found path whith " << path.size()-1 << " edges";
}