/* $Header: https://svn.mpia.de/gulli/TwiceAsNice/branch/unstable/TwiceAsNice/basda-mocca/src/device/mocca/NDeviceDummy/NDeviceDummy.cc 15660 2019-02-20 13:43:46Z briegel $
*/

#include <NDeviceDummy.h>
#include <Basda/DeviceFactory.h>
#include <Basda/Mocca/Constant.h>
#include <Nice/Time/Timer.h>
#include <boost/assign/list_of.hpp> // for 'map_list_of()'
#include <boost/assert.hpp>
#include <boost/array.hpp>
#include <Nice/Map/Helper.h>
#include <Nice/Property/Types.h>
#include <Nice/Property/BitSet.h>
#include <Basda/Mocca/PropertyName.h>
#include "PropertyName.h"

#undef LOG_NAME
#define LOG_NAME m_name

DEVICE_CREATE(Basda::MoccaNDeviceDummy);

Basda::MoccaNDeviceDummy::MoccaNDeviceDummy(const Nice::JointPtr & _config) :
        MoccaNDevice(_config)
{
    m_inMotion = false;

    Nice::JointPtrMap children = m_config->node("MOD")->children();

    Nice::NPoint v = Nice::NPoint(children.size(), 0.0);
    m_config->getWithDefault<Nice::NPoint >(
            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::NPoint>(
            Basda::Mocca::Dev::PropertyName::position(), init);
    m_config->getWithDefault<Nice::NPoint>(
            Basda::Mocca::Dev::PropertyName::absoluteEncoderPosition(), init);
    m_config->getWithDefault<Nice::NPoint>(
            Basda::Mocca::Dev::PropertyName::incrementalEncoderPosition(),
            init);
    m_config->getWithDefault<Nice::NPoint>(
            Basda::Mocca::Dev::PropertyName::velocity(), init);

    m_config->leaf(Basda::Mocca::Dev::PropertyName::position())
            ->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<double>(
            Basda::Mocca::DummyDev::PropertyName::datasheetVelocity(),
            100.0);
    m_config->leaf(
            Basda::Mocca::DummyDev::PropertyName::datasheetVelocity())
            ->getAttributeWithDefault<double>("MAX", 81916250.0);
    m_config->leaf(
            Basda::Mocca::DummyDev::PropertyName::datasheetVelocity())
            ->getAttributeWithDefault<double>("MIN", 0);
    m_config->leaf(
            Basda::Mocca::DummyDev::PropertyName::datasheetVelocity())
            ->getAttributeWithDefault<std::string>("UNIT", "steps/ms");

    std::map<std::string, Nice::JointPtr>::iterator it = children.begin();
    Nice::Point travelRange;

    while (it != children.end())
    {
        travelRange = (it->second)->node(
                Basda::Mocca::Dev::PropertyName::datasheetTravelRange())
                ->get<Nice::Point>();
        m_minPosition.push_back(travelRange.xp);
        m_maxPosition.push_back(travelRange.yp);
        it++;
    }

    Nice::NPoint home(children.size(), 0.0);
    m_commandedPosition = PositionBlock(home, 1.0);

    const double absEncResolutionStepsPerRev= m_config->getWithDefault<double>(
        Basda::Mocca::DummyDev::PropertyName::
        datasheetEncoderAbsoluteSteps(), 1.0);
    assert(0 < absEncResolutionStepsPerRev);
    m_absEncPosition = PositionBlock(home, absEncResolutionStepsPerRev);
    m_incEncPosition = PositionBlock(home, 1.0);
    Nice::Date now = Nice::Date::now();
    m_referenceTime = Nice::NPoint(2, now.toMicroSecondsDouble());

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

    m_tcpConnectionTimeout = Nice::Time::seconds(3600);

   // TODO 2013-04-23 With the following invocation NDeviceDummyTest fails:
    Nice::Application::distribute(m_config);
    D_LOG("device created")
}

Basda::MoccaNDeviceDummy::~MoccaNDeviceDummy()
{
    T_LOG("destruct");
}

Nice::Completion Basda::MoccaNDeviceDummy::getProgress()
{
    m_completion.setPercent(100);

    if (!m_timer.elapsed() && m_completion.state() == Nice::BUSY)
    {
        m_completion.setPercent(100
                    - (int) (100.0 * m_timer.now().toSecondsDouble()
                            / m_duration.toSecondsDouble()));
    }

    return m_completion;
}

void Basda::MoccaNDeviceDummy::startTimer(double _distance)
{
    double velocity = m_config->getAtNode<double>(
            Basda::Mocca::DummyDev::PropertyName::datasheetVelocity());

    m_duration = Nice::Time::milliSeconds(int(_distance / velocity));

    if (m_duration > Nice::Time::seconds(0.5))
    {
        m_timer.start(m_duration);
    }
}

void Basda::MoccaNDeviceDummy::waitInternal(const std::string _s,
        Basda::DeviceAsync & _async)
{
    if (m_duration > Nice::Time::seconds(0.5))
    {
        while (!m_timer.elapsed() && m_completion.state() == Nice::BUSY)
        {
            _async.asyncStatus(
                    _s,
                    (int)getProgress().percent());

            _async.asyncWait(
                    m_timer.now() < Nice::Time::seconds(0.33) ?
                            m_timer.now() : Nice::Time::seconds(0.33));
        }
        _async.asyncStatus(_s, 100);
        m_completion.setPercent(100);
    }
    else
    {
       _async.asyncStatus(_s, 100);
        m_completion.setPercent(100);
        _async.asyncWait(m_duration);
    }
}

void Basda::MoccaNDeviceDummy::open()
{
    D_LOG("OPEN");
    setIsReachable(true);
}

void Basda::MoccaNDeviceDummy::close()
{
    D_LOG("CLOSE");
    setIsReachable(false);
}

void Basda::MoccaNDeviceDummy::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(Basda::Command::Action::RESET, _async);
}

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

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

