/* $Header: https://svn.mpia.de/gulli/TwiceAsNice/branch/unstable/TwiceAsNice/basda-mocca/src/device/mpia/moconDummy/NDeviceDummy.cc 16132 2019-11-28 16:02:41Z mathar $
*/

#include "moconDummy/NDeviceDummy.h"
#include "moconDummy/ModuleDummy.h"
#include <mocon/ConfigIDs.h>
#include <mocon/ChatLineMock.h>
#include <core/CommandIDs.h>
#include <Basda/Mocca/PropertyName.h>
#include <Basda/Mocca/MPIA/PropertyName.h>
#include <Basda/DeviceFactory.h>
#include <Nice/Time/DateDummy.h>

#undef LOG_NAME
#define LOG_NAME m_name

DEVICE_CREATE(Basda::MoConNDeviceDummy);

Basda::MoConNDeviceDummy::MoConNDeviceDummy(const Nice::JointPtr & _config) :
        MoccaNDevice(_config)
{
    addProperties();
    createInstances();
    m_tcpConnectionTimeout = Nice::Time::seconds(3600);

    m_niceDateDummy = Nice::DateDummy::init();

    Nice::Application::distribute(m_config);
    D_LOG("device created")
}

Basda::MoConNDeviceDummy::~MoConNDeviceDummy()
{
    T_LOG("destruct");
    m_module.clear();
}

void Basda::MoConNDeviceDummy::setChatType(std::map<std::string, Nice::ChatLinePtr> _chat)
{
    m_chat = _chat;
    Nice::JointPtrMap children = m_config->node("MOD")->children();
    std::map<std::string, Nice::JointPtr>::iterator children_it =
            children.begin();

    while (children_it != children.end())
    {
        std::string address =
                (children_it->second)->getWithDefault<std::string>(
                        Basda::Mocca::MoCon::cfgConnectionAddress(),
                        "tcp://192.168.1.1:4000");
        int cardNo = (children_it->second)->getWithDefault<int>(
                Basda::Mocca::MoCon::cfgConnectionCard(), Basda::MOCON);
        int moduleNo = (children_it->second)->getWithDefault<int>(
                Basda::Mocca::MoCon::cfgConnectionModule(), 1);
        std::string connectionID = createConnectionID(address, cardNo, moduleNo);
        (m_module[connectionID])->setChatType(m_chat[address]);
        children_it++;
    }
}

std::string Basda::MoConNDeviceDummy::createConnectionID(const std::string &_address,
        const unsigned int _cardNo,
        const unsigned int _moduleNo)
{
    std::stringstream connectionID;
    connectionID << _address << ":" << _cardNo << ":" << _moduleNo;
    return connectionID.str();
}

void Basda::MoConNDeviceDummy::createInstances()
{
    m_externalProfileSize = 0;

    Nice::JointPtrMap children = m_config->node("MOD")->children();
    std::map<std::string, Nice::JointPtr>::iterator children_it = children.begin();
    std::map<std::string, MoConModuleDummyPtr>::iterator module_it =
            m_module.begin();

    while (children_it != children.end())
    {
        std::string address =
                (children_it->second)->getWithDefault<std::string>(
                        Basda::Mocca::MoCon::cfgConnectionAddress(),
                        "tcp://192.168.1.1:4000");
        int cardNo = (children_it->second)->getWithDefault<int>(
                Basda::Mocca::MoCon::cfgConnectionCard(), Basda::MOCON);
        int moduleNo = (children_it->second)->getWithDefault<int>(
                Basda::Mocca::MoCon::cfgConnectionModule(), 1);

        m_chat[address] = (new Basda::ChatLineMockMocon());

        std::string connectionID = createConnectionID(address, cardNo, moduleNo);
        if ((module_it = m_module.find(connectionID)) == m_module.end())
        {
            MoConModuleDummyPtr module;
//            D_LOG("create instance of module " << connectionID );
            module = new MoConModuleDummy(
                    m_name + "." + children_it->first,
                    m_config->node(std::string("MOD.") + children_it->first));
            m_module.insert(std::make_pair(connectionID, module));
        }
        children_it++;
    }
}

void Basda::MoConNDeviceDummy::open()
{
    D_LOG("open device " << m_config->name() << ".");
}

void Basda::MoConNDeviceDummy::close()
{
    D_LOG("close device " << m_config->name() << ".");
    setIsReachable(false);
}

void Basda::MoConNDeviceDummy::reset(Basda::DeviceAsync & _async)
{
    m_completion = Nice::Completion::exec<Basda::Command::Constant>(Command::RESET);
    m_completion.start(Basda::Command::Action::RESET);
    m_timer.start(Nice::Time::seconds(1.0));
    waitInternal("RESET", _async);
}

void Basda::MoConNDeviceDummy::version()
{
    E_LOG(VERSION);
}

void Basda::MoConNDeviceDummy::init(Basda::DeviceAsync & _async)
{
    m_completion = Nice::Completion::exec<Basda::Command::Constant>(Command::INIT);
    m_completion.start(Basda::Command::Action::INIT);

    std::map<std::string, MoConModuleDummyPtr>::iterator module_it = m_module.begin();

    unsigned int i = 0;
    while (module_it != m_module.end())
    {
        D_LOG("set reference time " << module_it->first)
        (module_it->second)->getReferenceTime();

        i++;
        module_it++;
    }

    m_timer.start(Nice::Time::seconds(1.0));
    waitInternal("INIT", _async);

    D_LOG("synchronize PC with MoCon clock")
    // the time gap between the reference time and the current time
    // has not to be to tiny in order to get a valid MoCon cycle time
    module_it = m_module.begin();
    while (module_it != m_module.end())
    {
        (module_it->second)->getCurrentTime();
        module_it++;
    }

    setIsReachable(true);
    D_LOG("device initialized");

    if ((m_completion.state() == Nice::EXEC) || (m_completion.state() == Nice::BUSY))
        m_completion.setDone();
}

void Basda::MoConNDeviceDummy::deinit(Basda::DeviceAsync & _async)
{
    m_completion = Nice::Completion::exec<Basda::Command::Constant>(Command::DEINIT);
    m_completion.start(Basda::Command::Action::DEINIT);
    m_timer.start(Nice::Time::seconds(1.0));
    waitInternal("DEINIT", _async);
}

void Basda::MoConNDeviceDummy::activate(Basda::DeviceAsync & _async)
{
    m_completion = Nice::Completion::exec<Basda::Command::Constant>(Command::ACTIVATE);
    m_completion.start(Basda::Command::Action::ACTIVATE);
    m_timer.start(Nice::Time::seconds(1.0));
    waitInternal("ACTIV", _async);
}

void Basda::MoConNDeviceDummy::deactivate(Basda::DeviceAsync & _async)
{
    m_completion = Nice::Completion::exec<Basda::Command::Constant>(Command::DEACTIVATE);
    m_completion.start(Basda::Command::Action::DEACTIVATE);
    m_timer.start(Nice::Time::seconds(1.0));
    waitInternal("DEACT", _async);
}

