Observer.cpp

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* Copyright (C) 2011-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    ArmarX::Core
* @author     Kai Welke (welke@kit.edu)
* @date       2011
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
#include "Observer.h"
 
#include <algorithm>
#include <condition_variable>
 
#include <boost/circular_buffer.hpp>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/util/OnScopeExit.h>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
#include <ArmarXCore/observers/exceptions/local/InvalidChannelException.h>
#include <ArmarXCore/observers/exceptions/local/InvalidCheckException.h>
#include <ArmarXCore/observers/exceptions/local/InvalidDataFieldException.h>
#include <ArmarXCore/observers/exceptions/user/InvalidDatafieldException.h>
#include <ArmarXCore/observers/exceptions/user/InvalidTypeException.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
 
using namespace armarx;
using namespace armarx::exceptions::local;
 
const std::string LAST_REFRESH_DELTA_CHANNEL = "_LastRefreshDelta";
 
struct Observer::Impl
{
    PeriodicTask<Observer>::pointer_type metaTask;
    bool logChannelUpdateRate = false;
 
 
    // available checks and channels
    StringConditionCheckMap availableChecks;
    std::mutex checksMutex;
 
    // available channels and data fields
    typename ConditionCheck::ChannelRegistry channelRegistry;
    using ChannelRegistryHistory =
        std::unordered_map<std::string,
                           boost::circular_buffer<std::pair<IceUtil::Time, ChannelRegistryEntry>>>;
    ChannelRegistryHistory channelHistory;
    mutable std::recursive_mutex historyMutex;
    int maxHistorySize;
    float maxHistoryRecordFrequency;
 
    int currentId;
    std::mutex idMutex;
 
    struct FilterData : IceUtil::Shared
    {
        DatafieldFilterBasePtr filter;
        DatafieldRefPtr original;
        DatafieldRefPtr filtered;
    };
 
    using FilterDataPtr = IceUtil::Handle<FilterData>;
 
    std::multimap<std::string, FilterDataPtr> orignalToFiltered;
    std::map<std::string, FilterDataPtr> filteredToOriginal;
    std::recursive_mutex filterMutex;
    std::map<std::string, IceUtil::Time> channelUpdateTimestamps;
    RunningTask<Observer>::pointer_type filterUpdateTask;
 
    struct FilterQueueData
    {
        VariantBasePtr value;
        std::string channelName;
        std::string datafieldName;
    };
 
    RunningTask<Observer>::pointer_type channelUpdateTask;
    std::map<std::string, std::set<std::string>> channelQueue;
    std::mutex channelQueueMutex;
    std::condition_variable idleChannelCondition;
 
    using FilterUpdateQueue = std::unordered_map<std::string, FilterQueueData>;
    FilterUpdateQueue filterQueue;
    std::mutex filterQueueMutex;
    std::condition_variable idleCondition;
 
    std::recursive_mutex workerUpdatesMutex;
 
    struct WorkerUpdate
    {
        WorkerUpdate(const std::string& name, std::function<void(void)> f);
        float ageInMs() const;
 
        std::string name;
        TimepointT time;
        std::function<void(void)> f;
    };
 
    mutable std::deque<WorkerUpdate> workerUpdates;
    std::thread worker;
    std::atomic_bool stopWorker;
};
 
Observer::Observer() : impl(new Impl)
{
}
 
Observer::~Observer()
{
}
 
// *******************************************************
// offering of channels, datafield, and checks
// *******************************************************
void
Observer::offerChannel(std::string channelName, std::string description)
{
    if (getState() < eManagedIceObjectInitialized)
    {
        throw LocalException()
            << "offerChannel() must not be called before the Observer is initalized (i.e. not in "
               "onInitObserver(), use onConnectObserver()";
    }
 
    std::unique_lock lock(channelsMutex);
 
    if (impl->channelRegistry.find(channelName) != impl->channelRegistry.end())
    {
        throw exceptions::user::InvalidChannelException(channelName);
    }
 
    ChannelRegistryEntry channel;
    channel.name = channelName;
    channel.description = description;
    channel.initialized = false;
 
    std::pair<std::string, ChannelRegistryEntry> entry;
    entry.first = channelName;
    entry.second = channel;
 
    impl->channelRegistry.insert(entry);
}
 
void
Observer::offerDataFieldWithDefault(std::string channelName,
                                    std::string datafieldName,
                                    const Variant& defaultValue,
                                    std::string description)
{
    if (getState() < eManagedIceObjectInitialized)
    {
        throw LocalException()
            << "offerDataFieldWithDefault() must not be called before the Observer is initalized "
               "(i.e. not in onInitObserver(), use onConnectObserver()";
    }
 
    std::unique_lock lock(channelsMutex);
 
    auto channelIt = impl->channelRegistry.find(channelName);
 
    if (channelIt == impl->channelRegistry.end())
    {
        throw exceptions::user::InvalidChannelException(channelName);
    }
 
    if (channelIt->second.dataFields.find(datafieldName) != channelIt->second.dataFields.end())
    {
        throw exceptions::user::DatafieldExistsAlreadyException(channelName, datafieldName);
    }
 
    DataFieldRegistryEntry dataField;
    dataField.identifier = new DatafieldRef(this, channelName, datafieldName, false);
    dataField.description = description;
    dataField.typeName = defaultValue.getTypeName();
    dataField.value = new TimedVariant();
    *dataField.value = defaultValue;
 
    std::pair<std::string, DataFieldRegistryEntry> entry;
    entry.first = datafieldName;
    entry.second = dataField;
 
    channelIt->second.dataFields.insert(entry);
}
 
void
Observer::offerDataField(std::string channelName,
                         std::string datafieldName,
                         VariantTypeId type,
                         std::string description)
{
    if (getState() < eManagedIceObjectInitialized)
    {
        throw LocalException()
            << "offerDataField() must not be called before the Observer is initalized (i.e. not in "
               "onInitObserver(), use onConnectObserver()";
    }
 
    std::unique_lock lock(channelsMutex);
 
    auto channelIt = impl->channelRegistry.find(channelName);
 
    if (channelIt == impl->channelRegistry.end())
    {
        throw exceptions::user::InvalidChannelException(channelName);
    }
 
    if (channelIt->second.dataFields.find(datafieldName) != channelIt->second.dataFields.end())
    {
        throw exceptions::user::InvalidDataFieldException(channelName, datafieldName);
    }
 
    DataFieldRegistryEntry dataField;
    dataField.identifier = new DatafieldRef(this, channelName, datafieldName, false);
    dataField.description = description;
    dataField.typeName = Variant::typeToString(type);
    dataField.value = new TimedVariant();
    dataField.value->setType(type);
 
    std::pair<std::string, DataFieldRegistryEntry> entry;
    entry.first = datafieldName;
    entry.second = dataField;
 
    impl->channelRegistry[channelName].dataFields.insert(entry);
}
 
bool
Observer::offerOrUpdateDataField(std::string channelName,
                                 std::string datafieldName,
                                 const Variant& value,
                                 const std::string& description)
{
    if (!existsDataField(channelName, datafieldName))
    {
        offerDataFieldWithDefault(channelName, datafieldName, value, description);
        return true;
    }
    else
    {
        setDataField(channelName, datafieldName, value);
        return false;
    }
}
 