void Basda::MoccaNDeviceDummy::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(Basda::Command::Action::DEINIT, _async);
}

void Basda::MoccaNDeviceDummy::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(Basda::Command::Action::ACTIVATE, _async);
}

void Basda::MoccaNDeviceDummy::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(Basda::Command::Action::DEACTIVATE, _async);
}

void Basda::MoccaNDeviceDummy::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::MoccaNDeviceDummy::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::MoccaNDeviceDummy::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::MoccaNDeviceDummy::prepareMoveSync(Nice::BitSet /*_motors*/)
{
    D_LOG("Not implemented yet!")
}

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

void Basda::MoccaNDeviceDummy::prepareDirectMove(const Nice::NPoint &/*_acceleration*/,
                       const Nice::NPoint &/*_velocity*/,
                       const Nice::NPoint &/*_deceleration*/)
{
    D_LOG("Not implemented yet!")
}

void Basda::MoccaNDeviceDummy::moveToHomeStart(Basda::DeviceAsync & /*_async*/)
{
    m_completion = Nice::Completion::exec<Basda::Command::Constant>(Command::MOVETOHOME);
    m_completion.start(Basda::Command::Action::MOVETOHOME);

    m_commandedPosition.start = m_commandedPosition.current;
    m_absEncPosition.start = m_absEncPosition.current;
    m_incEncPosition.start = m_incEncPosition.current;

    m_commandedPosition.target = m_commandedPosition.home;
    m_absEncPosition.target = m_absEncPosition.home;
    m_incEncPosition.target = m_incEncPosition.home;

    double distance = fabs((m_commandedPosition.target - m_commandedPosition.start).absmax());
    startTimer(distance);
}

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

