StateController.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::statechart
* @author     Mirko Waechter ( mirko.waechter at kit dot edu)
* @date       2012
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "StateController.h"
#include "StateControllerImpl.h"
#include "StateBaseImpl.h"
#include "RemoteState.h"
#include "StatechartManager.h"
#include "standardstates/FinalState.h"
 
#include <IceUtil/Time.h>
 
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/services/profiler/Profiler.h>  // for ProfilerPtr
 
#include <ArmarXCore/statechart/StatechartObjectFactories.h>
 
using namespace armarx;
using namespace StateUtilFunctions;
 
 
#define MAX_USER_CODE_DURATION 100 //milliseconds
 
 
 
StateController::StateController()
    : cimpl(new Impl)
{
}
 
StateController::StateController(const StateController& source) :
    IceUtil::Shared(source),
    Ice::Object(source),
    StateIceBase(source),
    Logging(source),
    StateBase(source),
    cimpl(new Impl)
{
    cimpl->transitionFunctions = source.cimpl->transitionFunctions;
    cimpl->localProfilers = source.cimpl->localProfilers;
    cimpl->profilersDisabled = source.cimpl->profilersDisabled;
}
 
StateController::~StateController()
{
    //    ARMARX_INFO << "~StateController "<< stateClassName << " " << tag.tagName << " " << (long)this /*<< "\n" << LogSender::createBackTrace() */<< std::endl;
    if (cimpl->__runningTask)
    {
        cimpl->__runningTask->stop();
    }
 
    {
        std::unique_lock lock(cimpl->__finishedMutex);
        cimpl->__finished = true;
    }
 
    cimpl->__finishedCondition.notify_all();
}
 
bool StateController::isFinished() const
{
    std::unique_lock lock(cimpl->__finishedMutex);
    return cimpl->__finished;
}
 
 
void StateController::waitForStateToFinish(int timeoutMs) const
{
    ARMARX_TRACE;
    ARMARX_INFO << "Waiting for state to finish";
    std::unique_lock lock(cimpl->__finishedMutex);
 
    if (__getParentState())
    {
        throw LocalException("You can only wait for toplevel states to finish!");
    }
 
    while (!cimpl->__finished)
    {
        if (timeoutMs == -1)
        {
            cimpl->__finishedCondition.wait(lock);
        }
        else
        {
            if (cimpl->__finishedCondition.wait_for(lock, std::chrono::milliseconds(timeoutMs)) == std::cv_status::timeout)
            {
                throw LocalException("Statechart did not finish in time");
            }
        }
    }
}
 
 
 
void
StateController::enter(const StringVariantContainerBaseMap& tempInputParameters)
{
    ARMARX_TRACE;
    __checkPhase(eDefined, __PRETTY_FUNCTION__);
 
    if (!isInitialized())
    {
        throw exceptions::local::eNotInitialized();
    }
 
    if (__hasSubstates() && !initState._ptr)
    {
        throw exceptions::local::eNullPointerException("initState was not set!");
    }
 
    for (auto& state : subStateList)
    {
        RemoteStatePtr remote = RemoteStatePtr::dynamicCast(state);
        if (remote)
        {
            ArmarXObjectSchedulerPtr objectScheduler = remote->getObjectScheduler();
            if (remote->getState() < eManagedIceObjectStarted)
            {
                ARMARX_INFO << "Waiting for remote state " << remote->getGlobalHierarchyString();
                objectScheduler->waitForDependencies();
            }
        }
    }
    ARMARX_VERBOSE << "starting state " << stateName <<  " with parameters:\n" << StateUtilFunctions::getDictionaryString(tempInputParameters) << flush;
 
    ARMARX_TRACE;
    //fill the inputDictionary of the entry state
    if (tempInputParameters.size() > 0)
    {
        createMapping()
        ->mapFromOutput("*", "*")
        ->_addSourceDictionary(eOutput, tempInputParameters)
        ->setTargetDictToGreedy(greedyInputDictionary)
        ->_applyMapping(inputParameters);
    }
 
    _baseOnEnter();
}
 
 
void
StateController::__substatesFinished(const EventPtr ev)
{
    ARMARX_TRACE;
    activeSubstate = nullptr; // line needed or not?! Pro: Finalstate will be broken on leaving of parentstate - Contra: baseOnExit() of finalstate wont be called.
    impl->triggeredEndstateEvent = ev;
    ARMARX_DEBUG << "Substate finished with event " << ev->eventName;
    if (__getParentState() && StateControllerPtr::dynamicCast(__getParentState()))
    {
        __finalize(ev);
    }
    else
    {
        clearSelfPointer();
        _baseOnExit();
        STATEINFO << "Statemachine finished with event '" <<  ev->eventName << "' in state '" << getGlobalHierarchyString() << "'\n"
                  << "The resulting output dictionary:\n" << StateUtilFunctions::getDictionaryString(getOutputParameters()) << flush ;
    }
}
 
 
// in function, so that it can be overridden in remoteStateWrapper
void StateController::__finalize(const EventPtr event)
{
    ARMARX_TRACE;
    __enqueueEvent(event);
}
 
 
void StateController::__enqueueEvent(const EventPtr event)
{
    ARMARX_TRACE;
    if (__getParentState())
    {
        ARMARX_TRACE;
        impl->manager->addEvent(event, __getParentState());
    }
}
 
 
 
 
 
 
 