void
Observer::offerOrUpdateDataFieldsFlatCopy(const std::string& channelName,
                                          const StringVariantBaseMap& valueMap)
{
    try
    {
        // fastest way is to just try to set the datafields
        maybeOfferChannelAndSetDataFieldsFlatCopy(channelName, "", valueMap, true);
    }
    catch (exceptions::user::InvalidDataFieldException& e)
    {
        ARMARX_INFO << deactivateSpam() << "Creating datafields for channel " << channelName;
        // on failure: do it the slow way
        ARMARX_DEBUG << deactivateSpam(1) << "failure when seting datafield "
                     << "using slow fallback";
        for (const auto& value : valueMap)
        {
            const std::string& datafieldName = value.first;
            if (!existsDataField(channelName, datafieldName))
            {
                try
                {
                    offerDataFieldWithDefault(
                        channelName, datafieldName, *VariantPtr::dynamicCast(value.second), "");
                }
                catch (exceptions::user::DatafieldExistsAlreadyException& e)
                {
                    setDataFieldFlatCopy(
                        channelName, datafieldName, VariantPtr::dynamicCast(value.second));
                }
            }
            else
            {
                setDataFieldFlatCopy(
                    channelName, datafieldName, VariantPtr::dynamicCast(value.second));
            }
        }
    }
    updateChannel(channelName);
}
 
void
Observer::offerConditionCheck(std::string checkName, ConditionCheck* conditionCheck)
{
    std::unique_lock lock(impl->checksMutex);
 
    if (impl->availableChecks.find(checkName) != impl->availableChecks.end())
    {
        throw InvalidCheckException(checkName);
    }
 
    std::pair<std::string, ConditionCheckPtr> entry;
    entry.first = checkName;
    entry.second = conditionCheck;
 
    impl->availableChecks.insert(entry);
}
 
void
Observer::removeChannel(std::string channelName)
{
    {
        std::unique_lock lock(channelsMutex);
 
        auto iter = impl->channelRegistry.find(channelName);
 
        if (iter == impl->channelRegistry.end())
        {
            return;
        }
 
        DataFieldIdentifierPtr id =
            new DataFieldIdentifier(getName(), LAST_REFRESH_DELTA_CHANNEL, channelName);
        removeDatafield(id);
        impl->channelRegistry.erase(iter);
        ARMARX_INFO << "Removed channel " << channelName;
    }
 
    {
        std::unique_lock lock(impl->historyMutex);
        impl->channelHistory.erase(channelName);
    }
}
 
void
Observer::removeDatafield(DataFieldIdentifierBasePtr id)
{
    {
        std::unique_lock lock(impl->filterMutex);
        // Remove filter
        DataFieldIdentifierPtr idptr = DataFieldIdentifierPtr::dynamicCast(id);
        std::string idStr = idptr->getIdentifierStr();
        auto itFilter = impl->filteredToOriginal.find(idStr);
 
        if (itFilter != impl->filteredToOriginal.end())
        {
            DatafieldRefPtr refPtr = DatafieldRefPtr::dynamicCast(itFilter->second->original);
            auto range = impl->orignalToFiltered.equal_range(
                refPtr->getDataFieldIdentifier()->getIdentifierStr());
 
            for (auto it = range.first; it != range.second; it++)
                if (it->second->filtered->getDataFieldIdentifier()->getIdentifierStr() == idStr)
                {
                    impl->orignalToFiltered.erase(it);
                    break;
                }
 
            impl->filteredToOriginal.erase(itFilter);
        }
    }
 
    std::unique_lock lock(channelsMutex);
 
    auto itChannel = impl->channelRegistry.find(id->channelName);
 
    if (itChannel == impl->channelRegistry.end())
    {
        return;
    }
 
    itChannel->second.dataFields.erase(id->datafieldName);
}
 
// *******************************************************
// utility methods for sensordatalistener
// *******************************************************
std::set<std::string>
Observer::updateDatafieldFilter(const std::string& channelName,
                                const std::string& datafieldName,
                                const VariantBasePtr& value)
{
    std::vector<Impl::FilterQueueData> filterData;
    std::set<std::string> foundFilterFields;
    {
        std::unique_lock lock(impl->filterMutex);
        const std::string id = getName() + "." + channelName + "." + datafieldName;
        //    DatafieldRefPtr ref = new DatafieldRef(this, channelName, datafieldName);
        auto range = impl->orignalToFiltered.equal_range(id);
 
        //IceUtil::Time start = IceUtil::Time::now();
        //bool found = false;
        TimedVariantPtr origTV = TimedVariantPtr::dynamicCast(value);
        auto t = origTV ? origTV->getTime() : TimeUtil::GetTime();
        long tLong = t.toMicroSeconds();
 
        for (auto it = range.first; it != range.second; it++)
        {
            it->second->filter->update(tLong, value);
 
            foundFilterFields.insert(it->second->filtered->datafieldName);
 
            TimedVariantPtr tv =
                new TimedVariant(VariantPtr::dynamicCast(it->second->filter->getValue()), t);
            filterData.emplace_back(
                Impl::FilterQueueData{tv,
                                      it->second->filtered->channelRef->channelName,
                                      it->second->filtered->datafieldName});
            //found = true;
        }
    }
 
    for (auto& elem : filterData)
    {
        setDataFieldFlatCopy(
            elem.channelName, elem.datafieldName, VariantPtr::dynamicCast(elem.value));
    }
 
    /*if(found)
    {
        IceUtil::Time end = IceUtil::Time::now();
        IceUtil::Time duration = end - start;
        ARMARX_IMPORTANT << deactivateSpam(0.1f) << channelName << ":" << datafieldName << ": all filters calc microseconds: " << duration.toMicroSeconds();
    }*/
 
    return foundFilterFields;
}
 
void
Observer::scheduleDatafieldFilterUpdate(const std::string& channelName,
                                        const std::string& datafieldName,
                                        const VariantBasePtr& value)
{
    if (impl->orignalToFiltered.size() == 0)
    {
        return; // no filters installed anyway ...nothing todo
    }
    const std::string id = getName() + "." + channelName + "." + datafieldName;
    {
        std::unique_lock lock(impl->filterMutex);
        if (impl->orignalToFiltered.count(id) == 0)
        {
            return; // no filter for this datafield installed ...nothing todo
        }
    }
    std::unique_lock lock(impl->filterQueueMutex);
    impl->filterQueue[id] = {value, channelName, datafieldName};
    impl->idleCondition.notify_all();
}
 
void
Observer::updateFilters()
{
    while (!impl->filterUpdateTask->isStopped())
    {
        Impl::FilterUpdateQueue queue;
        {
            std::unique_lock lock(impl->filterQueueMutex);
            queue.swap(impl->filterQueue);
        }
        std::unordered_map<std::string, std::set<std::string>> channels;
        for (const Impl::FilterUpdateQueue::value_type& elem : queue)
        {
            auto foundFilterFields = updateDatafieldFilter(
                elem.second.channelName, elem.second.datafieldName, elem.second.value);
            channels[elem.second.channelName].insert(foundFilterFields.begin(),
                                                     foundFilterFields.end());
        }
        for (const auto& channel : channels)
        {
            if (channel.second.size() > 0)
            {
                updateChannel(channel.first, channel.second);
            }
        }
        std::unique_lock lock(impl->filterQueueMutex);
        if (impl->filterQueue.size() == 0)
        {
            impl->idleCondition.wait(lock);
        }
    }
}
 
void
Observer::setDataFieldFlatCopy(const DataFieldRegistry::iterator& dataFieldIter,
                               const VariantPtr& value)
{
    ARMARX_CHECK_EXPRESSION(value) << "Datafieldvariant is NULL!";
    TimedVariantPtr tval = TimedVariantPtr::dynamicCast(value);
    if (tval)
    {
        dataFieldIter->second.value = std::move(tval);
    }
    else
    {
        dataFieldIter->second.value = new TimedVariant(value, TimeUtil::GetTime());
    }
}
 