void Basda::MoccaNDeviceDummy::moveToHomeSingleStart(int _motor,
        Basda::DeviceAsync & _async)
{
    if (_motor > -1)
    {
        m_completion = Nice::Completion::exec<Basda::Command::Constant>(Command::MOVETOHOMESINGLE);
        m_completion.start(Basda::Command::Action::MOVETOHOMESINGLE);

        m_commandedPosition.start = m_commandedPosition.current;
        m_absEncPosition.start = m_absEncPosition.current;
        m_incEncPosition.start = m_incEncPosition.current;

        m_commandedPosition.target         = m_commandedPosition.current;
        m_commandedPosition.target[_motor] = m_commandedPosition.home[_motor];

        m_absEncPosition.target         = m_absEncPosition.current;
        m_absEncPosition.target[_motor] = m_absEncPosition.home[_motor];

        m_incEncPosition.target         = m_incEncPosition.current;
        m_incEncPosition.target[_motor] = m_incEncPosition.home[_motor];

        double distance = fabs(m_commandedPosition.target[_motor] - m_commandedPosition.start[_motor]);
        startTimer(distance);
    }
    else
        moveToHomeStart(_async);
}

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

void Basda::MoccaNDeviceDummy::moveToLimitStart(const Nice::NPoint &_pos,
        Basda::DeviceAsync & /*_async*/)
{
    N_LOG("Moving to limit " << _pos)

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

    size_t s = _pos.size();

    m_commandedPosition.start = m_commandedPosition.current;
    m_absEncPosition.start = m_absEncPosition.current;
    m_incEncPosition.start = m_incEncPosition.current;


    for (size_t i = 0; i < s; i++)
    {
        if (_pos[i] < 0)
        {
            m_commandedPosition.target[i] = m_minPosition[i];
            m_absEncPosition.target[i] = m_minPosition[i] * m_absEncPosition.resolutionCmdPosToEncPos;
            m_incEncPosition.target[i] = m_minPosition[i] * m_incEncPosition.resolutionCmdPosToEncPos;
        }
        else if (_pos[i] > 0)
        {
            m_commandedPosition.target[i] = m_maxPosition[i];
            m_absEncPosition.target[i] = m_maxPosition[i] * m_absEncPosition.resolutionCmdPosToEncPos;
            m_incEncPosition.target[i] = m_maxPosition[i] * m_incEncPosition.resolutionCmdPosToEncPos;
        }
    }

    double distance = fabs((m_commandedPosition.target - m_commandedPosition.start).absmax());
    startTimer(distance);
}

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

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

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

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

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

void Basda::MoccaNDeviceDummy::stop()
{
    N_LOG("Stopping");
    D_LOG("doing something to stop");
    m_completion.setStop();
}

void Basda::MoccaNDeviceDummy::abort()
{
    N_LOG("Aborting");
    D_LOG("doing something to abort");
    m_completion.setAbort();
}

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

bool Basda::MoccaNDeviceDummy::isReachable()
{
    Nice::Guard guard(m_mutex);
    return m_isReachable;
}

void Basda::MoccaNDeviceDummy::moveAbsoluteStart(const Nice::NPoint &_pos,
        Basda::DeviceAsync & /*_async*/)
{
    N_LOG("Moving absolute to " << _pos);

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

    m_commandedPosition.start = m_commandedPosition.current;
    m_absEncPosition.start = m_absEncPosition.current;
    m_incEncPosition.start = m_incEncPosition.current;

    m_commandedPosition.target = _pos;
    m_absEncPosition.target = _pos * m_absEncPosition.resolutionCmdPosToEncPos;
    m_incEncPosition.target = _pos * m_incEncPosition.resolutionCmdPosToEncPos;

    size_t s = m_commandedPosition.current.size();

    for (size_t i = 0; i < s; i++)
    {
        if (m_commandedPosition.target[i] < m_minPosition[i])
        {
            m_commandedPosition.target[i] = m_minPosition[i];
            m_absEncPosition.target[i] = m_minPosition[i] * m_absEncPosition.resolutionCmdPosToEncPos;
            m_incEncPosition.target[i] = m_minPosition[i] * m_incEncPosition.resolutionCmdPosToEncPos;
        }
        else if (m_commandedPosition.target[i] > m_maxPosition[i])
        {
            m_commandedPosition.target[i] = m_maxPosition[i];
            m_absEncPosition.target[i] = m_maxPosition[i] * m_absEncPosition.resolutionCmdPosToEncPos;
            m_incEncPosition.target[i] = m_maxPosition[i] * m_incEncPosition.resolutionCmdPosToEncPos;
        }
    }

    double distance = fabs((m_commandedPosition.target - m_commandedPosition.start).absmax());
    startTimer(distance);
}

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