bool StateController::__applyMappings(const StateControllerPtr& srcState, const TransitionIceBase& t, const EventPtr& event, TransitionError& error)
{
    ARMARX_TRACE;
    srcState->setStatePhase(eDefined);
 
    // get source dictionaries and apply the mapping
    const StringVariantContainerBaseMap parentSetInputParams = __getSetInputAndLocalParameters();
    ARMARX_DEBUG << "Set parameters of parent " << stateName << " during transition: " << StateUtilFunctions::getDictionaryString(parentSetInputParams);
    StringVariantContainerBaseMap srcSetOutputParams;
 
    if (srcState)
    {
        srcSetOutputParams = StateUtilFunctions::getSetValues(srcState->getOutputParameters());
    }
 
    auto applyMapping = [&](const ParameterMappingIceBasePtr & icepm, StateParameterMap & targetMap, bool greedy)
    {
        ARMARX_TRACE;
        PMPtr pm = PMPtr::dynamicCast(icepm);
 
        if (pm)
        {
            pm->setTargetDictToGreedy(greedy);
 
            //            ARMARX_DEBUG_S << "The following output parameters of source state " << srcState->getStateName() << " are available: " << StateUtilFunctions::getDictionaryString(srcSetOutputParams);
            pm->_addSourceDictionary(eParent, parentSetInputParams)
            ->_addSourceDictionary(eOutput, srcSetOutputParams)
            ->_addSourceDictionary(eEvent, event->properties)
            ->_applyMapping(targetMap);
        }
    };
    applyMapping(t.mappingToParentStatesLocal, localParameters, false);
    applyMapping(t.mappingToParentStatesOutput, outputParameters, false);
    applyMapping(t.mappingToNextStatesInput, StateBasePtr::dynamicCast(t.destinationState)->inputParameters, StateBasePtr::dynamicCast(t.destinationState)->greedyInputDictionary);
    //    ARMARX_DEBUG << "The following input parameters from parent state " << getStateName() << " are available: " << StateUtilFunctions::getDictionaryString(inputParameters);
    std::string paramCheckOutput;
 
    auto transitionId = getTransitionID(t);
    auto it = cimpl->transitionFunctions.find(transitionId);
    if (it != cimpl->transitionFunctions.end())
    {
        ARMARX_TRACE;
        transitionFunction& f = it->second;
        f(this, StateIceBasePtr::dynamicCast(t.destinationState), StateIceBasePtr::dynamicCast(t.sourceState));
    }
 
    if (!checkForCompleteParameters(StateBasePtr::dynamicCast(t.destinationState)->inputParameters, &paramCheckOutput))
    {
        ARMARX_TRACE;
        error.errorType = eTransitionErrorInput;
        error.infos.push_back(StateBasePtr::dynamicCast(t.destinationState)->stateName);
        error.infos.push_back(paramCheckOutput);
        __printTransitionError(error, event);
        return false;
    }
 
 
    return true;
}
 
