/* $Header: https://oberon.oapd.inaf.it/shins/trunk/tan/basda-mocca/src/service/wheel/MoccaWheelServiceWorker.cpp 3436 2020-09-04 10:04:29Z depa $
*/

#include <MoccaWheelServiceWorker.h>
#include <Basda/Mocca/NServiceWorkerCore.hh>
#include <Basda/Mocca/PropertyName.h>
#include <Basda/Mocca/MPIA/PropertyName.h>

template class Basda::MoccaNServiceWorkerCore<double, Nice::Int, Nice::Point,
        Basda::MoccaWheelServiceWorkerSkeleton>;

#undef LOG_NAME
#define LOG_NAME this->m_service->name()

int Basda::MoccaWheelServiceWorker::WheelDeviceAsyncImpl::asyncWait(
        const Nice::Time & _t)
{
    return m_service->Basda::ServiceWorker::timedWaitForNewState(_t);
}

int Basda::MoccaWheelServiceWorker::WheelDeviceAsyncImpl::asyncStatus(
        const std::string & _s, int _d)
{
    m_service->Basda::ServiceWorker::sendStateStatus(_s, _d);
    return 0;
}

int Basda::MoccaWheelServiceWorker::WheelDeviceCmdAsyncImpl::asyncStatus(
        const std::string & _s, int _d)
{
    int status = 0;
    // if the command name is not NOSTATUS save the current command name for later use
//    if (_s.compare("NOSTATUS") != 0) //needed for uploadProfile
//    {
//    Basda::MoccaNServiceWorkerCore<DevType, SvcType, TransType, SkelType>::DeviceAsyncImpl::m_s =
//            _s;
//
//    Basda::MoccaNServiceWorkerCore<DevType, SvcType, TransType, SkelType>::DeviceAsyncImpl::m_d =
//                _d;
//    }
//
    if (_d > Basda::Completion::STATUS)
    {
        Basda::MoccaWheelServiceWorker::WheelDeviceAsyncImpl::m_s = _s;
        Basda::MoccaWheelServiceWorker::WheelDeviceAsyncImpl::m_d = _d;
        Basda::MoccaWheelServiceWorker::WheelDeviceAsyncImpl::m_service
                ->cmdAction(_d, this->m_s);
    }

    // While a [i] command is executed, a [o] command can be called in parallel.
    // When the [o] command is done, it set back the completion bar to 0% (DONE)
    // and the completion of the [i] command is not displayed any more
    // (especially the ping pong of the moveToHome command).
    // The following IF checks the whether a [o] command was executed while
    // a [i] command is running and sets back the completion of the [i] command
    // by overwriting the global CMD completion with the running command completion
    if ((m_service->m_completion != m_service->m_cmd->get<Nice::Completion>()))
        Basda::MoccaWheelServiceWorker::WheelDeviceAsyncImpl::m_service
                ->cmdAction(this->m_d, this->m_s);

//
//    // if the command name is not NOSTATUS, update the status information
//    if (_s.compare("NOSTATUS") != 0) //needed for uploadProfile
//    {

    try
    {
        D_LOG("update status parameter: positions")
        Basda::MoccaWheelServiceWorker::WheelDeviceAsyncImpl::m_service
                ->getPosition();
        D_LOG("update status parameter: velocity")
        Basda::MoccaWheelServiceWorker::WheelDeviceAsyncImpl::m_service
                ->getVelocity();
        D_LOG("update status parameter: limits")
        Basda::MoccaWheelServiceWorker::WheelDeviceAsyncImpl::m_service
                ->isAtLimit();
        D_LOG("update status parameter: motion flags")
        Basda::MoccaWheelServiceWorker::WheelDeviceAsyncImpl::m_service
                ->isMoving();
        D_LOG("update status parameter: home flags")
        Basda::MoccaWheelServiceWorker::WheelDeviceAsyncImpl::m_service
                ->isAtHome();
        D_LOG("update status parameter: named position")
        Basda::MoccaWheelServiceWorker::WheelDeviceAsyncImpl::m_service
                ->checkForNamedPosition();
        D_LOG("update status parameter: position switch")
        try
        {
            Basda::MoccaWheelServiceWorker::WheelDeviceAsyncImpl::m_service
                    ->getPositionSwitchStatus();
        }

        catch (::Basda::Dev::MoccaCmdRejectedByDeviceException &e)
        {
            D_LOG("MoCon reject request to update position switch status because another service is updating the position switch status. Position switch status will be updated next time.")
        }
    }

    catch (Nice::Exception &e)
    {
        E_LOG(e);
        E_THROW(::Basda::Svc::MoccaServiceException,
                "Error during updating status parameter.")
    }

    if (m_s == Basda::Command::Action::MOVETOHOME)
    {
        Basda::MoccaWheelServiceWorker::WheelDeviceAsyncImpl::m_service
                ->checkFullTurn();
    }

//    }
    return status;
}

#undef LOG_NAME
#define LOG_NAME Basda::ServiceWorker::name()