void
Observer::setDataField(const std::string& channelName,
                       const std::string& datafieldName,
                       const Variant& value,
                       bool triggerFilterUpdate)
{
    VariantBasePtr valuePtr;
    {
        std::unique_lock lock(channelsMutex);
        auto itChannel = impl->channelRegistry.find(channelName);
 
        if (itChannel == impl->channelRegistry.end())
        {
            throw exceptions::user::InvalidChannelException(channelName);
        }
 
        DataFieldRegistry::iterator itDF = itChannel->second.dataFields.find(datafieldName);
 
        if (itDF == itChannel->second.dataFields.end())
        {
            throw exceptions::user::InvalidDataFieldException(channelName, datafieldName);
        }
 
        if (dynamic_cast<const TimedVariant*>(&value))
        {
            itDF->second.value = value.clone();
        }
        else
        {
            itDF->second.value = new TimedVariant(value, TimeUtil::GetTime());
        }
        valuePtr = itDF->second.value;
    }
    if (triggerFilterUpdate)
    {
        scheduleDatafieldFilterUpdate(channelName, datafieldName, valuePtr);
    }
    //*dataFieldValue = value;
}
 
void
Observer::setDataFieldFlatCopy(const std::string& channelName,
                               const std::string& datafieldName,
                               const VariantPtr& value,
                               bool triggerFilterUpdate)
{
    {
        std::unique_lock lock(channelsMutex);
 
        auto itChannel = impl->channelRegistry.find(channelName);
 
        if (itChannel == impl->channelRegistry.end())
        {
            throw exceptions::user::InvalidChannelException(channelName);
        }
 
        DataFieldRegistry::iterator itDF = itChannel->second.dataFields.find(datafieldName);
 
        if (itDF == itChannel->second.dataFields.end())
        {
            throw exceptions::user::InvalidDataFieldException(channelName, datafieldName);
        }
 
        setDataFieldFlatCopy(itDF, value);
    }
 
    if (triggerFilterUpdate)
    {
        scheduleDatafieldFilterUpdate(channelName, datafieldName, value);
    }
}
 
void
Observer::updateDatafieldTimestamps(
    const std::string& channelName,
    const std::unordered_map<std::string, Ice::Long>& datafieldValues)
{
    std::unique_lock lock(channelsMutex);
 
    auto itChannel = impl->channelRegistry.find(channelName);
 
    if (itChannel == impl->channelRegistry.end())
    {
        throw exceptions::user::InvalidChannelException(channelName);
    }
 
    for (const auto& elem : datafieldValues)
    {
        DataFieldRegistry::iterator itDF = itChannel->second.dataFields.find(elem.first);
 
        if (itDF == itChannel->second.dataFields.end())
        {
            throw exceptions::user::InvalidDataFieldException(channelName, elem.first);
        }
 
 
        TimedVariantPtr oldValue = TimedVariantPtr::dynamicCast(itDF->second.value);
        if (!oldValue || (elem.second > oldValue->getTimestamp() && oldValue->getInitialized()))
        {
            itDF->second.value = new TimedVariant(VariantPtr::dynamicCast(itDF->second.value),
                                                  IceUtil::Time::microSeconds(elem.second));
        }
    }
}
 
void
Observer::updateDatafieldTimestamps(const std::string& channelName, Ice::Long timestamp)
{
    std::unique_lock lock(channelsMutex);
 
    auto itChannel = impl->channelRegistry.find(channelName);
 
    if (itChannel == impl->channelRegistry.end())
    {
        throw exceptions::user::InvalidChannelException(channelName);
    }
 
    for (auto& elem : itChannel->second.dataFields)
    {
 
        TimedVariantPtr oldValue = TimedVariantPtr::dynamicCast(elem.second.value);
        if (!oldValue || (timestamp > oldValue->getTimestamp() && oldValue->getInitialized()))
        {
            elem.second.value = new TimedVariant(VariantPtr::dynamicCast(elem.second.value),
                                                 IceUtil::Time::microSeconds(timestamp));
        }
    }
}
 
void
Observer::maybeOfferChannelAndSetDataFieldsFlatCopy(const std::string& channelName,
                                                    const std::string& description,
                                                    const StringVariantBaseMap& datafieldValues,
                                                    bool triggerFilterUpdate)
{
    {
        std::lock_guard lock(channelsMutex);
        auto itChannel = impl->channelRegistry.find(channelName);
        if (itChannel == impl->channelRegistry.end())
        {
            //add
            auto& channel = impl->channelRegistry[channelName];
            channel.name = channelName;
            channel.description = description;
            channel.initialized = false;
 
            itChannel = impl->channelRegistry.find(channelName);
        }
 
        //use map merge algorithm since it is faster (n+m instead n*log(m))
        auto targ = itChannel->second.dataFields.begin();
        auto vals = datafieldValues.begin();
        while (targ != itChannel->second.dataFields.end() && vals != datafieldValues.end())
        {
            if (vals->first > targ->first)
            {
                ++targ;
            }
            else if (vals->first == targ->first)
            {
                setDataFieldFlatCopy(targ, VariantPtr::dynamicCast(vals->second));
                ++vals;
                ++targ;
            }
            else
            {
                throw exceptions::user::InvalidDataFieldException(channelName, vals->first);
            }
        }
        if (vals != datafieldValues.end())
        {
            throw exceptions::user::InvalidDataFieldException(channelName, vals->first);
        }
    }
    if (triggerFilterUpdate)
    {
        for (const auto& elem : datafieldValues)
        {
            scheduleDatafieldFilterUpdate(channelName, elem.first, elem.second);
        }
    }
}
 
void
Observer::setDataFieldsFlatCopy(
    const std::string& channelName,
    const std::unordered_map<::std::string, ::armarx::VariantBasePtr>& datafieldValues,
    bool triggerFilterUpdate)
{
    {
        std::unique_lock lock(channelsMutex);
 
        auto itChannel = impl->channelRegistry.find(channelName);
 
        if (itChannel == impl->channelRegistry.end())
        {
            throw exceptions::user::InvalidChannelException(channelName);
        }
 
        for (const auto& elem : datafieldValues)
        {
            DataFieldRegistry::iterator itDF = itChannel->second.dataFields.find(elem.first);
 
            if (itDF == itChannel->second.dataFields.end())
            {
                throw exceptions::user::InvalidDataFieldException(channelName, elem.first);
            }
 
            setDataFieldFlatCopy(itDF, VariantPtr::dynamicCast(elem.second));
        }
    }
    if (triggerFilterUpdate)
    {
        for (const auto& elem : datafieldValues)
        {
            scheduleDatafieldFilterUpdate(channelName, elem.first, elem.second);
        }
    }
}
 
void
Observer::setDataFieldsFlatCopy(const std::string& channelName,
                                const StringVariantBaseMap& datafieldValues,
                                bool triggerFilterUpdate)
{
    {
        std::unique_lock lock(channelsMutex);
 
        auto itChannel = impl->channelRegistry.find(channelName);
 
        if (itChannel == impl->channelRegistry.end())
        {
            throw exceptions::user::InvalidChannelException(channelName);
        }
 
        for (const auto& elem : datafieldValues)
        {
            DataFieldRegistry::iterator itDF = itChannel->second.dataFields.find(elem.first);
 
            if (itDF == itChannel->second.dataFields.end())
            {
                throw exceptions::user::InvalidDataFieldException(channelName, elem.first);
            }
            ARMARX_CHECK_EXPRESSION(elem.second) << "Datafieldname: " << elem.first;
            setDataFieldFlatCopy(itDF, VariantPtr::dynamicCast(elem.second));
        }
    }
 
    if (triggerFilterUpdate)
    {
        for (const auto& elem : datafieldValues)
        {
            scheduleDatafieldFilterUpdate(channelName, elem.first, elem.second);
        }
    }
}
 