void StateController::__printTransitionError(const TransitionError& transitionError, const EventPtr& event) const
{
    ARMARX_TRACE;
    switch (transitionError.errorType)
    {
        case eTransitionErrorInput:
        {
            ARMARX_WARNING << "Could not execute transition!\nReturned reason: " + StateUtilFunctions::transitionErrorToString(transitionError.errorType) + "\nExplanations:\n"
                           << "\t- The input parameters of the destination state were not fully set.\n\tDestination state: '" << transitionError.infos.at(0) << "'\n" <<
                           transitionError.infos.at(1);
        }
        break;
 
        case eTransitionErrorUnexpectedEvent:
        {
            ARMARX_WARNING << "Could not execute transition!\nReturned reason: " + StateUtilFunctions::transitionErrorToString(transitionError.errorType) + "\nExplanations:\n"
                           << "\t- The state '" << stateName << "' does not expect the event '" << event->eventName << "' while in substate '" << StateBasePtr::dynamicCast(activeSubstate)->stateName << "'.\n" << flush;
        }
        break;
 
        default:
        {
            ARMARX_WARNING << "Could not execute transition!\nReturned reason: " + StateUtilFunctions::transitionErrorToString(transitionError.errorType) << flush;
        }
    }
}
 
 
void StateController::__processEvent(const EventPtr event, bool buffered)
{
    ARMARX_TRACE;
    STATEINFO << "TRANSITION due to Event '" << event->eventName  << "' (EventReceiver: '" << event->eventReceiverName << "')..." << flush;
    boost::recursive_mutex::scoped_lock lock(impl->__processEventMutex);
 
    if (!isInitialized())
    {
        throw exceptions::local::eNotInitialized();
    }
 
    //check if the current state is breakable and if any parent state has events to process
    if (!buffered
        && __hasActiveSubstate() && !StateIceBasePtr::dynamicCast(activeSubstate)->unbreakable
        && !__getUnbreakableBufferStati())
    {
        ARMARX_WARNING << "a parent state has events to process. this event will be skipped" << flush;
        return;
    }
    ARMARX_TRACE;
 
    TransitionError transitionError;
    transitionError.errorType = eTransitionErrorUndefined;
 
    TransitionIceBase t;
 
    if (__findValidTransition(event, StateIceBasePtr::dynamicCast(activeSubstate), t, transitionError))
    {
        ARMARX_TRACE;
        StateControllerPtr src = StateControllerPtr::dynamicCast(activeSubstate);
 
        if (src && src->__hasSubstates() && src->__hasActiveSubstate())
        {
            ARMARX_INFO << "<<<< Trying to break substates of " << src->stateName << flush;
 
            // Check if the active substate is unbreakable
            if (src->__breakActiveSubstate(event))
            {
                src->_baseOnExit();
            }
            else  // substate of src unbreakable
            {
                HiddenTimedMutex::ScopedLock lock(impl->__eventUnbreakableBufferMutex);
                cimpl->__unbreakableBuffer.push(event);
                __notifyEventBufferedDueToUnbreakableState();
 
                ARMARX_INFO << "got event while in unbreakable substate: parent stateName:" <<  stateName << flush;
                return;
            }
        }
        else if (src)
        {
            src->_baseOnExit();
        }
 
        if (t.fromAll)
        {
            // required by StatechartLogger
            // source state is empty if event is received as fromAll
            t.sourceState = activeSubstate;
        }
 
        // set to NULL in case mapping could not be applied, so onExit wont be called twice
        activeSubstate = nullptr;
        bool destinationStateInitialized = StateBasePtr::dynamicCast(t.destinationState)->waitForInitialization();
        ARMARX_CHECK_EXPRESSION(destinationStateInitialized) << StateBasePtr::dynamicCast(t.destinationState)->stateName;
 
        if (__applyMappings(src, t, event, transitionError))
        {
            ARMARX_TRACE;
            activeSubstate = t.destinationState;
            if (t.sourceState && activeSubstate && !cimpl->profilersDisabled)
            {
                for (const Profiler::ProfilerPtr& localProfiler : cimpl->localProfilers)
                {
                    localProfiler->logStatechartTransition(getGlobalHierarchyString(), StateIceBasePtr::dynamicCast(t.sourceState),  StateIceBasePtr::dynamicCast(t.destinationState), t.evt->eventName);
                    localProfiler->logStatechartTransitionWithParameters(t);
                }
            }
 
 
            StateControllerPtr::dynamicCast(t.destinationState)->_baseOnEnter();
 
            //check if unbreakable state has caused a lock
            if (src && src->unbreakable && src->eventsDelayed)
            {
                ARMARX_TRACE;
                if (__getParentState() &&  !__getParentState()->__getUnbreakableBufferStati())
                {
                    // check if next state is unbreakable as well
                    if (StateIceBasePtr::dynamicCast(t.destinationState)->unbreakable)
                    {
                        StateIceBasePtr::dynamicCast(t.destinationState)->eventsDelayed = true;
                    }
                    else
                    {
                        __getParentState()->__processBufferedEvents();
                    }
                }
            }
 
 
 
            // Transition done
            return;
        }
    }
    ARMARX_TRACE;
 
    //no fitting transition found->throw
    if (activeSubstate)
    {
        __printTransitionError(transitionError, event);
        //throw eUnexpectedEvent(event, activeSubstate);
    }
    else
    {
        ARMARX_WARNING << "The state '" << stateName << "' does not have an activeSubstate and therefore cannot process any events." << flush;
    }
}
 
bool StateController::__findValidTransition(const EventPtr& event, const StateIceBasePtr& sourceState, TransitionIceBase& resultTransition, TransitionError& error) const
{
    ARMARX_TRACE;
    // loop through transition table to find the correct transition fitting to the event, activeState and DestinationState
 
    error.errorType = eTransitionNoError;
 
    // first check if there is a transition fitting to the input
 
    int selectedTransitionIndex = -1;
 
    for (unsigned int i = 0; i < transitions.size(); i++)
    {
        ARMARX_TRACE;
        const TransitionIceBase& transition = transitions.at(i);
 
        if (
            (transition.sourceState == sourceState || transition.fromAll) // correct source state
            && transition.evt->eventName == event->eventName // correct event
            && (sourceState->stateName == event->eventReceiverName || event->eventReceiverName == EVENTTOALL) // correct eventreceiver
        )
        {
            selectedTransitionIndex = i;
            break;
        }
    }
    ARMARX_TRACE;
 
    if (selectedTransitionIndex == -1)
    {
        error.errorType = eTransitionErrorUnexpectedEvent;
        return false;
    }
 
    const TransitionIceBase& transition = transitions.at(selectedTransitionIndex);
 
    resultTransition = transition;
    error.errorType = eTransitionNoError;
    return true;
}
 