Basda::MoccaWheelServiceWorker::MoccaWheelServiceWorker(
        const Nice::JointPtr & _config, const MoccaNDevicePtr &_device) :
        Basda::MoccaNServiceWorkerCore<double, Nice::Int, Nice::Point,
                Basda::MoccaWheelServiceWorkerSkeleton>(_config, _device)
{
    D_LOG("construct");

    bool warning = true;
    m_scanAllReferenceSwitches = false;
    m_homeReferenceSwitchFound = false;
    m_positionOffset = 0;
    m_startPosition = 0;

    //
    // apply a bridge in order to get notice of the coordinate transformation changes
    Nice::PropertyBridgeMemFn<Nice::Any, MoccaWheelServiceWorker> *bridge =
      new Nice::PropertyBridgeMemFn<Nice::Any, MoccaWheelServiceWorker>(
                                                                    this, &MoccaWheelServiceWorker::transformationFactorChanged,
                                                                    m_id);
    Nice::JointPtrMap children = m_device->config()->node(
                                                          Basda::Mocca::Dev::PropertyName::transformation())->children();
    std::map<std::string, Nice::JointPtr>::iterator jt = children.begin();
    while (jt != children.end())
      {
        (jt->second)->leaf()->applyBridge(
                                          bridge, "MoccaNServiceWorkerCore_SCALE_" + (jt->first));
        jt++;
      }
    transformationFactorChanged(Nice::Any());

    m_config->leaf("CMD.MOVETONAMEDPOSITION")
            ->applyAttribute<std::vector<std::string> >(
            "NAMED_POSITION+" + Nice::QtWidgetAttributeName::select(),
            m_namedPosAbsNameString);
    Basda::MoccaWheelMoveToNamedPosition& cmdMoveToNamedPosition = m_config
            ->leaf("CMD.MOVETONAMEDPOSITION")
            ->ref<Basda::MoccaWheelMoveToNamedPosition>();
    cmdMoveToNamedPosition.namedposition = 0;

    std::map<unsigned int, Nice::BitSet> namedSwitchesMap;
    Nice::JointPtrMap namedPositionSwitchChildren = m_device->config()->node(
            Basda::Mocca::Dev::PropertyName::namedPositionSwitch())->children();

    std::map<std::string, Nice::JointPtr>::iterator namedPositionSwitch_it =
            namedPositionSwitchChildren.begin();
    while (namedPositionSwitch_it != namedPositionSwitchChildren.end())
    {
        std::stringstream ss;
        ss << namedPositionSwitch_it->first;
        unsigned int index;
        ss >> index;
        namedSwitchesMap.insert(
                std::make_pair(
                        index,
                        (namedPositionSwitch_it->second)->get<Nice::BitSet>()));
        namedPositionSwitch_it++;
    }
    for (int i = 0; i < (int) namedSwitchesMap.size(); i++)
    {
        m_namedSwitches.push_back(namedSwitchesMap.at(i));
    }
    m_config->leaf("CMD")
                ->getAttributeWithDefault<Nice::SeqBitSet>(
                Basda::Mocca::Svc::AttributeName::namedReference(),
                m_namedSwitches);
    m_scannedSwitches = m_namedSwitches; // first we guess that everything works

    std::map<unsigned int, Nice::Int> namedPositionAbsMap;
    Nice::JointPtrMap namedPositionAbsChildren = m_device->config()->node(
            Basda::Mocca::Dev::PropertyName::namedPositionAbs())->children();

    std::map<std::string, Nice::JointPtr>::iterator namedPositionAbs_it =
            namedPositionAbsChildren.begin();
    while (namedPositionAbs_it != namedPositionAbsChildren.end())
    {
        std::stringstream ss;
        ss << namedPositionAbs_it->first;
        unsigned int index;
        ss >> index;
        namedPositionAbsMap.insert(
                std::make_pair(
                        index,
                        (namedPositionAbs_it->second)->get<Nice::Int>()));
        namedPositionAbs_it++;
    }
    for (int i = 0; i < (int) namedPositionAbsMap.size(); i++)
    {
        m_namedPositions.push_back(namedPositionAbsMap.at(i));
    }

    std::string defaultUnit = m_device->config()->Nice::Joint::getWithDefault<std::string>(
            Basda::Mocca::Dev::PropertyName::positionUnit(), m_basicUnit,
            warning);
    defaultUnit = boost::to_upper_copy(defaultUnit);

    Basda::ServiceWorker::config()->Nice::Joint::getWithDefault<Nice::Int>(
            Basda::Mocca::Svc::PropertyName::positionRemainder(), 0);
    Basda::ServiceWorker::config()->leaf(
            Basda::Mocca::Svc::PropertyName::positionRemainder())
            ->Nice::Property::applyAttribute<std::string>(
            Basda::Mocca::Svc::AttributeName::unit(), defaultUnit);
    Basda::ServiceWorker::config()->leaf(
            Basda::Mocca::Svc::PropertyName::positionRemainder())
            ->Nice::Property::applyAttribute<int>(
            Nice::QtWidgetAttributeName::prec(), m_unitNamePrecMap[defaultUnit]);
    Basda::ServiceWorker::config()->leaf(
            Basda::Mocca::Svc::PropertyName::positionRemainder())
            ->Nice::Property::applyAttribute<std::map<std::string, int> >(
            Nice::QtWidgetAttributeName::unitPrec(), m_unitNamePrecMap);

    m_device->config()->Nice::Joint::getWithDefault<Nice::Int>(
            Basda::Mocca::Dev::PropertyName::cfgFullTurn(), 1, warning);
    m_device->config()->Nice::Joint::getWithDefault<Nice::Int>(
            Basda::Mocca::Dev::PropertyName::cfgSwitchWidth(), 60, warning);
    m_device->config()->getWithDefault<bool>(Basda::Mocca::Dev::PropertyName::cfgOneWay(),
                                   true, warning);
    m_device->config()->getWithDefault<double>(Basda::Mocca::Dev::PropertyName::cfgScanVelocity(),
                                       100, warning);

    m_deviceAsync.m_service = this;
    m_deviceCmdAsync.m_service = this;
    m_wheelDeviceAsync.m_service = this;
    m_wheelDeviceCmdAsync.m_service = this;

    Nice::Application::distribute(Basda::ServiceWorker::config()->node("CFG"));
    Nice::Application::distribute(Basda::ServiceWorker::config()->node("CMD"));
    Nice::Application::distribute(Basda::ServiceWorker::config()->node("VAR"));
}

Basda::MoccaWheelServiceWorker::~MoccaWheelServiceWorker()
{
    D_LOG("Destruct");
}

