/**
* 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::PathPlanner
* @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 "PathPlanner.h"

#include <MemoryX/interface/memorytypes/MemoryEntities.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>

#include <VirtualRobot/XML/RobotIO.h>

armarx::PathPlanner::PathPlanner():
    safetyMargin{50.f},
    objects{}
{
}

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

void armarx::PathPlanner::onConnectComponent()
{
    workingMemoryPrx = getProxy<memoryx::WorkingMemoryInterfacePrx>(getProperty<std::string>("WorkingMemoryName").getValue());
    commonStoragePrx = workingMemoryPrx->getCommonStorage();
    fileManager.reset( new memoryx::GridFileManager(commonStoragePrx) );

//    debugDrawer = getProxy<armarx::DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerName").getValue());
}

void armarx::PathPlanner::setCollisionObjects(const ::armarx::ObjectPositionBaseList& list, const ::Ice::Current&)
{
    clearCollisionObjects();
    addCollisionObjects(list);
}

void armarx::PathPlanner::addCollisionObjects(const ::armarx::ObjectPositionBaseList& list, const ::Ice::Current&)
{
    ARMARX_VERBOSE << "Adding " << list.size() << " collision objects";

    //strong except guarantee
    std::vector<ObjectData> newObjects;

    for(const auto& elem: list)
    {
        ARMARX_VERBOSE << "Object: " << newObjects.size();

        ObjectData newObject;
        newObject.object = elem.objectClassBase;

        ARMARX_VERBOSE << "Object: name " << newObject.object->getName() << ", id " << newObject.object->getId();

        memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(elem.objectClassBase);
        if(!objectClass)
        {
            std::stringstream s;
            s << "Can't use object class with ice id " << elem.objectClassBase->ice_id();
            ARMARX_ERROR_S << s.str();
            throw armarx::InvalidArgumentException{s.str()};
        }

        ARMARX_VERBOSE << "Adding SimoxObjectWrapper";
        memoryx::EntityWrappers::SimoxObjectWrapperPtr sw = objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
        ARMARX_VERBOSE << "Getting ManipulationObject name and object";
        VirtualRobot::ManipulationObjectPtr orgMo = sw->getManipulationObject();
        std::string moName = orgMo->getName();
        ARMARX_VERBOSE << "Cloning ManipulationObject";
        VirtualRobot::ManipulationObjectPtr mo = orgMo->clone(moName);

         ARMARX_VERBOSE << "Moving object";
        //move the object to the given position
        const auto objectPose = armarx::PosePtr::dynamicCast(elem.objectPose);
        if(!objectPose)
        {
            std::stringstream s;
            s << "Can't convert pose of " << objectClass->getName() << " to armarx::Pose.";
            ARMARX_ERROR_S << s.str();
            throw armarx::InvalidArgumentException{s.str()};
        }
        mo->setGlobalPose(objectPose->toEigen());

        ARMARX_VERBOSE << "Get collision object";
        VirtualRobot::CollisionModelPtr colModel = mo->getCollisionModel();

        newObject.colModel = colModel;
        newObjects.push_back(newObject);
        ARMARX_VERBOSE << "Object: " << newObjects.size() - 1 << " done.";
    }

    //copy
    std::copy(newObjects.begin(), newObjects.end(), std::back_inserter(objects));

    ARMARX_VERBOSE << "Added " << newObjects.size() << " collision objects. (Total: " << objects.size() << ")";
}

void armarx::PathPlanner::clearCollisionObjects(const ::Ice::Current&)
{
    ARMARX_VERBOSE << "Cleared " << objects.size() << " objects.";
    objects.clear();
}

void armarx::PathPlanner::setAgent(const ::memoryx::AgentInstanceBasePtr& newAgent, const std::string& agentColModelName, const ::Ice::Current&)
{
    ARMARX_VERBOSE << "Setting agent '" << newAgent->getName() << "' with id " << newAgent->getId();
    //get agent
    std::string agentFilePath = newAgent->getAgentFilePath();
    //strong except guarantee
     ARMARX_VERBOSE << "Loading agent: " << agentFilePath;
    auto agent2 = VirtualRobot::RobotIO::loadRobot(agentFilePath, VirtualRobot::RobotIO::eFull);
     ARMARX_VERBOSE << "Agent loaded.";
    if(!agent2)
    {
        std::stringstream s;
        s << "Can't load agent from: " << agentFilePath;
        ARMARX_ERROR_S << s.str();
        throw armarx::InvalidArgumentException{s.str()};
    }

    //get collision model
    if(!agent2->hasRobotNode(agentColModelName) || !agent2->getRobotNode(agentColModelName)->getCollisionModel())
    {
        std::stringstream s;
        s << "Agent has no collision model with name " << agentColModelName;
        ARMARX_ERROR_S << s.str();
        throw armarx::InvalidArgumentException{s.str()};
    }
    agent = agent2;
    agentCollisionModel = agent->getRobotNode(agentColModelName)->getCollisionModel();//->clone();
    agentZCoord = agent->getGlobalPose()(2,3);
     ARMARX_VERBOSE << "Setting agent...done";
}

void armarx::PathPlanner::setSafetyMargin(::Ice::Float margin, const ::Ice::Current&)
{
    if(margin < 0)
    {
        std::stringstream s;
        s << "Invalid margin: " << margin << " < 0.0";
        ARMARX_ERROR_S << s.str();
        throw armarx::InvalidArgumentException{s.str()};
    }
    safetyMargin=margin;
}

bool armarx::PathPlanner::isPositionValid(armarx::Vector3 position) const
{
    if(!agentCollisionModel)
    {
        throw std::logic_error{"no agent collision model"};
    }
    if(objects.empty())
    {
        return true;
    }

    //set pose
    Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
    pose.block<3,3>(0,0) = VirtualRobot::MathTools::axisangle2eigen3f(Eigen::Vector3f{0.f, 0.f, 1.f}, position.z);
    pose(0,3) = position.x;
    pose(1,3) = position.y;
    pose(3,3) = agentZCoord;
    agent->setGlobalPose(pose);

    for (const auto& object: objects)
    {
        float dist = VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->calculateDistance(
                        agentCollisionModel,
                        object.colModel);
        if(dist < safetyMargin)
        {
            return false;
        }
    }
    return true;
}