TransitionError StateController::__validateTransition(const TransitionIceBase& transition, const EventPtr event, const StateIceBasePtr& sourceState, const StateIceBasePtr& destinationState) const
{
    ARMARX_TRACE;
    TransitionError error;
 
    if (
        (transition.sourceState != sourceState && !transition.fromAll) // wrong source state
        || transition.evt->eventName != event->eventName // wrong event
        || (sourceState && sourceState->stateName != event->eventReceiverName && event->eventReceiverName != EVENTTOALL) // wrong eventreceiver
    )
    {
        error.errorType = eTransitionErrorUnexpectedEvent;
        return error;
    }
 
 
    if (transition.sourceState && transition.fromAll) // this should never happen
    {
        error.errorType = eTransitionErrorUndefined;
        return error;
    }
 
 
    // get source dictionaries and apply the mapping
    PMPtr transitionMapping = PMPtr::dynamicCast(transition.mappingToNextStatesInput);
 
    ARMARX_TRACE;
    if (transitionMapping && sourceState)
    {
        ARMARX_TRACE;
        const StringVariantContainerBaseMap parentSetInputParams = __getSetInputAndLocalParameters();
        const StringVariantContainerBaseMap srcSetOutputParams = StateUtilFunctions::getSetValues(sourceState->outputParameters);
        transitionMapping->setTargetDictToGreedy(StateBasePtr::dynamicCast(transition.destinationState)->greedyInputDictionary);
        StateParameterMap inputCopy;
        StateUtilFunctions::copyDictionary(destinationState->inputParameters, inputCopy);
        transitionMapping->_addSourceDictionary(eParent, parentSetInputParams)
        ->_addSourceDictionary(eOutput, srcSetOutputParams)
        ->_addSourceDictionary(eEvent, event->properties)
        ->_applyMapping(inputCopy);
        std::string paramCheckOutput;
 
        if (!checkForCompleteParameters(inputCopy, &paramCheckOutput))
        {
            error.errorType = eTransitionErrorInput;
            error.infos.push_back(destinationState->stateName + ": " + paramCheckOutput);
            return error;
        }
    }
 
    error.errorType = eTransitionNoError;
    return error;
}
 
unsigned int StateController::__getUnbreakableBufferSize() const
{
    return cimpl->__unbreakableBuffer.size();
}
 
void StateController::__notifyEventBufferedDueToUnbreakableState(bool eventBuffered)
{
    ARMARX_TRACE;
    cimpl->__eventBufferedDueToUnbreakableState = eventBuffered;
 
    for (unsigned int i = 0; i < subStateList.size(); ++i)
    {
        ARMARX_TRACE;
        StateControllerPtr state = StateControllerPtr::dynamicCast(subStateList.at(i));
 
        if (state->__getUnbreakableBufferSize() > 0) // check if an event in this state is buffered. if it is, the eventBuffered must be set to true, otherwise dont change it
        {
            eventBuffered = true;
        }
 
        StateControllerPtr::dynamicCast(subStateList.at(i))->__notifyEventBufferedDueToUnbreakableState(eventBuffered);
    }
}
 
 
 
StateControllerPtr StateController::__getParentState() const
{
    ARMARX_TRACE;
    StateControllerPtr ptr = StateControllerPtr::dynamicCast(impl->__parentState);
 
    if (!ptr && impl->__parentState)
    {
        // We can try to call the factory
        auto newParentAsController = IceGeneratedState::createInstance();
        armarx::StateIceBase* base = newParentAsController.get();
        *base = *impl->__parentState;
        ARMARX_WARNING << "Had to call factory manually for StateIceBase in "
                       << impl->__parentState->globalStateIdentifier;
        return newParentAsController;
    }
 
    return ptr;
}
 