void Basda::MoConNDeviceDummy::addProperties()
{
    Nice::JointPtrMap children = m_config->node("MOD")->children();

    if (children.size() == 0)
    {
        std::string errorMsg = "No modules defined!";
        F_THROW(::Basda::DevException, errorMsg);
    }

    std::vector<double> v(children.size(), 0);
    m_config->getWithDefault<std::vector<double> >(
            Basda::Mocca::Dev::PropertyName::limit(), v);

    Nice::BitSet bs((Nice::U32)children.size(), 0);
    m_config->getWithDefault<Nice::BitSet>(
            Basda::Mocca::Dev::PropertyName::atHome(), bs);
    m_config->getWithDefault<Nice::BitSet>(
            Basda::Mocca::Dev::PropertyName::moving(), bs);

    Nice::NPoint init(children.size(), 0.0);
    m_config->getWithDefault<Nice::NPointVar>(
            Basda::Mocca::MoCon::varMoconEncoderPosition(), init);
    m_config->getWithDefault<Nice::NPointVar>(
            Basda::Mocca::Dev::PropertyName::absoluteEncoderPosition(), init);
    m_config->getWithDefault<Nice::NPointVar>(
            Basda::Mocca::Dev::PropertyName::incrementalEncoderPosition(),
            init);
    m_config->getWithDefault<Nice::NPointVar>(
            Basda::Mocca::Dev::PropertyName::velocity(), init);

    m_config->leaf(Basda::Mocca::MoCon::varMoconEncoderPosition())
            ->getAttributeWithDefault<std::string>("UNIT", "steps");
    m_config->leaf(Basda::Mocca::Dev::PropertyName::absoluteEncoderPosition())
            ->getAttributeWithDefault<std::string>("UNIT", "steps");
    m_config->leaf(
            Basda::Mocca::Dev::PropertyName::incrementalEncoderPosition())
            ->getAttributeWithDefault<std::string>("UNIT", "steps");
    m_config->leaf(Basda::Mocca::Dev::PropertyName::velocity())
            ->getAttributeWithDefault<std::string>("UNIT", "steps/ms");

//    m_config->getWithDefault<Nice::NPoint>(
//            "CFG.VEL", Nice::NPoint(children.size(), 0.0));
    m_isReachable = false;
    m_config->setAtNode<bool>(Basda::Mocca::Dev::PropertyName::isReachable(),
                              m_isReachable);
}

Nice::Completion Basda::MoConNDeviceDummy::getProgress()
{
    int percent = 100; // for while loop entrance
    int tmp = 1000;

    std::map<std::string, MoConModuleDummyPtr>::iterator module_it =
            m_module.begin();

    while (module_it != m_module.end())
    {
        tmp = int((module_it->second)->getProgress().percent());

        if (tmp < percent)
        {
            percent = tmp;
        }
        module_it++;
    }

    m_completion.setPercent((int)percent);

    return m_completion;
}

void Basda::MoConNDeviceDummy::waitInternal(const std::string _cmd,
        Basda::DeviceAsync & _async)
{
    D_LOG("waitInternal " << _cmd);

    std::map<std::string, MoConModuleDummyPtr>::iterator jt = m_module.begin();

    Nice::BitSet motionComplete;
    motionComplete = 0;
    _async.asyncStatus(_cmd, 0);

    while (!isMotionComplete(motionComplete))
    {
        jt = m_module.begin();
        //update completion bar and status information like pos, vel
        _async.asyncStatus(_cmd, (int)getProgress().percent());

        for (int i = 0; i < motionComplete.depth && jt != m_module.end(); i++)
        {
            if (motionComplete.test(i) == 0)
                i = motionComplete.depth;
            else
                jt++;
        }

        if (jt != m_module.end())
            (jt->second)->waitForEvent(Nice::Time::milliSeconds(500), _async);
    }
    _async.asyncStatus(_cmd, 100);
    m_completion.setPercent(100);
}

void Basda::MoConNDeviceDummy::chat(std::string _commandID,
        std::string _cardNo,
        std::string _moduleNo,
        std::string _selectID,
        std::string _parameter,
        std::string /*_numberOfLines*/,
        Basda::DeviceAsync & /*_async*/)
{
    m_cardNo = _cardNo;
    m_moduleNo = _moduleNo;
    m_commandID = _commandID;
    unsigned int MPIA_SELECT_NOT_EXIST = 0; // temporary defined here but is defined in mpia/core/CommandID.h

    unsigned int selectID;
    std::stringstream s;
    s << _selectID;
    s >> selectID;
    W_LOG("Deprecated method! chat on card " << m_cardNo << " module " << m_moduleNo);
    if (!_parameter.empty()) // with parameter
    {
        if (selectID == MPIA_SELECT_NOT_EXIST) // without select ID
        {
            D_LOG(m_cardNo << " " << m_commandID << " " << m_moduleNo << " " << _parameter);
        }
        else // with select ID
        {
            D_LOG(m_cardNo << " " << m_commandID << " " << m_moduleNo << " " << _selectID << " " << _parameter);
        }
    }
    else // without parameter
    {
        if (selectID == MPIA_SELECT_NOT_EXIST) // without selectID
        {
            D_LOG(m_cardNo << " " << m_commandID << " " << m_moduleNo);
        }
        else // with select ID
        {
            D_LOG(m_cardNo << " " << m_commandID << " " << m_moduleNo << " " << " " << _selectID);
        }
    }
}

std::string Basda::MoConNDeviceDummy::dontParse(Basda::DeviceAsync & /*_async*/)
{
    W_LOG("Depreciated method! dontParse");
    std::stringstream s;
    s << m_cardNo << " " << m_commandID << " " << m_moduleNo << " 1";
    return s.str();
}

std::string Basda::MoConNDeviceDummy::receiveDataRaw(std::string _commandID,
        std::string _cardNo,
        std::string _moduleNo,
        std::string /*_numberOfLines*/,
        Basda::DeviceAsync & /*_async*/)
{
    W_LOG("Deprecated method! chat on card " << _cardNo << " module " << _moduleNo);
    std::stringstream s;
    s << _cardNo << " " << _commandID << " " << _moduleNo << " 1";
    return s.str();
}

void Basda::MoConNDeviceDummy::moveToHomeStart(Basda::DeviceAsync & /*_async*/)
{
    D_LOG("moveToHomeStart");

    m_completion = Nice::Completion::exec<Basda::Command::Constant>(Command::MOVETOHOME);
    m_completion.start(Basda::Command::Action::MOVETOHOME);

    if (m_completion.state() == Nice::BUSY)
    {
        std::map<std::string, MoConModuleDummyPtr>::iterator module_it =
                m_module.begin();

        try
        {
            while (module_it != m_module.end())
            {
                D_LOG("Motor " << module_it->first << " is moving to home.");
                (module_it->second)->setMotionComplete(false);
                (module_it->second)->moveToHome();
                module_it++;
            }
        }

        catch (::Basda::MoCon::OutOfRangeException &e)
        {
            stop();
            (module_it->second)->setMotionComplete(true);
            std::string errorMsg = "MoCon motion error. Stopped movement of device "
                    + m_config->name();
            m_completion.setError(errorMsg, e);
            E_THROW(::Basda::MoConFunctionException, e, errorMsg)
        }
    }
}

void Basda::MoConNDeviceDummy::moveToHomeWait(Basda::DeviceAsync & _async)
{
    D_LOG("moveToHomeWait")
    waitInternal(Basda::Command::Action::MOVETOHOME, _async);
    m_completion.setDone();
}

void Basda::MoConNDeviceDummy::moveToHomeSingleStart(int _motor,
        Basda::DeviceAsync & _async)
{
    D_LOG("moveToHomeSingleStart " << _motor);

    m_completion = Nice::Completion::exec<Basda::Command::Constant>(Command::MOVETOHOMESINGLE);
    m_completion.start(Basda::Command::Action::MOVETOHOMESINGLE);

    if (_motor > -1)
    {
        int motor = 0;
        if (m_completion.state() == Nice::BUSY)
        {
            std::map<std::string, MoConModuleDummyPtr>::iterator module_it =
                    m_module.begin();

            try
            {
                while (module_it != m_module.end())
                {
                    if (motor == _motor)
                    {
                        D_LOG("Motor " << module_it->first << " is moving to home.");
                        (module_it->second)->setMotionComplete(false);
                        (module_it->second)->moveToHome();
                    }
                    motor++;
                    module_it++;
                }
            }

            catch (::Basda::MoCon::OutOfRangeException &e)
            {
                stop();
                (module_it->second)->setMotionComplete(true);
                std::string errorMsg = "MoCon motion error. Stopped movement of device "
                        + m_config->name();
                m_completion.setError(errorMsg, e);
                E_THROW(::Basda::MoConFunctionException, e, errorMsg)
            }
        }
    }
    else
        moveToHomeStart(_async);
}

void Basda::MoConNDeviceDummy::moveToHomeSingleWait(Basda::DeviceAsync & _async)
{
    D_LOG("moveToHomeSingleWait")
    waitInternal(Basda::Command::Action::MOVETOHOMESINGLE, _async);
    m_completion.setDone();
}