void Basda::MoccaNDeviceDummy::moveRelativeStart(const Nice::NPoint &_pos,
        Basda::DeviceAsync & /*_async*/)
{
    N_LOG("Moving relative by " << _pos)

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

    m_commandedPosition.start = m_commandedPosition.current;
    m_absEncPosition.start = m_absEncPosition.current;
    m_incEncPosition.start = m_incEncPosition.current;

    m_commandedPosition.target = m_commandedPosition.current + _pos;
    m_absEncPosition.target = m_absEncPosition.current + _pos * m_absEncPosition.resolutionCmdPosToEncPos;
    m_incEncPosition.target = m_incEncPosition.current + _pos * m_incEncPosition.resolutionCmdPosToEncPos;

    size_t s = m_commandedPosition.current.size();

    for (size_t i = 0; i < s; i++)
    {
        if (m_commandedPosition.target[i] < m_minPosition[i])
        {
            m_commandedPosition.target[i] = m_minPosition[i];
            m_absEncPosition.target[i] = m_minPosition[i] * m_absEncPosition.resolutionCmdPosToEncPos;
            m_incEncPosition.target[i] = m_minPosition[i] * m_incEncPosition.resolutionCmdPosToEncPos;
        }
        else if (m_commandedPosition.target[i] > m_maxPosition[i])
        {
            m_commandedPosition.target[i] = m_maxPosition[i];
            m_absEncPosition.target[i] = m_maxPosition[i] * m_absEncPosition.resolutionCmdPosToEncPos;
            m_incEncPosition.target[i] = m_maxPosition[i] * m_incEncPosition.resolutionCmdPosToEncPos;
        }
    }

    double distance = fabs((m_commandedPosition.target - m_commandedPosition.start).absmax());
    startTimer(distance);
}

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

Nice::NPoint Basda::MoccaNDeviceDummy::getPosition()
{
    return getDeviceEncoderPosition();
}

Nice::NPoint Basda::MoccaNDeviceDummy::getDeviceEncoderPosition()
{
    D_LOG("Get position of all motors.")
    Nice::NPoint distance = m_commandedPosition.target - m_commandedPosition.start;
    m_commandedPosition.current = m_commandedPosition.start + distance * getProgress().percent() / 100.0;

    size_t s = m_commandedPosition.current.size();

    for (size_t i = 0; i < s; i++)
    {
        if (m_commandedPosition.current[i] < m_minPosition[i])
            m_commandedPosition.current[i] = m_minPosition[i];
        else if (m_commandedPosition.current[i] > m_maxPosition[i])
            m_commandedPosition.current[i] = m_maxPosition[i];
    }

    m_config->setAtNode<Nice::NPoint>(
                Basda::Mocca::Dev::PropertyName::position(), m_commandedPosition.current);
    return m_commandedPosition.current;
}

Nice::NPoint Basda::MoccaNDeviceDummy::getAbsoluteEncoderPosition()
{
    D_LOG("Get absolute encoder position of all motors.")
    Nice::NPoint distance = m_absEncPosition.target - m_absEncPosition.start;
    m_absEncPosition.current = m_absEncPosition.start + distance * getProgress().percent() / 100.0;

    m_config->setAtNode<Nice::NPoint>(
                    Basda::Mocca::Dev::PropertyName::absoluteEncoderPosition(), m_absEncPosition.current);
    return m_absEncPosition.current;
}

Nice::NPoint Basda::MoccaNDeviceDummy::getIncrementalEncoderPosition()
{
    D_LOG("Get incremental encoder position of all motors.")
    Nice::NPoint distance = m_incEncPosition.target - m_incEncPosition.start;
    m_incEncPosition.current = m_incEncPosition.start + distance * getProgress().percent() / 100.0;

    m_config->setAtNode<Nice::NPoint>(
                    Basda::Mocca::Dev::PropertyName::incrementalEncoderPosition(), m_incEncPosition.current);
    return m_incEncPosition.current;
}