bool StateController::__breakActiveSubstate(const EventPtr event)
{
    ARMARX_TRACE;
    bool result = true;
    boost::recursive_mutex::scoped_lock lock(impl->__processEventMutex);
 
    if (__hasActiveSubstate())
    {
        result = StateControllerPtr::dynamicCast(activeSubstate)->_baseOnBreak(event);
 
        if (result) //active substate is broken -> set to NULL
        {
            activeSubstate = nullptr;
        }
    }
 
    return result;
}
 
 
 
 
void
StateController::__processBufferedEvents()
{
    ARMARX_TRACE;
    while (__getUnbreakableBufferSize() > 0)
    {
        ARMARX_VERBOSE << "processing " << cimpl->__unbreakableBuffer.front()->eventName << " in unbreakableBuffer  - eventprocessorstate:" << stateName << " - activeState: " << StateBasePtr::dynamicCast(activeSubstate)->stateName << flush;
        impl->__eventUnbreakableBufferMutex.lock();
        EventPtr event = cimpl->__unbreakableBuffer.front();
        cimpl->__unbreakableBuffer.pop();
        impl->__eventUnbreakableBufferMutex.unlock();
        __processEvent(event, true);
 
        if (__getUnbreakableBufferSize() == 0)
        {
            ARMARX_TRACE;
            __notifyEventBufferedDueToUnbreakableState(false);
        }
    }
 
    if (__getParentState())
    {
        StateControllerPtr::dynamicCast(__getParentState())->__processBufferedEvents();
    }
}
 
 
void StateController::__waitForRemoteStates() const
{
    ARMARX_TRACE;
    for (unsigned int i = 0; i < subStateList.size(); ++i)
    {
        RemoteStatePtr remoteStatePtr = RemoteStatePtr::dynamicCast(subStateList.at(i));
 
        if (remoteStatePtr)
        {
            ARMARX_TRACE;
            ARMARX_INFO << "Waiting for RemoteState " << remoteStatePtr->getName();
 
            while (!(remoteStatePtr->getState() >= eManagedIceObjectStarted))
            {
                usleep(10);
            }
 
            ARMARX_INFO << "RemoteState " << remoteStatePtr->getName() << "started.";
            break;
        }
    }
}
 
bool StateController::__checkExistenceOfTransition(const TransitionIceBase& transition)
{
    for (unsigned int i = 0; i < transitions.size(); ++i)
    {
        ARMARX_TRACE;
        if (transitions[i].evt->eventName == transition.evt->eventName
            && (transition.fromAll || transitions[i].sourceState == transition.sourceState)
            &&  transitions[i].destinationState == transition.destinationState
           )
        {
            return true;
        }
    }
 
    return false;
}
 