void Basda::MoConNDeviceDummy::moveToLimitStart(const Nice::NPoint &_limit,
        Basda::DeviceAsync & /*_async*/)
{
    D_LOG("moveToLimit");

    m_completion = Nice::Completion::exec<Basda::Command::Constant>(Command::MOVETOLIMIT);
    m_completion.start(Basda::Command::Action::MOVETOLIMIT);

    std::map<std::string, MoConModuleDummyPtr>::iterator module_it =
            m_module.begin();

    std::vector<double>::const_iterator jt;
    for (jt = _limit.begin(), module_it = m_module.begin();
            jt != _limit.end() && module_it != m_module.end(); module_it++, jt++)
    {
        if (int(*jt) > 0)
        {
            D_LOG(module_it->first << " is moving to positive software limit");
        }
        else if (int(*jt) < 0)
        {
            D_LOG(module_it->first << " is moving to negative software limit");
        }
        (module_it->second)->setMotionComplete(false);
        (module_it->second)->moveToLimit(int(*jt));
    }
}

void Basda::MoConNDeviceDummy::moveToLimitWait(Basda::DeviceAsync & _async)
{
    D_LOG("moveToLimitWait")
    waitInternal(Basda::Command::Action::MOVETOLIMIT, _async);
    m_completion.setDone();
}

void Basda::MoConNDeviceDummy::moveToPositiveLimitStart(Basda::DeviceAsync & /*_async*/)
{
    E_LOG("not implemented");
}

void Basda::MoConNDeviceDummy::moveToPositiveLimitWait(Basda::DeviceAsync & /*_async*/)
{
    E_LOG("not implemented");
}

void Basda::MoConNDeviceDummy::moveToNegativeLimitStart(Basda::DeviceAsync & /*_async*/)
{
    E_LOG("not implemented");
}

void Basda::MoConNDeviceDummy::moveToNegativeLimitWait(Basda::DeviceAsync & /*_async*/)
{
    E_LOG("not implemented");
}

void Basda::MoConNDeviceDummy::stop()
{
    D_LOG("Stopping");
    std::map<std::string, MoConModuleDummyPtr>::iterator module_it =
            m_module.begin();

    while (module_it != m_module.end())
    {
        D_LOG("Stop moving " << module_it->first)
        (module_it->second)->stop();
        module_it++;
    }
    m_completion.setStop();
    m_completion.setDone();
}

void Basda::MoConNDeviceDummy::abort()
{
    D_LOG("Aborting");
    std::map<std::string, MoConModuleDummyPtr>::iterator module_it =
            m_module.begin();

    while (module_it != m_module.end())
    {
        D_LOG("Abort moving " << module_it->first)
        (module_it->second)->abort();
        module_it++;
    }
    m_completion.setAbort();
    m_completion.setDone();
}

void Basda::MoConNDeviceDummy::setIsReachable(const bool state)
{
    Nice::Guard guard(m_mutex);
    m_config->setAtNode<bool>(Basda::Mocca::Dev::PropertyName::isReachable(),
                              state);
    m_isReachable = state;
}

bool Basda::MoConNDeviceDummy::isReachable()
{
    D_LOG("Device " << m_config->name() << " is " << (m_isReachable ? "" : "not ") << "reachable.");
    Nice::Guard guard(m_mutex);
    return m_isReachable;
}

void Basda::MoConNDeviceDummy::prepareMoveSync(Nice::BitSet /*_motors*/)
{
    D_LOG("Not implemented yet!")
}

void Basda::MoConNDeviceDummy::startMoveSync()
{
    D_LOG("Not implemented yet!")
}

void Basda::MoConNDeviceDummy::prepareDirectMove(const Nice::NPoint &_acceleration,
                       const Nice::NPoint &_velocity,
                       const Nice::NPoint &_deceleration)
{
    D_LOG("prepareDirectMove");

    std::vector<double>::const_iterator acceleration_it;
    std::vector<double>::const_iterator velocity_it;
    std::vector<double>::const_iterator deceleration_it;
    std::map<std::string, MoConModuleDummyPtr>::iterator module_it;

    // set for the velocity of each motor
    for (acceleration_it = _acceleration.begin(), velocity_it = _velocity.begin(), deceleration_it = _deceleration.begin(), module_it = m_module.begin();
            acceleration_it != _acceleration.end() && velocity_it != _velocity.end() && deceleration_it != _deceleration.end() && module_it != m_module.end();
            acceleration_it++, velocity_it++, deceleration_it++, module_it++)
    {
        if (*acceleration_it != 0 && *velocity_it != 0)
        {
            D_LOG( "Set acceleration of motor " << module_it->first << " to " << *acceleration_it << " rev/min")
            (module_it->second)->setBasicSettings(
                    Basda::SelectBasicSettings::MOCON_SELECT_ACCELERATION, *acceleration_it);

            D_LOG( "Set velocity of motor " << module_it->first << " to " << *velocity_it << " rev/min")
            (module_it->second)->setBasicSettings(
                    Basda::SelectBasicSettings::MOCON_SELECT_VELOCITY, *velocity_it);

            D_LOG( "Set deceleration of motor " << module_it->first << " to " << *deceleration_it << " rev/min")
            (module_it->second)->setBasicSettings(
                    Basda::SelectBasicSettings::MOCON_SELECT_DECELERATION, *deceleration_it);
        }
    }
}

void Basda::MoConNDeviceDummy::moveAbsoluteStart(const Nice::NPoint &_absolutePositions,
        Basda::DeviceAsync & /*_async*/)
{
    D_LOG("moveAbsolute ");
    m_completion = Nice::Completion::exec<Basda::Command::Constant>(Command::MOVEABSOLUTE);
    m_completion.start(Basda::Command::Action::MOVEABSOLUTE);

    std::map<std::string, MoConModuleDummyPtr>::iterator module_it =
            m_module.begin();

    try
    {
        Nice::NPoint startPosition = getDeviceEncoderPosition();
        Nice::NPoint::iterator startPosition_it;
        Nice::NPoint::const_iterator absolutePositions_it;
        int i = 0;

        // all motors from the vector targetPosition and maximal the motors in
        // the moConPtrMap will move
        for (startPosition_it = startPosition.begin(), absolutePositions_it = _absolutePositions.begin(), module_it = m_module.begin();
                startPosition_it != startPosition.end() && absolutePositions_it != _absolutePositions.end() && module_it != m_module.end();
                absolutePositions_it++, module_it++, i++)
        {
            if (startPosition != *absolutePositions_it)
            {
                D_LOG( "Motor " << module_it->first << " is moving to absolute position " << *absolutePositions_it << " steps.");
                (module_it->second)->setMotionComplete(false);
                (module_it->second)->moveAbsolute((int) *absolutePositions_it);
            }
            else
            {
                D_LOG(m_config->name() << " [" << i << "] is already at the target position. Will not send extra move command.")
            }
        }
    }

    catch (::Basda::MoCon::OutOfRangeException &e)
    {
        stop();
        (module_it->second)->setMotionComplete(true);
        std::string errorMsg = "MoCon motion error. Stopped movement of device "
                + m_config->name();
        m_completion.setError(errorMsg, e);
        E_THROW(::Basda::MoConFunctionException, e, errorMsg)
    }
}

void Basda::MoConNDeviceDummy::moveAbsoluteWait(Basda::DeviceAsync & _async)
{
    D_LOG("moveAbsoluteWait")
    waitInternal(Basda::Command::Action::MOVEABSOLUTE, _async);
    m_completion.setDone();
}