void
Observer::updateRefreshRateChannel(const std::string& channelName)
{
    auto& oldUpdateTime = impl->channelUpdateTimestamps[channelName];
    auto now = IceUtil::Time::now();
 
    try
    {
        setDataFieldFlatCopy(LAST_REFRESH_DELTA_CHANNEL,
                             channelName,
                             new Variant((now - oldUpdateTime).toMilliSecondsDouble()),
                             false);
    }
    catch (exceptions::user::InvalidDataFieldException& e)
    {
        offerDataFieldWithDefault(LAST_REFRESH_DELTA_CHANNEL,
                                  channelName,
                                  0.0,
                                  "Update delta of channel '" + channelName + "'");
    }
    oldUpdateTime = now;
}
 
void
Observer::updateChannel(const std::string& channelName,
                        const std::set<std::string>& updatedDatafields)
{
    if (!existsChannel(channelName))
    {
        throw exceptions::user::InvalidChannelException(channelName);
    }
    std::unique_lock lock(impl->channelQueueMutex);
    std::set<std::string>& dfs = impl->channelQueue[channelName];
    dfs.insert(updatedDatafields.begin(), updatedDatafields.end());
    impl->idleChannelCondition.notify_all();
}
 
void
Observer::channelUpdateFunction()
{
 
    while (!impl->channelUpdateTask->isStopped())
    {
        std::map<std::string, std::set<std::string>> queue;
        {
            std::unique_lock lock(impl->channelQueueMutex);
            queue.swap(impl->channelQueue);
        }
        for (const auto& elem : queue)
        {
            doChannelUpdate(elem.first, elem.second);
        }
        std::unique_lock lock(impl->channelQueueMutex);
        if (impl->channelQueue.size() == 0)
        {
            impl->idleChannelCondition.wait(lock);
        }
    }
}
 
void
Observer::addToChannelHistory(const std::pair<IceUtil::Time, ChannelRegistryEntry>& historyEntry,
                              const std::string& channelName)
{
    if (impl->maxHistorySize == 0)
    {
        return;
    }
    std::unique_lock lockHistory(impl->historyMutex);
    auto historyIt = impl->channelHistory.find(channelName);
    if (historyIt == impl->channelHistory.end())
    {
        impl->channelHistory[channelName].set_capacity(impl->maxHistorySize);
        historyIt = impl->channelHistory.find(channelName);
    }
    auto now = TimeUtil::GetTime();
    if (historyIt->second.empty() ||
        now > historyIt->second.rbegin()->first +
                  IceUtil::Time::secondsDouble(1.0 / impl->maxHistoryRecordFrequency))
    {
        historyIt->second.push_back(historyEntry);
    }
}
 
void
Observer::doChannelUpdate(const std::string& channelName,
                          const std::set<std::string>& updatedDatafields)
{
 
 
    try
    {
        ChannelRegistryEntry entry;
        std::pair<IceUtil::Time, ChannelRegistryEntry> historyEntry;
        {
            std::unique_lock lock_channels(channelsMutex);
            // check if channels exists
            auto iterChannel = impl->channelRegistry.find(channelName);
 
            if (iterChannel == impl->channelRegistry.end())
            {
                throw exceptions::user::InvalidChannelException(channelName);
            }
 
 
            if (impl->logChannelUpdateRate)
            {
                updateRefreshRateChannel(channelName);
            }
 
            // update initialized state
            const DataFieldRegistry& dataFields = iterChannel->second.dataFields;
            DataFieldRegistry::const_iterator iterDataFields = dataFields.begin();
            bool channelInitialized = true;
 
            while (iterDataFields != dataFields.end())
            {
                channelInitialized &= iterDataFields->second.value->getInitialized();
                iterDataFields++;
            }
 
            iterChannel->second.initialized = channelInitialized;
            if (iterChannel->second.conditionChecks.size() > 0)
            {
                entry = iterChannel->second;
            }
            historyEntry = std::make_pair(TimeUtil::GetTime(), iterChannel->second);
        }
        addToChannelHistory(historyEntry, channelName);
        // evaluate checks
        ConditionCheckRegistry::iterator iterChecks = entry.conditionChecks.begin();
 
        while (iterChecks != entry.conditionChecks.end())
        {
            bool found = false;
            if (updatedDatafields.size() > 0)
            {
                // check if this check belongs to an updated datafield.
                for (const auto& elem : entry.dataFields)
                {
                    const DataFieldRegistryEntry& datafieldEntry = elem.second;
                    if (updatedDatafields.count(datafieldEntry.identifier->datafieldName))
                    {
                        found = true;
                        break;
                    }
                }
            }
            else
            {
                found = true;
            }
 
            try
            {
                if (found)
                {
                    evaluateCheck(ConditionCheckPtr::dynamicCast(iterChecks->second), entry);
                }
            }
            catch (...)
            {
                ARMARX_ERROR << "Evaluating condition for channel " << channelName << " failed!";
                handleExceptions();
            }
 
            iterChecks++;
        }
    }
    catch (...)
    {
        //        ARMARX_ERROR << "Upating channel " << channelName << " failed!";
        //        handleExceptions();
    }
}
 
// *******************************************************
// Component hooks
// *******************************************************
void
Observer::onInitComponent()
{
    impl->maxHistorySize = getProperty<int>("MaxHistorySize");
    impl->maxHistoryRecordFrequency = getProperty<float>("MaxHistoryRecordFrequency");
 
    {
        std::unique_lock lock(impl->idMutex);
        impl->currentId = 0;
    }
 
    onInitObserver();
    impl->filterUpdateTask =
        new RunningTask<Observer>(this, &Observer::updateFilters, "Filter update task");
    impl->filterUpdateTask->start();
    impl->channelUpdateTask =
        new RunningTask<Observer>(this, &Observer::channelUpdateFunction, "Channel update task");
    impl->channelUpdateTask->start();
}
 
void
Observer::onConnectComponent()
{
    impl->logChannelUpdateRate = getProperty<bool>("CreateUpdateFrequenciesChannel").getValue();
    if (impl->logChannelUpdateRate && !existsChannel(LAST_REFRESH_DELTA_CHANNEL))
    {
        offerChannel(
            LAST_REFRESH_DELTA_CHANNEL,
            "Metachannel with the last channel update deltas of all channels in milliseconds");
    }
    impl->channelHistory.clear();
    // subclass init
    onConnectObserver();
    //    offerDataFieldWithDefault(channelName, LAST_REFRESH_DELTA_CHANNEL, 0.0, );
    //    impl->channelUpdateTimestamps[channelName] = IceUtil::Time::now();
 
    std::unique_lock lock(channelsMutex);
    auto proxy = ObserverInterfacePrx::checkedCast(getProxy());
    // update the proxy in all existing datafield refs
    for (auto& channel : impl->channelRegistry)
    {
        ChannelRegistryEntry& c = channel.second;
        for (auto& df : c.dataFields)
        {
            DataFieldRegistryEntry& d = df.second;
            d.identifier->channelRef->observerProxy = proxy;
        }
    }
    impl->metaTask = new PeriodicTask<Observer>(this, &Observer::metaUpdateTask, 50);
    if (impl->logChannelUpdateRate)
    {
        impl->metaTask->start();
    }
}
 