void Basda::MoccaWheelServiceWorker::scanPositionSwitches()
{
    I_LOG(Basda::ServiceWorker::name() << " scan all position switches.");

    Nice::BitSet switchMask = m_device->config()
            ->Nice::Joint::getWithDefault<Nice::BitSet>(
            Basda::Mocca::Dev::PropertyName::cfgSwitchMask(),
            Nice::BitSet(8));

    Nice::NPoint fullTurn = Nice::NPoint(
            1,
            (double) m_device->config()
                    ->Nice::Joint::getWithDefault<Nice::Int>(
                    Basda::Mocca::Dev::PropertyName::cfgFullTurn(), 0));
    std::string unit = m_device->config()
            ->Nice::Joint::getAtNode<std::string>(
            Basda::Mocca::Dev::PropertyName::positionUnit());

    Nice::SeqBitSet positionSwitches;
    Nice::SeqInt positions;

    double scanVelocity = m_device->config()->getAtNode<double>(
            Basda::Mocca::Dev::PropertyName::cfgScanVelocity());

    Nice::JointPtrMap children = m_device->config()->node("MOD")->children();
    std::map<std::string, Nice::JointPtr>::iterator children_it =
            children.begin();
    double basicVelocityStraight = m_device->config()->getAtNode<double>(
            std::string("MOD.") + children_it->first + "."
                    + Basda::Mocca::MoCon::cfgDatasheetBasicVelocityStraight());

    D_LOG("Set scan velocity to : " << scanVelocity)
    // turn the wheel once and record the position reference switches
    m_device->setVelocitySetting(Nice::NPoint(1, scanVelocity));
    m_device->moveRelativeStart(fullTurn, m_wheelDeviceCmdAsync);

    Nice::BitSet lastPositionSwitch = switchMask;
    Nice::Int positionSwitchRangeStart = 0;
    Nice::Int positionSwitchRangeEnd = 0;
    int depth = switchMask.depth;

    bool storePositionSwitch = false;

    while (isMoving())
    {
        try
        {
            Nice::Int position = getPosition();
            Nice::BitSet positionSwitch = getPositionSwitchStatus()[0];

            checkForNamedPosition(); // to keep the GUI/property tree up to date

            // mask out the ignoring bits and compare it with the switch mask
            if (positionSwitch != switchMask)
            {
                // when a new signal was received (previous position switch is equal to the switch mask)
                if (lastPositionSwitch == switchMask)
                {
                    // store the current position switch status
                    lastPositionSwitch = positionSwitch;
                    positionSwitchRangeStart = position;
                    positionSwitchRangeEnd = position;
                }
                else
                {
                    Nice::BitSet bs;
                    bs = lastPositionSwitch & positionSwitch;
                    // find the position with as many as possible signals ( looking for maximum number of zeros)
                    if ((depth - bs.count())
                            > (depth - lastPositionSwitch.count()))
                    {
                        positionSwitchRangeStart = position;
                        positionSwitchRangeEnd = position;
                    }
                    else if ((depth - bs.count())
                            == (depth - lastPositionSwitch.count()))
                        positionSwitchRangeEnd = position;
                    // add as many signals to the position switch
                    // by default no signal is represented as 1 and a signal is a 0
                    // therefore we can add the new signal by AND to the lastPositionSwitch
                    // and save it as lastPositionSwitch in any iteration
                    // (we will not loose any information)
                    lastPositionSwitch = bs;
                }
                storePositionSwitch = true;
            }
            else
            {
                if (storePositionSwitch)
                {
                    positionSwitches.push_back(lastPositionSwitch);
                    int positionSwitchCenter = (int)(positionSwitchRangeStart
                                    + ((double) (positionSwitchRangeEnd
                                            - positionSwitchRangeStart) / 2.0));
                    positions.push_back(positionSwitchCenter);
                    I_LOG("found position reference switch [signal@position]: " << lastPositionSwitch << "@" << positionSwitchCenter)
                    storePositionSwitch = false;
                }
                lastPositionSwitch = switchMask;
            }
        }

        catch (::Basda::Dev::MoccaCmdRejectedByDeviceException &e)
        {
            D_LOG("MoCon reject request to update position switch status because another service is updating the position switch status. Position switch status will be ignored for that position.")
        }

        m_wheelDeviceCmdAsync.asyncStatus(
                "Scan Position Switches", (int) m_device->getProgress().percent());
    }
    D_LOG("Set velocity back to " << basicVelocityStraight)
    m_device->setVelocitySetting(Nice::NPoint(1, basicVelocityStraight));

    // find the best match between scanned position reference switches and the configured named positions
    if (m_completion.state() == Nice::BUSY)
    {
        // vector of scanned position switches will be compared with the
        // vector of named position by shifting the values of scanned position
        // switches vector by one.
        // The values compared using XOR. The result is stored in the
        // vector positionSwitchError first and than in positionSwitchMatrix
        // to get a matrix
        std::vector<Nice::SeqBitSet> positionSwitchMatrix;

        int bestMatch = 0;
        std::vector<int> errorSum;
        int minError = (int) m_namedSwitches.size()
                * m_namedSwitches[0].depth + 1;
        {
            for (int i = 0; i < (int) positionSwitches.size(); i++)
            {
                Nice::SeqBitSet positionSwitchError;
                errorSum.push_back(0);
                positionSwitchError.push_back(positionSwitches[i]);
                for (int j = 0; j < (int) m_namedSwitches.size(); j++)
                {
                    Nice::BitSet error = positionSwitches[(i + j)
                            % positionSwitches.size()] ^ m_namedSwitches[j];
                    errorSum[i] += error.count();
                    positionSwitchError.push_back(error);
                }
                if (errorSum[i] < minError)
                {
                    minError = errorSum[i];
                    bestMatch = i;
                }
                positionSwitchMatrix.push_back(positionSwitchError);
            }
        }

        if (positionSwitchMatrix.size() == 0)
        {
            m_scannedSwitches.clear();
            m_brokenSwitches.clear();
            m_scannedPositions.clear();

            std::string errorMsg = "No position reference switches found. In case of simulation you have to use libBasdaMoccaMpiaNDeviceDummy!";
            E_LOG(errorMsg)
            E_THROW(Basda::Svc::MoccaWheelReferenceSwitchNotFoundException, errorMsg)
        }
        I_LOG("Matrix shows the signal error of the scanned position signals.")
        I_LOG("First row shows the known named positions.")
        I_LOG("The first column shows the starting value of the scanned position.")
        I_LOG("The scanned position signals are rotated and compared with the named positions.")
        I_LOG("            " << m_namedSwitches)
        for (int i = 0; i < (int) positionSwitchMatrix.size(); i++)
        {
            I_LOG(i << ": " << positionSwitchMatrix[i] << " number of errors: " << errorSum[i])
        }
        I_LOG("Best match is at line: " << bestMatch)

        m_scannedSwitches.clear();
        m_brokenSwitches.clear();
        m_scannedPositions.clear();
        for (int i = 0; i < (int) positionSwitches.size(); i++)
        {
            m_brokenSwitches.push_back(
                    positionSwitchMatrix[bestMatch][i + 1]);
            m_scannedSwitches.push_back(
                    positionSwitches[(i + bestMatch)
                            % positionSwitches.size()]);
            m_scannedPositions.push_back(
                    positions[(i + bestMatch) % positions.size()]);
        }
        I_LOG("Configured signals are [reference switch number:signal@position]:")
        int i;
        std::stringstream configuredSignals;
        for (i = 0; i < (int) m_namedPositions.size() - 1; i++)
        {
            configuredSignals << i << ":" << m_namedSwitches[i] << "@"
                                << m_namedPositions[i] << ", ";
        }
        configuredSignals << i << ":" << m_namedSwitches[i] << "@"
                        << m_namedPositions[i];
        I_LOG(configuredSignals.str())

        I_LOG("Scanned signals are [reference switch number:signal@position]:")
        std::stringstream scannedSignals;
        for (i = 0; i < (int) m_scannedSwitches.size() - 1; i++)
        {
            scannedSignals << i << ":" << m_scannedSwitches[i] << "@"
                    << m_scannedPositions[i] << ", ";
        }
        scannedSignals << i << ":" << m_scannedSwitches[i] << "@"
                << m_scannedPositions[i];
        I_LOG(scannedSignals.str())

        if (minError > 0)
        {
            std::stringstream brokenSignals;
            W_LOG("Broken signals found at [reference switch number:signal@position]:")
            int j;
            for (j = 0; j < (int) positionSwitches.size() - 1; j++)
            {
                brokenSignals << j << ":" << m_brokenSwitches[j]
                        << "@" << m_scannedPositions[j] << ", ";
            }
            brokenSignals << j << ":" << m_brokenSwitches[j] << "@"
                    << m_scannedPositions[j];
            W_LOG(brokenSignals.str())
        }

        I_LOG(Basda::ServiceWorker::name() << " scanning of position switches done.");
    }

    m_device->getProgress().setPercent(100);
    m_device->getProgress().setDone();
}