void
StateController::_baseOnEnter()
{
    ARMARX_TRACE;
    if (!isInitialized())
    {
        throw exceptions::local::eNotInitialized();
    }
 
    {
        HiddenTimedMutex::ScopedLock lock(impl->__stateMutex);
 
        if (cimpl->__runningTask)
        {
            ARMARX_TRACE;
            try
            {
                if (cimpl->__runningTask->isRunning())
                {
                    ARMARX_VERBOSE << "Waiting for running task of " << stateName << " to finish";
                }
 
                cimpl->__runningTask->stop(); // this blocks until the thread has finished
            }
            catch (IceUtil::ThreadSyscallException& e)
            {
 
            }
        }
 
        eventsDelayed = false;
 
        //reset local & output parameters, so that they are NOT set in case we have been in this state before
        ARMARX_DEBUG << "Resetting local and output parameters of state " << stateName;
        StateUtilFunctions::unsetParameters(localParameters);
        StateUtilFunctions::unsetParameters(outputParameters);
 
        // std::ARMARX_INFO << StateUtil::getDictionaryString(inputParameters);
        std::string paramCheckOutput;
 
        if (!checkForCompleteParameters(inputParameters, &paramCheckOutput))
        {
            throw LocalException("Not all required inputparameters of the state '" + stateName + "' are set:\n" + paramCheckOutput);
        }
 
        // overwrite status of last visit
        impl->cancelEnteringSubstates = false;
 
        impl->triggeredEndstateEvent = nullptr;
 
        impl->visitCounter++;
 
        STATEINFO << "Entering State '" << getLocalHierarchyString() << "' (id: " << impl->localUniqueId << ")" << flush;
        IceUtil::Time executionStart = IceUtil::Time::now();
 
        try
        {
            ARMARX_TRACE;
            setStatePhase(eEntering);
            if (!cimpl->profilersDisabled)
            {
                for (const Profiler::ProfilerPtr& localProfiler : cimpl->localProfilers)
                {
                    localProfiler->logEvent(Profiler::Profiler::EventType::eFunctionStart, getGlobalHierarchyString(), getStateName());
                    localProfiler->logStatechartInputParameters(getGlobalHierarchyString(), inputParameters);
                }
            }
            onEnter();
        }
        catch (exceptions::local::eStateAlreadyLeft& e)
        {
        }
        catch (...)
        {
            ARMARX_ERROR << "onEnter() of State " << globalStateIdentifier << " of class " << stateClassName << " failed. Ignoring substates and sending Failure.\n" << GetHandledExceptionString();
 
            lock.unlock();
            setStatePhase(eEntered);
            __enqueueEvent(new Failure(stateName));
            return;
 
        }
 
        setStatePhase(eEntered);
        if (!cimpl->profilersDisabled)
        {
            for (const Profiler::ProfilerPtr& localProfiler : cimpl->localProfilers)
            {
                localProfiler->logStatechartLocalParameters(getGlobalHierarchyString(), localParameters);
            }
        }
 
        IceUtil::Time duration = IceUtil::Time::now() - executionStart;
 
        if (duration.toMilliSeconds() > MAX_USER_CODE_DURATION)
        {
            ARMARX_WARNING << "onEnter() of state '" + stateName + "' took more than " << MAX_USER_CODE_DURATION << " ms (In fact: " << duration.toMilliSeconds() << " ms).  The onEnter() method should not calculate complex operations." << flush;
        }
    }
 
    {
        ARMARX_TRACE;
        HiddenTimedMutex::ScopedLock lock(impl->__stateMutex);
 
        if (!impl->cancelEnteringSubstates)
        {
            if (__hasSubstates())
            {
                impl->cancelEnteringSubstates = false;
 
                if (!initState)
                {
                    ARMARX_WARNING << "No initial substate set in '" << stateName << "'" << flush;
                }
                else
                {
                    StringVariantContainerBaseMap combinedMap =  __getSetInputAndLocalParameters();
                    ARMARX_DEBUG << "Source for initial state: " << StateUtilFunctions::getDictionaryString(combinedMap);
 
                    if (initialStateMapping)
                    {
                        PMPtr mapping = ParameterMappingPtr::dynamicCast(initialStateMapping);
                        mapping->_addSourceDictionary(eParent, combinedMap)
                        ->setTargetDictToGreedy(StateBasePtr::dynamicCast(initState)->greedyInputDictionary)
                        ->_applyMapping(StateBasePtr::dynamicCast(initState)->inputParameters);
                    }
 
                    activeSubstate = initState;
                    lock.unlock();
                    if (!cimpl->profilersDisabled && activeSubstate)
                    {
                        for (const Profiler::ProfilerPtr& localProfiler : cimpl->localProfilers)
                        {
                            localProfiler->logStatechartTransition(getGlobalHierarchyString(), nullptr, StateIceBasePtr::dynamicCast(activeSubstate), "InitialTransition");
 
                            TransitionIceBase t;
                            t.sourceState = nullptr;
                            t.destinationState = activeSubstate;
                            EventBasePtr e(new EventBase());
                            e->eventName = "InitialTransition";
                            t.evt = e;
                            localProfiler->logStatechartTransitionWithParameters(t);
                        }
                    }
                    StateControllerPtr::dynamicCast(initState)->_baseOnEnter();
 
                }
            }
        }
        else
        {
            STATEINFO << "Entering substates of " << stateName << " has been canceled";
        }
    }
 
    // triggers to run the run()-Function in a separate thread
    _startRun();
}
 
void
StateController::_startRun()
{
    ARMARX_TRACE;
    HiddenTimedMutex::ScopedLock lock(impl->__stateMutex);
 
    if (!impl->__useRunFunction)
    {
        return;
    }
 
    // only execute if still active state and not changed to another state in onEnter()
    if (!impl->__parentState || impl->__parentState->activeSubstate.get() == this)
    {
        cimpl->__runningTask = new RunningTask<StateController>(this, &StateController::_baseRun, stateClassName + "RunningTask");
        cimpl->__runningTask->start();
    }
}
 
void
StateController::_baseRun()
{
    ARMARX_TRACE;
    try
    {
        run();
    }
    catch (exceptions::local::eStateAlreadyLeft& e)
    {
    }
    catch (...)
    {
        ARMARX_ERROR << GetHandledExceptionString();
        if (getStatePhase() == eEntered)
        {
            ARMARX_ERROR_S << "run() of State " << globalStateIdentifier << " of class " << stateClassName << " failed for unhandled reason. Ignoring substates and sending Failure\n" << GetHandledExceptionString();
 
            __enqueueEvent(new Failure(stateName));
        }
 
        return;
    }
}
 