void Basda::MoConNDeviceDummy::moveRelativeStart(const Nice::NPoint &_relativePositions,
        Basda::DeviceAsync & /*_async*/)
{
    D_LOG("moveRelative");

    m_completion = Nice::Completion::exec<Basda::Command::Constant>(Command::MOVERELATIVE);
    m_completion.start(Basda::Command::Action::MOVERELATIVE);

    std::map<std::string, MoConModuleDummyPtr>::iterator module_it =
            m_module.begin();

    try
    {
        Nice::NPoint::const_iterator it;
        int i = 0;

        // each motor in the argument map have to move to the relative position
        for (it = _relativePositions.begin(), module_it = m_module.begin();
                it != _relativePositions.end() && module_it != m_module.end();
                it++, module_it++)
        {
            // search for the given motor name in the map
            D_LOG( "Motor " << module_it->first << " is moving to relative position " << *it << " steps.");
            if (*it != 0)
            {
                (module_it->second)->setMotionComplete(false);
                (module_it->second)->moveRelative((int) *it);
            }
            else
            {
                D_LOG(m_config->name() << " [" << i << "] is already at the target position. Will not send extra move command.")
            }
        }
    }

    catch (::Basda::MoCon::OutOfRangeException &e)
    {
        stop();
        (module_it->second)->setMotionComplete(true);
        std::string errorMsg = "MoCon motion error. Stopped movement of device "
                + m_config->name();
        m_completion.setError(errorMsg, e);
        E_THROW(::Basda::MoConFunctionException, e, errorMsg)
    }
}

void Basda::MoConNDeviceDummy::moveRelativeWait(Basda::DeviceAsync & _async)
{
    D_LOG("moveRelativeWait")
    waitInternal(Basda::Command::Action::MOVERELATIVE, _async);
    m_completion.setDone();
}

Nice::NPoint Basda::MoConNDeviceDummy::getPosition()
{
    W_LOG("Deprecated method! Will be removed in the next version! Use getDeviceEncoderPosition() instead.")
    return getDeviceEncoderPosition();
}

Nice::NPoint Basda::MoConNDeviceDummy::getDeviceEncoderPosition()
{
    D_LOG("Get position of all motors.");

    Nice::NPoint currentPosition;

    std::map<std::string, MoConModuleDummyPtr>::iterator module_it =
            m_module.begin();
    size_t i = 0;
    while (module_it != m_module.end())
    {
        currentPosition.push_back((module_it->second)->getMEncPosition());
        module_it++;
        i++;
    }
    m_config->setAtNode<Nice::NPointVar>(
            Basda::Mocca::MoCon::varMoconEncoderPosition(), currentPosition);

    return currentPosition;
}

Nice::NPoint Basda::MoConNDeviceDummy::getAbsoluteEncoderPosition()
{
    D_LOG("Get absolute encoder position of all motors.");

    Nice::NPoint absEncPosition;

    std::map<std::string, MoConModuleDummyPtr>::iterator module_it =
            m_module.begin();

    while (module_it != m_module.end())
    {
        absEncPosition.push_back((module_it->second)->getAEncPosition());
        module_it++;
    }

    m_config->setAtNode<Nice::NPointVar>(
                    Basda::Mocca::Dev::PropertyName::absoluteEncoderPosition(), absEncPosition);
    return absEncPosition;
}

Nice::NPoint Basda::MoConNDeviceDummy::getIncrementalEncoderPosition()
{
    D_LOG("Get incremental encoder position of all motors.");

    Nice::NPoint incEncPosition;

    std::map<std::string, MoConModuleDummyPtr>::iterator module_it =
            m_module.begin();

    size_t i = 0;
    while (module_it != m_module.end())
    {
        incEncPosition.push_back((module_it->second)->getIEncPosition());
        module_it++;
        i++;
    }

    m_config->setAtNode<Nice::NPointVar>(
                    Basda::Mocca::Dev::PropertyName::incrementalEncoderPosition(), incEncPosition);
    return incEncPosition;
}

void Basda::MoConNDeviceDummy::setPosition(const Nice::NPoint &_position)
{
    D_LOG("Setting position to " << _position);

    std::map<std::string, MoConModuleDummyPtr>::iterator module_it =
            m_module.begin();

    size_t i = 0;
    while (module_it != m_module.end())
    {
        (module_it->second)->setPosition((int)_position[i]);
        module_it++;
        i++;
    }
}

Nice::NPoint Basda::MoConNDeviceDummy::getVelocity()
{
    Nice::NPoint velocity;

    std::map<std::string, MoConModuleDummyPtr>::iterator module_it =
            m_module.begin();

    size_t i = 0;
    while (module_it != m_module.end())
    {
        velocity.push_back((module_it->second)->getVelocity());
        module_it++;
        i++;
    }

    m_config->getWithDefault<Nice::NPointVar>(
            Basda::Mocca::Dev::PropertyName::velocity(), velocity);

    return velocity;
}

Nice::SeqSeqDouble Basda::MoConNDeviceDummy::getBasicSettings()
{
    D_LOG("Get basic setting of all motors.");
    Nice::SeqSeqDouble basicSettings;

    std::map<std::string, MoConModuleDummyPtr>::iterator module_it = m_module.begin();

    // ask for the basic settings of each motor
    while (module_it != m_module.end())
    {
        (module_it->second)->getBasicSettings();
        module_it++;
    }

    Nice::JointPtrMap children = m_config->node("MOD")->children();
    std::map<std::string, Nice::JointPtr>::iterator children_it =
            children.begin();

    while (children_it != children.end())
    {
        Nice::SeqDouble basicSetting;

        basicSetting.push_back(m_config->getAtNode<Nice::U16>(
                std::string("MOD.") + children_it->first + "."
                        + Basda::Mocca::MoCon::cfgDatasheetBasicProfile()));
        basicSetting.push_back(m_config->getAtNode<Nice::U16>(
                std::string("MOD.") + children_it->first + "."
                        + Basda::Mocca::MoCon::cfgDatasheetBasicResolution()));
        basicSetting.push_back(m_config->getAtNode<double>(
                std::string("MOD.") + children_it->first + "."
                        + Basda::Mocca::MoCon::cfgDatasheetBasicVelocityStraight()));
        basicSetting.push_back(m_config->getAtNode<double>(
                std::string("MOD.") + children_it->first + "."
                        + Basda::Mocca::MoCon::cfgDatasheetBasicVelocityStart()));
        basicSetting.push_back(m_config->getAtNode<Nice::U32>(
                std::string("MOD.") + children_it->first + "."
                        + Basda::Mocca::MoCon::cfgDatasheetBasicVelocityAcceleration()));
        basicSetting.push_back(m_config->getAtNode<Nice::U32>(
                std::string("MOD.") + children_it->first + "."
                        + Basda::Mocca::MoCon::cfgDatasheetBasicVelocityDeceleration()));
        basicSetting.push_back(m_config->getAtNode<Nice::U32>(
                std::string("MOD.") + children_it->first + "."
                        + Basda::Mocca::MoCon::cfgDatasheetBasicJerk()));

        basicSettings.push_back(basicSetting);
        children_it++;
    }

    return basicSettings;
}

void Basda::MoConNDeviceDummy::setVelocitySetting(const Nice::NPoint &_velocity)
{
    D_LOG("setVelocitySetting");

    std::vector<double>::const_iterator velocity_it;
    std::map<std::string, MoConModuleDummyPtr>::iterator module_it;

    // set for the velocity of each motor
    for (velocity_it = _velocity.begin(), module_it = m_module.begin();
            velocity_it != _velocity.end() && module_it != m_module.end();
            velocity_it++, module_it++)
    {
        D_LOG( "Set velocity of motor " << module_it->first << " to " << *velocity_it << " rev/min")
        (module_it->second)->setBasicSettings(
                Basda::SelectBasicSettings::MOCON_SELECT_VELOCITY, *velocity_it);
    }
}

void Basda::MoConNDeviceDummy::setAccelerationSetting(const Nice::NPoint &_acceleration)
{
    D_LOG("setAccelerationSetting");

    std::vector<double>::const_iterator acceleration_it;
    std::map<std::string, MoConModuleDummyPtr>::iterator module_it;

    // set for the acceleration of each motor
    for (acceleration_it = _acceleration.begin(), module_it = m_module.begin();
            acceleration_it != _acceleration.end() && module_it != m_module.end();
            acceleration_it++, module_it++)
    {
        D_LOG( "Set acceleration of motor " << module_it->first << " to " << *acceleration_it << " rev/min")
        (module_it->second)->setBasicSettings(
                Basda::SelectBasicSettings::MOCON_SELECT_ACCELERATION, *acceleration_it);
    }
}