void
Observer::postOnConnectComponent()
{
    ARMARX_INFO << "starting worker thread";
    impl->stopWorker = false;
    ARMARX_CHECK_EXPRESSION(!impl->worker.joinable());
    impl->worker = std::thread{[&] { runWorker(); }};
}
 
void
Observer::preOnDisconnectComponent()
{
    ARMARX_INFO << "stopping worker thread";
    impl->stopWorker = true;
    if (impl->worker.joinable())
    {
        impl->worker.join();
    }
}
 
void
Observer::onExitComponent()
{
    onExitObserver();
    if (impl->metaTask)
    {
        impl->metaTask->stop();
    }
    if (impl->filterUpdateTask)
    {
        impl->filterUpdateTask->stop(false);
    }
    impl->idleCondition.notify_all();
    if (impl->filterUpdateTask)
    {
        impl->filterUpdateTask->stop(true);
    }
    if (impl->channelUpdateTask)
    {
        impl->channelUpdateTask->stop(false);
    }
    impl->idleChannelCondition.notify_all();
    if (impl->channelUpdateTask)
    {
        impl->channelUpdateTask->stop(true);
    }
    impl->channelHistory.clear();
}
 
PropertyDefinitionsPtr
Observer::createPropertyDefinitions()
{
    return PropertyDefinitionsPtr(new ObserverPropertyDefinitions(getConfigIdentifier()));
}
 
void
Observer::metaUpdateTask()
{
    updateChannel(LAST_REFRESH_DELTA_CHANNEL);
}
 
// *******************************************************
// private methods
// *******************************************************
int
Observer::generateId()
{
    std::unique_lock lock(impl->idMutex);
 
    return impl->currentId++;
}
 
ConditionCheckPtr
Observer::createCheck(const CheckConfiguration& configuration) const
{
    std::string checkName = configuration.checkName;
 
    // create check from elementary condition
    StringConditionCheckMap::const_iterator iterChecks = impl->availableChecks.find(checkName);
 
    if (iterChecks == impl->availableChecks.end())
    {
        std::string reason =
            "Invalid condition check \"" + checkName + "\" for observer \"" + getName() + "\".";
        throw InvalidConditionException(reason.c_str());
    }
 
    ARMARX_CHECK_EXPRESSION(iterChecks->second);
    ConditionCheckPtr check = ConditionCheckPtr::dynamicCast(iterChecks->second)
                                  ->createInstance(configuration, impl->channelRegistry);
 
    return check;
}
 
CheckIdentifier
Observer::registerCheck(const ConditionCheckPtr& check)
{
    ARMARX_CHECK_EXPRESSION(check);
    ARMARX_CHECK_EXPRESSION(check->configuration.dataFieldIdentifier);
    // create identifier
    int id = generateId();
    CheckIdentifier identifier;
    identifier.uniqueId = id;
    identifier.channelName = check->configuration.dataFieldIdentifier->channelName;
    identifier.observerName = getName();
 
    // add to conditions list
    std::pair<int, ConditionCheckPtr> entry;
    entry.first = id;
    entry.second = check;
    impl->channelRegistry[check->configuration.dataFieldIdentifier->channelName]
        .conditionChecks.insert(entry);
 
    return identifier;
}
 
void
Observer::evaluateCheck(const ConditionCheckPtr& check, const ChannelRegistryEntry& channel) const
{
    check->evaluateCondition(channel.dataFields);
}
 
// ////////////////////////////////////////////////////////////////////////// //
void
Observer::runWorker()
{
    ARMARX_INFO << "observer worker->start";
    ARMARX_ON_SCOPE_EXIT
    {
        ARMARX_INFO << "observer worker->stop";
    };
    std::deque<Impl::WorkerUpdate> toProcess;
    while (!impl->stopWorker)
    {
        preWorkerJobs();
        ARMARX_ON_SCOPE_EXIT
        {
            postWorkerJobs();
        };
        {
            std::lock_guard g{impl->workerUpdatesMutex};
            std::swap(impl->workerUpdates, toProcess);
        }
        if (toProcess.empty())
        {
            ARMARX_DEBUG << deactivateSpam()
                         << "no worker jobs (message only posted every 10 seconds";
            std::this_thread::sleep_for(std::chrono::milliseconds{1});
            continue;
        }
 
        float minage = std::numeric_limits<float>::infinity();
        float maxage = 0;
 
        struct Counters
        {
            float age = 0;
            float time = 0;
            std::size_t n = 0;
        };
 
        std::map<std::string, Counters> counters;
        {
            std::lock_guard lock(channelsMutex);
            for (auto& f : toProcess)
            {
                ARMARX_TRACE;
                float age = f.ageInMs();
                Counters& counter = counters[f.name];
                counter.age += age;
                ++counter.n;
                TimepointT start = Now();
                f.f();
                counter.time += TimeDeltaInMs(start);
                minage = std::min(minage, age);
                maxage = std::max(maxage, age);
            }
        }
        if (maxage <= 0)
        {
            minage = 0;
        }
        ARMARX_DEBUG << "observer worker got " << toProcess.size() << "\tjobs ages min/max "
                     << minage << "\t" << maxage << "\n"
                     << ARMARX_STREAM_PRINTER
        {
            for (const auto& [key, c] : counters)
            {
                out << "    " << key << "\t# " << c.n << "\tavg age " << (c.age / c.n)
                    << "\tavg time " << (c.time / c.n) << "\n";
            }
        };
        toProcess.clear();
    }
}
 
void
Observer::addWorkerJob(const std::string& name, std::function<void(void)>&& f) const
{
    std::lock_guard g{impl->workerUpdatesMutex};
    impl->workerUpdates.emplace_back(name, std::move(f));
}
 
void
Observer::addWorkerJob(const std::string& name, std::function<void(void)>&& f)
{
    std::lock_guard g{impl->workerUpdatesMutex};
    impl->workerUpdates.emplace_back(name, std::move(f));
}
 
void
Observer::preWorkerJobs()
{
}
 
void
Observer::postWorkerJobs()
{
}
 
Observer::TimepointT
Observer::Now()
{
    return ClockT::now();
}
 
float
Observer::TimeDeltaInMs(Observer::TimepointT t0)
{
    return std::chrono::duration_cast<std::chrono::nanoseconds>(Now() - t0).count() * 1e-6f;
}
 
// ////////////////////////////////////////////////////////////////////////// //
 
std::string
Observer::getObserverName() const
{
    return getName();
}
 
void
Observer::getObserverName_async(const AMD_ObserverInterface_getObserverNamePtr& amd,
                                const Ice::Current&) const
{
    addWorkerJob("Observer::getObserverName", amd, &Observer::getObserverName);
}
 
// //////////////////
CheckIdentifier
Observer::installCheck(const CheckConfiguration& configuration)
{
    // create condition check
    ConditionCheckPtr check;
    {
        std::unique_lock lock_checks(impl->checksMutex);
        std::unique_lock lock_channels(channelsMutex);
        check = createCheck(configuration);
    }
    ARMARX_CHECK_EXPRESSION(check);
    // insert into registry
    CheckIdentifier identifier;
    {
        std::unique_lock lock_conditions(channelsMutex);
        identifier = registerCheck(check);
    }
 
    ARMARX_DEBUG << "Installed check " << identifier.uniqueId << flush;
 
    // perform initial check
    {
        std::unique_lock lock_channels(channelsMutex);
        ARMARX_CHECK_EXPRESSION(configuration.dataFieldIdentifier);
        // retrieve channel
        ChannelRegistryEntry channel =
            impl->channelRegistry[configuration.dataFieldIdentifier->channelName];
 
        // evaluate
        evaluateCheck(check, channel);
    }
 
    return identifier;
}
 