void
StateController::_baseOnExit()
{
    ARMARX_TRACE;
    HiddenTimedMutex::ScopedLock lock(impl->__stateMutex);
 
    // substates must not be entered if onexit has already been called
    impl->cancelEnteringSubstates = true;
 
    if (cimpl->__runningTask)
    {
        cimpl->__runningTask->stop(false);
        //runningTask = NULL;
    }
 
    if (!isInitialized())
    {
        throw exceptions::local::eNotInitialized();
    }
 
    if (__hasActiveSubstate())
    {
        throw exceptions::local::eStatechartLogicError("baseOnExit was called before substates were finished - baseOnBreak must be called instead. ActiveSubstate: " + StateBasePtr::dynamicCast(activeSubstate)->stateName);
    }
 
 
    STATEINFO << "Leaving State '" <<  getLocalHierarchyString() << "' (id: " << impl->localUniqueId << ")" << flush;
 
    IceUtil::Time executionStart = IceUtil::Time::now();
 
    try
    {
        ARMARX_TRACE;
        setStatePhase(eExiting);
        onExit();
        if (!cimpl->profilersDisabled)
        {
            for (const Profiler::ProfilerPtr& localProfiler : cimpl->localProfilers)
            {
                localProfiler->logEvent(Profiler::Profiler::EventType::eFunctionReturn, getGlobalHierarchyString(), getStateName());
                localProfiler->logStatechartLocalParameters(getGlobalHierarchyString(), localParameters);
                localProfiler->logStatechartOutputParameters(getGlobalHierarchyString(), outputParameters);
            }
        }
    }
    catch (...)
    {
        ARMARX_ERROR << "onExit() of State " << globalStateIdentifier << " of class " << stateClassName << " failed\n" << GetHandledExceptionString();
    }
 
    IceUtil::Time duration = IceUtil::Time::now() - executionStart;
    _removeInstalledConditions();
 
 
    setStatePhase(eExited);
 
 
 
    if (duration.toMilliSeconds() > MAX_USER_CODE_DURATION)
    {
        ARMARX_WARNING << "onExit() of state '" + stateName + "' took more than " << MAX_USER_CODE_DURATION << " ms (In fact: " << duration.toMilliSeconds() << " ms).  The onEnter() method should not calculate complex operations." << flush;
    }
 
    std::string paramCheckOutput;
 
    if (!checkForCompleteParameters(getOutputParameters(), &paramCheckOutput))
    {
        throw LocalException("Not all required Outputparameters of state '" + stateName + "' are set:\n" + paramCheckOutput);
    }
 
    //reset input parameters, so that they are NOT set when we reenter this state
    StateUtilFunctions::unsetParameters(inputParameters);
    __copyDefaultValuesToInput();
 
    {
        std::unique_lock lock(cimpl->__finishedMutex);
        cimpl->__finished = true;
        cimpl->__finishedCondition.notify_all();
    }
}
 
 
bool
StateController::_baseOnBreak(const EventPtr evt)
{
    ARMARX_TRACE;
    HiddenTimedMutex::ScopedLock lock(impl->__stateMutex);
 
    if (cimpl->__runningTask)
    {
        cimpl->__runningTask->stop(false);
        //runningTask = NULL;
    }
 
    if (!isInitialized())
    {
        throw exceptions::local::eNotInitialized();
    }
 
 
 
    bool result = true;
 
    if (unbreakable)
    {
        eventsDelayed = true;
        result = false;
    }
    else if (subStateList.size() > 0 && __hasActiveSubstate())
    {
        // Check if the active substate is unbreakable
        if (StateBasePtr::dynamicCast(activeSubstate)->unbreakable)
        {
            // signal to upper states that this state cannot be broken
            StateBasePtr::dynamicCast(activeSubstate)->eventsDelayed = true;
            result = false;
        }
        else
        {
            result = __breakActiveSubstate(evt);
        }
    }
 
    if (result)
    {
        ARMARX_TRACE;
        STATEINFO << "Breaking State '" <<  getLocalHierarchyString() << "' (id: " << impl->localUniqueId << ")" << flush;
 
        if (cimpl->__runningTask)
        {
            cimpl->__runningTask->stop(false);
            //runningTask = NULL;
        }
 
        IceUtil::Time executionStart = IceUtil::Time::now();
 
        try
        {
            setStatePhase(eBreaking);
            onBreak();
            if (!cimpl->profilersDisabled)
                for (const Profiler::ProfilerPtr& localProfiler : cimpl->localProfilers)
                {
                    localProfiler->logEvent(Profiler::Profiler::EventType::eFunctionBreak, getGlobalHierarchyString(), getStateName());
                    localProfiler->logStatechartLocalParameters(getGlobalHierarchyString(), localParameters);
                    localProfiler->logStatechartOutputParameters(getGlobalHierarchyString(), outputParameters);
                }
            setStatePhase(eDefined);
        }
        catch (...)
        {
            ARMARX_ERROR << "onBreak() of State " << globalStateIdentifier << " of class " << stateClassName << " failed. Ignoring substates and sending Failure.\n" << GetHandledExceptionString();
 
        }
 
        _removeInstalledConditions();
 
        IceUtil::Time duration = IceUtil::Time::now() - executionStart;
 
        if (duration.toMilliSeconds() > MAX_USER_CODE_DURATION)
        {
            ARMARX_WARNING << "onEnter() of state '" + stateName + "' took more than " << MAX_USER_CODE_DURATION << " ms (In fact: " << duration.toMilliSeconds() << " ms).  The onEnter() method should not calculate complex operations." << flush;
        }
    }
 
    if (result)
    {
        //reset input parameters, so that they are NOT set when we reenter this state
        StateUtilFunctions::unsetParameters(inputParameters);
        __copyDefaultValuesToInput();
    }
 
    {
        std::unique_lock lock(cimpl->__finishedMutex);
        cimpl->__finished = true;
        cimpl->__finishedCondition.notify_all();
    }
 
    return result;
}
 
 
void StateController::addProfilerRecursive(Profiler::ProfilerPtr profiler, int recursiveLevels)
{
    ARMARX_TRACE;
    cimpl->localProfilers.insert(profiler);
    if (recursiveLevels < 0)
    {
        // fall through: the value -1 means that all substates should be profiled
    }
    else if (recursiveLevels == 0)
    {
        return;
    }
    else if (recursiveLevels > 0)
    {
        recursiveLevels--;
    }
 
    for (AbstractStateIceBasePtr state : subStateList)
    {
        StateControllerPtr stateController = StateControllerPtr::dynamicCast(state);
 
        if (stateController)
        {
            stateController->addProfilerRecursive(profiler, recursiveLevels);
        }
    }
}
 