void Basda::MoccaNDeviceDummy::setPosition(const Nice::NPoint &_position)
{
    N_LOG("Setting position to " << _position)

    m_commandedPosition.current = _position;
    //needed for getPosition
    m_commandedPosition.start = _position;
    m_commandedPosition.target = _position;

    m_incEncPosition.current = _position * m_incEncPosition.resolutionCmdPosToEncPos;
    m_incEncPosition.start = _position * m_incEncPosition.resolutionCmdPosToEncPos;
    m_incEncPosition.target = _position * m_incEncPosition.resolutionCmdPosToEncPos;

    m_config->setAtNode<Nice::NPoint>(
                Basda::Mocca::Dev::PropertyName::position(), m_commandedPosition.current);
    m_config->setAtNode<Nice::NPoint>(
                    Basda::Mocca::Dev::PropertyName::incrementalEncoderPosition(), m_incEncPosition.current);
}

Nice::NPoint Basda::MoccaNDeviceDummy::getVelocity()
{
    return m_config->getAtNode<Nice::NPoint>(
            Basda::Mocca::Dev::PropertyName::velocity());;
}

Nice::SeqSeqDouble Basda::MoccaNDeviceDummy::getBasicSettings()
{
    C_LOG("Not implemented yet.")
    return Nice::SeqSeqDouble();
}

void Basda::MoccaNDeviceDummy::setVelocitySetting(const Nice::NPoint &_velocity)
{
    m_config->setAtNode<Nice::NPoint>(
                Basda::Mocca::DummyDev::PropertyName::datasheetVelocity(), _velocity);
}

void Basda::MoccaNDeviceDummy::setAccelerationSetting(const Nice::NPoint &/*_acceleration*/)
{
    C_LOG("Not implemented yet.")
}

void Basda::MoccaNDeviceDummy::setDecelerationSetting(const Nice::NPoint &/*_deceleration*/)
{
    C_LOG("Not implemented yet.")
}

void Basda::MoccaNDeviceDummy::setJerkSetting(const Nice::NPoint &/*_jerk*/)
{
    C_LOG("Not implemented yet.")
}

bool Basda::MoccaNDeviceDummy::isAtLimit(Nice::NPoint & _limit)
{
    size_t s = m_commandedPosition.current.size();
    _limit.resize(s);
    bool status = false;
    for (size_t i = 0; i < s; i++)
    {
        if (m_minPosition.isNull() || m_maxPosition.isNull())
        {
            _limit[i] = 0.0;
        }
        else if (Nice::Helper::fuzzyCompare(m_commandedPosition.current[i], m_minPosition[i]))
        {
            _limit[i] = -1.0;
            status = true;
        }
        else if (Nice::Helper::fuzzyCompare(m_commandedPosition.current[i], m_maxPosition[i]))
        {
            _limit[i] = 1.0;
            status = true;
        }
        else
        {
            _limit[i] = 0.0;
        }
    }

    m_config->node(Basda::Mocca::Dev::PropertyName::limit())
            ->set<Nice::NPoint>(_limit);
    return status;
}

bool Basda::MoccaNDeviceDummy::isMoving(Nice::BitSet &_isMoving)
{
    _isMoving.setDepth((Nice::U32)m_commandedPosition.current.size());
    size_t s = m_commandedPosition.current.size();
    for (size_t i = 0; i < s; i++)
    {
        _isMoving[i] = true;
    }
    m_config->setAtNode<Nice::BitSet>(Basda::Mocca::Dev::PropertyName::moving(),
                                      _isMoving);
    return m_inMotion;
}