void Basda::MoccaWheelServiceWorker::validateScannedPositionSwitches()
{
    std::string unit = m_device->config()->Nice::Joint::getAtNode<std::string>(
            Basda::Mocca::Dev::PropertyName::positionUnit());

    Nice::BitSet positionSwitchMask = m_device->config()
                    ->Nice::Joint::getAtNode<Nice::BitSet>(
                    Basda::Mocca::Dev::PropertyName::cfgSwitchMask());

    int currentPosition = getPosition();

    int nextNamedPosition = 0;
    int fullTurn = m_device->config()
                    ->Nice::Joint::getAtNode<Nice::Int>(
                    Basda::Mocca::Dev::PropertyName::cfgFullTurn());
    int minDistanceToNextNamedPosition = fullTurn;
    int distanceToNextNamedPosition = 0;

    for (int i = 0; i < (int)m_namedPositions.size(); i++)
    {
        distanceToNextNamedPosition = (fullTurn + m_namedPositions[i] - currentPosition) % fullTurn;
        if (distanceToNextNamedPosition > 0 && distanceToNextNamedPosition < minDistanceToNextNamedPosition)
        {
            nextNamedPosition = i;
            minDistanceToNextNamedPosition = distanceToNextNamedPosition;
        }
    }

    for (int i = 0; i < (int)m_namedPositions.size(); i++)
    {
        if (m_completion.state() != Nice::BUSY) // in case scanning was interrupted
            break;

        int next = (i + nextNamedPosition) % (int)m_namedPositions.size();
        I_LOG("Validate named position " << next << " (" << m_namedPositions[next] << ").")
        moveAbsolute(m_namedPositions[next], unit);
        checkForNamedPosition(); // to keep the GUI/property tree up to date
        Nice::SeqBitSet positionSwitch;
        bool error = false;
        do
        {
            try
            {
                positionSwitch = getPositionSwitchStatus();
                error = false;
            }

            catch (::Basda::Dev::MoccaCmdRejectedByDeviceException &e)
            {
                Nice::Time wait = Nice::Time::seconds(0.1);
                D_LOG("MoCon reject request to update position switch status because another service is updating the position switch status. Position switch status will be updated in " << wait << " seconds.")
                m_wheelDeviceAsync.asyncWait(wait);
                error = true;
            }
        }
        while (error);

        if (m_completion.state() != Nice::BUSY) // in case homing was interrupted
            break;

        if (positionSwitch[0] == m_scannedSwitches[next])
        {
            I_LOG("Named Position " << next << ":" << m_scannedSwitches[next] << "@" << m_namedPositions[next] << " is valid.")
        }
        else
        {
            W_LOG("Named Position " << next << ":" << m_scannedSwitches[next] << "@" << m_namedPositions[next] << " could not verified.")
            W_LOG("Reference Position switches (" << positionSwitch[0] << ") and named position (" << m_scannedSwitches[next] << ") does not match. Reference Switches will be ignored and validation of named position will not be done.")
            m_scannedSwitches[next] = positionSwitchMask;
        }
    }
}

#if 0
 the following two methods use the m_scannedPositions to validate the founded
 position reference switches
 since the dicroic wheel has to position very precisely, the named positions
 from the config file has to be used!
 these methods are exchanged by a new method: validateScannedPositionSwitches
void Basda::MoccaWheelServiceWorker::validateScannedPositionSwitches()
{
    m_positionSwitchWidth = m_device->config()
            ->Nice::Joint::getAtNode<Nice::Int>(
            Basda::Mocca::Dev::PropertyName::cfgSwitchWidth());

    Nice::BitSet positionSwitchMask = m_device->config()
                    ->Nice::Joint::getAtNode<Nice::BitSet>(
                    Basda::Mocca::Dev::PropertyName::cfgSwitchMask());

    std::string unit = m_device->config()->Nice::Joint::getAtNode<std::string>(
            Basda::Mocca::Dev::PropertyName::positionUnit());

    int currentPosition = getPosition();

    int nextNamedPosition = 0;
    int minDistanceToNextNamedPosition = m_device->config()
            ->Nice::Joint::getAtNode<Nice::Int>(
            Basda::Mocca::Dev::PropertyName::cfgFullTurn());
    int distanceToNextNamedPosition = 0;

    for (int i = 0; i < (int)m_scannedPositions.size(); i++)
    {
        distanceToNextNamedPosition = (m_scannedPositions[i] - currentPosition);
        if (distanceToNextNamedPosition > 0 && distanceToNextNamedPosition < minDistanceToNextNamedPosition)
        {
            nextNamedPosition = i;
            minDistanceToNextNamedPosition = distanceToNextNamedPosition;
        }
    }

    for (int i = 0; i < (int)m_scannedPositions.size(); i++)
    {
        if (m_completion.state() != Nice::BUSY) // in case homing was interrupted
            break;

        int next = (i + nextNamedPosition) % (int)m_scannedPositions.size();
        I_LOG("Validate named position " << next << " (" << m_scannedPositions[next] << ").")
        int namedPosition = searchForNamedPosition(next, -m_positionSwitchWidth,
                                                   unit);
        if (m_completion.state() != Nice::BUSY) // in case homing was interrupted
            break;

        if (namedPosition <= m_scannedPositions[next] + m_positionSwitchWidth)
        {
            if (namedPosition != m_scannedPositions[next])
            {
                N_LOG("Due to the position correction slightly shift of the named position detected.")
                N_LOG("Named position " << next << ":" << m_scannedSwitches[next] << " found at " << namedPosition)
            }
            else
            {
                I_LOG("Named Position " << next << ":" << m_scannedSwitches[next] << "@" << m_scannedPositions[next] << " is valid.")
            }
            m_scannedPositions[next] = namedPosition;
        }
        else
        {
            W_LOG("Named Position " << next << ":" << m_scannedSwitches[next] << "@" << m_scannedPositions[next] << " could not verified.")
            W_LOG("Reference Position switches and named position does not match. Reference Switches will be ignored and validation of named position will not be done.")
            m_scannedSwitches[next] = positionSwitchMask;
        }
    }
}