void Basda::MoConNDeviceDummy::setDecelerationSetting(const Nice::NPoint &_deceleration)
{
    D_LOG("setDecelerationSetting");

    std::vector<double>::const_iterator deceleration_it;
    std::map<std::string, MoConModuleDummyPtr>::iterator module_it;

    // set for the deceleration of each motor
    for (deceleration_it = _deceleration.begin(), module_it = m_module.begin();
            deceleration_it != _deceleration.end() && module_it != m_module.end();
            deceleration_it++, module_it++)
    {
        D_LOG( "Set deceleration of motor " << module_it->first << " to " << *deceleration_it << " rev/min")
        (module_it->second)->setBasicSettings(
                Basda::SelectBasicSettings::MOCON_SELECT_DECELERATION, *deceleration_it);
    }
}

void Basda::MoConNDeviceDummy::setJerkSetting(const Nice::NPoint &_jerk)
{
    D_LOG("setJerkSetting");

    std::vector<double>::const_iterator jerk_it;
    std::map<std::string, MoConModuleDummyPtr>::iterator module_it;

    // set for the jerk of each motor
    for (jerk_it = _jerk.begin(), module_it = m_module.begin();
            jerk_it != _jerk.end() && module_it != m_module.end();
            jerk_it++, module_it++)
    {
        D_LOG( "Set jerk of motor " << module_it->first << " to " << *jerk_it << " rev/min")
        (module_it->second)->setBasicSettings(
                Basda::SelectBasicSettings::MOCON_SELECT_JERK, *jerk_it);
    }
}

bool Basda::MoConNDeviceDummy::isAtLimit(Nice::NPoint & _limit)
{
    std::map<std::string, MoConModuleDummyPtr>::iterator module_it =
            m_module.begin();
    Nice::BitSet bs;
    size_t i = 0;

    bs.setDepth((unsigned int) m_module.size());

    _limit.clear();
    while (module_it != m_module.end())
    {
        _limit.push_back((module_it->second)->isAtLimit());
        bs[i] = (bool) fabs(_limit[i]);
        module_it++;
        i++;
    }
    m_config->node(Basda::Mocca::Dev::PropertyName::limit())
            ->set<Nice::NPointVar>(_limit);

    return (bs.count() == (unsigned int) bs.depth);
}

bool Basda::MoConNDeviceDummy::isMoving(Nice::BitSet &_isMoving)
{
    std::map<std::string, MoConModuleDummyPtr>::iterator it = m_module.begin();

    int i = 0;
    _isMoving.setDepth((unsigned int) m_module.size());
    while (it != m_module.end())
    {
        _isMoving[i] = (it->second)->isMoving();
        it++;
        i++;
    }

    m_config->setAtNode<Nice::BitSet>(Basda::Mocca::Dev::PropertyName::moving(),
                                      _isMoving);

    D_LOG("isMoving " << _isMoving);
    return (_isMoving.count() > 0);
}

bool Basda::MoConNDeviceDummy::isAtHome(Nice::BitSet &_atHome)
{
    D_LOG("isAtHome");
    std::map<std::string, MoConModuleDummyPtr>::iterator it = m_module.begin();
    int i = 0;

    _atHome.setDepth((unsigned int) m_module.size());

    while (it != m_module.end())
    {
        _atHome[i] = (it->second)->isAtHome();
        it++;
        i++;
    }

    m_config->node(Basda::Mocca::Dev::PropertyName::atHome())->set<Nice::BitSet>(
            _atHome);

    return _atHome.count() == _atHome.getDepth();
}

bool Basda::MoConNDeviceDummy::isMotionComplete(Nice::BitSet &_motion)
{
    D_LOG("isMotionComplete");
    std::map<std::string, MoConModuleDummyPtr>::iterator it = m_module.begin();

    int i = 0;
    _motion.setDepth((unsigned int) m_module.size());
    while (it != m_module.end())
    {
        // set the MOTION_COMPLETE flag to true if one of the module has done the motion
        _motion[i] = (it->second)->isMotionComplete();
        it++;
        i++;
    }

    m_config->setAtNode<Nice::BitSet>(
            Basda::Mocca::MoCon::varMotionComplete(), _motion);

    Nice::BitSet _bs;
    _bs = _motion;
    _bs = ~_bs;

    return (_bs.value == 0);
}

void Basda::MoConNDeviceDummy::setLinkTimeout(Nice::Time _tcpConnectionTimeout)
{
    m_tcpConnectionTimeout = _tcpConnectionTimeout;
}


Nice::SeqTime Basda::MoConNDeviceDummy::getLinkTimeout()
{
    return Nice::SeqTime(2, m_tcpConnectionTimeout);
}

Nice::NPoint Basda::MoConNDeviceDummy::getReferenceTime()
{
    D_LOG("Get reference time from all modules.");
    std::map<std::string, MoConModuleDummyPtr>::iterator it = m_module.begin();

    Nice::NPoint referenceTime;

    // each motor in the argument map have to move home
    while (it != m_module.end())
    {
        D_LOG("synchronize time of module " << it->first);
        referenceTime.push_back((double) (it->second)->getReferenceTime());
        it++;
    }
    return referenceTime;
}

Nice::NPoint Basda::MoConNDeviceDummy::getCurrentTime()
{
    D_LOG("Get current time from all modules.");
    std::map<std::string, MoConModuleDummyPtr>::iterator it = m_module.begin();

    Nice::NPoint currentTime;

    // each motor in the argument map have to move home
    while (it != m_module.end())
    {
        D_LOG("get current time of module " << it->first);
        currentTime.push_back((double) (it->second)->getCurrentTime());
        it++;
    }
    return currentTime;
}

void Basda::MoConNDeviceDummy::checkMotionError()
{
    W_LOG("Not implemented yet.")
}

