/*
 * 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 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    RobotAPI::ArmarXObjects::ObjectPoseObserver
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */

#include "ObjectPoseObserver.h"

#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/meta/EnumNames.hpp>

#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotConfig.h>

#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/FramedPose.h>

#include "ice_conversions.h"


namespace armarx
{

    ObjectPoseObserverPropertyDefinitions::ObjectPoseObserverPropertyDefinitions(std::string prefix) :
        armarx::ObserverPropertyDefinitions(prefix)
    {
    }

    armarx::PropertyDefinitionsPtr ObjectPoseObserver::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs(new ObjectPoseObserverPropertyDefinitions(getConfigIdentifier()));

        defs->defineOptionalProperty<std::string>("ObjectPoseTopicName", "ObjectPoseTopic", "Name of the Object Pose Topic.");

        defs->optional(visu.enabled, "visu.enabled", "Enable or disable visualization of objects.");
        defs->optional(visu.inGlobalFrame, "visu.inGlobalFrame", "If true, show global poses. If false, show poses in robot frame.");
        defs->optional(visu.alpha, "visu.alpha", "Alpha of objects (1 = solid, 0 = transparent).");

        return defs;
    }

    std::string ObjectPoseObserver::getDefaultName() const
    {
        return "ObjectPoseObserver";
    }

    void ObjectPoseObserver::onInitObserver()
    {
        usingTopicFromProperty("ObjectPoseTopicName");
    }

    void ObjectPoseObserver::onConnectObserver()
    {
        this->robot = RobotState::addRobot("robot", VirtualRobot::RobotIO::RobotDescription::eStructure);

        createRemoteGuiTab();
        RemoteGui_startRunningTask();
    }

    void ObjectPoseObserver::onDisconnectComponent()
    {
    }

    void ObjectPoseObserver::onExitObserver()
    {
    }


    void ObjectPoseObserver::createRemoteGuiTab()
    {
        using namespace armarx::RemoteGui::Client;

        tab.visuEnabled.setValue(visu.enabled);
        tab.visuInGlobalFrame.setValue(visu.inGlobalFrame);

        tab.visuAlpha.setRange(0, 1.0);
        tab.visuAlpha.setValue(visu.alpha);

        GroupBox visuGroup;
        {
            GridLayout grid;
            int row = 0;
            grid.add(Label("Enabled"), {row, 0}).add(tab.visuEnabled, {row, 1});
            row++;
            grid.add(Label("Global Frame"), {row, 0}).add(tab.visuInGlobalFrame, {row, 1});
            row++;
            grid.add(Label("Alpha"), {row, 0}).add(tab.visuAlpha, {row, 1});
            row++;

            visuGroup.setLabel("Visualization");
            visuGroup.addChild(grid);
        }

        VBoxLayout root = {visuGroup, VSpacer()};
        RemoteGui_createTab(getName(), root, &tab);
    }

    void ObjectPoseObserver::RemoteGui_update()
    {
        visu.enabled = tab.visuEnabled.getValue();
        visu.inGlobalFrame = tab.visuInGlobalFrame.getValue();
        visu.alpha = tab.visuAlpha.getValue();
    }


    void ObjectPoseObserver::reportProviderAvailable(const std::string& providerName, const Ice::Current&)
    {
        static const bool refresh = true;
        fetchProviderInfo(providerName, refresh);
    }

    void ObjectPoseObserver::reportObjectPoses(
        const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
    {
        ARMARX_VERBOSE << "Received object poses from '" << providerName << "'.";

        static const bool refresh = false;
        fetchProviderInfo(providerName, refresh);

        objpose::ObjectPoseSeq objectPoses;
        {
            std::scoped_lock lock(dataMutex);
            RobotState::synchronizeLocalClone("robot");

            for (const auto& provided : providedPoses)
            {
                objpose::ObjectPose& pose = objectPoses.emplace_back();
                pose.fromProvidedPose(provided, robot);
            }
        }

        {
            std::scoped_lock lock(dataMutex);
            this->objectPoses[providerName] = objectPoses;
            handleProviderUpdate(providerName);
        }
    }

    void ObjectPoseObserver::fetchProviderInfo(const std::string& providerName, bool refresh)
    {
        {
            std::scoped_lock lock(dataMutex);
            if (!refresh && providers.count(providerName) > 0)
            {
                return;  // Do nothing.
            }
        }

        objpose::ObjectPoseProviderPrx provider = getProviderProxy(providerName);
        if (!provider)
        {
            ARMARX_WARNING << "Received availability signal from provider '" << providerName << "', "
                           << "but could not get provider proxy.";
            return;
        }
        objpose::ProviderInfo info = provider->getProviderInfo();

        {
            std::scoped_lock lock(dataMutex);
            if (providers.count(providerName) == 0)
            {
                std::stringstream ss;
                for (const auto& id : info.supportedObjects)
                {
                    ss << "- " << id << "\n";
                }
                ARMARX_VERBOSE << "New provider '" << providerName << "' available.\n"
                               << "Supported objects: \n" << ss.str();
            }
            providers[providerName] = info;

            if (updateCounters.count(providerName) == 0)
            {
                updateCounters[providerName] = 0;
            }
        }

        if (!existsChannel(providerName))
        {
            offerChannel(providerName, "Channel of provider '" + providerName + "'.");
        }
        offerOrUpdateDataField(providerName, "objectType", objpose::ObjectTypeEnumNames.to_name(info.objectType), "");
    }


    objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPoses(const Ice::Current&)
    {
        std::scoped_lock lock(dataMutex);
        objpose::data::ObjectPoseSeq result;
        for (const auto& [name, poses] : objectPoses)
        {
            for (const auto& pose : poses)
            {
                result.push_back(pose.toIce());
            }
        }
        return result;
    }

    objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
    {
        std::scoped_lock lock(dataMutex);
        return objpose::toIce(objectPoses.at(providerName));
    }


    void ObjectPoseObserver::requestObjects(const objpose::ObjectIDSeq& objectIDs, Ice::Long relativeTimeoutMS, const Ice::Current&)
    {
        std::map<std::string, objpose::ObjectIDSeq> requests;

        {
            std::scoped_lock lock(dataMutex);
            for (const auto& objectID : objectIDs)
            {
                for (const auto& [name, info] : providers)
                {
                    // ToDo: optimize look up.
                    if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
                    {
                        requests[name].push_back(objectID);
                    }
                }
            }
        }

        for (const auto& [providerName, objects] : requests)
        {
            objpose::ObjectPoseProviderPrx provider = getProviderProxy(providerName);
            if (provider)
            {
                ARMARX_VERBOSE << "Requesting provider '" << providerName << "' for objects " << objects;
                provider->requestObjects(objects, relativeTimeoutMS);
            }
        }
    }


    objpose::ProviderInfoMap ObjectPoseObserver::getAvailableProvidersWithInfo(const Ice::Current&)
    {
        std::scoped_lock lock(dataMutex);
        return providers;
    }

    Ice::StringSeq ObjectPoseObserver::getAvailableProviderNames(const Ice::Current&)
    {
        std::scoped_lock lock(dataMutex);
        return simox::get_keys(providers);
    }

    objpose::ProviderInfo ObjectPoseObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
    {
        try
        {
            return providers.at(providerName);
        }
        catch (const std::out_of_range&)
        {
            std::stringstream ss;
            ss << "No provider with name '" << providerName << "' available.\n";
            ss << "Available are:\n";
            for (const auto& [name, _] : providers)
            {
                ss << "- '" << name << "'\n";
            }
            throw std::out_of_range(ss.str());
        }
    }

    bool ObjectPoseObserver::hasProvider(const std::string& providerName, const Ice::Current&)
    {
        std::scoped_lock lock(dataMutex);
        return providers.count(providerName) > 0;
    }


    Ice::Int ObjectPoseObserver::getUpdateCounterByProvider(const std::string& providerName, const Ice::Current&)
    {
        std::scoped_lock lock(dataMutex);
        return updateCounters.at(providerName);
    }

    StringIntDictionary ObjectPoseObserver::getAllUpdateCounters(const Ice::Current&)
    {
        return updateCounters;
    }


    void ObjectPoseObserver::handleProviderUpdate(const std::string& providerName)
    {
        // Initialized to 0 on first access.
        updateCounters[providerName]++;
        if (providers.count(providerName) == 0)
        {
            providers[providerName] = objpose::ProviderInfo();
        }

        if (!existsChannel(providerName))
        {
            offerChannel(providerName, "Channel of provider '" + providerName + "'.");
        }
        offerOrUpdateDataField(providerName, "updateCounter", Variant(updateCounters.at(providerName)), "Counter incremented for each update.");
        offerOrUpdateDataField(providerName, "candidateCount", Variant(int(objectPoses.at(providerName).size())), "Number of provided object poses.");

        if (visu.enabled)
        {
            visProviderUpdate(providerName);
        }
    }


    void ObjectPoseObserver::visProviderUpdate(const std::string& providerName)
    {
        viz::Layer layer = arviz.layer(providerName);

        for (const objpose::ObjectPose& objectPose : objectPoses.at(providerName))
        {
            const objpose::ObjectID id = objectPose.objectID;
            std::string key = id.dataset + "/" + id.name;

            std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id.dataset, id.name);
            if (!objectInfo)
            {
                ARMARX_WARNING << "Cannot visualize object '" << key << "'.";
                continue;
            }

            Eigen::Matrix4f pose = visu.inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;

            viz::Object object = viz::Object(id.dataset + "/" + id.name)
                                 .file(objectInfo->package(), objectInfo->simoxXML().relativePath)
                                 .pose(pose);
            if (visu.alpha < 1)
            {
                object.overrideColor(simox::Color::white().with_alpha(visu.alpha));
            }

            layer.add(object);
        }

        arviz.commit(layer);
    }

    objpose::ObjectPoseProviderPrx ObjectPoseObserver::getProviderProxy(const std::string& providerName)
    {
        return getProxy<objpose::ObjectPoseProviderPrx>(providerName, false, "", false);
    }

}