void
Observer::installCheck_async(const AMD_ObserverInterface_installCheckPtr& amd,
                             const CheckConfiguration& configuration,
                             const Ice::Current&)
{
    addWorkerJob("Observer::installCheck", amd, &Observer::installCheck, configuration);
}
 
// //////////////////
void
Observer::removeCheck(const CheckIdentifier& id)
{
    std::unique_lock lock(channelsMutex);
 
    // find channel
    auto iter = impl->channelRegistry.find(id.channelName);
 
    if (iter == impl->channelRegistry.end())
    {
        return;
    }
 
    // find condition
    ConditionCheckRegistry::iterator iterCheck = iter->second.conditionChecks.find(id.uniqueId);
 
    // remove condition
    if (iterCheck != iter->second.conditionChecks.end())
    {
        iter->second.conditionChecks.erase(iterCheck);
    }
 
    ARMARX_DEBUG << "Removed check " << id.uniqueId << flush;
}
 
void
Observer::removeCheck_async(const AMD_ObserverInterface_removeCheckPtr& amd,
                            const CheckIdentifier& id,
                            const Ice::Current&)
{
    addWorkerJob("Observer::removeCheck", amd, &Observer::removeCheck, id);
}
 
// //////////////////
TimedVariantBasePtr
Observer::getDataField(const DataFieldIdentifierBasePtr& identifier, const Ice::Current& c) const
{
    //ARMARX_DEBUG << BOOST_CURRENT_FUNCTION;
    return getDatafieldByName(identifier->channelName, identifier->datafieldName);
}
 
void
Observer::getDataField_async(const AMD_ObserverInterface_getDataFieldPtr& amd,
                             const DataFieldIdentifierBasePtr& identifier,
                             const Ice::Current& c) const
{
    addWorkerJob("Observer::getDataField", amd, &Observer::getDataField, identifier, c);
}
 
// //////////////////
TimedVariantBasePtr
Observer::getDatafieldByName(const std::string& channelName, const std::string& datafieldName) const
{
    //ARMARX_DEBUG << BOOST_CURRENT_FUNCTION;
    std::unique_lock lock(channelsMutex);
 
    auto it = impl->channelRegistry.find(channelName);
    if (it == impl->channelRegistry.end())
    {
        throw exceptions::user::InvalidChannelException(channelName);
    }
 
    auto itDF = it->second.dataFields.find(datafieldName);
    if (itDF == it->second.dataFields.end())
    {
        throw exceptions::user::InvalidDataFieldException(channelName, datafieldName);
    }
 
    //    VariantPtr var = VariantPtr::dynamicCast(impl->channelRegistry[identifier->channelName].dataFields[identifier->datafieldName].value);
    TimedVariantPtr tv = TimedVariantPtr::dynamicCast(itDF->second.value);
    if (!tv)
    {
        ARMARX_IMPORTANT << "could not cast timed variant: " << itDF->second.value->ice_id();
    }
    return tv;
}
 
void
Observer::getDatafieldByName_async(const AMD_ObserverInterface_getDatafieldByNamePtr& amd,
                                   const std::string& channelName,
                                   const std::string& datafieldName,
                                   const Ice::Current&) const
{
    addWorkerJob("Observer::getDatafieldByName",
                 amd,
                 &Observer::getDatafieldByName,
                 channelName,
                 datafieldName);
}
 
// //////////////////
DatafieldRefBasePtr
Observer::getDataFieldRef(const DataFieldIdentifierBasePtr& identifier) const
{
    //ARMARX_DEBUG << BOOST_CURRENT_FUNCTION;
    std::unique_lock lock(channelsMutex);
    auto it = impl->channelRegistry.find(identifier->channelName);
    if (it == impl->channelRegistry.end())
    {
        throw exceptions::user::InvalidChannelException(identifier->channelName);
    }
 
    auto itDF = it->second.dataFields.find(identifier->datafieldName);
    if (itDF == it->second.dataFields.end())
    {
        throw exceptions::user::InvalidDataFieldException(identifier->channelName,
                                                          identifier->datafieldName);
    }
    //    itDF->second.identifier->channelRef->observerProxy = ObserverInterfacePrx::uncheckedCast(getProxy());
    return DatafieldRefPtr::dynamicCast(itDF->second.identifier);
}
 
void
Observer::getDataFieldRef_async(const AMD_ObserverInterface_getDataFieldRefPtr& amd,
                                const DataFieldIdentifierBasePtr& identifier,
                                const Ice::Current&) const
{
    addWorkerJob("Observer::getDataFieldRef", amd, &Observer::getDataFieldRef, identifier);
}
 
// //////////////////
DatafieldRefBasePtr
Observer::getDatafieldRefByName(const std::string& channelName,
                                const std::string& datafieldName) const
{
    return getDataFieldRef(new DataFieldIdentifier(getName(), channelName, datafieldName));
}
 
void
Observer::getDatafieldRefByName_async(const AMD_ObserverInterface_getDatafieldRefByNamePtr& amd,
                                      const std::string& channelName,
                                      const std::string& datafieldName,
                                      const Ice::Current&) const
{
    addWorkerJob("Observer::getDatafieldRefByName",
                 amd,
                 &Observer::getDatafieldRefByName,
                 channelName,
                 datafieldName);
}
 
// //////////////////
TimedVariantBaseList
Observer::getDataFields(const DataFieldIdentifierBaseList& identifiers, const Ice::Current& c)
{
    TimedVariantBaseList result;
 
    DataFieldIdentifierBaseList::const_iterator iter = identifiers.begin();
 
    std::unique_lock lock(channelsMutex);
    while (iter != identifiers.end())
    {
        result.push_back(getDataField(*iter, c));
        iter++;
    }
 
    return result;
}
 
void
Observer::getDataFields_async(const AMD_ObserverInterface_getDataFieldsPtr& amd,
                              const DataFieldIdentifierBaseList& identifiers,
                              const Ice::Current& c)
{
    addWorkerJob("Observer::getDataFields", amd, &Observer::getDataFields, identifiers, c);
}
 
// //////////////////
StringTimedVariantBaseMap
Observer::getDatafieldsOfChannel(const std::string& channelName) const
{
    //ARMARX_DEBUG << BOOST_CURRENT_FUNCTION;
    StringTimedVariantBaseMap result;
    std::unique_lock lock(channelsMutex);
    auto it = impl->channelRegistry.find(channelName);
    if (it == impl->channelRegistry.end())
    {
        throw exceptions::user::InvalidChannelException(channelName);
    }
    DataFieldRegistry fields = it->second.dataFields;
    for (auto& field : fields)
    {
        result[field.first] = TimedVariantBasePtr::dynamicCast(field.second.value);
    }
    return result;
}
 
void
Observer::getDatafieldsOfChannel_async(const AMD_ObserverInterface_getDatafieldsOfChannelPtr& amd,
                                       const std::string& channelName,
                                       const Ice::Current&) const
{
    addWorkerJob(
        "Observer::getDatafieldsOfChannel", amd, &Observer::getDatafieldsOfChannel, channelName);
}
 