void Basda::MoConNDeviceDummy::startExternalProfile(
        const Nice::Date &_date,
        const Nice::NPoint &_positionSequence,
        unsigned int _frequency,
        unsigned int _maxSamplesPerSegment,
        unsigned int _faultTolerance,
        Basda::DeviceAsync &_async)
{
    I_LOG("Derotation requested starting at " << _date)
    U2_LOG("Derotation requested at " << _date << " with parameter: frequency:" << _frequency << " _maxSamplesPerSegment:" << _maxSamplesPerSegment << " _faultTolerance:" << _faultTolerance);
    m_externalProfileInfo = -1;
    unsigned int minSamplesPerSegment = 5;
    double contingency = 1.1;  // 10% contingency to the MoCon simulation
                               // decrease the number the trigger will raise too late
                               // increase the number the trigger will raise too early
    std::map<std::string, MoConModuleDummyPtr>::iterator module_it = m_module.begin();
    Nice::Date currentBlockEndDate;

    // trajectory service (derotator) has only a single motor
    if (module_it != m_module.end())
    {
        m_completion = Nice::Completion::exec<Basda::Command::Constant>(Command::STARTEXTERNALPROFILE);
        m_completion.start(Basda::Command::Action::STARTEXTERNALPROFILE);

        _async.asyncStatus(Basda::Command::Action::STARTEXTERNALPROFILE, 0);

        Nice::BitSet motionComplete;
        (module_it->second)->setMotionComplete(false);
        try
        {
            (module_it->second)->clearExternalProfileBuffer();
        }

        catch (Basda::MoCon::NoBufferDefinedException &e)
        {
//                D_LOG("Initial clear buffer for next profile. Exception ignored.");
        }

        Nice::Time segmentUploadTime = (module_it->second)->initExternalProfile(
                _frequency, _maxSamplesPerSegment, minSamplesPerSegment,
                _faultTolerance);

        Nice::JointPtrMap children = m_config->node("MOD")->children();
        std::map<std::string, Nice::JointPtr>::iterator children_it = children
                .begin();
        Nice::U32 segmentsPerBlock =
                (children_it->second)->getAtNode<Nice::U32>(
                        Basda::Mocca::MoCon::cfgDatasheetSegmentsPerBlock());

        double maxNumberOfSegmentsPerBlock = segmentsPerBlock
                * _maxSamplesPerSegment / minSamplesPerSegment;

        Nice::Time blockUploadTime = segmentUploadTime
                * maxNumberOfSegmentsPerBlock;

        generateExternalProfile(_date, _positionSequence);
        //bufferSize = 1024*1024 / 8(modules) / 4(32 bit = long int) / 5(traj values)
        (module_it->second)->setExternalProfileBufferSize((unsigned int) (6000), true);

        Nice::Date uploadFinished = m_niceDateDummy->now() + blockUploadTime;

        if (uploadFinished < _date)
            currentBlockEndDate = (module_it->second)->uploadExternalProfile();
        else
        {
            std::string errorMsg = "External profile start is too close ("
                    + _date.toString()
                    + "). Segment uploading will take longer ("
                    + uploadFinished.toString() + ")";
            m_completion.setError(
                    errorMsg,
                    Basda::Dev::MoccaUploadExternalProfileException());
            E_THROW( Basda::Dev::MoccaUploadExternalProfileException, errorMsg)
        }
        (module_it->second)->setExternalProfileStart(
                Basda::ValueStartMode::MOCON_MODE_START_FROM_BEGINNING, _date);

        I_LOG("Wait for External Profile execution.");
        waitForNextTrigger(_date - blockUploadTime * contingency, _async);
        I_LOG("External Profile starts now.")

        Nice::Date nextTrigger;
        // in case the stop button is pressed, the Mocon will send a motion complete event
        // and the trajectory handling procedure will stop as well
        while (!isMotionComplete(motionComplete))
        {
            // if the next trigger is in the future than wait for it
            if ((currentBlockEndDate - (blockUploadTime * contingency))
                    > m_niceDateDummy->now())
            {
                nextTrigger = currentBlockEndDate
                        - (blockUploadTime * contingency);
                if (!(module_it->second)->isExternalProfileDone())
                {
                    U2_LOG("Current block ends at " << currentBlockEndDate);
    //                U7_LOG("currentBlockEndDate - blockUploadTime * contingency " << currentBlockEndDate - blockUploadTime * contingency);
                    U2_LOG("Next trigger will be at " << nextTrigger);
                }
                waitForNextTrigger(nextTrigger, _async);

                if (!(module_it->second)->isExternalProfileDone())
                {
                    U2_LOG("Trigger raised for next block at " << nextTrigger << ". Now is it: " << Nice::Date::now());
                }
            }

            // upload blocks only when the block end date is in the future
            if (currentBlockEndDate > m_niceDateDummy->now()
                    && !isMotionComplete(motionComplete)
                    && !(module_it->second)->isExternalProfileDone())
            {
                U2_LOG("Upload next block of segments now.");
                currentBlockEndDate = (module_it->second)->uploadExternalProfile();
                if (!(module_it->second)->isExternalProfileDone()) // no log when trajectory is done
                {
                    U2_LOG("Next block of segments are uploaded. Next block should start at " << currentBlockEndDate);
                }
            }

            // in case the block end date is in the past (because the whole buffer
            // is uploaded) we will wait till we receive the trajectory event
            // or throw an exception when the motion is done but the trajectory not completed
            if (currentBlockEndDate < m_niceDateDummy->now())
            {
                if (!(module_it->second)->isExternalProfileDone())
                {
                    if (!isMotionComplete(motionComplete))
                        (module_it->second)->setExternalProfileStop();
                    break;
//                    std::string errorMsg = "Trigger to upload next external profile segment was too late. Start external profile again.";
//                    m_completion.setError(
//                            errorMsg,
//                            Basda::Dev::MoccaUploadExternalProfileException());
//                    E_THROW(Basda::Dev::MoccaUploadExternalProfileException, errorMsg)
                }
                else
                {
                    // optimize predict trajectory end
                    nextTrigger = m_niceDateDummy->now() + Nice::Time::milliSeconds(500);
//                    U2_LOG("wait for completion event till " << nextTrigger)
                    waitForNextTrigger(nextTrigger, _async); // is needed to wait for
                                                             // the completion event
                                                             // at the end of the profile
//                    U2_LOG("wait for completion event done.")
                }
            }
        }

        try
        {
            D_LOG("clear buffer")
            (module_it->second)->clearExternalProfileBuffer();
        }

        catch (Basda::MoCon::NoBufferDefinedException &e)
        {
//           D_LOG("Initial clear buffer for next profile. Exception ignored.");
        }

        if (!(module_it->second)->isExternalProfileDone())
        {
            std::string errorMsg = "Trigger to upload next external profile segment was too late. Start external profile again.";
            m_completion.setError(
                    errorMsg,
                    Basda::Dev::MoccaUploadExternalProfileException());
            E_THROW(Basda::Dev::MoccaUploadExternalProfileException, errorMsg)
        }
        else
        {
            I_LOG("trajectory done")
            _async.asyncStatus(Basda::Command::Action::STARTEXTERNALPROFILE, 100);
        }

        if ((m_completion.state() == Nice::EXEC) || (m_completion.state() == Nice::BUSY))
            m_completion.setDone();
    }
}