int Basda::MoccaWheelServiceWorker::searchForNamedPosition(int _namedPosition,
                                                           int width,
                                                           std::string _unit)
{
    if (!m_scannedPositions.empty() && !m_scannedSwitches.empty())
    {
        if (m_completion.state() != Nice::BUSY) // in case homing was interrupted
        {
            return m_scannedPositions[_namedPosition] + 2*width;
        }

        if (width > m_positionSwitchWidth)
        {
            return m_scannedPositions[_namedPosition] + width;
        }
        else
        {
            moveAbsolute(m_scannedPositions[_namedPosition] + width, _unit);
            checkForNamedPosition(); // to keep the GUI/property tree up to date
            Nice::SeqBitSet positionSwitch;
            bool error = false;
            do
            {
                try
                {
                    positionSwitch = getPositionSwitchStatus();
                    error = false;
                }

                catch (::Basda::Dev::MoccaCmdRejectedByDeviceException &e)
                {
                    Nice::Time wait = Nice::Time::seconds(0.1);
                    D_LOG("MoCon reject request to update position switch status because another service is updating the position switch status. Position switch status will be updated in " << wait << " seconds.")
                    m_wheelDeviceAsync.asyncWait(wait);
                    error = true;
                }
            }
            while (error);

            if (positionSwitch[0] != m_scannedSwitches[_namedPosition])
            {
                return searchForNamedPosition(_namedPosition,
                                              width + m_positionSwitchWidth / 4,
                                              _unit);
            }
            else
                return m_scannedPositions[_namedPosition] + width;
        }
    }
}
#endif /* 0 */

void Basda::MoccaWheelServiceWorker::scanAllReferenceSwitches()
{
    Basda::ServiceWorkerSkeleton::cmdStart(Basda::Command::Action::MOVETOHOME);
    try
    {
        m_scanAllReferenceSwitches = true;

        scanPositionSwitches();

        if (m_completion.state() == Nice::BUSY)
        {
            if (!m_homeReferenceSwitchFound)
            {
                Nice::Int fullTurn = m_device->config()
                        ->Nice::Joint::getAtNode<Nice::Int>(
                        Basda::Mocca::Dev::PropertyName::cfgFullTurn());
                m_positionOffset = 0;
                int numberOfCorrectSwitches = 0;
                std::stringstream ss_offset;
                for (int i = 0; i < (int) m_scannedPositions.size(); i++)
                {
                    int offset;

                    offset = (fullTurn + m_namedPositions[i]
                            - (m_scannedPositions[i] % fullTurn)) % fullTurn;
                    if (offset > fullTurn/2)
                        offset = offset - fullTurn;

                    ss_offset << i << ":" << m_namedSwitches[i] << "|"
                            << offset << "  ";
//                    U3_LOG("target: " << m_namedPositions[i] << " scan: " << m_scannedPositions[i] << " offset: " << offset << " number of errors: " << m_brokenSwitches[i].count())
                    if (m_brokenSwitches[i].count() == 0)
                    {
                        m_positionOffset += offset;
                        numberOfCorrectSwitches++;
                    }
                }
                I_LOG("Determined position offsets are [reference switch number:signal|offset]:")
                I_LOG(ss_offset.str())

                if (numberOfCorrectSwitches != 0)
                    m_positionOffset = boost::math::iround(
                            m_positionOffset / (double) numberOfCorrectSwitches);
                else
                    m_positionOffset = boost::math::iround(
                            m_positionOffset / (double) m_namedPositions.size());
                I_LOG("based on scanning determined average position offset: " << m_positionOffset)
                Nice::Int currentPosition = getPosition();
                I_LOG("current position: " << currentPosition << " (equivalent to single turn position: " << currentPosition % fullTurn << ")")
                Nice::Int correctedPosition = (currentPosition + m_positionOffset)
                        % fullTurn;

                std::string unit = m_device->config()
                        ->Nice::Joint::getAtNode<std::string>(
                        Basda::Mocca::Dev::PropertyName::positionUnit());
                m_device->setPosition(Nice::NPoint(1, (double) correctedPosition));
                I_LOG("corrected position: " << correctedPosition)
            }

            validateScannedPositionSwitches();

            if (m_completion.state() == Nice::BUSY)
            {
                for (int i = 0; i < (int)m_scannedSwitches.size(); i++)
                {
                    std::stringstream ss;
                    ss << Basda::Mocca::Dev::PropertyName::namedPositionSwitch() + "."
                            << i;
                    if (m_device->config()->exist(ss.str()))
                    {
                        m_device->config()->setAtNode(ss.str(), m_scannedSwitches[i]);
                        m_namedSwitches[i] = m_scannedSwitches[i];
                    }
                }

                // the dicroic wheel needs to position very precisely.
                // therefor the config file named position values will not be
                // overwritten with the scanned values
//                for (int i = 0; i < (int)m_scannedPositions.size(); i++)
//                {
//                    std::stringstream ss;
//                    ss << Basda::Mocca::Dev::PropertyName::namedPositionAbs() + "."
//                            << i;
//                    if (m_device->config()->exist(ss.str()))
//                    {
//                        m_device->config()->setAtNode(ss.str(), m_scannedPositions[i]);
//                        m_namedPositions[i] = m_scannedPositions[i];
//                    }
//                }
//                m_config->leaf("CMD")->attribute(
//                        Basda::Mocca::Svc::AttributeName::namedPositionAbs())
//                        ->set<Nice::SeqInt>(m_scannedPositions);

                status();
                m_scanAllReferenceSwitches = false;
                I_LOG(Basda::ServiceWorker::name() << " move home done. ");
            }
            else
            {
                I_LOG("Scan procedure was interrupted.")
            }
        }
        else
        {
            I_LOG("Scan procedure was interrupted.")
        }
        Basda::ServiceWorkerSkeleton::cmdDone();
    }

    catch (::Basda::Dev::MoccaDeviceException &e)
    {
        m_scanAllReferenceSwitches = false;
        E_LOG(e)
        Basda::ServiceWorkerSkeleton::cmdError(
                "::Basda::Svc::MoccaServiceException");
        E_THROW(Basda::Svc::MoccaMotionException, e)
    }

    catch (::Basda::Svc::MoccaUnitNotKnownException &e)
    {
        m_scanAllReferenceSwitches = false;
        E_LOG(e)
        Basda::ServiceWorkerSkeleton::cmdError(
                "::Basda::Svc::MoccaUnitNotKnownException");
        E_THROW(Basda::Svc::MoccaMotionException, e)
    }

    catch (::Basda::Svc::MoccaTransformationNotInvertableException &e)
    {
        m_scanAllReferenceSwitches = false;
        E_LOG(e)
        Basda::ServiceWorkerSkeleton::cmdError(
                "::Basda::Svc::MoccaTransformationNotInvertableException");
        E_THROW(Basda::Svc::MoccaMotionException, e)
    }

    catch (::Basda::Svc::MoccaMotionException &e)
    {
        m_scanAllReferenceSwitches = false;
        E_THROW(Basda::Svc::MoccaMotionException, e, "Motion exception. Home search not completed.")
    }

}

