/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * 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 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    ArmarXCore::units
 * @author     Manfred Kroehnert (manfred dot kroehnert at kit dot edu)
 * @date       2013
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */

#pragma once

#include <RobotAPI/components/units/SensorActorUnit.h>

#include <RobotAPI/interface/units/HandUnitInterface.h>

#include <ArmarXCore/core/application/properties/Properties.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>

#include <VirtualRobot/VirtualRobot.h>


namespace armarx
{
    /**
     * \class HandUnitPropertyDefinitions
     * \brief Defines all necessary properties for armarx::HandUnit
     */
    class HandUnitPropertyDefinitions:
        public ComponentPropertyDefinitions
    {
    public:
        HandUnitPropertyDefinitions(std::string prefix):
            ComponentPropertyDefinitions(prefix)
        {
            defineRequiredProperty<std::string>("RobotFileName", "VirtualRobot XML file in which the endeffector is is stored.");
            defineRequiredProperty<std::string>("EndeffectorName", "Name of the endeffector as stored in the XML file (will publish values on EndeffectorName + 'State')");
        }
    };


    /**
     * @ingroup Component-HandUnit HandUnit
     * \brief Base unit for high-level access to robot hands.
     * \ingroup RobotAPI-SensorActorUnits
     *
     * An instance of a HandUnit provides means to open, close, and preshape hands.
     * It uses the HandUnitListener Ice interface to report updates of its current state
     *
     */

    /**
     * @brief The HandUnit class
     * @ingroup Component-HandUnit
     */
    class HandUnit :
        virtual public HandUnitInterface,
        virtual public SensorActorUnit
    {
    public:
        // inherited from Component
        std::string getDefaultName() const override
        {
            return "HandUnit";
        }

        /**
         * Retrieve proxy for publishing State information and call
         * armarx::PlatformUnit::onInitPlatformUnit().
         * \see armarx::Component::onInitComponent()
         */
        void onInitComponent() override;
        /**
         * Calls armarx::PlatformUnit::onStartPlatformUnit().
         * \see armarx::Component::onConnectComponent()
         */
        void onConnectComponent() override;
        /**
         * Calls armarx::PlatformUnit::onExitPlatformUnit().
         * \see armarx::Component::onExitComponent()
         */
        void onExitComponent() override;

        /**
         *
         */
        virtual void onInitHandUnit() = 0;
        /**
         *
         */
        virtual void onStartHandUnit() = 0;
        /**
         *
         */
        virtual void onExitHandUnit() = 0;

        /**
         * Send command to the hand to form a specific shape position. The shapes are defined in the robot.xml file.
         */
        void setShape(const std::string& shapeName, const Ice::Current& c = Ice::emptyCurrent) override;

        /**
         * @brief setShapeWithObjectInstance Send command to the hand to form a specific shape position.
         * While trying to form the specified shape, collison checking with the named object instance is performed.
         * The resulting joint angles of the hand might differ from the predefined joint angles for the given shape.
         *
         * @param shapeName Name of the well known shape that the hand should form
         * @param graspedObjectInstanceName name of the object instance which is used to check for collisions while setting the shape
         */
        void setShapeWithObjectInstance(const std::string& shapeName, const std::string& objectInstanceName, const Ice::Current& c = Ice::emptyCurrent) override;

        /**
         * \return a list of strings for shape positions which can be used together with HandUnit::shape().
         */
        SingleTypeVariantListBasePtr getShapeNames(const Ice::Current& c = Ice::emptyCurrent) override;

        NameValueMap getShapeJointValues(const std::string& shapeName, const Ice::Current& c = Ice::emptyCurrent) override;
        NameValueMap getCurrentJointValues(const Ice::Current& c = Ice::emptyCurrent) override;

        void setObjectGrasped(const std::string& objectName, const Ice::Current&) override;
        void setObjectReleased(const std::string& objectName, const Ice::Current&) override;
        void setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current&) override;

        std::string describeHandState(const Ice::Current&) override;

        /**
         * \see armarx::PropertyUser::createPropertyDefinitions()
         */
        PropertyDefinitionsPtr createPropertyDefinitions() override;

        void reloadPreshapes(const Ice::Current&) override;

    protected:
        /**
         * HandUnitListener proxy for publishing state updates
         */
        HandUnitListenerPrx listenerPrx;
        /**
         * Ice Topic name on which armarx::HandUnit::listenerPrx publishes information
         */
        std::string listenerChannelName;
        /**
         * List containing the names of all valid shapes.
         */
        SingleTypeVariantListPtr shapeNames;

        std::string kinematicUnitName;

        VirtualRobot::RobotPtr robot;
        VirtualRobot::EndEffectorPtr eef;

        std::string robotName;
        std::string tcpName;

        std::map <std::string, float> handJoints;

        std::string graspedObject;


        // HandUnitInterface interface
    public:
        std::string getHandName(const Ice::Current& = Ice::emptyCurrent) override;
        void setJointForces(const NameValueMap& targetJointForces, const Ice::Current&) override;
        void sendJointCommands(const NameCommandMap& targetJointCommands, const Ice::Current&) override;
    };
}