void Basda::MoConNDeviceDummy::shinsStartExternalProfile(
        const Nice::Date &_date,
        const Nice::NPoint &_positionSequence,
        unsigned int _frequency,
        unsigned int _maxSamplesPerSegment,
        unsigned int _faultTolerance,
        Basda::DeviceAsync &_async)
{
    I_LOG("Derotation requested starting at " << _date)
    U2_LOG("Derotation requested at " << _date << " with parameter: frequency:" << _frequency << " _maxSamplesPerSegment:" << _maxSamplesPerSegment << " _faultTolerance:" << _faultTolerance);
    m_externalProfileInfo = -1;
    unsigned int minSamplesPerSegment = 5;
    double contingency = 1.1;  // 10% contingency to the MoCon simulation
                               // decrease the number the trigger will raise too late
                               // increase the number the trigger will raise too early
    std::map<std::string, MoConModuleDummyPtr>::iterator module_it = m_module.begin();
    Nice::Date currentBlockEndDate;

    // trajectory service (derotator) has only a single motor
    if (module_it != m_module.end())
    {
        m_completion = Nice::Completion::exec<Basda::Command::Constant>(Command::SHINSSTARTEXTERNALPROFILE);
        m_completion.start(Basda::Command::Action::SHINSSTARTEXTERNALPROFILE);

        _async.asyncStatus(Basda::Command::Action::SHINSSTARTEXTERNALPROFILE, 0);

        Nice::BitSet motionComplete;
        (module_it->second)->setMotionComplete(false);
        try
        {
            (module_it->second)->clearExternalProfileBuffer();
        }

        catch (Basda::MoCon::NoBufferDefinedException &e)
        {
//                D_LOG("Initial clear buffer for next profile. Exception ignored.");
        }

        Nice::Time segmentUploadTime = (module_it->second)->initExternalProfile(
                _frequency, _maxSamplesPerSegment, minSamplesPerSegment,
                _faultTolerance);

        Nice::JointPtrMap children = m_config->node("MOD")->children();
        std::map<std::string, Nice::JointPtr>::iterator children_it = children
                .begin();
        Nice::U32 segmentsPerBlock =
                (children_it->second)->getAtNode<Nice::U32>(
                        Basda::Mocca::MoCon::cfgDatasheetSegmentsPerBlock());

        double maxNumberOfSegmentsPerBlock = segmentsPerBlock
                * _maxSamplesPerSegment / minSamplesPerSegment;

        Nice::Time blockUploadTime = segmentUploadTime
                * maxNumberOfSegmentsPerBlock;

        generateExternalProfile(_date, _positionSequence);
        //bufferSize = 1024*1024 / 8(modules) / 4(32 bit = long int) / 5(traj values)
        (module_it->second)->setExternalProfileBufferSize((unsigned int) (6000), true);

        Nice::Date uploadFinished = m_niceDateDummy->now() + blockUploadTime;

        if (uploadFinished < _date)
            currentBlockEndDate = (module_it->second)->uploadExternalProfile();
        else
        {
            std::string errorMsg = "External profile start is too close ("
                    + _date.toString()
                    + "). Segment uploading will take longer ("
                    + uploadFinished.toString() + ")";
            m_completion.setError(
                    errorMsg,
                    Basda::Dev::MoccaUploadExternalProfileException());
            E_THROW( Basda::Dev::MoccaUploadExternalProfileException, errorMsg)
        }
        (module_it->second)->setExternalProfileStart(
                Basda::ValueStartMode::MOCON_MODE_START_FROM_BEGINNING, _date);

        I_LOG("Wait for External Profile execution.");
        shinsWaitForNextTrigger(_date - blockUploadTime * contingency, _async);
        I_LOG("External Profile starts now.")

        Nice::Date nextTrigger;
        // in case the stop button is pressed, the Mocon will send a motion complete event
        // and the trajectory handling procedure will stop as well
        while (!isMotionComplete(motionComplete))
        {
            // if the next trigger is in the future than wait for it
            if ((currentBlockEndDate - (blockUploadTime * contingency))
                    > m_niceDateDummy->now())
            {
                nextTrigger = currentBlockEndDate
                        - (blockUploadTime * contingency);
                if (!(module_it->second)->isExternalProfileDone())
                {
                    U2_LOG("Current block ends at " << currentBlockEndDate);
    //                U7_LOG("currentBlockEndDate - blockUploadTime * contingency " << currentBlockEndDate - blockUploadTime * contingency);
                    U2_LOG("Next trigger will be at " << nextTrigger);
                }
                shinsWaitForNextTrigger(nextTrigger, _async);

                if (!(module_it->second)->isExternalProfileDone())
                {
                    U2_LOG("Trigger raised for next block at " << nextTrigger << ". Now is it: " << Nice::Date::now());
                }
            }

            // upload blocks only when the block end date is in the future
            if (currentBlockEndDate > m_niceDateDummy->now()
                    && !isMotionComplete(motionComplete)
                    && !(module_it->second)->isExternalProfileDone())
            {
                U2_LOG("Upload next block of segments now.");
                currentBlockEndDate = (module_it->second)->uploadExternalProfile();
                if (!(module_it->second)->isExternalProfileDone()) // no log when trajectory is done
                {
                    U2_LOG("Next block of segments are uploaded. Next block should start at " << currentBlockEndDate);
                }
            }

            // in case the block end date is in the past (because the whole buffer
            // is uploaded) we will wait till we receive the trajectory event
            // or throw an exception when the motion is done but the trajectory not completed
            if (currentBlockEndDate < m_niceDateDummy->now())
            {
                if (!(module_it->second)->isExternalProfileDone())
                {
                    if (!isMotionComplete(motionComplete))
                        (module_it->second)->setExternalProfileStop();
                    break;
//                    std::string errorMsg = "Trigger to upload next external profile segment was too late. Start external profile again.";
//                    m_completion.setError(
//                            errorMsg,
//                            Basda::Dev::MoccaUploadExternalProfileException());
//                    E_THROW(Basda::Dev::MoccaUploadExternalProfileException, errorMsg)
                }
                else
                {
                    // optimize predict trajectory end
                    nextTrigger = m_niceDateDummy->now() + Nice::Time::milliSeconds(500);
//                    U2_LOG("wait for completion event till " << nextTrigger)
                    shinsWaitForNextTrigger(nextTrigger, _async); // is needed to wait for
                                                             // the completion event
                                                             // at the end of the profile
//                    U2_LOG("wait for completion event done.")
                }
            }
        }

        try
        {
            D_LOG("clear buffer")
            (module_it->second)->clearExternalProfileBuffer();
        }

        catch (Basda::MoCon::NoBufferDefinedException &e)
        {
//           D_LOG("Initial clear buffer for next profile. Exception ignored.");
        }

        if (!(module_it->second)->isExternalProfileDone())
        {
            std::string errorMsg = "Trigger to upload next external profile segment was too late. Start external profile again.";
            m_completion.setError(
                    errorMsg,
                    Basda::Dev::MoccaUploadExternalProfileException());
            E_THROW(Basda::Dev::MoccaUploadExternalProfileException, errorMsg)
        }
        else
        {
            I_LOG("trajectory done")
            _async.asyncStatus(Basda::Command::Action::SHINSSTARTEXTERNALPROFILE, 100);
        }

        if ((m_completion.state() == Nice::EXEC) || (m_completion.state() == Nice::BUSY))
            m_completion.setDone();
    }
}

void Basda::MoConNDeviceDummy::waitForNextTrigger(Nice::Date _date,
        Basda::DeviceAsync &_async)
{
    // @toDo: stop also the while loop when the Mocon cycle time is reached
    // for a next upload
    Nice::BitSet motionComplete;
    Nice::Time time;

    while (_date > m_niceDateDummy->now() && !isMotionComplete(motionComplete))
    {
//        U6_LOG("next trigger is " << _date << ". current time is " << m_niceDateDummy->now())
        time = _date - m_niceDateDummy->now();
        if (time > Nice::Time::milliSeconds(500))
            time = Nice::Time::milliSeconds(500);

        if (time > Nice::Time::microSeconds(1))
            _async.asyncWait(time);

        int index = (m_module.begin()->second)->getExternalProfileInfo();
        if (index > -1)
        {
            if (index != m_externalProfileInfo)
            {
                U2_LOG("Trajectory segment " << index << " of " << m_externalProfileSize << " is in progress. ")
                m_externalProfileInfo = index;
            }
            _async.asyncStatus(Basda::Command::Action::STARTEXTERNALPROFILE,
                               (int) getProgress().percent());
        }

//        int index = (m_module.begin()->second)->getExternalProfileInfo();
//        if (index > -1)
//            _async.asyncStatus(Basda::Command::Action::STARTEXTERNALPROFILE,
//                               (int) (100.0 / m_externalProfileSize * index));
    }
}

void Basda::MoConNDeviceDummy::shinsWaitForNextTrigger(Nice::Date _date, Basda::DeviceAsync &_async)
{
    // @toDo: stop also the while loop when the Mocon cycle time is reached
    // for a next upload
    Nice::BitSet motionComplete;
    Nice::Time time;

    while (_date > m_niceDateDummy->now() && !isMotionComplete(motionComplete))
    {
//        U6_LOG("next trigger is " << _date << ". current time is " << m_niceDateDummy->now())
        time = _date - m_niceDateDummy->now();
        if (time > Nice::Time::milliSeconds(500))
            time = Nice::Time::milliSeconds(500);

        if (time > Nice::Time::microSeconds(1))
            _async.asyncWait(time);

        int index = (m_module.begin()->second)->getExternalProfileInfo();
        if (index > -1)
        {
            if (index != m_externalProfileInfo)
            {
                U2_LOG("Trajectory segment " << index << " of " << m_externalProfileSize << " is in progress. ")
                m_externalProfileInfo = index;
            }
            _async.asyncStatus(Basda::Command::Action::SHINSSTARTEXTERNALPROFILE,
                               (int) getProgress().percent());
        }

//        int index = (m_module.begin()->second)->getExternalProfileInfo();
//        if (index > -1)
//            _async.asyncStatus(Basda::Command::Action::STARTEXTERNALPROFILE,
//                               (int) (100.0 / m_externalProfileSize * index));
    }
}

void Basda::MoConNDeviceDummy::generateExternalProfile(
        const Nice::Date &_date,
        const Nice::NPoint &_positionSequence)
{
        U2_LOG("Generate trajectory starting at " << _date)
        std::map<std::string, MoConModuleDummyPtr>::iterator module_it = m_module.begin();

        // trajectory service has only a single motor
        if (module_it != m_module.end())
        {
            try
            {
                (module_it->second)->calculateTransition(_date, _positionSequence);
                (module_it->second)->fitTrajectory();
                (module_it->second)->validateExternalProfile();
                m_externalProfileSize = (module_it->second)->bufferExternalProfile();
            }

            catch (Basda::MoCon::NoBufferDefinedException &e)
            {
//               D_LOG("Initial clear buffer for next profile. Exception ignored.");
            }
        }
}