// //////////////////
ChannelRegistryEntry
Observer::getChannel(const std::string& channelName) const
{
    //ARMARX_DEBUG << BOOST_CURRENT_FUNCTION;
    std::unique_lock lock(channelsMutex);
    auto it = impl->channelRegistry.find(channelName);
    if (it == impl->channelRegistry.end())
    {
        throw exceptions::user::InvalidChannelException(channelName);
    }
    return it->second;
}
 
void
Observer::getChannel_async(const AMD_ObserverInterface_getChannelPtr& amd,
                           const std::string& channelName,
                           const Ice::Current&) const
{
    addWorkerJob("Observer::getChannel", amd, &Observer::getChannel, channelName);
}
 
// //////////////////
ChannelRegistry
Observer::getAvailableChannels(bool includeMetaChannels)
{
    //ARMARX_DEBUG << BOOST_CURRENT_FUNCTION;
    std::unique_lock lock(channelsMutex);
 
    ChannelRegistry result(impl->channelRegistry.begin(), impl->channelRegistry.end());
    if (!includeMetaChannels)
    {
        result.erase(LAST_REFRESH_DELTA_CHANNEL);
    }
    return result;
}
 
void
Observer::getAvailableChannels_async(const AMD_ObserverInterface_getAvailableChannelsPtr& amd,
                                     bool includeMetaChannels,
                                     const Ice::Current&)
{
    addWorkerJob("Observer::getAvailableChannels",
                 amd,
                 &Observer::getAvailableChannels,
                 includeMetaChannels);
}
 
// //////////////////
StringConditionCheckMap
Observer::getAvailableChecks()
{
    std::unique_lock lock(impl->checksMutex);
 
    return impl->availableChecks;
}
 
void
Observer::getAvailableChecks_async(const AMD_ObserverInterface_getAvailableChecksPtr& amd,
                                   const Ice::Current&)
{
    addWorkerJob("Observer::getAvailableChecks", amd, &Observer::getAvailableChecks);
}
 
// //////////////////
bool
Observer::existsChannel(const std::string& channelName) const
{
    //ARMARX_DEBUG << BOOST_CURRENT_FUNCTION;
    std::unique_lock lock(channelsMutex);
    return impl->channelRegistry.find(channelName) != impl->channelRegistry.end();
}
 
void
Observer::existsChannel_async(const AMD_ObserverInterface_existsChannelPtr& amd,
                              const std::string& channelName,
                              const Ice::Current&) const
{
    addWorkerJob("Observer::existsChannel", amd, &Observer::existsChannel, channelName);
}
 
// //////////////////
bool
Observer::existsDataField(const std::string& channelName, const std::string& datafieldName) const
{
    //ARMARX_DEBUG << BOOST_CURRENT_FUNCTION;
    std::unique_lock lock(channelsMutex);
    auto itChannel = impl->channelRegistry.find(channelName);
 
    if (itChannel == impl->channelRegistry.end())
    {
        return false;
    }
 
    return itChannel->second.dataFields.find(datafieldName) != itChannel->second.dataFields.end();
}
 
void
Observer::existsDataField_async(const AMD_ObserverInterface_existsDataFieldPtr& amd,
                                const std::string& channelName,
                                const std::string& datafieldName,
                                const Ice::Current&) const
{
    addWorkerJob(
        "Observer::existsDataField", amd, &Observer::existsDataField, channelName, datafieldName);
}
 
// //////////////////
DatafieldRefBasePtr
Observer::createFilteredDatafield(const DatafieldFilterBasePtr& filter,
                                  const DatafieldRefBasePtr& datafieldRef)
{
    //ARMARX_DEBUG << BOOST_CURRENT_FUNCTION;
    //    if( auto it = impl->orignalToFiltered.find(datafieldRef) != impl->orignalToFiltered.end())
    //    {
    //        return it->second.filtered;
    //    }
    ARMARX_CHECK_EXPRESSION(datafieldRef);
 
 
    std::string filteredName = datafieldRef->datafieldName + "_" + filter->ice_id();
 
 
    int i = 1;
 
    while (existsDataField(datafieldRef->channelRef->channelName, filteredName))
    {
        //        ARMARX_IMPORTANT << "Checking if datafield " << filteredName << " exists";
        filteredName =
            datafieldRef->datafieldName + "_" + filter->ice_id() + "_" + ValueToString(i);
        i++;
    }
 
    return createNamedFilteredDatafield(filteredName, filter, datafieldRef);
}
 
void
Observer::createFilteredDatafield_async(const AMD_ObserverInterface_createFilteredDatafieldPtr& amd,
                                        const DatafieldFilterBasePtr& filter,
                                        const DatafieldRefBasePtr& datafieldRef,
                                        const Ice::Current&)
{
    addWorkerJob("Observer::createFilteredDatafield",
                 amd,
                 &Observer::createFilteredDatafield,
                 filter,
                 datafieldRef);
}
 
// //////////////////
DatafieldRefBasePtr
Observer::createNamedFilteredDatafield(const std::string& filterDatafieldName,
                                       const DatafieldFilterBasePtr& filter,
                                       const DatafieldRefBasePtr& datafieldRef)
{
    //ARMARX_DEBUG << BOOST_CURRENT_FUNCTION;
    DatafieldRefPtr ref = DatafieldRefPtr::dynamicCast(datafieldRef);
    ARMARX_CHECK_EXPRESSION(ref);
    ARMARX_CHECK_EXPRESSION(filter);
    if (!filter->checkTypeSupport(ref->getDataField()->getType()))
    {
        auto types = filter->getSupportedTypes();
        std::string suppTypes = "supported types";
 
        for (auto t : types)
        {
            suppTypes += Variant::typeToString(t) + ", ";
        }
 
        ARMARX_WARNING << suppTypes;
        throw exceptions::user::UnsupportedTypeException(ref->getDataField()->getType());
    }
 
    // if filter with that name exists -> remove it
    if (existsDataField(datafieldRef->channelRef->channelName, filterDatafieldName))
    {
        DatafieldRefPtr filteredRef = new DatafieldRef(ref->getChannelRef(), filterDatafieldName);
        removeFilteredDatafield(filteredRef);
    }
 
    // calculate initial value
    auto var = ref->getDataField();
    filter->update(var->getTime().toMicroSeconds(), var);
 
    // create datafield for new filter
    offerDataFieldWithDefault(datafieldRef->channelRef->channelName,
                              filterDatafieldName,
                              *VariantPtr::dynamicCast(filter->getValue()),
                              "Filtered value of " +
                                  ref->getDataFieldIdentifier()->getIdentifierStr());
 
    //create and store the refs for the filters
    ChannelRefPtr channel = ChannelRefPtr::dynamicCast(ref->channelRef);
    channel->refetchChannel();
    DatafieldRefPtr filteredRef = new DatafieldRef(ref->getChannelRef(), filterDatafieldName);
    Impl::FilterDataPtr data = new Impl::FilterData;
    data->filter = filter;
    data->original = ref;
    data->filtered = filteredRef;
    std::unique_lock lock(impl->filterMutex);
    impl->orignalToFiltered.insert(
        std::make_pair(ref->getDataFieldIdentifier()->getIdentifierStr(), data));
    impl->filteredToOriginal[filteredRef->getDataFieldIdentifier()->getIdentifierStr()] = data;
    return filteredRef;
}
 