void StateController::removeProfilerRecursive(Profiler::ProfilerPtr profiler, int recursiveLevels)
{
    ARMARX_TRACE;
    cimpl->localProfilers.erase(profiler);
    if (recursiveLevels < 0)
    {
        // fall through: the value -1 means that all substates should be profiled
    }
    else if (recursiveLevels == 0)
    {
        return;
    }
    else if (recursiveLevels > 0)
    {
        recursiveLevels--;
    }
 
    for (AbstractStateIceBasePtr state : subStateList)
    {
        StateControllerPtr stateController = StateControllerPtr::dynamicCast(state);
 
        if (stateController)
        {
            stateController->addProfilerRecursive(profiler, recursiveLevels);
        }
    }
}
 
 
 
bool StateController::isRunningTaskStopped() const
{
    ARMARX_TRACE;
    if (!cimpl->__runningTask)
    {
        return true;
    }
 
    return cimpl->__runningTask->isStopped();
}
 
bool StateController::isRunningTaskFinished() const
{
    if (!cimpl->__runningTask)
    {
        return true;
    }
    return cimpl->__runningTask->isFinished();
}
 
void StateController::waitForRunningTaskToFinish() const
{
    ARMARX_TRACE;
    if (cimpl->__runningTask)
    {
        cimpl->__runningTask->join();
    }
}
 
void StateController::disableStateReporting(bool disable)
{
    cimpl->profilersDisabled = disable;
}
 
void StateController::addTransitionFunction(const TransitionIceBase& t, StateController::transitionFunction function)
{
    ARMARX_INFO << "Regstering transition code for " << t.evt->eventName;
    cimpl->transitionFunctions[getTransitionID(t)] = function;
}
 
std::string StateController::getTransitionID(const TransitionIceBase& t) const
{
    ARMARX_CHECK_EXPRESSION(t.evt);
    return getTransitionID(t.evt->eventName, (t.sourceState ? StateBasePtr::dynamicCast(t.sourceState)->stateName : std::string("")));
}
 
std::string StateController::getTransitionID(const std::string& eventName, const std::string sourceStateName) const
{
    return sourceStateName + ":" + eventName;
}
 
bool StateController::findTransition(const std::string& eventName, const std::string sourceStateName, TransitionIceBase& transition)
{
    ARMARX_TRACE;
    for (const TransitionIceBase& t : transitions)
    {
        if (t.evt->eventName == eventName && StateBasePtr::dynamicCast(t.sourceState)->stateName == sourceStateName)
        {
            transition = t;
            return true;
        }
    }
    ARMARX_INFO << "Could not find transition in " << transitions.size() << " transitions";
    return false;
}
 
 
bool
StateController::__getUnbreakableBufferStati() const
{
    ARMARX_TRACE;
    // commented due to performance issues
    //        const StateBase * curParentState = this;
    //        while(curParentState)
    //        {
    //            if(curParentState->__getUnbreakableBufferSize() > 0){
    //                return false;
    //            }
    //            curParentState = curParentState->__parentState;
    //        }
    //        return true;
    return !cimpl->__eventBufferedDueToUnbreakableState;
}
 
 
void StateController::disableRunFunction()
{
    ARMARX_TRACE;
    for (unsigned int i = 0; i < subStateList.size(); i++)
    {
        StateControllerPtr::dynamicCast(subStateList[i])->disableRunFunction();
    }
 
    HiddenTimedMutex::ScopedLock lock(impl->__stateMutex);
    impl->__useRunFunction = false;
 
    if (cimpl->__runningTask && cimpl->__runningTask->isRunning())
    {
        ARMARX_VERBOSE << "State with name '" << stateName << "' is waiting for the RunFunction";
        cimpl->__runningTask->stop();
    }
 
    cimpl->__runningTask = nullptr;
}