void Basda::MoConNDeviceDummy::changeExternalProfile(
        const Nice::Date &_date,
        const Nice::NPoint &_positionSequence)
{
    I_LOG("Trajectory will be changed at " << _date)
    std::map<std::string, MoConModuleDummyPtr>::iterator module_it = m_module.begin();

    // trajectory service has only a single motor
    if (module_it != m_module.end())
    {
        try
        {
            double basicResolution = m_config->getAtNode<Nice::U16>(
                            std::string("MOD.") + m_config->node("MOD")->children().begin()->first + "."
                                    + Basda::Mocca::MoCon::cfgDatasheetBasicResolution());
            double basicVelocity = m_config->getAtNode<double>(
                            std::string("MOD.") + m_config->node("MOD")->children().begin()->first + "."
                                    + Basda::Mocca::MoCon::cfgDatasheetBasicVelocityStraight());

            bool transitionIsNotValid = true;
            Nice::Time transitionLength = Nice::Time::seconds(
                    (getDeviceEncoderPosition()[0] - _positionSequence[0])
                    / (basicVelocity * basicResolution / 60.0));

            if (transitionLength < Nice::Time::seconds(1))
                transitionLength = Nice::Time::seconds(1);

            while (transitionIsNotValid)
            {
                try
                {
                    (module_it->second)->calculateTransition(_date, _positionSequence, transitionLength);
                    (module_it->second)->fitTrajectory();
                    (module_it->second)->validateExternalProfile();
                    m_externalProfileSize = (module_it->second)->bufferExternalProfile();
                    I_LOG("Trajectory changed successfully.")
                    transitionIsNotValid = false;
                }

                catch (::Basda::MoCon::SegmentLengthUnderrunException &e)
                {
                    transitionLength += Nice::Time::seconds(1);
                    D_LOG("SegmentLengthUnderrunException. Procedure increases time to " << transitionLength << " and try to fit again.")
                }

                catch (Basda::MoCon::ProfileTransitionException &e)
                {
                    transitionLength += Nice::Time::seconds(1);
                    D_LOG("Transition to short. Procedure increases time to " << transitionLength << " and try to fit again.")
                }

                catch (Basda::MoCon::InvalidProfileException &e)
                {
                    transitionLength += Nice::Time::seconds(1);
                    D_LOG(e)
                    D_LOG("Transition to short. Procedure increases time to " << transitionLength << " and try to fit again.")
                }

                catch (Basda::MoCon::ProfileTransitionStartToEarlyException &e)
                {
                    D_LOG("Transition to new trajectory was not feasible.")
                    E_THROW(Basda::Dev::MoccaChangeExternalProfileException, "Transition to new trajectory was not feasible.")
                }
            }
        }

        catch (Basda::MoCon::NoBufferDefinedException &e)
        {
//                D_LOG("Initial clear buffer for next profile. Exception ignored.");
        }

    }
}

void Basda::MoConNDeviceDummy::prepareExternalProfile(const Nice::SeqSeqInt &_segmentsX,
                                                const Nice::SeqSeqInt &_segmentsY,
                                                Basda::DeviceAsync &_async)
{
    m_completion = Nice::Completion::exec<Basda::Command::Constant>(Command::PREPAREEXTERNALPROFILE);
    m_completion.start(Basda::Command::Action::PREPAREEXTERNALPROFILE);

    (m_module.begin()->second)->uploadExternalProfile(_segmentsX);
    ((++m_module.begin())->second)->uploadExternalProfile(_segmentsY);

    _async.asyncStatus(Basda::Command::Action::PREPAREEXTERNALPROFILE, 100);
    m_completion.setDone();
}

void Basda::MoConNDeviceDummy::startExternalProfile(Basda::DeviceAsync &_async)
{
    m_completion = Nice::Completion::exec<Basda::Command::Constant>(Command::STARTEXTERNALPROFILE);
    m_completion.start(Basda::Command::Action::STARTEXTERNALPROFILE);

    std::map<std::string, MoConModuleDummyPtr>::iterator moduleX_it = m_module.begin();
    std::map<std::string, MoConModuleDummyPtr>::iterator moduleY_it = m_module.begin();
    moduleY_it++;

    (moduleX_it->second)->setExternalProfileStart(
                    Basda::ValueStartMode::MOCON_MODE_START_FROM_BEGINNING, Nice::Date::microSeconds(-1));
    (moduleX_it->second)->setMotionComplete(false);

    (moduleY_it->second)->setExternalProfileStart(
                    Basda::ValueStartMode::MOCON_MODE_START_FROM_BEGINNING, Nice::Date::microSeconds(-1));
    (moduleY_it->second)->setMotionComplete(false);

    _async.asyncStatus(Basda::Command::Action::STARTEXTERNALPROFILE, 100);
    m_completion.setDone();
}

void Basda::MoConNDeviceDummy::shinsStartExternalProfile(Basda::DeviceAsync &_async)
{
    m_completion = Nice::Completion::exec<Basda::Command::Constant>(Command::SHINSSTARTEXTERNALPROFILE);
    m_completion.start(Basda::Command::Action::SHINSSTARTEXTERNALPROFILE);

    std::map<std::string, MoConModuleDummyPtr>::iterator moduleX_it = m_module.begin();
    std::map<std::string, MoConModuleDummyPtr>::iterator moduleY_it = m_module.begin();
    moduleY_it++;

    (moduleX_it->second)->setExternalProfileStart(
                    Basda::ValueStartMode::MOCON_MODE_START_FROM_BEGINNING, Nice::Date::microSeconds(-1));
    (moduleX_it->second)->setMotionComplete(false);

    (moduleY_it->second)->setExternalProfileStart(
                    Basda::ValueStartMode::MOCON_MODE_START_FROM_BEGINNING, Nice::Date::microSeconds(-1));
    (moduleY_it->second)->setMotionComplete(false);

    _async.asyncStatus(Basda::Command::Action::SHINSSTARTEXTERNALPROFILE, 100);
    m_completion.setDone();
}

void Basda::MoConNDeviceDummy::setBrokenSwitches(Nice::BitSet _brokenPositionSwitchMask, int _fromPosition, int _toPosition)
{
    std::map<std::string, MoConModuleDummyPtr>::iterator it = m_module.begin();

    // each motor in the argument map have to move home
    while (it != m_module.end())
    {
        it->second->setBrokenSwitches(_brokenPositionSwitchMask, _fromPosition, _toPosition);
        it++;
    }
}

void Basda::MoConNDeviceDummy::setWheelPositionOffset(int _positionOffset)
{
    std::map<std::string, MoConModuleDummyPtr>::iterator it = m_module.begin();

    // each motor in the argument map have to move home
    while (it != m_module.end())
    {
        it->second->setWheelPositionOffset(_positionOffset);
        it++;
    }
}

Nice::SeqBitSet Basda::MoConNDeviceDummy::getPositionSwitchStatus()
{
    D_LOG("Get status of all position switches from all motors.")
    Nice::SeqBitSet positionSwitchStatus;

    Nice::BitSet mask = m_config->getWithDefault<Nice::BitSet>(Basda::Mocca::Dev::PropertyName::cfgSwitchMask(), Nice::BitSet(8, 255));

    std::map<std::string, MoConModuleDummyPtr>::iterator it = m_module.begin();

    // each motor in the argument map have to move home
    while (it != m_module.end())
    {
        positionSwitchStatus.push_back(it->second->getPositionSwitchStatus() & mask);
        it++;
    }

    return positionSwitchStatus;
}

void Basda::MoConNDeviceDummy::dumpStatusInformation(Basda::DeviceAsync &/*_asynch*/,
                                                     bool /*_compareParameter*/)
{
    C_LOG("Not implemented yet.")
}

void Basda::MoConNDeviceDummy::setCollision(Nice::SeqPoint _collisions)
{
    D_LOG("Indicate all motors that have a collision.");
    std::map<std::string, MoConModuleDummyPtr>::iterator module_it = m_module.begin();

    unsigned int i = 0;
    while ((module_it != m_module.end()) && (i < _collisions.size()))
    {
        module_it->second->setCollision(_collisions[i].x() != 0, _collisions[i].y() != 0);
        module_it++;
        i++;
    }
}