void Basda::MoccaWheelServiceWorker::moveToHome()
{
    Basda::ServiceWorkerSkeleton::cmdStart(Basda::Command::Action::MOVETOHOME);

    I_LOG(Basda::ServiceWorker::name() << " search for home position.");

    try
    {
        m_homeReferenceSwitchFound = false;
        m_startPosition = getPosition();
        m_device->moveToHome(m_wheelDeviceCmdAsync);
        m_homeReferenceSwitchFound = true;
        I_LOG(Basda::ServiceWorker::name() << " home search done.");
    }

    catch (::Basda::Svc::MoccaWheelHomeSwitchNotFoundException &e)
    {
        E_LOG(e)
        Basda::ServiceWorkerSkeleton::cmdError(
                "::Basda::Svc::MoccaServiceException");
        E_THROW(Basda::Svc::MoccaMotionException, e)
    }
}

void Basda::MoccaWheelServiceWorker::moveAbsolute(int _requestedPosition,
                                                  std::string _unit)
{
    if (!m_scanAllReferenceSwitches)
    {
        I_LOG(Basda::ServiceWorker::name() << " move absolute to " << _requestedPosition << " " << _unit);
        Basda::ServiceWorkerSkeleton::cmdStart(Basda::Command::Action::MOVEABSOLUTE);
    }

    // _requestedPosition does not get transformed in steps by base class
    // int requestedPositionInSteps = coordinateTransformation(_requestedPosition, _unit);
    // I_LOG("Position has been transformed to " << requestedPositionInSteps << " STEPS");

    try
    {

      m_device->moveAbsolute(optimizeAbsolutePosition(_requestedPosition, _unit), m_wheelDeviceCmdAsync);
      //m_device->moveAbsolute(optimizeAbsolutePosition(requestedPositionInSteps, "STEPS"), m_wheelDeviceCmdAsync);//, _unit), m_wheelDeviceCmdAsync);
        if (!m_scanAllReferenceSwitches)
        {
            I_LOG(Basda::ServiceWorker::name() << " move absolute to " << _requestedPosition << " " << _unit <<" done.");
        }
    }

    catch (::Basda::Dev::MoccaDeviceException &e)
    {
        E_LOG(e)
        Basda::ServiceWorkerSkeleton::cmdError(
                "::Basda::Svc::MoccaServiceException");
        E_THROW(Basda::Svc::MoccaMotionException, e)
    }

    catch (::Basda::Svc::MoccaUnitNotKnownException &e)
    {
        E_LOG(e)
        Basda::ServiceWorkerSkeleton::cmdError(
                "::Basda::Svc::MoccaUnitNotKnownException");
        E_THROW(Basda::Svc::MoccaMotionException, e)
    }

    catch (::Basda::Svc::MoccaTransformationNotInvertableException &e)
    {
        E_LOG(e)
        Basda::ServiceWorkerSkeleton::cmdError(
                "::Basda::Svc::MoccaTransformationNotInvertableException");
        E_THROW(Basda::Svc::MoccaMotionException, e)
    }
}

void Basda::MoccaWheelServiceWorker::moveRelative(int _requestedPosition, std::string _unit)
{
    if (!m_scanAllReferenceSwitches)
    {
        I_LOG(Basda::ServiceWorker::name() << " move relative by " << _requestedPosition << " " << _unit);
        Basda::ServiceWorkerSkeleton::cmdStart(Basda::Command::Action::MOVERELATIVE);
    }

    // _requestedPosition does not get transformed in steps by base class
    // int requestedPosition = coordinateTransformation(_requestedPosition, _unit);
    // I_LOG("Position has been transformed to " << requestedPosition << " STEPS");

    try
    {
        bool oneway = m_device->config()->getAtNode<bool>(
                Basda::Mocca::Dev::PropertyName::cfgOneWay());

        Nice::Int fullTurn = m_device->config()
                ->Nice::Joint::getAtNode<Nice::Int>(
                Basda::Mocca::Dev::PropertyName::cfgFullTurn());

//        Nice::Int currentPosition = config()
//                ->Nice::Joint::getAtNode<Nice::Int>(
//                Basda::Mocca::Svc::PropertyName::deviceEncoderPosition() + "."
//                        + m_basicUnit);
        double requestedPositionInSteps = coordinateTranslateInverse(_requestedPosition, _unit);
        //double requestedPositionInSteps = coordinateTranslateInverse(requestedPosition, "STEPS");//, _unit);

        double relativePositionInSteps;

        if (oneway)
        {
            if (requestedPositionInSteps < 0)
                relativePositionInSteps = fullTurn + ((Nice::Int)requestedPositionInSteps % fullTurn);
            else
                relativePositionInSteps = ((Nice::Int)requestedPositionInSteps % fullTurn);
        }
        else // twoway
        {
            if (requestedPositionInSteps < 0)
                relativePositionInSteps = -(abs((Nice::Int)requestedPositionInSteps) % fullTurn);
            else
                relativePositionInSteps = ((Nice::Int)requestedPositionInSteps % fullTurn);
        }

        m_device->moveRelative(relativePositionInSteps, m_wheelDeviceCmdAsync);
        if (!m_scanAllReferenceSwitches)
        {
            I_LOG(Basda::ServiceWorker::name() << " move relative by " << _requestedPosition << " " << _unit << " done.");
        }
    }

    catch (::Basda::Dev::MoccaDeviceException &e)
    {
        E_LOG(e)
        Basda::ServiceWorkerSkeleton::cmdError(
                "::Basda::Svc::MoccaServiceException");
        E_THROW(Basda::Svc::MoccaMotionException, e)
    }

    catch (::Basda::Svc::MoccaUnitNotKnownException &e)
    {
        E_LOG(e)
        Basda::ServiceWorkerSkeleton::cmdError(
                "::Basda::Svc::MoccaUnitNotKnownException");
        E_THROW(Basda::Svc::MoccaMotionException, e)
    }

    catch (::Basda::Svc::MoccaTransformationNotInvertableException &e)
    {
        E_LOG(e)
        Basda::ServiceWorkerSkeleton::cmdError(
                "::Basda::Svc::MoccaTransformationNotInvertableException");
        E_THROW(Basda::Svc::MoccaMotionException, e)
    }
}