void
Observer::createNamedFilteredDatafield_async(
    const AMD_ObserverInterface_createNamedFilteredDatafieldPtr& amd,
    const std::string& filterDatafieldName,
    const DatafieldFilterBasePtr& filter,
    const DatafieldRefBasePtr& datafieldRef,
    const Ice::Current&)
{
    addWorkerJob("Observer::createNamedFilteredDatafield",
                 amd,
                 &Observer::createNamedFilteredDatafield,
                 filterDatafieldName,
                 filter,
                 datafieldRef);
}
 
// //////////////////
void
Observer::removeFilteredDatafield(const DatafieldRefBasePtr& datafieldRef)
{
    //ARMARX_DEBUG << BOOST_CURRENT_FUNCTION;
    bool remove = true;
    DatafieldRefPtr ref;
    {
        std::unique_lock lock(impl->filterMutex);
        ref = DatafieldRefPtr::dynamicCast(datafieldRef);
        const std::string idStr = ref->getDataFieldIdentifier()->getIdentifierStr();
        auto it = impl->filteredToOriginal.find(idStr);
        remove = (it != impl->filteredToOriginal.end());
    }
 
    if (remove && ref)
    {
        removeDatafield(ref->getDataFieldIdentifier());
    }
}
 
void
Observer::removeFilteredDatafield_async(const AMD_ObserverInterface_removeFilteredDatafieldPtr& amd,
                                        const DatafieldRefBasePtr& datafieldRef,
                                        const Ice::Current&)
{
    addWorkerJob(
        "Observer::removeFilteredDatafield", amd, &Observer::removeFilteredDatafield, datafieldRef);
}
 
// //////////////////
ChannelHistory
Observer::getChannelHistory(const std::string& channelName,
                            Ice::Float timestepMs,
                            const Ice::Current& c) const
{
    //ARMARX_DEBUG << BOOST_CURRENT_FUNCTION;
    return getPartialChannelHistory(
        channelName, 0, std::numeric_limits<long>::max(), timestepMs, c);
}
 
void
Observer::getChannelHistory_async(const AMD_ObserverInterface_getChannelHistoryPtr& amd,
                                  const std::string& channelName,
                                  Ice::Float timestepMs,
                                  const Ice::Current& c) const
{
    addWorkerJob("Observer::getChannelHistory",
                 amd,
                 &Observer::getChannelHistory,
                 channelName,
                 timestepMs,
                 c);
}
 
// //////////////////
ChannelHistory
Observer::getPartialChannelHistory(const std::string& channelName,
                                   Ice::Long startTimestamp,
                                   Ice::Long endTimestamp,
                                   Ice::Float timestepMs,
                                   const Ice::Current& c) const
{
    //ARMARX_DEBUG << BOOST_CURRENT_FUNCTION;
    ARMARX_IMPORTANT << "Waiting for mutex";
    std::unique_lock lock_channels(impl->historyMutex);
    ARMARX_IMPORTANT << "GOT mutex";
    ChannelHistory result;
    auto historyIt = impl->channelHistory.find(channelName);
    if (historyIt == impl->channelHistory.end())
    {
        return result;
    }
    ARMARX_IMPORTANT << "found channel";
 
    auto lastInsertIt = result.begin();
    Ice::Long lastUsedTimestep = 0;
    float timestepUs = timestepMs * 1000;
    for (auto& entry : historyIt->second)
    {
        long timestamp = entry.first.toMicroSeconds();
        if (timestamp > endTimestamp)
        {
            break;
        }
 
        if (timestamp > startTimestamp && timestamp - lastUsedTimestep > timestepUs)
        {
            lastInsertIt = result.emplace_hint(result.end(),
                                               std::make_pair(timestamp, entry.second.dataFields));
            lastUsedTimestep = timestamp;
        }
    }
    return result;
}
 
void
Observer::getPartialChannelHistory_async(
    const AMD_ObserverInterface_getPartialChannelHistoryPtr& amd,
    const std::string& channelName,
    Ice::Long startTimestamp,
    Ice::Long endTimestamp,
    Ice::Float timestepMs,
    const Ice::Current& c) const
{
    addWorkerJob("Observer::getPartialChannelHistory",
                 amd,
                 &Observer::getPartialChannelHistory,
                 channelName,
                 startTimestamp,
                 endTimestamp,
                 timestepMs,
                 c);
}
 
// //////////////////
TimedVariantBaseList
Observer::getDatafieldHistory(const std::string& channelName,
                              const std::string& datafieldName,
                              Ice::Float timestepMs,
                              const Ice::Current& c) const
{
    //ARMARX_DEBUG << BOOST_CURRENT_FUNCTION;
    return getPartialDatafieldHistory(
        channelName, datafieldName, 0, std::numeric_limits<long>::max(), timestepMs, c);
}
 
void
Observer::getDatafieldHistory_async(const AMD_ObserverInterface_getDatafieldHistoryPtr& amd,
                                    const std::string& channelName,
                                    const std::string& datafieldName,
                                    Ice::Float timestepMs,
                                    const Ice::Current& c) const
{
    addWorkerJob("Observer::getDatafieldHistory",
                 amd,
                 &Observer::getDatafieldHistory,
                 channelName,
                 datafieldName,
                 timestepMs,
                 c);
}
 
// //////////////////
TimedVariantBaseList
Observer::getPartialDatafieldHistory(const std::string& channelName,
                                     const std::string& datafieldName,
                                     Ice::Long startTimestamp,
                                     Ice::Long endTimestamp,
                                     Ice::Float timestepMs,
                                     const Ice::Current& c) const
{
    //ARMARX_DEBUG << BOOST_CURRENT_FUNCTION;
    std::unique_lock lock_channels(impl->historyMutex);
    TimedVariantBaseList result;
    auto historyIt = impl->channelHistory.find(channelName);
    if (historyIt == impl->channelHistory.end())
    {
        return result;
    }
    if (!existsDataField(channelName, datafieldName))
    {
        return result;
    }
 
    Ice::Long lastUsedTimestep = 0;
    float timestepUs = timestepMs * 1000;
    for (auto& entry : historyIt->second)
    {
        long timestamp = entry.first.toMicroSeconds();
        if (timestamp > endTimestamp)
        {
            break;
        }
 
        if (timestamp > startTimestamp && timestamp - lastUsedTimestep > timestepUs)
        {
            auto datafieldIt = entry.second.dataFields.find(datafieldName);
            if (datafieldIt == entry.second.dataFields.end())
            {
                continue;
            }
            else
            {
                TimedVariantPtr tvar = TimedVariantPtr::dynamicCast(datafieldIt->second.value);
                if (tvar)
                {
                    result.emplace_back(tvar);
                    lastUsedTimestep = timestamp;
                }
            }
        }
    }
    return result;
}
 
void
Observer::getPartialDatafieldHistory_async(
    const AMD_ObserverInterface_getPartialDatafieldHistoryPtr& amd,
    const std::string& channelName,
    const std::string& datafieldName,
    Ice::Long startTimestamp,
    Ice::Long endTimestamp,
    Ice::Float timestepMs,
    const Ice::Current& c) const
{
    addWorkerJob("Observer::getPartialDatafieldHistory",
                 amd,
                 &Observer::getPartialDatafieldHistory,
                 channelName,
                 datafieldName,
                 startTimestamp,
                 endTimestamp,
                 timestepMs,
                 c);
}
 
// ////////////////////////////////////////////////////////////////////////// //
Observer::Impl::WorkerUpdate::WorkerUpdate(const std::string& name, std::function<void(void)> f) :
    name{name}, time{Now()}, f{std::move(f)}
{
}
 
float
Observer::Impl::WorkerUpdate::ageInMs() const
{
    return TimeDeltaInMs(time);
}