bool Basda::MoccaNDeviceDummy::isAtHome(Nice::BitSet &_atHome)
{
    _atHome.setDepth((Nice::U32)m_commandedPosition.current.size());
    size_t s = m_commandedPosition.current.size();
    for (size_t i = 0; i < s; i++)
    {
        if (m_commandedPosition.current[i] == 0)
            _atHome[i] = true;
        else
            _atHome[i] = false;
    }

    Nice::NPoint home(m_commandedPosition.current.size(), 0.0);
    bool h = false;
    if (m_commandedPosition.current == home)
        h = true;

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

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

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

Nice::NPoint Basda::MoccaNDeviceDummy::getReferenceTime()
{
    return m_referenceTime;
}

Nice::NPoint Basda::MoccaNDeviceDummy::getCurrentTime()
{
    Nice::Date now = Nice::Date::now();
    return Nice::NPoint(2, now.toMicroSecondsDouble());
}

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

void Basda::MoccaNDeviceDummy::startExternalProfile(
        const Nice::Date & _startDate,
        const Nice::NPoint & _positionSequence,
        unsigned int _frequency,
        unsigned int _samplesPerSegment,
        unsigned int _faultTolerance,
        Basda::DeviceAsync & _async)
{
    m_duration = Nice::Time::seconds(_positionSequence.size() /  _frequency);
    
    m_completion = Nice::Completion::exec<Basda::Command::Constant>(Command::STARTEXTERNALPROFILE);
    m_completion.start(Basda::Command::Action::STARTEXTERNALPROFILE);
//    m_completion.setFinish(Nice::Date::now()+m_duration);
    m_timer.start(m_duration);
    N_LOG(_startDate << " Duration:" << m_duration << " " << !m_timer.elapsed());
    waitInternal(Basda::Command::Action::STARTEXTERNALPROFILE, _async);
}

void Basda::MoccaNDeviceDummy::shinsStartExternalProfile(
        const Nice::Date & _startDate,
        const Nice::NPoint & _positionSequence,
        unsigned int _frequency,
        unsigned int _samplesPerSegment,
        unsigned int _faultTolerance,
        Basda::DeviceAsync & _async)
{
    m_duration = Nice::Time::seconds(_positionSequence.size() /  _frequency);
    
    m_completion = Nice::Completion::exec<Basda::Command::Constant>(Command::SHINSSTARTEXTERNALPROFILE);
    m_completion.start(Basda::Command::Action::SHINSSTARTEXTERNALPROFILE);
//    m_completion.setFinish(Nice::Date::now()+m_duration);
    m_timer.start(m_duration);
    N_LOG(_startDate << " Duration:" << m_duration << " " << !m_timer.elapsed());
    waitInternal(Basda::Command::Action::SHINSSTARTEXTERNALPROFILE, _async);
}

void Basda::MoccaNDeviceDummy::changeExternalProfile(
        const Nice::Date &/*_date*/,
        const Nice::NPoint &/*_positionSequence*/)
{
    I_LOG("External profile simulation is not implemented yet")
}

void Basda::MoccaNDeviceDummy::prepareExternalProfile(const Nice::SeqSeqInt &/*_segmentsX*/,
                                                const Nice::SeqSeqInt &/*_segmentsY*/,
                                                Basda::DeviceAsync &/*_async*/)
{
    I_LOG("External profile simulation is not implemented yet")
}

void Basda::MoccaNDeviceDummy::startExternalProfile(Basda::DeviceAsync &/*_async*/)
{
    I_LOG("External profile simulation is not implemented yet")
}

void Basda::MoccaNDeviceDummy::shinsStartExternalProfile(Basda::DeviceAsync &/*_async*/)
{
    I_LOG("Shins External profile simulation is not implemented yet")
}

Nice::SeqBitSet Basda::MoccaNDeviceDummy::getPositionSwitchStatus()
{
    Nice::SeqBitSet sbs;
    Nice::BitSet bs;
//    Nice::BitSet bs(32, 254); //RETURNS : 0xFE
    bs.setDepth(m_config->getWithDefault(Basda::Mocca::Dev::PropertyName::cfgSwitchDepth(), 32));
    bs.setValue(254); //RETURNS : 0xFE

    sbs.push_back(bs);
    return sbs;
}

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

void Basda::MoccaNDeviceDummy::setCollision(Nice::SeqPoint /*_collisions*/)
{
    C_LOG("Not implemented yet.")
}