void Basda::MoccaWheelServiceWorker::moveToNamedPosition(int _namedPosition)
{
    I_LOG(Basda::ServiceWorker::name() << " move to named position " << _namedPosition);
    Basda::ServiceWorkerSkeleton::cmdStart("MoveRef");

    std::stringstream ss;
    ss << Basda::Mocca::Dev::PropertyName::namedPositionAbs() << "."
            << _namedPosition;

    if (m_device->config()->exist(ss.str()))
    {
        try
        {
            Nice::BitSet positionSwitchMask = m_device->config()
                            ->Nice::Joint::getAtNode<Nice::BitSet>(
                            Basda::Mocca::Dev::PropertyName::cfgSwitchMask());

            Nice::Int requestedPositionInSteps = m_device->config()->node(ss.str())
                    ->get<Nice::Int>();

            m_device->moveAbsolute(
                    optimizeAbsolutePosition(requestedPositionInSteps,
                                             m_basicUnit), m_wheelDeviceCmdAsync);

            if (!m_scannedSwitches.empty() && m_scannedSwitches[_namedPosition] != positionSwitchMask)
            {
                Nice::SeqBitSet positionSwitch;
                bool error = false;
                do
                {
                    try
                    {
                        positionSwitch = getPositionSwitchStatus();
                        error = false;
                    }

                    catch (::Basda::Dev::MoccaCmdRejectedByDeviceException &e)
                    {
                        Nice::Time wait = Nice::Time::seconds(0.1);
                        D_LOG("MoCon reject request to update position switch status because another service is updating the position switch status. Position switch status will be updated in " << wait << " seconds.")
                        m_wheelDeviceAsync.asyncWait(wait);
                        error = true;
                    }
                }
                while (error);

                if (positionSwitch[0] != m_namedSwitches[_namedPosition])
                {
                    std::stringstream errorMsgSs;
                    errorMsgSs << "Could not verify named position. "
                    		      "Received signal (" << positionSwitch[0] <<
								  ") is not what it should be (" <<
								  m_namedSwitches[_namedPosition] << ").";
                    const std::string errorMsg= errorMsgSs.str();
                    E_LOG(errorMsg)
                    Basda::ServiceWorkerSkeleton::cmdError(
                            "::Basda::Svc::MoccaWheelReferenceSwitchNotFoundException");
                    E_THROW(Basda::Svc::MoccaMotionException, errorMsg)
                }
                else
                {
                    I_LOG(Basda::ServiceWorker::name() << " move to named position " << _namedPosition << " done. Position is valid.");
                }
            }
            else
            {
                W_LOG("Named position could not be validated while home search. Skip validation of the current position (should be named position " <<  _namedPosition << "). Motion done.")
            }
        }

        catch (::Basda::Dev::MoccaDeviceException &e)
        {
            E_LOG(e)
            Basda::ServiceWorkerSkeleton::cmdError(
                    "::Basda::Svc::MoccaServiceException");
            E_THROW(Basda::Svc::MoccaMotionException, e)
        }

        catch (::Basda::Svc::MoccaUnitNotKnownException &e)
        {
            E_LOG(e)
            Basda::ServiceWorkerSkeleton::cmdError(
                    "::Basda::Svc::MoccaUnitNotKnownException");
            E_THROW(Basda::Svc::MoccaMotionException, e)
        }

        catch (::Basda::Svc::MoccaTransformationNotInvertableException &e)
        {
            E_LOG(e)
            Basda::ServiceWorkerSkeleton::cmdError(
                    "::Basda::Svc::MoccaTransformationNotInvertableException");
            E_THROW(Basda::Svc::MoccaMotionException, e)
        }
    }
    else
    {
        std::stringstream errorMsg;
        errorMsg << "no reference point defined at " << m_device->config()->node( Basda::Mocca::Dev::PropertyName::namedPositionAbs() )->path() << "." << _namedPosition;
        E_LOG(errorMsg.str())
        Basda::ServiceWorkerSkeleton::cmdError(
                "::Basda::Svc::MoccaWheelReferenceSwitchNotFoundException");
        E_THROW(Basda::Svc::MoccaMotionException, errorMsg.str())
    }
}

Nice::Int Basda::MoccaWheelServiceWorker::getPosition()
{
    Nice::Int fullTurn = m_device->config()
            ->Nice::Joint::getAtNode<Nice::Int>(
            Basda::Mocca::Dev::PropertyName::cfgFullTurn());

    Nice::Int currentPosition = Basda::MoccaNServiceWorkerCore<double, Nice::Int, Nice::Point,
                    Basda::MoccaWheelServiceWorkerSkeleton>::getPosition();

    m_config->Nice::Joint::setAtNode<Nice::Int>(
                Basda::Mocca::Svc::PropertyName::positionRemainder(), currentPosition % fullTurn);

    return currentPosition;
}

double Basda::MoccaWheelServiceWorker::optimizeAbsolutePosition(
        int _requestedPosition, std::string _unit)
{
    bool oneway = m_device->config()->getAtNode<bool>(
            Basda::Mocca::Dev::PropertyName::cfgOneWay());

    Nice::Int fullTurn = m_device->config()
            ->Nice::Joint::getAtNode<Nice::Int>(
            Basda::Mocca::Dev::PropertyName::cfgFullTurn());
    Nice::Int currentPosition = config()
            ->Nice::Joint::getAtNode<Nice::Int>(
            Basda::Mocca::Svc::PropertyName::deviceEncoderPosition() + "."
                    + m_basicUnit);
    double requestedPositionInSteps = coordinateTransformationInverse(_requestedPosition,
                                                             _unit);

    double fullTurnPositionValue;
    double remainingPositionValue;

    double absolutePositionInSteps;

    if (oneway)
    {
        if ((currentPosition % fullTurn) > ((Nice::Int)requestedPositionInSteps % fullTurn))
            fullTurnPositionValue = (currentPosition / fullTurn + 1) * fullTurn;
        else
            fullTurnPositionValue = (currentPosition / fullTurn) * fullTurn;

        remainingPositionValue = ((Nice::Int)requestedPositionInSteps % fullTurn);
    }
    else // twoway
    {
        fullTurnPositionValue = (currentPosition / fullTurn) * fullTurn;

        if (requestedPositionInSteps < 0)
            remainingPositionValue = -(abs((Nice::Int)requestedPositionInSteps) % fullTurn);
        else
            remainingPositionValue = ((Nice::Int)requestedPositionInSteps % fullTurn);
    }

    absolutePositionInSteps = fullTurnPositionValue + remainingPositionValue;

    return absolutePositionInSteps;
}

void Basda::MoccaWheelServiceWorker::checkFullTurn()
{
    Nice::Int currentPosition = getPosition();
    Nice::Int fullTurn = m_device->config()->Nice::Joint::getAtNode<Nice::Int>(
            Basda::Mocca::Dev::PropertyName::cfgFullTurn());

    if ((currentPosition - m_startPosition) > fullTurn)
    {
        E_THROW(Basda::Svc::MoccaWheelHomeSwitchNotFoundException,
                "Home reference switch not found.")
    }
}

std::string Basda::MoccaWheelServiceWorker::checkForNamedPosition()
{
    D_LOG("Check whether current position is a named position.")

    std::string namedPosition = "not defined";

    if (m_device->config()->exist(Basda::Mocca::Dev::PropertyName::namedPositionAbs()))
    {
        std::string defaultUnit = m_device->config()->Nice::Joint::getAtNode<std::string>(
                Basda::Mocca::Dev::PropertyName::positionUnit());
//        SvcType requestedPosition = coordinateTransformationInverse(
//                Basda::ServiceWorker::config()->Nice::Joint::getAtNode<SvcType>(
//                        Basda::Mocca::Svc::PropertyName::deviceEncoderPosition()
//                                + "." + defaultUnit),
        Nice::Int requestedPosition = m_config->Nice::Joint::getAtNode<Nice::Int>(
                    Basda::Mocca::Svc::PropertyName::positionRemainder());

        Nice::JointPtrMap children = m_device->config()->node(
                Basda::Mocca::Dev::PropertyName::namedPositionAbs())->children();
        Nice::JointPtrMap::iterator it = children.begin();
        while (it != children.end())
        {
            if (it->second->get<Nice::Int>() == requestedPosition)
            {
                namedPosition = it->second->leaf()->getAttribute<std::string>("NAME");
                break;
            }
            it++;
        }
        Basda::ServiceWorker::config()->Nice::Joint::setAtNode<std::string>(
                    Basda::Mocca::Svc::PropertyName::namedPosition(), namedPosition);
    }

    return namedPosition;
}

void Basda::MoccaWheelServiceWorker::status()
{
    Basda::MoccaNServiceWorkerCore<double, Nice::Int, Nice::Point,
                    Basda::MoccaWheelServiceWorkerSkeleton>::status();
    try
    {
        getPositionSwitchStatus();
    }

    catch (::Basda::Dev::MoccaCmdRejectedByDeviceException &e)
    {
        D_LOG("MoCon reject request to update position switch status because another service is updating the position switch status. Position switch status will be updated next time.")
    }
}

// for wheelIntegrationTest
Nice::Int Basda::MoccaWheelServiceWorker::getPositionOffset()
{
    return m_positionOffset;
}

template<> Nice::Point Basda::MoccaNServiceWorkerCore<double, Nice::Int,
        Nice::Point, Basda::MoccaWheelServiceWorkerSkeleton>::getDefaultTransformation()
{
    return Nice::Point(1, 0);
}

void Basda::MoccaWheelServiceWorker::transformationFactorChanged(const Nice::Any & /*_a*/)
{
  N_LOG("Transformation factor changed." << Basda::Mocca::Dev::PropertyName::transformation());

  Nice::JointPtrMap children = m_device->config()->node(
                                                        Basda::Mocca::Dev::PropertyName::transformation())->children();
  std::map<std::string, Nice::JointPtr>::iterator it = children.begin();

  N_LOG("Number of units available = " << children.size());
  m_transformation.clear();
  while (it != children.end())
    {
      I_LOG("UNIT " << boost::to_upper_copy(it->first)
            << " - TRANS FACTOR " << (it->second)->get<Nice::Point>());
      m_transformation[boost::to_upper_copy(it->first)] = (it->second)->get<Nice::Point>();
      it++;
    }
}

#if 0
Nice::Int Basda::MoccaWheelServiceWorker::coordinateTransformation(double _value, std::string _unit)
{
  _unit = boost::to_upper_copy(_unit);
  Nice::MapStringPoint::iterator unitNamePrecIt = m_transformation.find(_unit);
  if (unitNamePrecIt == m_transformation.end() )
    {
      std::stringstream unitNames;
      unitNamePrecIt = m_transformation.begin();
      while (unitNamePrecIt != m_transformation.end())
        {
          unitNames << unitNamePrecIt->first << " ";
          unitNamePrecIt++;
        }
      //E_THROW(Sharknir::DrotUnitNotKnownException, "Unit " << _unit << " is not known. Known units are: " << unitNames.str());
    }
  return _value * m_transformation[_unit].xp + m_transformation[_unit].yp;
}
#endif

double Basda::MoccaWheelServiceWorker::coordinateTranslateInverse(Nice::Int _value, std::string _unit)
{
  _unit = boost::to_upper_copy(_unit);
  Nice::MapStringPoint::iterator unitNamePrecIt = m_transformation.find(_unit);
  if (unitNamePrecIt == m_transformation.end() )
    {
      std::stringstream unitNames;
      unitNamePrecIt = m_transformation.begin();
      while (unitNamePrecIt != m_transformation.end())
        {
          unitNames << unitNamePrecIt->first << " ";
          unitNamePrecIt++;
        }
      E_THROW(Basda::Svc::MoccaUnitNotKnownException, "Unit " << _unit << " is not known. Known units are: " << unitNames.str());
    }

  I_LOG("Position inverted and translated to " << (_value / m_transformation[_unit].xp) << " STEPS");
  return _value / m_transformation[_unit].xp;
}

double Basda::MoccaWheelServiceWorker::coordinateTransformationInverse(Nice::Int _value, std::string _unit)
{
  _unit = boost::to_upper_copy(_unit);
  Nice::MapStringPoint::iterator unitNamePrecIt = m_transformation.find(_unit);
  if (unitNamePrecIt == m_transformation.end() )
    {
      std::stringstream unitNames;
      unitNamePrecIt = m_transformation.begin();
      while (unitNamePrecIt != m_transformation.end())
        {
          unitNames << unitNamePrecIt->first << " ";
          unitNamePrecIt++;
        }
      E_THROW(Basda::Svc::MoccaUnitNotKnownException, "Unit " << _unit << " is not known. Known units are: " << unitNames.str());
    }

  I_LOG("Position inverted and transformed to " << (int)(_value / m_transformation[_unit].xp
                                                    + m_transformation[_unit].yp)
        << " STEPS");
  return _value / m_transformation[_unit].xp
    + m_transformation[_unit].yp;
}

