// $Header$

/* obtained from
 * https://svn.mpia.de/gulli/TwiceAsNice/branch/unstable/ln/laos-drot/src/Service/LaosDrotServiceWorker.cc 15070 2018-01-26 22:16:34Z mathar $
 * substituting Laos namespace with Sharknir namespace
 */

/** @file
 * @since 2015-08-22
 * @author kittmann
 * @author laudisio adapted the code to SHARK-NIR derotator*/

#include <pthread.h>

#include <iostream>
#include <fstream> // file of polynomials debug purposes

#include <cmath>

#include <Ltcs/Iif/IifSide.h>
#include <Ltcs/Iif/PropertyName.h>

#include <SharknirDrotServiceWorker.h>
#include <Sharknir/Drot/SharknirDrotServiceWorkerInterface.h>
#include <Sharknir/Drot/SharknirDrotServiceWorkerException.h>
#include <Sharknir/Drot/PropertyName.h>
#include <Sharknir/Drot/SharknirDrotServiceWorkerInternalData.h>

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

#undef LOG_NAME
#define LOG_NAME name()

#ifndef DR2D
/** 180 divided by pi. Defined in sofam.h */
#define DR2D 57.2957795130823208767981548141
#endif

const char * LOGNAME_FORMAT = "%Y-%m-%d-%H-%M-%S";

double gDebug1 = 1.0, gDebug2 = 1.0, gDebug3 = 1.0;
double gDebug4 = 0.0, gDebug5 = 0.0, gDebug6 = 0.0;
double gDebug7 = 0.0, gDebug8 = 0.0, gDebug9 = 0.0;

bool gDrotEnabled = true;
bool gShinsProfile = true;
bool gTanProfile = false;

double Degree = M_PI/180;

namespace bms = Basda::Mocca::Svc;

std::string tilde = " ~ ";
std::string stepsString = "STEPS";
std::string absencString = "ABSENC";
std::string degreeString = "DEGREE";

template class Basda::MoccaNServiceWorkerCore<double, double, Nice::Point, Sharknir::DrotServiceWorkerSkeleton>;


int Sharknir::DrotServiceWorker::DeviceAsyncImpl::asyncWait(const Nice::Time & _t)
{
    return m_worker->timedWaitForNewState(_t);
}

int Sharknir::DrotServiceWorker::DeviceAsyncImpl::asyncStatus(const std::string & _s, int _d)
{
    m_worker->cmdAction(_d, _s);
    return 0;
}

/* check reached position against commanded abs enc position  */
void Sharknir::DrotServiceWorker::callback_check_reached_position(const Nice::Completion& _c)
{
    U6_LOG(__FUNCTION__);
    // Nice::Completion c = _c.get<Nice::Completion>();
    switch (_c.state())
    {
    case Nice::OK:
        U9_LOG("Calling deltaCorrection ... omissis")
//        if(deltaCorrection())
//            setPosition(getAbsoluteEncoderPosition(), "ABSENC");
        U9_LOG("... callback done")
        m_CBqueue.pop_front();
        break;
    case Nice::EXEC:
        if(_c.action() == "Moving Absolute" || _c.action() == "Moving Relative")
            m_CBqueue.push_back(1);
        break;
    case Nice::BUSY:
        if(_c.action() == "Moving Absolute" || _c.action() == "Moving Relative")
            m_CBqueue.push_back(1);
        break;
    default:
        W_LOG("None of the foreseen states")
        break;
    }
}

void * Sharknir::DrotServiceWorker::absenc_thread(void * pMe)
{
    Sharknir::DrotServiceWorker & Me = *((Sharknir::DrotServiceWorker *)pMe);
    while(true)
    {
        float AbsoluteEncoder = Me.m_SimulatedAbsoluteEncoder;
        break;
        while(Me.m_SimulatedAbsoluteEncoder != Me.m_SimulatedDestination)
        {
            float increment = 10.9226f;
            bool increasing = Me.m_SimulatedAbsoluteEncoder < Me.m_SimulatedDestination;
            if(increasing && Me.m_SimulatedDestination - Me.m_SimulatedAbsoluteEncoder < increment)
            {
                increment = Me.m_SimulatedDestination - Me.m_SimulatedAbsoluteEncoder;
            }
            else if(!increasing && Me.m_SimulatedAbsoluteEncoder - Me.m_SimulatedDestination < increment)
            {
                increment = Me.m_SimulatedAbsoluteEncoder - Me.m_SimulatedDestination;
            }
            if(increasing)AbsoluteEncoder += increment;
            else AbsoluteEncoder -= increment;
            Me.m_SimulatedAbsoluteEncoder = AbsoluteEncoder;
            usleep(10000);
        }
        usleep(1000000);
    }
    return NULL;
}


/** @brief ctor
  * @param _config
  * @param _device  */
Sharknir::DrotServiceWorker::DrotServiceWorker(const Nice::JointPtr & _config, const Basda::MoccaNDevicePtr & _device):
    Basda::MoccaNServiceWorkerCore<double, double, Nice::Point, Sharknir::DrotServiceWorkerSkeleton>(_config, _device)
{
    I_LOG("Constructor1");
    m_bDerotationRunning = false;

    m_ControlFlags.bLogMisalignment = false;
    m_ControlFlags.bLogDerotationPositions = true;
    m_ControlFlags.bTraceDebug = false;
    m_drotMode = DROT_MODE_PUPIL;

    bool warning = true;
    m_config->Nice::Joint::getWithDefault<Nice::Endpoint>("IIF", Nice::Endpoint());
    m_iif = 0;
    m_microsteps = 256; // same as STEPPER.MICROSTEPS.VALUE:U32 (MoCon 1 152 6 -> 1 152 6 2 1 256)

    m_SimulatedAbsoluteEncoder = 41757; // Equivalent to STEPS 14861875
    m_SimulatedDestination = 41757;

//    pthread_create(&m_absenc_tid, NULL, absenc_thread, this);

    m_deviceAsync.m_worker = this;

    // apply a bridge in order to get notice of the coordinate transformation changes
    Nice::PropertyBridgeMemFn<Nice::Any, DrotServiceWorker> * bridge =
        new Nice::PropertyBridgeMemFn<Nice::Any, DrotServiceWorker>(this, &DrotServiceWorker::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())
    {
        U9_LOG("F: Children -> " << jt->first);
        (jt->second)->leaf()->applyBridge(bridge, "DrotServiceWorker_SCALE_" + (jt->first));
        jt++;
    }

    transformationFactorChanged(Nice::Any());

    m_absencX = m_transformation[absencString].xp;
    m_absencY = m_transformation[absencString].yp;

    U9_LOG("Transformatin ABSENC : [" << m_absencX << ", " << m_absencY << "]");

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

    m_config->Nice::Joint::getWithDefault<int>(Sharknir::Drot::Svc::PropertyName::cfgFaultTolerance(), 25, warning);
    m_config->Nice::Joint::getWithDefault<int>(Sharknir::Drot::Svc::PropertyName::cfgFrequency(), 100, warning);
    m_config->Nice::Joint::getWithDefault<int>(Sharknir::Drot::Svc::PropertyName::cfgSamplesPerSegment(), 100, warning);

    m_config->Nice::Joint::getWithDefault<int>(Sharknir::Drot::Svc::PropertyName::datasheetBacklash(), 500, warning);

    m_config->Nice::Joint::getWithDefault<bool>("VAR.IIF", false);

    /* sign of the motor (steps) relative to the polynomials (angle).
     * A value of +1 or -1 depending on whether increase of motor steps should
     * be correlated with an increase of the rotator polynomial angle, or with
     * a decrease. For the K-mirror the value is +0.5 or -0.5, because
     * the action on the motor axis should be halved to rotate the beam by
     * the angle proposed by the derotator polynomials.
     * For the pathFinder GWS case, the LBT trajectory decrease whilelooking at a southern star,
     * we have to flip the sign of the MoCon trajectory
     * DROT.SVC.CFG.DIRECTION in the *drot.derotation-svc.cfg files.  */
    m_config->Nice::Joint::getWithDefault<float>(Sharknir::Drot::Svc::PropertyName::cfgDirection(), 1.0, warning);
    m_direction = m_config->Nice::Joint::getAtNode<float>(Sharknir::Drot::Svc::PropertyName::cfgDirection());
    // m_config->Nice::Joint::getWithDefault<float>("CFG.ABSENC_DIRECTION", 1.0, warning);
    // m_absEncDir = (int)m_config->Nice::Joint::getAtNode<float>("CFG.ABSENC_DIRECTION");

    I_LOG("Configured motor speed factor " << m_direction << " ~ " << Sharknir::Drot::Svc::PropertyName::cfgDirection());
    // I_LOG("Configured absolute encoder direction " << m_absEncDir << " ~ " << "CFG.ABSENC_DIRECTION");


    m_startDate = Nice::Date::now();     // just for initialization
    m_positionSequence = Nice::NPoint();

    setTrajFinDate(m_startDate);

    m_config->Nice::Joint::getWithDefault<Nice::U64>(Sharknir::Drot::Svc::PropertyName::datasheetResolution(), 320000, warning);

    m_resolutionProperty = m_config->node(Sharknir::Drot::Svc::PropertyName::datasheetResolution());

    U9_LOG("m_resolutionProperty : " << m_resolutionProperty << " ~ " << Sharknir::Drot::Svc::PropertyName::datasheetResolution())


    if(Basda::ServiceWorker::config()->exist("CFG.UI.POSITION"))
    {
        Nice::Int i = Basda::ServiceWorker::config()->getAtNode<Nice::Int>("CFG.UI.POSITION");
        Basda::ServiceWorker::config()->leaf(bms::PropertyName::position())->applyAttribute("DEFAULT", i);
    }

    Basda::ServiceWorker::config()->Nice::Joint::getWithDefault<Nice::Time> ( "CFG.INTERVAL", Nice::Time::seconds ( 1.0));

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

    //U9_LOG("adding bridge for moveabsolute callback")
    // strPropName = "CMD.MOVERELATIVE";
    // bridgeRelMove = new Nice::PropertyBridgeMemFn<Nice::Any, DrotServiceWorker>(this, &DrotServiceWorker::callback_check_reached_position, m_id);
    // // U9_LOG(config()->parent()->toString());
    // if(config()->exist(strPropName))
    // {
    //     prop = config()->leaf(strPropName);
    //     U9_LOG("Created property " << prop->get<Nice::Completion>().state());
    //     if(prop->get<Nice::Completion>().state() == Nice::OK)
    //         U9_LOG(prop->get<Nice::Completion>());
    // }
    // prop->applyBridge(bridgeRelMove, "SharknirDrotServiceWorker_" + strPropName);

    initConfig();

    I_LOG("Contructor done")
}


void Sharknir::DrotServiceWorker::setDerotationParameters1(int _numPolynomials, double _samplingRate, double _frequency)
{
    U6_LOG(__FUNCTION__ << " ~ Num Polys " << m_numberOfPolynomials << " -> " << _numPolynomials << "; Sampling Rate " << m_samplingRate << " -> " << _samplingRate << "; Frequency " << m_frequency << " -> " << _frequency );
    m_numberOfPolynomials = _numPolynomials;
    m_samplingRate = _samplingRate;
    m_frequency = _frequency;
}

void Sharknir::DrotServiceWorker::setDerotationParameters2(int _polynomialOrder, int _faultTolerance, int _samplesPerSegment)
{
    U6_LOG(__FUNCTION__  << " ~ Polynomial Order " << m_polynomialOrder << " -> " << _polynomialOrder << "; fault Tolerance " << m_faultTolerance << " -> " << _faultTolerance << "; Samples per segment " << m_samplesPerSegment << " -> " <<  _samplesPerSegment);
    m_polynomialOrder = _polynomialOrder;
    m_faultTolerance = _faultTolerance;
    m_samplesPerSegment = _samplesPerSegment;
}

void Sharknir::DrotServiceWorker::setDerotationParameters3(double debug1, double debug2, double debug3)
{
    U6_LOG(__FUNCTION__ << " ~ Debug1 " << gDebug1 << " -> " << debug1 << "; Debug2 " << gDebug2 << "-> " <<  debug2 << "; Debug3 " << gDebug3 << "-> " << debug3);
    gDebug1 = debug1;
    gDebug2 = debug2;
    gDebug3 = debug3;
}


void Sharknir::DrotServiceWorker::setDerotationParameters4(double debug4, double debug5, double debug6)
{
    U6_LOG(__FUNCTION__ << " ~ Debug4 " << gDebug4 << " -> " << debug4 << "; Debug5 " << gDebug5 << "-> " <<  debug5 << "; Debug6 " << gDebug6 << "-> " << debug6);
    m_referencePos = gDebug4 = debug4;
    m_slitNarrowPos = gDebug5 = debug5;
    m_slitWidePos = gDebug6 = debug6;
}

void Sharknir::DrotServiceWorker::initConfig()
{
    if(m_ControlFlags.bTraceDebug){ U9_LOG(__FUNCTION__);};
    Nice::Point trange;
    Nice::JointPtrMap children = m_device->config()->node("MOD")->children();
    std::map<std::string, Nice::JointPtr>::iterator it = children.begin();

    // Nice::SeqString m_address;
    // Nice::SeqInt m_moduleNumber;
    // if(it != children.end())
    // {
    //     m_address.push_back((it->second)->getAtNode<std::string>(Basda::Mocca::Dev::PropertyName::connectionAddress()));
    //     std::stringstream ss;
    //     int moduleNo;
    //     ss << (it->second)->getAtNode<std::string>(Basda::Mocca::Dev::PropertyName::connectionModule());
    //     ss >> moduleNo;
    //     m_moduleNumber.push_back(moduleNo);
    // }

    U8_LOG("Setting travel range property...")

    double travelRangeMax = 0;
    double travelRangeMin = 0;
    //if(m_device->config()->exist(Basda::Mocca::Dev::PropertyName::cfgFullTurn()))
    if(it != children.end()) // There is one MOD
    {
        trange = it->second->getAtNode<Nice::Point>(Basda::Mocca::Dev::PropertyName::datasheetTravelRange());
        travelRangeMin = trange.x();
        travelRangeMax = trange.y();
        U8_LOG("Travelrange interval [" << (int)travelRangeMin << "," << (int)travelRangeMax << "]");
        m_travelRange = travelRangeMax - travelRangeMin;
        m_travelRangeMin = (int)travelRangeMin;
        U8_LOG("Travelrange " << m_travelRange);

       // m_fullTurn = it->second->getAtNode<int>(Basda::Mocca::Dev::PropertyName::cfgFullTurn())*m_transformation["ABSENC"].xp + 0.5;

        m_microsteps = it->second->getAtNode<int>("CFG.MICROSTEPS");
        m_absencLead = it->second->getAtNode<int>("CFG.ABSENCLEAD");
    }
    else
    {
        E_THROW("Cannot find MOD in config.")
    }

    // U8_LOG("Full Turn : " << m_fullTurn << "; Microsteps : " << m_microsteps);

    /* CFG.NUMBER_OF_POLYNOMIALS in the 5 configuration files of the services  */
    m_numberOfPolynomials = m_config->Nice::Joint::getAtNode<int>( Ltcs::Iif::Svc::PropertyName::cfgNumberOfPolynomials());

    I_LOG("Configured number of polynomials " << m_numberOfPolynomials);

    /* read configuration with the sampling rate, typically 1 second, from the IIF configuration
     * m_samplingRate = m_iif->config()->Nice::Joint::getAtNode<int>(Ltcs::Iif::Svc::PropertyName::cfgSamplingRate());
     * or, actually below, take it from the configuration of the individual motor */
    m_samplingRate = m_config->Nice::Joint::getAtNode<double>(Ltcs::Iif::Svc::PropertyName::cfgSamplingRate());

    m_frequency = m_config->Nice::Joint::getAtNode<int>(Sharknir::Drot::Svc::PropertyName::cfgFrequency());

    /* polynomial order, usually set to 2 in CFG.POLYNOMIAL_ORDER */
    m_polynomialOrder = m_config->Nice::Joint::getAtNode<int>(Ltcs::Iif::Svc::PropertyName::cfgPolynomialOrder());

    m_faultTolerance = m_config->Nice::Joint::getAtNode<int>(Sharknir::Drot::Svc::PropertyName::cfgFaultTolerance());

    m_samplesPerSegment = m_config->Nice::Joint::getAtNode<int>(Sharknir::Drot::Svc::PropertyName::cfgSamplesPerSegment());

    m_referencePos = m_config->Nice::Joint::getAtNode<double>("CFG.POSITION.REFERENCE");
    m_slitNarrowPos = m_config->Nice::Joint::getAtNode<double>("CFG.POSITION.SLITNARROW");
    m_slitWidePos = m_config->Nice::Joint::getAtNode<double>("CFG.POSITION.SLITWIDE");
}


/*!****************************************
 * @brief dtor
 * Removes bridges that have been created by the ctor.*/
Sharknir::DrotServiceWorker::~DrotServiceWorker()
{
    U6_LOG(__FUNCTION__);

    Nice::JointPtrMap children = m_device->config()->node(Basda::Mocca::Dev::PropertyName::transformation())->children();
    std::map<std::string, Nice::JointPtr>::iterator it = children.begin();
    while(it != children.end())
    {
        (it->second)->leaf()->removeBridge("DrotServiceWorker_SCALE_" + (it->first));
        it++;
    }
}

/**********************************************/
void Sharknir::DrotServiceWorker::init()
{
    U6_LOG(".init -> " << state() << " ~ " << Basda::ServiceWorker::state() << ":~:" << Basda::ServiceWorker::config()->leaf("STATE"));

    if(m_config->getAtNode<Nice::Endpoint>("IIF") != Nice::Endpoint())
    {
        try
        {
            Nice::Endpoint p = m_config->getAtNode<Nice::Endpoint>("IIF");
            Nice::AppConfigClient::create(p, m_config->node("IIF.SVC"));
            m_iif = Ltcs::IifServiceWorkerInterface::create(m_config->node("IIF.SVC"));
            /* we do NOT want to store the parallactic angle here, but only when the derotation started. */
            m_config->Nice::Joint::setAtNode<bool>("VAR.IIF", true);
        }
        catch (Nice::Exception & e)
        {
            E_LOG ("IIF service not found: " << m_config->getAtNode<std::string>("IIF") << "!");
            m_config->Nice::Joint::setAtNode<bool>("VAR.IIF", false);
        }
    }
    else
    {
        Nice::Endpoint p = m_config->getAtNode<Nice::Endpoint>("IIF");
        U1_LOG("Endpoint : " << p);
        m_config->Nice::Joint::setAtNode<bool>("VAR.IIF", false);
    }

    try
    {
        Basda::MoccaNServiceWorkerCore<double, double, Nice::Point, Sharknir::DrotServiceWorkerSkeleton>::init();
    }
    catch (Ice::ConnectionRefusedException & e)
    {
        E_THROW ( Basda::ServiceInitException, e.what())
    }
    catch (const Basda::Dev::OpenException & ex)
    {
        E_THROW(::Basda::ServiceInitException, ex, "Can't initiate service " << m_name << ".");
    }
    catch (const Basda::Dev::InitException & ex)
    {
        E_THROW(::Basda::ServiceInitException, ex, "Can't initiate service " << m_name << ".");
    }

    if(m_iif == 0)
    {
        W_LOG("m_iif null in drot service worker init")
    }

    //    Basda::ServiceWorker::sendStateStatus("INIT", -1);
}


/** @brief This method activate the device. Reimplements parent method to deal with
  * values bigger then travel range, by calling remiplemented setPosition (see below) */
void Sharknir::DrotServiceWorker::activate()
{
    U6_LOG(__FUNCTION__);
    U6_LOG(Basda::ServiceWorker::name() << " --activate-> " << Basda::ServiceWorker::state() << "::" << Basda::ServiceWorker::config()->leaf("STATE"));
    U8_LOG("Using Sharknir::DrotServiceWorker::activate()");
    m_device->activate(deviceAsyncImpl());

    //U8_LOG("abs2incinit() Exists : " << m_device->config()->exist(Basda::Mocca::Dev::PropertyName::abs2incinit()));
    /** check on the value of CFG.ABSTOINC_INIT in properties tree
     *  @param abs2incinit() ifit exists, use its value as unit for return value of
     *  getAbsoluteEncoderPosition() (see below) */
    //if(m_device->config()->exist(Basda::Mocca::Dev::PropertyName::abs2incinit()))
    {
    //    std::string unit = m_device->config()->getAtNode<std::string>(Basda::Mocca::Dev::PropertyName::abs2incinit());
        /* returns position from absolute encoder */
        double pos = getAbsoluteEncoderPosition();
        // m_travelRangeABSENC = convertPosition(m_travelRange, false, stepsString, unit);
    //    U8_LOG("Travel range " << m_travelRangeABSENC << " " << unit);
        /* calculate floating point modulus of pos against travelrange in ABSENC */

        U8_LOG("Call to getAbsoluteEncoderPosition() returned " << pos);
        // if(pos > m_travelRangeABSENC)
        // {
        //     pos = std::fmod(pos, m_travelRangeABSENC);
        //     U9_LOG("Absolute encoder position again " << pos << " [ENC]");
        // }

        // since absolute encoder counts increases while stepper count decreases
        // pos = m_travelRangeABSENC - pos;
        // U9_LOG("Absolute encoder delta " << pos);
        setPosition(pos, absencString);
    }
    N_LOG("Service started");

    // U9_LOG("adding bridge for moveabsolute callback")
    // // add bridge to correct for positionning after motion
    // Nice::PropertyBridgeMemFn<Nice::Any, DrotServiceWorker> * bridgeAbsMove =
    //     new Nice::PropertyBridgeMemFn<Nice::Any, DrotServiceWorker>(this, &DrotServiceWorker::callback_check_reached_position, m_id);
    // Nice::PropertyPtr prop;
    // // U9_LOG(config()->parent()->toString());
    // if(config()->exist("CMD.MOVEABSOLUTE"))
    // {
    //     prop = config()->leaf("CMD.MOVEABSOLUTE");
    //     U9_LOG("Created property " << prop->get<Nice::Completion>().state());
    //     if(prop->get<Nice::Completion>().state() == Nice::OK)
    //         U9_LOG(prop->get<Nice::Completion>());
    // }
    // prop->applyBridge(bridgeAbsMove, "SharknirDrotServiceWorker_MOVEABSOLUTE");
    // U9_LOG("Created bridge")

    Basda::ServiceWorker::sendStateStatus("ACTIV", -1);

    // std::string strPropName = "CMD.MOVEABSOLUTE.EXEC";
    // Nice::PropertyPtr prop;
    // std::string bridgeName = "derot-abs-motion";
    // prop = this->config()->leaf(strPropName);
    //
    //
    // m_CBFun = boost::bind(&niceCallback::callback_drot_move_absolute, m_ptrCBObj, _1, boost::bind(&SharknirDrotServiceWorker::deltaCorrection, this));
    //
    // prop->applyBridge(
    //     new Nice::PropertyBridgeMemFn<Nice::Completion, SharknirDrotServiceWorker, boost::function<void(const Nice::Completion&)> SharknirDrotServiceWorker::* >
    //         (this, &SharknirDrotServiceWorker::m_CBFun, prop->id()), bridgeName);
    U8_LOG(__FUNCTION__);
}


/** @brief Change the absolute position of all motors.
  * The motor will not move, but the current absolute position will be
  * set to the given position value.
  * The new position value can only be inside the travel range.
  * Reimplements parent method to deal with values bigger then travel range
  * @param _unit the unit in which the position value should be converted
  * @param _position a vector with the new absolute position of all motors  */
void Sharknir::DrotServiceWorker::setPosition(double _position, std::string _unit)
{
    U6_LOG(__FUNCTION__ << " ~ " << _position << " ~ " << _unit);

    double transformedPos = 0;
    try
    {
        transformedPos = coordinateTransformationInverse(_position, _unit); //Transformation to STEPS (or maybe CFG.POSITION.UNIT)
        U8_LOG("Position set to " << (int)transformedPos << " steps");

        m_device->setPosition(transformedPos);
    }
    catch (::Basda::Svc::MoccaUnitNotKnownException &e)
    {
        E_LOG(e);
        Basda::ServiceWorkerSkeleton::cmdError("::Basda::Svc::MoccaUnitNotKnownException");
    }
}


/* The motor is moved to 0 position in STEPS  */
void Sharknir::DrotServiceWorker::moveToHome()
{
    U6_LOG(__FUNCTION__);
    int pos = 0;
    std::string unit = "STEPS";
    try
    {
        moveAbsolute(pos, unit);
    }
    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);
    }
}

/** A moveRelative with no correction
  * @param _pos  [description]
  * @param _unit [description]  */
bool Sharknir::DrotServiceWorker::deltaCorrection0(/*double _pos, std::string _unit*/)
{
    // W_LOG("apply delta correction by relative of " << _pos << " " << _unit);
    U6_LOG(__FUNCTION__);
    double posAbsEnc = 0;
    double posSteps = 0;
    double posStepsAbsEnc = 0;
    bool corrected = true;
    const unsigned int maxCorrections = 10;
    unsigned int count = 0;
    int comulCorrection = 0;
    // use actual position instead of input position since the device could have been stopped
    posSteps = Basda::MoccaNServiceWorkerCore<double, double, Nice::Point, DrotServiceWorkerSkeleton>::getDeviceEncoderPosition("STEPS");
    U9_LOG("reached position in steps " << (int)posSteps);
    posStepsAbsEnc = coordinateConversionFromSteps(posSteps, "ABSENC");
    U9_LOG("reached position in steps converted to ABSENC " << coordinateConversionFromSteps(posSteps, "ABSENC"));
    posAbsEnc = getAbsoluteEncoderPosition();
    U9_LOG("reached position in from absolute encoder " << posAbsEnc);

    while(std::abs(posAbsEnc - posStepsAbsEnc) >= 0.5)
    {
        W_LOG("apply correction of " << posAbsEnc - posStepsAbsEnc);
        Basda::MoccaNServiceWorkerCore<double, double, Nice::Point, DrotServiceWorkerSkeleton>::moveRelative(posAbsEnc - posStepsAbsEnc, "ABSENC");
        comulCorrection += posAbsEnc - posStepsAbsEnc;
        count++;
        corrected = true;
        posSteps = Basda::MoccaNServiceWorkerCore<double, double, Nice::Point, DrotServiceWorkerSkeleton>::getDeviceEncoderPosition("STEPS");
        posAbsEnc = getAbsoluteEncoderPosition();
        U9_LOG("reached position in steps " << posSteps);
        U9_LOG("reached position in steps converted to ABSENC " << coordinateConversionFromSteps(posSteps, "ABSENC"));
        U9_LOG("reached position from absolute encoder " << posAbsEnc);
        if(count >= maxCorrections)
        {
            break;                // shark
            corrected = false;
        }
    }
    U9_LOG("corrected? " << corrected);
    U9_LOG("Comulative correction " << comulCorrection << " ABSENC");
    U9_LOG("Number of iterations " << count);
    U9_LOG("DONE");
    return corrected;
}

bool Sharknir::DrotServiceWorker::deltaCorrection(double _pos, std::string _unit)
{
    // W_LOG("apply delta correction by relative of " << _pos << " " << _unit);
    U6_LOG(__FUNCTION__);
    double posAbsEnc = 0;
    double posSteps = 0;
    double posStepsAbsEnc = 0;
    bool corrected = true;
    const unsigned int maxCorrections = 10;
    unsigned int count = 0;
    int comulCorrection = 0;
    // use actual position instead of input position since the device could have been stopped

    // while(corrections < maxCorrections)
    // {

    // }

    // retPos = getAbsoluteEncoderPosition();    // Same as MoCon getAEncActualPosition (1 171 5 25 1 or 1 171 5)
    // if(_unit != "ABSENC")
    // {
    //     // Converting retPos to STEPS which is the standard unit
    //     unitNamePrecIt = m_transformation.find("ABSENC");
    //     retPos = (retPos - unitNamePrecIt->second.yp)/unitNamePrecIt->second.xp;
    //     if(_unit == "STEPS")// STEPS is currently default unit
    //     {
    //         U9_LOG(__FUNCTION__ << " ~ Steps are : " << (int)retPos);
    //         return retPos;
    //     }
    //     else // DEGREE or other
    //     {
    //         unitNamePrecIt = m_transformation.find(_unit);
    //         retPos = retPos*unitNamePrecIt->second.xp + unitNamePrecIt->second.yp;
    //         U9_LOG(__FUNCTION__ << " ~ " << _unit << " are : " << retPos);
    //     }
    // }
    // U8_LOG(__FUNCTION__ << " ~ " << _unit << " = " << (int)retPos);

    posSteps = Basda::MoccaNServiceWorkerCore<double, double, Nice::Point, DrotServiceWorkerSkeleton>::getDeviceEncoderPosition("STEPS");
    U9_LOG("reached position in steps " << (int)posSteps);
    posStepsAbsEnc = coordinateConversionFromSteps(posSteps, "ABSENC");
    U9_LOG("reached position in steps converted to ABSENC " << coordinateConversionFromSteps(posSteps, "ABSENC"));
    posAbsEnc = getAbsoluteEncoderPosition();
    U9_LOG("reached position in from absolute encoder " << posAbsEnc);

    while(std::abs(posAbsEnc - posStepsAbsEnc) >= 0.5)
    {
        W_LOG("apply correction of " << posAbsEnc - posStepsAbsEnc);
        Basda::MoccaNServiceWorkerCore<double, double, Nice::Point, DrotServiceWorkerSkeleton>::moveRelative(posAbsEnc - posStepsAbsEnc, "ABSENC");
        comulCorrection += posAbsEnc - posStepsAbsEnc;
        count++;
        corrected = true;
        posSteps = Basda::MoccaNServiceWorkerCore<double, double, Nice::Point, DrotServiceWorkerSkeleton>::getDeviceEncoderPosition("STEPS");
        posAbsEnc = getAbsoluteEncoderPosition();
        U9_LOG("reached position in steps " << posSteps);
        U9_LOG("reached position in steps converted to ABSENC " << coordinateConversionFromSteps(posSteps, "ABSENC"));
        U9_LOG("reached position from absolute encoder " << posAbsEnc);
        if(count >= maxCorrections)
        {
            break;                // shark
            corrected = false;
        }
    }
    U9_LOG("corrected? " << corrected);
    U9_LOG("Comulative correction " << comulCorrection << " ABSENC");
    U9_LOG("Number of iterations " << count);
    U9_LOG("DONE");
    return corrected;
}


/** @brief as first, call (?) base class moveAbsolute, then reset the commanded position _pos in ABSENC with the position given by the absolute encoder
  * @param _pos  commanded position to reach
  * @param _unit unit for commanded position  */
void Sharknir::DrotServiceWorker::moveAbsolute(double _pos, std::string _unit)
{
    if(_unit == stepsString)
    {
        U6_LOG(__FUNCTION__ << tilde << (int)_pos << tilde << _unit);
    }
    else
    {
        U6_LOG(__FUNCTION__ << tilde << _pos << tilde << _unit);
    }

    m_SimulatedDestination = convertPosition(_pos, true, _unit, absencString);
    U9_LOG("m_SimulatedDestination = " << m_SimulatedDestination);
    if(gDrotEnabled)
    {
        Basda::MoccaNServiceWorkerCore<double, double, Nice::Point, DrotServiceWorkerSkeleton>::moveAbsolute(_pos, _unit);
    }
    else
    {
        W_LOG("Derotator movement is currently disabled");
    }
    //m_device->moveAbsolute(Nice::NPoint(1, startPositionInSteps - backlashInSteps * rotationDirection), m_deviceCmdAsync);
    U9_LOG("m_SimulatedAbsoluteEncoder = " << m_SimulatedAbsoluteEncoder);

    resetPosition();

    U9_LOG("moveAbsolute DONE");
}


void Sharknir::DrotServiceWorker::moveAbsoluteWait(const Nice::Time & _wait)
{
    U6_LOG(__FUNCTION__);
    W_LOG("Provide Implementation for Sharknir::DrotServiceWorker::moveAbsoluteWait");
}


void Sharknir::DrotServiceWorker::moveRelative(double _pos, std::string _unit)
{
    U6_LOG(__FUNCTION__ << tilde << _pos << tilde << _unit);

    double retPos = getAbsoluteEncoderPosition();

    //retPos += convertPosition(_pos, false, _unit, absencString);
    double retPosSteps = convertPosition(retPos, true, absencString, stepsString) + convertPosition(_pos, false, _unit, stepsString);
    U9_LOG("Final pos will be : " << (int)(retPosSteps + 0.5) << " STEPS");

    moveAbsolute(retPosSteps, stepsString);
}


void Sharknir::DrotServiceWorker::moveRelativeWait(const Nice::Time & _wait)
{
    U6_LOG(__FUNCTION__);
    W_LOG("Provide Implementation for Sharknir::DrotServiceWorker::moveRelativeWait");
}


/**********************************************/
void Sharknir::DrotServiceWorker::idleOnline()
{
    U6_LOG(__FUNCTION__);
    while( !Basda::ServiceWorker::timedWaitForNewState ( Basda::ServiceWorker::config()->Nice::Joint::getAtNode <Nice::Time> ( "CFG.INTERVAL")))
    {
        status();
        if(m_iif)
        {
            try
            {
                /* we do NOT store angles, because that is only done when starting... */
                // Yes we do show the current angle !
                double pstrt = m_iif->getParallacticAngle();
                setPangStrtAng(pstrt);
                m_config->Nice::Joint::setAtNode<bool>("VAR.IIF", true);
            }
            catch (Nice::Exception & e)
            {
                E_LOG ("IIF service connection lost: " << m_config->getAtNode<std::string>("IIF") << "!");
                m_config->Nice::Joint::setAtNode<bool>("VAR.IIF", false);
                m_iif = 0;
                setPangStrtAng(-9999, Nice::Date());
            }
            catch(...)
            {
                U6_LOG(__FUNCTION__);
            }
        }
        else
        {
            U9_LOG("m_iif zero")
        }
    }
}



Nice::SeqDouble Sharknir::DrotServiceWorker::OsGetDerotationProfile(const Nice::Date & start_date, int number_of_polynomials, float time_interval)
{
    const Ltcs::IifSide sdx(Ltcs::SIDE_LEFT);
    double startMJD = start_date.toMJD();

    if(number_of_polynomials > 10800 || number_of_polynomials < 2)
    {
        throw(std::invalid_argument("number of polynomials must be between [2-10880]"));
    }
    else if(number_of_polynomials*time_interval > 10800)
    {
        throw(std::invalid_argument("total time cannot be greater then 3h"));
    }

    double good_interval = time_interval;
    if(good_interval > 1)good_interval = 1.0;
    int good_number_of_polynomials = number_of_polynomials;
    if(time_interval > 1)good_number_of_polynomials = number_of_polynomials*time_interval;

    Nice::SeqSeqDouble rotatorPolynomials;
    try
    {
        I_LOG("requesting " << number_of_polynomials << " polynomials for " << sdx.toString() << " side starting " << start_date << "; MJD " << startMJD )
        rotatorPolynomials = m_iif->getRotatorPolynomials(startMJD, good_number_of_polynomials, good_interval, 2, sdx.toString());
    }
    catch(const Ltcs::RotatorPolynomialException & e)
    {
        E_LOG(e.what());
        throw e;
    }
    catch(...)
    {
        std::string what = boost::current_exception_diagnostic_information();
        std::string toErase = "std::exception::what:";
        size_t pos = what.find(toErase);
        if(pos != std::string::npos)what.erase(pos, toErase.length());
        what.erase(std::remove(what.begin(), what.end(), '\n'), what.end());
        std::stringstream log_msg;
        log_msg << __FUNCTION__ << " ~ " << what;
        E_LOG(log_msg.str());
        usleep(200000);
        throw;
    }

    Nice::SeqDouble result1, result2;
    int numberOfPolynomials = rotatorPolynomials.size();
    if(numberOfPolynomials == good_number_of_polynomials)
    {
        double position = 0, start_position = 0, pstep = 0;
        start_position = rotatorPolynomials[0][1];
        for(unsigned int i = 0; i < numberOfPolynomials; i++)
        {
            /* SEE DNOTES IN CREATETRAJECTORY */
            const std::vector<double> & polynomials = rotatorPolynomials[i];
            pstep = polynomials[1] - start_position - position;
            position = polynomials[1] - start_position;

            // pstep is supposed to be a small correction, if greater than Pi then a full turn occurred.
            if(pstep > M_PI)
                position  -=  2.*M_PI;
            else if(pstep < -M_PI)
                position  +=  2.*M_PI;

            result1.push_back(position * 180./M_PI);

        }
        for(int i = 0; i < numberOfPolynomials; i += time_interval)
        {
            result2.push_back(result1[i]);
        }
    }
    else
    {
        W_LOG(__FUNCTION__ << " : Could  not retrieve requested polynomials");
    }
    return result2;
}


/** @brief Get IIF rotator polynomials and convert them to MoCon steps.
  *  Note that the entire calculation is unaware of a start position (in steps).
  *  So the trajectory is starting (virtually) at step 0. It is left
  *  to getReady() to change the steps into absolute steps for the MoCon.
  * @param _startDate Datum of the start of the trajectory (in the future) */
void Sharknir::DrotServiceWorker::createTrajectory(const Nice::Date & _startDate)
{
    U6_LOG(__FUNCTION__ << tilde << _startDate);
    const int frequency = m_frequency;
    const int polynomialOrder = m_polynomialOrder;
    const Nice::U64 resolution = m_resolutionProperty->get<Nice::U64>();
    m_stepsPerRad    = (double)resolution / (2.0 * M_PI);
    m_stepsPerDegree = m_stepsPerRad / DR2D;
    int numberOfPolynomials = m_numberOfPolynomials;
    /* retrieve the polynomials from the TCS
     * At this point name() is LIRCS_DROT or DX_HWS_DROT or SX_HWS_DROT or DX_GWS_DROT ...
     * as set in the configuration file for the DROT.SVC.LOAD.
     * and we let the ltcs side figure out whether this means to take the left or the right polynomial.
     * Now for LIRCS the sdx.s becomes "Both", and the LBCS function needs to figure out
     * how to average the two polynomials. */
    // const Ltcs::IifSide sdx( name()) ;
    const Ltcs::IifSide sdx(Ltcs::SIDE_LEFT);
    /* need to transform start time to mjd to match the interface.. */
    double startMJD = _startDate.toMJD();
    Nice::SeqSeqDouble rotatorPolynomials;
    double par_ang = m_iif->getParallacticAngle();
    try
    {
        I_LOG("requesting " << numberOfPolynomials << " polynomials for " << sdx.toString() << " side starting " << _startDate << "; MJD " << startMJD << " ~ ParAng : " << par_ang)
        rotatorPolynomials = m_iif->getRotatorPolynomials(startMJD, numberOfPolynomials, 1./frequency, polynomialOrder, sdx.toString());
    }
    catch(const Ltcs::RotatorPolynomialException & e)
    {
        E_LOG(e.what() );
        throw e;
    }
    catch(...)
    {
        std::string what = boost::current_exception_diagnostic_information();
        std::string toErase = "std::exception::what:";
        size_t pos = what.find(toErase);
        if(pos != std::string::npos)what.erase(pos, toErase.length());
        what.erase(std::remove(what.begin(), what.end(), '\n'), what.end());
        std::stringstream log_msg;
        log_msg << __FUNCTION__ << " ~ " << what;
        E_LOG(log_msg.str());
        usleep(200000);
        throw;
    }


            if(gDebug2 == 7)
            {
                std::string line;
                int i = 0;
                std::ifstream ifstrm;
                std::istringstream isstream;
                rotatorPolynomials.clear();
                U9_LOG("Loading file " << "/home/shinsmgr/drot_test/start_polyn_test_00.txt")
                std::string test_file = "/home/shinsmgr/drot_test/start_polyn_test_00.txt";
                bool firstValue = true;
                double FirstValue;
                if(boost::filesystem::exists(boost::filesystem::path(test_file)))
                {
                    rotatorPolynomials.clear();
                    Nice::SeqDouble record(4, 0);
                    ifstrm.open(test_file.c_str());
                    if(std::getline(ifstrm, line))
                    {
                        isstream.str(line);
                        double read_value = 0;
                        isstream >> read_value;
                        record[0] = read_value;
                    }
                    while(std::getline(ifstrm, line))
                    {
                        isstream.clear();
                        isstream.str(line);
                        double read_value = 0;
                        isstream >> read_value;
                        if(isstream.fail())
                        {
                            E_LOG("Failed reading line " << i + 1 << "; read_data = " << line);
                            break;
                        }
                        else
                        {
                            if(firstValue)
                            {
                                FirstValue = read_value;
                                firstValue = false;
                            }
                            record[1] = read_value;
                            rotatorPolynomials.push_back(record);
                        }
                        i++;
                        line = "";
                    }
                    ifstrm.close();
                }
                else if(!test_file.empty())
                {
                    E_LOG("File " << test_file << " does not exists");
                }
            }
            if(gDebug2 == 6)
            {
                std::string line;
                int i = 0;
                std::ifstream ifstrm;
                std::istringstream isstream;
                rotatorPolynomials.clear();
                U9_LOG("Loading file " << "/home/shinsmgr/drot_test/start_polyn_test_00.txt")
                std::string test_file = "/home/shinsmgr/drot_test/start_polyn_test_00.txt";
                bool firstValue = true;
                double FirstValue;
                if(boost::filesystem::exists(boost::filesystem::path(test_file)))
                {
                    rotatorPolynomials.clear();
                    Nice::SeqDouble record(0, 4);
                    ifstrm.open(test_file.c_str());
                    while(std::getline(ifstrm, line))
                    {
                        isstream.clear();
                        isstream.str(line);
                        for(int jj = 0; jj < 4; jj++)
                        {
                            double read_value = 0;
                            isstream >> read_value;
                            if(isstream.fail())
                            {
                                E_LOG("Failed reading line " << i + 1 << "; element : " << jj + 1 << "; read_data = " << line);
                                break;
                            }
                            else
                            {
                                if(firstValue)
                                {
                                    FirstValue = read_value;
                                    firstValue = false;
                                }
                                record[jj] = read_value;
                            }
                        }
                        rotatorPolynomials.push_back(record);
                        i++;
                        line = "";
                    }
                    ifstrm.close();
                }
                else if(!test_file.empty())
                {
                    E_LOG("File " << test_file << " does not exists");
                }
            }


    I_LOG("got " << rotatorPolynomials.size() << " polynomials")
    if(rotatorPolynomials.size() < 2)
    {
        E_THROW(Sharknir::DrotPolynomialToShortException, "LBT rotator polynomial number too low! Check requested number of polynomials.")
    }

    /* numberOfPolynomials were those (read above, constant) in the configuration file.
     * Overwrite them by the potentially smaller number retrieved from the TCS. */
    numberOfPolynomials = rotatorPolynomials.size();

    std::vector<double> firstPolynomials = rotatorPolynomials[0];
//    char poly_names[4][16] = {"Time MJD", "Angle", "Velocity", "Acceleration/2"};

    m_startDate = Nice::Date::fromMJD(firstPolynomials[0]);
    if(rotatorPolynomials.size() > 0)
    {
        U9_LOG("polynomials obtained from " << m_startDate  << " on; rotatorPolynomials.size() : " << rotatorPolynomials.size() << "/" << rotatorPolynomials[0].size());
    }
    /* proposed start angle by IIF polynomials, in radians */

    m_startAngle = firstPolynomials[1];


    if(m_ControlFlags.bLogDerotationPositions)
    {
        std::vector<double>::iterator itDouble;
        Nice::SeqSeqDouble::const_iterator itSeq;
        std::stringstream log_msg;
        for(itSeq = rotatorPolynomials.begin(); itSeq < rotatorPolynomials.end(); itSeq++)
        {
            log_msg << std::setprecision(12) << (*itSeq)[0] << "\t";
            log_msg << std::setprecision(8) << (*itSeq)[1] << "\t";
            log_msg << std::setprecision(8) << (*itSeq)[2] << "\t";
            log_msg << std::setprecision(8) << (*itSeq)[3] << "\n";
        }
        std::stringstream path;
        path << "/home/shinsmgr/drot_test/start_polyn_" << m_derotStartDate.toString(0, true, LOGNAME_FORMAT) << ".txt";
        std::ofstream out(path.str().c_str());
        out << log_msg.str();
        out.close();
    }

#if 0
    std::ofstream streamTrajectory("/opt/tan/var/log/TCS_trajectory.txt");
    for(int i = 0; i < rotatorPolynomials.size(); i++)
    {
        for(int j = 0; j < rotatorPolynomials[i].size(); j++)
        {
            streamTrajectory << rotatorPolynomials[i][j] << " ";
        }
        streamTrajectory << std::endl;
    }
    streamTrajectory.close();
#endif


    double timeSteps = 1.0 / (double)frequency;                       // seconds
    int numberOfSamples = (int) (numberOfPolynomials * m_samplingRate * frequency);

    double ang_rad = 0;
    double ang_step = 0;
    /* minimum and maximum value of steps (looking for curvature...) */
    /* convert the list of piece-wise polynomials into motor steps */
    double p = 0;
    m_positionSequence.clear();
    std::stringstream angle_steps, poly;
//    angle_steps << "{";
//    poly << "{";
    for(unsigned int i = 0; i < numberOfPolynomials; i++)
    {
        /* note that polynomial Orders >=3 (jerk..) are ignored here, even ifconfigured .... */

        /* Angle p in radian at the start of the segment relative to overall start.
         * If this is i > 0, there is a previous value p. in rare cases the polynomials may
         * wrap around -pi or +2*pi ifthe trajectory passes through some sort of zero
         * or -180 degrees of the parallactic angle. We try to remove any steps of 2*pi
         * by comparing the new value, rotatorPolynomials[i][1]-startAngle with the previous value.
         * RJM 2016-12-14
         * const double pstep = (i==0) ? 0. : ( rotatorPolynomials[i][1] - startAngle -p) ; */
        const std::vector<double> & polynomials = rotatorPolynomials[i];
        const double pstep = polynomials[1] - m_startAngle - p;
        //p += pstep;
        p = polynomials[1] - m_startAngle;

        // pstep is supposed to be a small correction, if greater than Pi then a full turn occurred.
        if(pstep > M_PI)
             p  -=  2.*M_PI;
        else if(pstep < -M_PI)
             p  +=  2.*M_PI;

        double v  =  0.0;
        if( polynomialOrder > 0)
            v  =  polynomials[2];

        double aby2  =  0.0;
        if( polynomialOrder > 1)
            aby2  =  polynomials[3];

        int iterations = (int)(m_samplingRate * frequency);
        for(int j = 0; j < iterations; j++)
        {
            /* time in seconds */
            const double tn  = j * timeSteps;
            /* angle in radians */
            ang_rad  = p + tn*v + tn*tn*aby2;
            /* position in steps (mocon/NDevice.cc multiplies for microsteps) */
            ang_step = m_direction * ang_rad * m_stepsPerRad/m_microsteps;
            m_positionSequence.push_back(ang_step);
            angle_steps << ang_step;
            poly << polynomials[1];
            if(i < numberOfPolynomials - 1){angle_steps << "\t"; poly << "\t";}
        }
    }
//    angle_steps << "}";
//    poly << "}";
    if(numberOfPolynomials <= 300 && false)
    {
        U8_LOG(angle_steps.str());
        U8_LOG(poly.str());
    }
    else if(false)
    {
        std::ofstream out("/home/shinsmgr/drot_test/Polynomials.txt");
        out << angle_steps.str() << "\n" << poly.str() << "\n";
        out.close();
    }
    U8_LOG("m_direction : " << m_direction << "; m_stepsPerRad : " << (int)m_stepsPerRad << "; StartingValue/ParAng : " << m_startAngle/Degree << "/" << par_ang << "; FromParallacticAngle : " << rotatorPolynomials[0][1]/Degree - par_ang << " ~ sum : " << par_ang + rotatorPolynomials[0][1]/Degree);

    /* IIF polynomials value at the end of the trajectory relative to the start (radians) */
    m_ang_rad = ang_rad;
    /* Motor position in units of steps relative to the start position. */
    m_ang_steps = ang_step;

    /* note: the following logs do *not* incorporate the strange sign convention
     * of one of the GWS bearings! To get those one would need to use the convertPosition(). */
//    I_LOG("derotation angle at end will be " << m_positionSequence[numberOfSamples - 1] << " ABSENC")
//    I_LOG("derotation angle at end will be " << coordinateTransformation(m_positionSequence[numberOfSamples - 1]*m_microsteps, degreeString) << " DEGREE")
//    I_LOG("derotation angle min and max will be " << (int)(m_positionSequenceRange[0]*m_microsteps) << " " << (int)(m_positionSequenceRange[1]*m_microsteps << " STEPS")
}


/** @brief Move to the start/initial position of the rotator trajectory.
  *  Also convert the relative positions in m_positionSequence to absolute positions.
  * @param _startPosition Start position of derotation.
  * @param _unit The unit of the _startPosition value.  */
void Sharknir::DrotServiceWorker::getReady(double _startPosition, std::string _unit)
{
    U6_LOG(__FUNCTION__);
    I_LOG("get Ready start position " << _startPosition << " " << _unit)
    int i = 0;
    double x =1.0;
    /* in some cases it can be that the motion is so slow that the first position values are 0.
     * searching for the first position changes to specify the moving direction
     * TODO: this should better be done by looking into the 2nd term of the polynomials... */
    while((i < (int)m_positionSequence.size() - 1) && (m_positionSequence[0] == m_positionSequence[i]))
    {
        i++;
    }

    if(i < (int)m_positionSequence.size())
    {
        x = m_positionSequence[0] - m_positionSequence[i];
    }

    int rotationDirection = ((x > 0) - (x < 0)) *  m_direction;
    double currentPositionInSteps = m_config->Nice::Joint::getAtNode<double>(Basda::Mocca::Svc::PropertyName::deviceEncoderPosition() + "." + "STEPS");
    double startPositionInSteps = coordinateTransformationInverse(_startPosition, _unit);

//    I_LOG("start steps " << (int)startPositionInSteps);
    int backlashInSteps = m_config->Nice::Joint::getAtNode<int>(Sharknir::Drot::Svc::PropertyName::datasheetBacklash());

//    if(startPositionInSteps != currentPositionInSteps)
    {
        I_LOG("Derotator is moving to start position " << (int)startPositionInSteps)

        moveAbsolute(startPositionInSteps, stepsString);
        //m_device->moveAbsolute(Nice::NPoint(1, startPositionInSteps - backlashInSteps * rotationDirection), m_deviceCmdAsync);
//        I_LOG("moved to start position " << (int)startPositionInSteps)
        // Nice::CompletionState state = m_device->getProgress().state();
        // if(state != Nice::OK)
        // {
        //     I_LOG("Removing backlash failed. Completion state is: " << state);
        //     E_THROW(Sharknir::DrotCommandExecutionFailedException, "Removing backlash failed. Completion state is: " << state)
        // }
        // m_device->moveAbsolute(Nice::NPoint(1, startPositionInSteps), m_deviceCmdAsync);
        // I_LOG("moved to absolute start position " << startPositionInSteps)
        // state = m_device->getProgress().state();
        // if(state != Nice::OK)
        // {
        //     I_LOG("Moving to start position failed. Completion state is: " << state);
        //     E_THROW(Sharknir::DrotCommandExecutionFailedException, "Moving to start position failed. Completion state is: " << state)
        // }
    }
    // else
    // {
    //     D_LOG("Derotator removing backlash");
    //     m_device->moveAbsolute(Nice::NPoint(1, currentPositionInSteps - backlashInSteps * rotationDirection), m_deviceCmdAsync);
    //     Nice::CompletionState state = m_device->getProgress().state();
    //     if(state != Nice::OK)
    //     {
    //         E_THROW(Sharknir::DrotCommandExecutionFailedException, "Removing backlash failed. Completion state is: " << state)
    //     }
    //     m_device->moveAbsolute(Nice::NPoint(1, currentPositionInSteps), m_deviceCmdAsync);
    //     state = m_device->getProgress().state();
    //     if(state != Nice::OK)
    //     {
    //         E_THROW(Sharknir::DrotCommandExecutionFailedException, "Moving to start position failed. Completion state is: " << state)
    //     }
    // }
    currentPositionInSteps = getDeviceEncoderPosition("STEPS");

    //I_LOG("currpos steps " << currentPositionInSteps)
    /* at this point all the relative positions in steps are converted to absolute positions as understood by the MoCon motion profiler.... */
    m_positionSequence += currentPositionInSteps/m_microsteps;

    I_LOG("m_positionSequence[0] = "<< m_positionSequence[0])
    try
    {
        /* store nominal motor start angle into the property (radians).
         * This includes the offsets, for example -60 for the GWS RJM 2017-03-19 */
        const double angDeg = convertPosition(_startPosition, true, _unit, degreeString);
        setMotStrtAng(angDeg);///DR2D);

        /* store parallactic Angle in the property in units of degrees (!) from the DD of the IIF,
         * Need the parallactic angle with a KNOWN motor offset, stored by getReady(), to construct the WCS matrix, RJM 2016-12-14 */
        const double pstrt = m_iif->getParallacticAngle();
        setPangStrtAng(pstrt);

        I_LOG("Motor start angle " << angDeg << " deg; Starting parallactic angle : " << pstrt << " deg")
    }
    catch(...)
    {
        /* this will likely not succed on the rotator stages of GWS and MHWS, which does not matter
         * because these are not monitored by GEIRS */
        E_LOG("No connection to IIF");
    }

    D_LOG("Derotator is starting trajectory at " << currentPositionInSteps << " steps")
}

/** @brief Get an estimate of the time that is left on the trajectory.
  * This subtracts the time "now" from the estimated finish time that was
  * deposited in the property tree at the start of the derotation.
  * @return The estimated residual derotation time.
  * @warning If used before the derotation started this will return zero seconds.
  * @since 2017-04-05
  * @author R. J. Mathar */
Nice::Time Sharknir::DrotServiceWorker::timeLeft()
{
    U6_LOG(__FUNCTION__);
    /* estimated finish date */
    const Nice::Date tFin = getTrajFinDate();

    /* current time */
    const Nice::Date curr = Nice::Date::now();

    U8_LOG("current time " << curr);
    U8_LOG("finish time " << tFin);

    if( curr > tFin)
    {
        /* already finished */
        return Nice::Time::seconds(0);
    }
    else
    {
        /* time left is difference from now up to the esimated finish time */
        return (tFin - curr);
    }
}

/** @brief Get an estimate of the time that is left on the trajectory till hitting the limit switch.
  *
  * Optionally truncate the list of positions such that the trajectory fits into the travel range.
  *
  * This is a static evaluation of the profile; it does not look at what
  * the current motor positions are or whether parts of the trajectory are already executed..
  *
  * There is a basda-mocca/src/device/mpia/mocon/Module.cc with validateExternalProfile()
  * that does a similar job. This is called by mpia/mocon/NDevice.cc, which is the
  * base class of this here, within generateExternalProfile().
  *
  * @param _startPosition Start position of derotation.
  *   If the function is called *after* getReady(), the offset of the start position
  *   is already added to the local m_positionSequence, and the paramter _startPosition
  *   of this function here should be set to zero to avoid double application of the offset.
  * @param _unit The unit of the _startPosition value.
  * @param trunc If true, truncate the polynomials.
  *   This means parts of the trajectory are deleted at the end (ifneeded) such
  *   that remaining first part can be excuted within the limit switches.
  *   This is useful to have a dynamic adaptation into the residual travel range;
  *   then validateExternalProfile() does not block execution of the entire trajectory.
  * @return The time estimate according to the profile and distance to the limit.
  *    This is the time interval needed to execute the trajectory starting at the
  *    start position until either the down-loaded polynomials are exhausted or
  *    until one of the limit switches is reached, whichever happens first.
  * @since 2017-04-05
  * @author R. J. Mathar */
Nice::Time Sharknir::DrotServiceWorker::timeLeft(int frequency, double _startPosition, std::string _unit, bool trunc)
{
    /* same computation as above in createTrajectory(). One sample in the polynomial
     * equals a time difference of timeSteps (seconds). */
    U6_LOG(__FUNCTION__ << tilde << frequency << tilde << _startPosition << tilde << _unit << tilde << trunc);
    const double timeSteps = 1.0 / frequency;
    const double startPositionInSteps = coordinateTransformationInverse(_startPosition, _unit);

    /* time in seconds since start of trajectory */
    int t = 0;
    int pos = 0;
    /* walk along the trajectory and see whether any of the 2 limits are violated */
    I_LOG("m_positionSequence.size() = " << m_positionSequence.size());
    I_LOG("startPositionInSteps = " << (int)startPositionInSteps);
    for(unsigned int j = 0; false && j < m_positionSequence.size(); j++)
    {
        /* get position update at item j in units of steps along the trajectory */
        // int pos = (int) (m_positionSequence[j]+startPositionInSteps) ;
        pos = (int) (m_positionSequence[j] + startPositionInSteps);
        t = (int)(j*timeSteps);
        /* compare with travel range; break loop ifthe steps step beyond . */
        if(pos < m_travelRangeMin || pos > m_travelRangeMin + m_travelRange)
        {
            t = (int)(j*timeSteps);

            if(trunc && j > 5/timeSteps)
            {
                /* stop roughly 5 seconds before the limit.... */
                I_LOG("stop roughly 5 seconds before the limit...." << tilde << j);
                m_positionSequence.resize(j - (int)(5/timeSteps));
                t = (int) ( m_positionSequence.size()*timeSteps);
                I_LOG("Trajectory chopped after " << t << " seconds at " << pos << " steps");
            }
            break;
        }
    }
    I_LOG("Final position will be : " << convertPosition(pos, false, stepsString, degreeString) << "°");

    // I_LOG("Maximum derotation time to limit switch   " << (int)(t/60) << " minutes and " << (t - ((int)(t/60))*60) << " seconds");
    I_LOG("Time left = " << Nice::Time::seconds(t))
    return Nice::Time::seconds(t);
}

/* VIRTUAL BASDA */
// int Sharknir::DrotServiceWorker::OsExtimateDerotation(const Nice::Date & _startDate, double _startPosition, std::string _unit, bool _autoStart)
// {
//     int finalSize = 0;
//     double effStartPosition = 0.;
//     OsExtimateDerotation(_startDate, _startPosition, _unit, _autoStart, finalSize, effStartPosition);
//     return finalSize;
// }

int Sharknir::DrotServiceWorker::sharkDerotatorBacklashProcedure()
{
    return m_device->sharkDerotatorBacklashProcedure(1);
}


int Sharknir::DrotServiceWorker::OsExtimateStartingPosition(const Nice::Date & _startDate, double _positionAngle, int _correction, bool _autoStart)
{
    U6_LOG(__FUNCTION__ << tilde << _startDate << tilde << _positionAngle << tilde << tilde << _autoStart);


    Nice::Date now = Nice::Date::now();

    if(now > _startDate - Nice::Time::seconds(20))
    {
        createTrajectory(now);

        double ref = convertPosition(m_referencePos, true, absencString, degreeString);
        // m_startAngle [-M_PI, M_PI] ?
        double startAngle = convertPosition(270 - m_startAngle*360/M_PI, false, degreeString, absencString);
        double finalAngle = startAngle + m_referencePos; // should be true north
        if(_correction == 1)finalAngle += m_slitNarrowPos;
        else if(_correction == 2)finalAngle += m_slitWidePos;
    }


    int result = EXIT_FAILURE;
    return result;
}


void Sharknir::DrotServiceWorker::OsExtimateDerotation(Nice::Date & _startDate, double _positionAngle, int _correction, bool _autoStart, int & _finalSize, double & _effStartPositionSteps)
{
    U6_LOG(__FUNCTION__ << tilde << _startDate << tilde << _positionAngle << tilde << _correction << tilde << _autoStart);

    // 00:01:40 per 2400 samples
    double currentPosition, posAngle, parAngle, slitAngle, destinationPosition;
    int timeEsteem;
    currentPosition = getPosition(absencString);
    Nice::Date now = Nice::Date::now();
    Nice::Date effectiveStart = _startDate;
    Nice::Time delta = Nice::Time::seconds(10);

    destinationPosition = currentPosition;
    if(_autoStart)
    {
        timeEsteem = delta;
        if(_startDate < now + Nice::Time::seconds(timeEsteem) + delta)effectiveStart = now + Nice::Time::seconds(timeEsteem) + delta;
    }
    else
    {
        posAngle = convertPosition(_positionAngle, false, degreeString, absencString);
        parAngle = m_iif->getParallacticAngle();
        slitAngle = m_slitNarrowPos;
        if(_correction == 2)slitAngle == m_slitWidePos;
        destinationPosition = m_referencePos + convertPosition(gDebug1*parAngle + gDebug2*_positionAngle + gDebug3*slitAngle, false, degreeString, absencString);
        destinationPosition += 65536*5;
        double travelRangeMin = convertPosition(m_travelRangeMin, true, stepsString, absencString);
        while(destinationPosition > travelRangeMin)destinationPosition -= 32768;
        if(destinationPosition > currentPosition)timeEsteem = ((destinationPosition - currentPosition)/32768.)*130 + 5.5; // 83 deg/min + 5 seconds
        else timeEsteem = ((currentPosition - destinationPosition)/32768.)*130 + 5.5; // 83 deg/min + 5 seconds
        U1_LOG(__FUNCTION__ << " travelRangeMin : " << travelRangeMin << " ~  timeEsteem : " << timeEsteem << " ~ destinationPosition : " << destinationPosition << " ~ currentPosition : " << currentPosition);

        if(_startDate < now + Nice::Time::seconds(timeEsteem) + delta)effectiveStart = now + Nice::Time::seconds(timeEsteem) + delta;
    }

    effectiveStart = effectiveStart.roundToSecPrecision();
    createTrajectory(effectiveStart);

#ifdef UNDEFINED
            if(gDebug2 == 5)
            {
                std::string line;
                int i = 0;
                std::ifstream ifstrm;
                std::istringstream isstream;
                m_positionSequence.clear();
                U9_LOG("Loading file " << "/home/shinsmgr/drot_test/derotation_test_00.txt")
                std::string test_file = "/home/shinsmgr/drot_test/derotation_test_00.txt";
                bool firstValue = true;
                double FirstValue;
                if(boost::filesystem::exists(boost::filesystem::path(test_file)))
                {
                    ifstrm.open(test_file.c_str());
                    while(std::getline(ifstrm, line))
                    {
                        isstream.clear();
                        isstream.str(line);
                        double read_value = 0;
                        isstream >> read_value;
                        if(isstream.fail())
                        {
                            E_LOG("Failed reading line " << i + 1 << "; read_data = " << line);
                            break;
                        }
                        else
                        {
                            if(firstValue)
                            {
                                FirstValue = read_value;
                                firstValue = false;
                            }
                            m_positionSequence.push_back(read_value - FirstValue);
                        }
                        i++;
                        line = "";
                    }
                    ifstrm.close();
                }
                else if(!test_file.empty())
                {
                    E_LOG("File " << test_file << " does not exists");
                }
            }
            else if(gDebug2 == 1 || gDebug2 == 2 || gDebug2 == 3 || gDebug2 == 4)
            {
                m_positionSequence.clear();
                double ampiezza = 45;
                double max_velocity = gDebug1;
                double durata = (int)(60*ampiezza*2*M_PI/max_velocity + 0.999999);
                double effective_max_velocity = 60*ampiezza*2*M_PI/durata;
                double max_acceleration = ampiezza*(60*2*M_PI/durata)*(60*2*M_PI/durata);
                double max_jerk = max_acceleration*(60*2*M_PI/durata);
                double single_step = 1/m_frequency;
                int num_poly = (int)(60*ampiezza*2*M_PI/(max_velocity*single_step) + 0.999999);
                double pos;
                double i;
                const double steps_per_degree = 40960000/360.;
                for(i = 0; i < 2*durata + single_step*1.1; i += single_step)
                {
                    if(gDebug2 == 1)
                        pos = (steps_per_degree/m_microsteps)*ampiezza*(1 - cos(i*2*M_PI/durata));
                    else if(gDebug2 == 2)
                        pos = -(steps_per_degree/m_microsteps)*ampiezza*(1 - cos(i*2*M_PI/durata));
                    else if(gDebug2 == 3)
                        pos = (steps_per_degree/m_microsteps)*ampiezza*sin(i*2*M_PI/durata);
                    else if(gDebug2 == 4)
                        pos = -(steps_per_degree/m_microsteps)*ampiezza*sin(i*2*M_PI/durata);
                    m_positionSequence.push_back(pos);
                }
                U2_LOG("Circular Derotation -> durata : " << durata << "; Num Poly : " << num_poly << "; max velocity : " << effective_max_velocity << "; max acceleration : " << max_acceleration << "; max_jerk : " << max_jerk);

                double starting_velocity = (m_positionSequence[1] - m_positionSequence[0])/single_step;
                double ending_velocity = (m_positionSequence[m_positionSequence.size() - 1] - m_positionSequence[m_positionSequence.size() - 2])/single_step;
                double mocon_cycle = 614.411;
            }
#endif
    double drot_invariant = 270; // ASSUMPTION : TrueParAng + DrotPoly = drot_invariant = 270;
    destinationPosition = m_referencePos + convertPosition(gDebug1*(drot_invariant - m_startAngle/Degree) + gDebug2*_positionAngle + gDebug3*slitAngle, false, degreeString, absencString);

    _finalSize = 0;
    _effStartPositionSteps = convertPosition(destinationPosition, true, absencString, stepsString);
    _effStartPositionSteps -= 40960000*5;
    while(_effStartPositionSteps < m_travelRangeMin)
    {
        _effStartPositionSteps += 20480000;
    }
    U5_LOG("_effStartPositionSteps : " << (int)_effStartPositionSteps << " ~  " << "m_referencePos : " << m_referencePos << "  ~  " << "DrotParAngle : " << drot_invariant - m_startAngle/Degree << "  ~ " << "ParAngle : " << parAngle << " ~  " << "Inv : " << m_startAngle + parAngle << "  ~  " << "PositionAngle : " << _positionAngle << " ~ " << "SlitAngle : " << slitAngle << " ~ " << " gDebug1/2/3 : " << gDebug1 << "/" << gDebug2 << "/" << gDebug3);
    double aboveRange, belowRange;
    double min, max;
    double prev_max, prev_min;

    const double safetyRange = 48*625;
    const double minRange = (((int)(m_travelRangeMin + safetyRange))/10)*10;
    const double maxRange = (((int)(m_travelRangeMin + m_travelRange - safetyRange))/10)*10;
    if(_autoStart)
    {
        // we don't know how long it will take to reach start position so we calculate twice
        for(int j = 0; j < 2; j++)
        {
            if(j == 1)
            {
                currentPosition = getPosition(stepsString);
                if(currentPosition > _effStartPositionSteps)timeEsteem = (currentPosition - _effStartPositionSteps)/40960000*260 + 5.5; // 83 deg/min + 5 seconds
                else timeEsteem = (_effStartPositionSteps - currentPosition)/40960000*260 + 5.5; // 83 deg/min + 5 seconds

                if(effectiveStart < now + Nice::Time::seconds(timeEsteem) + delta)effectiveStart = now + Nice::Time::seconds(timeEsteem) + delta;
                effectiveStart = effectiveStart.roundToSecPrecision();
                createTrajectory(effectiveStart);
                _finalSize = 0;
            }
            _effStartPositionSteps = getPosition(stepsString);
            std::vector<double>::iterator itDouble;
            // m_positionSequence is in MacroSteps;
            max = min = *m_positionSequence.begin();
            U9_LOG(__FUNCTION__ << " ~ min/max" << min << "/" << max);
            for(itDouble = m_positionSequence.begin(); itDouble < m_positionSequence.end(); itDouble++)
            {
                if(*itDouble < min)min = *itDouble;
                if(*itDouble > max)max = *itDouble;
                if((max - min)*m_microsteps > (maxRange - minRange))
                {
                    //ensure that final range is smaller than m_travelRange
                    max = prev_max;
                    min = prev_min;
                    break;
                }
                prev_max = max;
                prev_min = min;
                _finalSize++;
            }
            aboveRange = (max - m_positionSequence[0])*m_microsteps;
            belowRange = (m_positionSequence[0] - min)*m_microsteps;
            if(_effStartPositionSteps >= minRange + belowRange && _effStartPositionSteps <= maxRange - aboveRange)
            {
                // No need to move drot
            }
            else
            {
                if(_effStartPositionSteps < minRange + belowRange)
                {
                    _effStartPositionSteps = minRange + belowRange;
                }
                else if(_effStartPositionSteps > maxRange - aboveRange)
                {
                    _effStartPositionSteps = maxRange - aboveRange;
                }
            }
        }
    }
    else
    {
        std::vector<double>::iterator itDouble;
        std::vector<double> ranges(m_positionSequence.size(), 0);
        double min, max;
        double stepDouble;
        min = max = *m_positionSequence.begin()*m_microsteps + _effStartPositionSteps;
        double prev_max = max, prev_min = min;
        for(itDouble = m_positionSequence.begin(); itDouble < m_positionSequence.end(); itDouble++)
        {
            stepDouble = *itDouble*m_microsteps + _effStartPositionSteps;
            if(stepDouble < min)min = stepDouble;
            if(stepDouble > max)max = stepDouble;
            if(stepDouble < m_travelRangeMin || stepDouble > m_travelRangeMin + m_travelRange)break;
            prev_max = max;
            prev_min = min;
            _finalSize++;
        }
        aboveRange = prev_max - _effStartPositionSteps;
        belowRange = _effStartPositionSteps - prev_min;
    }
    if(_finalSize < m_positionSequence.size())
    {
        W_LOG("Derotation will be truncated to first " << _finalSize << " polynomials.");
    }
    U7_LOG("minRange = " << (int)minRange << "; maxRange = " << (int)maxRange << "; belowRange = " << (int)belowRange << "; aboveRange = " << (int)aboveRange << "; effectiveStartPosition = " << (int)_effStartPositionSteps);
    U7_LOG("Derotation will last for " << (int)(_finalSize/m_frequency) << " seconds");
    _startDate = effectiveStart;
}


int Sharknir::DrotServiceWorker::OsStartDerotationTest(double startPosDegree, double endPosDegree, double speedDegreeSec)
{
    U6_LOG(__FUNCTION__ << tilde << "startPosDegree : " << startPosDegree << tilde << "endPosDegree : " << endPosDegree << tilde << "sppedDegreeSec : " << speedDegreeSec);
    int result = EXIT_FAILURE;
    try
    {
        int finalSize = 0;
        double effectiveStartPosition = 0;
        effectiveStartPosition = convertPosition(startPosDegree, true, degreeString, stepsString);

        moveAbsolute(effectiveStartPosition, stepsString);
        moveAbsolute(effectiveStartPosition, stepsString);
        effectiveStartPosition = getPosition(stepsString);

        double effectiveSpeed = abs(speedDegreeSec);
        if(endPosDegree < startPosDegree)effectiveSpeed *= -1.;
        double duration = (endPosDegree - startPosDegree)/(effectiveSpeed/60);
        finalSize = duration*m_frequency;
        m_positionSequence.resize(finalSize);
        double step = convertPosition((effectiveSpeed/60)/m_frequency, false, degreeString, stepsString);
        for(int i = 0; i < finalSize; i++)
        {
            m_positionSequence[i] = i*step/m_microsteps;
        }
        double starting_ramp = m_device->shinsGetStartingRamp(m_positionSequence, 1000000./m_frequency);
        U9_LOG("starting_ramp : " << starting_ramp);
        moveRelative(starting_ramp, stepsString);
        m_positionSequence += effectiveStartPosition/m_microsteps;

        /* store nominal motor start angle into the property (radians).
        * This includes the offsets, for example -60 for the GWS RJM 2017-03-19 */
        const double angDeg = convertPosition(effectiveStartPosition, true, stepsString, degreeString);
        U7_LOG("Final size : " << finalSize << tilde << "Effective start position : " << (int)effectiveStartPosition << tilde << "Time duration : " << duration << tilde << "Step : " << step);

        setMotStrtAng(angDeg);
        /* store parallactic Angle in the property in units of degrees (!) from the DD of the IIF,
        * Need the parallactic angle with a KNOWN motor offset, stored by getReady(), to construct the WCS matrix, RJM 2016-12-14 */
        const double pstrt = m_iif->getParallacticAngle();
        setPangStrtAng(pstrt);
        I_LOG("Motor start angle " << angDeg << " deg; Starting parallactic angle : " << pstrt << " deg");
        int delay_seconds = 30;

        Nice::Time tDurat = Nice::Time::seconds(finalSize/m_frequency + finalSize/22. + delay_seconds + 5); // finalSize/22 = time to load values; 5 confidence
        Nice::Date t0 = Nice::Date::now();
        setTrajFinDate(t0 + tDurat);

        m_derotStartDate = Nice::Date::now() + Nice::Time::seconds(delay_seconds);
        m_derotStartDate = m_derotStartDate.roundToSecPrecision();

        if(m_ControlFlags.bLogDerotationPositions)
        {
            std::vector<double>::iterator itDouble;
            std::stringstream log_msg;
            for(itDouble = m_positionSequence.begin(); itDouble < m_positionSequence.end(); itDouble++)
            {
                log_msg << *itDouble << "\t";
                log_msg << convertPosition(*itDouble*m_microsteps, true, stepsString, absencString) << "\n";
            }
            std::stringstream path;
            path << "/home/shinsmgr/drot_test/start_derot_" << m_derotStartDate.toString(0, true, LOGNAME_FORMAT) << ".txt";
            std::ofstream out(path.str().c_str());
            out << log_msg.str();
            out.close();
        }
        I_LOG("Start profile");

        m_bDerotationRunning = true;
        U7_LOG("Starting External Profile ~ " << m_derotStartDate.timeString(true) << " ~ " << m_derotStartDate.dateString(true) << " ~ " << m_derotStartDate.toString(3, true, "%Y-%m-%d-%H-%M-%S"))
        if(gShinsProfile)
        {
            unsigned int faultTolerance = m_faultTolerance;
            if(gDrotEnabled == false)faultTolerance = 0;
            m_device->shinsStartExternalProfile(m_derotStartDate, m_positionSequence, m_frequency, m_samplesPerSegment, faultTolerance, deviceCmdAsyncImpl());
        }
        Nice::Date t1 = Nice::Date::now();
        U7_LOG("External Profile End. Trajectory started at : " << t0 << " ended at : " << t1 << "; Elapsed time : " << (t1 - t0));
        resetPosition();
        setTrajFinDate(t1);
        m_bDerotationRunning = false;
        SaveDerotationLog();
        result = EXIT_SUCCESS;
    }
    catch(...)
    {
        E_LOG(__FUNCTION__ << " ~ Error");
        m_drotMode = DROT_MODE_PUPIL;
        m_bDerotationRunning = false;
        std::string what = boost::current_exception_diagnostic_information();
        std::string toErase = "std::exception::what:";
        size_t pos = what.find(toErase);
        if(pos != std::string::npos)what.erase(pos, toErase.length());
        what.erase(std::remove(what.begin(), what.end(), '\n'), what.end());
        std::stringstream log_msg;
        log_msg << __FUNCTION__ << " ~ " << what;
        E_LOG(log_msg.str());
        usleep(200000);
        try{SaveDerotationLog();}catch(...){E_LOG(__FUNCTION__ << " ~ " << boost::current_exception_diagnostic_information()); }
    }

    U8_LOG(__FUNCTION__);
    return result;
}


void Sharknir::DrotServiceWorker::SaveDerotationLog()
{
    if(!m_positionTimes.empty())
    {
        std::stringstream path;
        std::vector<Nice::Date>::iterator itTime;
        std::vector<double>::iterator itPosition;
        path << "/home/shinsmgr/drot_test/derot_posit_" << m_derotStartDate.toString(0, true, LOGNAME_FORMAT) << ".txt";
        std::ofstream out(path.str().c_str());
        for(itTime = m_positionTimes.begin(), itPosition = m_derotPositions.begin(); itTime < m_positionTimes.end(); itTime++)
        {
            out << *itTime << "\t" << (int)*itPosition << "\n";
            itPosition++;
        }
        out.close();
        U8_LOG(__FUNCTION__ << " Completed ~ Saved : " << m_derotPositions.size() << " positions");
        m_positionTimes.clear();
        m_derotPositions.clear();
    }
}


int Sharknir::DrotServiceWorker::OsStartDerotation(const Nice::Date & _startDate, double _positionAngle, int _correction, bool _autoStart)
{
    U6_LOG(__FUNCTION__ << tilde << _startDate << tilde << _positionAngle << tilde << tilde << _autoStart);

    int result = EXIT_FAILURE;
    Basda::ServiceWorkerSkeleton::cmdStart("startProfile");
    try
    {
        m_derotStartDate = _startDate;
        int finalSize = 0; // In STEPS
        double effectiveStartPosition = 0;
        double startPosition = m_referencePos + convertPosition(_positionAngle, false, degreeString, absencString);
        if(_correction == 1)startPosition += convertPosition(m_slitNarrowPos, false, degreeString, absencString);
        else if(_correction == 2)startPosition += convertPosition(m_slitWidePos, false, degreeString, absencString);
        OsExtimateDerotation(m_derotStartDate, _positionAngle, _correction, _autoStart, finalSize, effectiveStartPosition);

        moveAbsolute(effectiveStartPosition, stepsString);
        moveAbsolute(effectiveStartPosition, stepsString);
        m_positionSequence.resize(finalSize);
        U9_LOG("Final Size : " << finalSize);

        double starting_ramp = m_device->shinsGetStartingRamp(m_positionSequence, 1000000./m_frequency);

        U9_LOG("starting_ramp : " << starting_ramp);
        moveAbsolute(effectiveStartPosition + starting_ramp, stepsString);   // moveRelative(starting_ramp, stepsString);
        m_positionSequence += effectiveStartPosition/m_microsteps;
        /* store nominal motor start angle into the property (radians).
        * This includes the offsets, for example -60 for the GWS RJM 2017-03-19 */
        const double angDeg = convertPosition(effectiveStartPosition, true, stepsString, degreeString);
        setMotStrtAng(angDeg);
        /* store parallactic Angle in the property in units of degrees (!) from the DD of the IIF,
        * Need the parallactic angle with a KNOWN motor offset, stored by getReady(), to construct the WCS matrix, RJM 2016-12-14 */
        const double pstrt = m_iif->getParallacticAngle();
        setPangStrtAng(pstrt);
        I_LOG("Motor start angle " << angDeg << " deg; Starting parallactic angle : " << pstrt << " deg");

        Nice::Time tDurat = Nice::Time::seconds(finalSize/m_frequency + finalSize/22. + 5); // finalSize/22 = time to load values; 5 confidence
        Nice::Date t0 = Nice::Date::now();
        setTrajFinDate(t0 + tDurat);

        if(m_ControlFlags.bLogDerotationPositions)
        {
            std::vector<double>::iterator itDouble;
            std::stringstream log_msg;
            for(itDouble = m_positionSequence.begin(); itDouble < m_positionSequence.end(); itDouble++)
            {
                log_msg << *itDouble << "\t";
                log_msg << convertPosition(*itDouble*m_microsteps, true, stepsString, absencString) << "\n";
            }
            std::stringstream path;
            path << "/home/shinsmgr/drot_test/start_derot_" << m_derotStartDate.toString(0, true, LOGNAME_FORMAT) << ".txt";
            std::ofstream out(path.str().c_str());
            out << log_msg.str();
            out.close();
        }
        I_LOG("Start profile");

        m_bDerotationRunning = true;
        U7_LOG("Starting External Profile")
        if(gDebug9 == 0 && gShinsProfile)
        {
            unsigned int faultTolerance = m_faultTolerance;
            if(gDrotEnabled == false)faultTolerance = 0;
            m_device->shinsStartExternalProfile(m_derotStartDate, m_positionSequence, m_frequency, m_samplesPerSegment, faultTolerance, deviceCmdAsyncImpl());
        }
        else if(gDebug9 == 1 && gTanProfile)
        {
            m_device->startExternalProfile(m_derotStartDate, m_positionSequence, m_frequency, m_samplesPerSegment, m_faultTolerance, deviceCmdAsyncImpl());
        }
        Nice::Date t1 = Nice::Date::now();
        U7_LOG("External Profile End. Trajectory started at : " << t0 << " ended at : " << t1 << "; Elapsed time : " << (t1 - t0));
        resetPosition();
        setTrajFinDate(t1);
        m_bDerotationRunning = false;
        SaveDerotationLog();
        result = EXIT_SUCCESS;
    }
    catch(...)
    {
        E_LOG(__FUNCTION__ << " ~ Error");
        m_bDerotationRunning = false;
        SaveDerotationLog();
        std::string what = boost::current_exception_diagnostic_information();
        std::string toErase = "std::exception::what:";
        size_t pos = what.find(toErase);
        if(pos != std::string::npos)what.erase(pos, toErase.length());
        what.erase(std::remove(what.begin(), what.end(), '\n'), what.end());
        std::stringstream log_msg;
        log_msg << __FUNCTION__ << " ~ " << what;
        E_LOG(log_msg.str());
        setTrajFinDate(Nice::Date::now());
        usleep(5000000);
//        throw;
    }
    U8_LOG(__FUNCTION__);
    return result;
}


// int Sharknir::DrotServiceWorker::OsStartDerotation(bool _autoStart)
// {
//     U6_LOG(__FUNCTION__);
//     int result = EXIT_FAILURE;
//     if(m_drotMode == DROT_MODE_PUPIL)
//     {
//         if(_autoStart)m_drotMode = DROT_MODE_FIELD_FREE;
//         else m_drotMode = DROT_MODE_FIELD_FIXED;
//         Nice::Date startDate = Nice::Date::now() + Nice::Time::seconds(5);

//         bool autostart = false;

//         if(m_drotMode == DROT_MODE_FIELD_FREE) //Extimate Derotation is run twice to take care of the unknown starting derotation time.
//         {
//             autostart = true;
//             int finalSize = 0;
//             double effectiveStartPosition = 0;
//             m_derotStartDate = startDate.roundToSecPrecision();
//             OsExtimateDerotation(startDate, getAbsoluteEncoderPosition(), absencString, autostart, finalSize, effectiveStartPosition);
//             moveAbsolute(effectiveStartPosition, stepsString);
//         }

//         startDate = Nice::Date::now() + Nice::Time::seconds(20);
//         result = OsStartDerotation(startDate, getAbsoluteEncoderPosition(), absencString, autostart);

//         m_drotMode = DROT_MODE_PUPIL;
//     }
//     else
//     {
//         std::stringstream log_msg;
//         log_msg << "Error : Derotator is already running";
//         E_LOG(log_msg.str());
//         throw(std::runtime_error(log_msg.str()));
//     }

//     U8_LOG(__FUNCTION__);
//     return result;
// }

int Sharknir::DrotServiceWorker::OsWaitDerotation(int seconds)
{
    U6_LOG(__FUNCTION__);
    int counter = 0;
    int status = EXIT_SUCCESS;
    while(m_drotMode != DROT_MODE_PUPIL)
    {
        counter++;
        usleep(100000);
        if(counter/10 >= seconds)
        {
            status = EXIT_FAILURE;
            break;
        }
    }
    U8_LOG(__FUNCTION__);
    return status;
}

/** @brief Get polynomials from the TCS, move to start position, and start the derotator motion.
  * @param _startDate Start time of the derotation.
  * @param _startPosition Start position of the bearing.
  * @param _unit Units of the start position.  */
void Sharknir::DrotServiceWorker::startDerotation(const Nice::Date & _startDate, double _startPosition, std::string _unit)
{
    U6_LOG(__FUNCTION__ << tilde << _startDate << tilde << _startPosition << tilde << _unit);
    // I_LOG("startDerotation ")

    Basda::ServiceWorkerSkeleton::cmdStart("startProfile");

// double Sharknir::DrotServiceWorker::convertPosition(double _position, bool _absolute, std::string _unitFrom, std::string _unitTo)
    int absencStartPosition = (int)convertPosition(_startPosition, true, _unit, absencString);
    double pos_angle = convertPosition(absencStartPosition - m_referencePos, true, absencString, degreeString);

    int result = OsStartDerotation(_startDate, pos_angle, 0, true);

    return;

    pthread_t tid1;
    m_bDerotationRunning = true;
//    pthread_create(&tid1, NULL, at_drot_exec, this);
    try
    {
        // get polynomials from TCS and convert to steps. This ignores _startPosition.
        createTrajectory(_startDate);
        int faultTolerance = m_faultTolerance;
        int frequency = m_frequency;
        int samplesPerSegment = m_samplesPerSegment;

        I_LOG("Derotation requested at  : " << _startDate << " with parameter: ");
        I_LOG("Segment Duration         : " << Nice::Time::seconds(samplesPerSegment/frequency));
        I_LOG("samplesPerSegment        : " << samplesPerSegment);
        I_LOG("faultTolerance           : " << faultTolerance);
        I_LOG("Frequency                : " << frequency);
        I_LOG("Derotation will start at : " << _startDate);
        I_LOG("Local   time   now    is : " << Nice::Date::now());
        I_LOG("Derotation duration      : " << Nice::Time::seconds(m_numberOfPolynomials/m_samplingRate));

        /* move to start position; store motor and parallactic start angle. */
        getReady(_startPosition, _unit);
        double effectiveStartPosition = getPosition(stepsString);

        /* tDurat is an estimate of the actual duration that is left on the trajectory (seconds);
         * the minimum of (i) the duration of the IIF polynomials and (ii) hitting the limits
         * of the travel range of the motor. If the polynomials reach further into
         * the future than what is left in the travel range, we eliminate the trail
         * of the polynomials by using a 'true' a the last argument to keep it
         * in the travel range of the motor. RJM 2017-04-05 */
        I_LOG("start position : " << effectiveStartPosition << " " << _unit);
        Nice::Time tDurat = timeLeft(frequency, effectiveStartPosition, _unit, true); // true -> truncate if necessary

        I_LOG("time left : " << tDurat)

//        usleep(5000000);
        Nice::Date t0 = Nice::Date::now();

        /* store estimated time when the trajectory will halt/finish/stop */
        setTrajFinDate(t0 + tDurat);

        m_derotStartDate = _startDate.roundToSecPrecision();
        if(m_ControlFlags.bLogDerotationPositions)
        {
            std::vector<double>::iterator itDouble;
            std::stringstream log_msg;
            for(itDouble = m_positionSequence.begin(); itDouble < m_positionSequence.end(); itDouble++)
            {
                log_msg << (int)*itDouble << "\t";
                log_msg << convertPosition(*itDouble*m_microsteps, true, stepsString, absencString) << "\n";
            }
            std::stringstream path;
            path << "/home/shinsmgr/drot_test/startDerot_" << m_derotStartDate.toString(0, true, LOGNAME_FORMAT) << ".txt";
            std::ofstream out(path.str().c_str());
            out << log_msg.str();
            out.close();
        }

        I_LOG("Start profile");
        m_device->startExternalProfile(m_derotStartDate, m_positionSequence, frequency, samplesPerSegment, faultTolerance, deviceCmdAsyncImpl());

        m_bDerotationRunning = false;
        SaveDerotationLog();
        I_LOG("Start profile done")
        Nice::Date t1 = Nice::Date::now();
        I_LOG("trajectory started at : " << t0 << "; ends at : " << t1 << "; elapsed time : " << (t1 - t0));

        setTrajFinDate(t1);

        const double lbtDeltaRad = m_ang_rad;
        const double lbtDeltaDeg = m_ang_rad * DR2D;
        const double deltaSteps  = m_ang_steps*m_microsteps;

        I_LOG("Delta angle of the LBT : " << lbtDeltaRad << " rad, (" << lbtDeltaDeg << " deg, " << deltaSteps << " steps)")

        const double currentPositionInSteps = getDeviceEncoderPosition("STEPS");
        const double derotatorDeltaSteps = currentPositionInSteps - m_positionSequence[0]*m_microsteps;
        const double derotatorDeltaRad = derotatorDeltaSteps / m_stepsPerRad * m_direction;
        const double derotatorDeltaDeg = derotatorDeltaSteps / m_stepsPerDegree * m_direction;

        I_LOG("delta angle of the GWS : " << derotatorDeltaRad << " rad, (" << derotatorDeltaDeg << "  deg, " << (int)derotatorDeltaSteps << " steps)")
        I_LOG("delta angle error : " << (lbtDeltaRad - derotatorDeltaRad) << "  rad, (" << (lbtDeltaDeg - derotatorDeltaDeg) << " deg, " << (deltaSteps - derotatorDeltaSteps) << " steps)")
    }
    catch (Sharknir::DrotCommandExecutionFailedException & e)
    {
        m_bDerotationRunning = false;
        SaveDerotationLog();
        I_LOG("Derotation was interrupted: " << e)
    }
    catch (Basda::Dev::MoccaUploadExternalProfileException & e)
    {
        m_bDerotationRunning = false;
        SaveDerotationLog();
        E_LOG(e)
        E_THROW(Basda::Svc::MoccaProfileException, e)
    }
    catch(...)
    {
        m_bDerotationRunning = false;
        SaveDerotationLog();
        std::string what = boost::current_exception_diagnostic_information();
        std::string toErase = "std::exception::what:";
        size_t pos = what.find(toErase);
        if(pos != std::string::npos)what.erase(pos, toErase.length());
        what.erase(std::remove(what.begin(), what.end(), '\n'), what.end());
        std::stringstream log_msg;
        log_msg << __FUNCTION__ << " ~ " << what;
        E_LOG(log_msg.str());
        throw;
    }
}


/** @brief Get polynomials from the TCS and start the derotator motion.
  * The difference to startDerotation() is that this function does not
  *  move the motor to any particular position but starts from where it is.
  *  The function is not overloading startDerotation() because this allows
  *  to have them both exposed to the Basda/ICE interface.
  * @param _startDate Start time of the derotation.
  * @since 2017-06-08  */
void Sharknir::DrotServiceWorker::startDerotationHere(const Nice::Date & _startDate)
{
    U6_LOG(__FUNCTION__ << tilde << _startDate);
    const std::string _unit("STEPS");
    const double _startPosition = m_config->Nice::Joint::getAtNode<double>(Basda::Mocca::Svc::PropertyName::deviceEncoderPosition() + "." + _unit);
    // forward to the specific signature above 
    startDerotation(_startDate, _startPosition, _unit);
}

/** @param _startDate
  * @param _startPositionAbsolute
  * @param _unit  */
void Sharknir::DrotServiceWorker::changeDerotationWithAbsoluteOffset(const Nice::Date & _startDate, double _startPositionAbsolute, std::string _unit)
{
    U6_LOG(__FUNCTION__ << " ~ startDate : " << _startDate << " ~ startPositionAbsolute : " << _startPositionAbsolute << " ~ Unit : " << _unit);
    
    U7_LOG(__FUNCTION__ << " ~ Under Development");

    return;

    createTrajectory(_startDate);

    int faultTolerance = m_faultTolerance;
    int frequency = m_frequency;
    int samplesPerSegment = m_samplesPerSegment;

    /* tDurat is an estimate of the actual duration that is left on the trajectory (seconds);
     * the minimum of (i) the duration of the IIF polynomials and (ii) hitting the limits
     * of the travel range of the motor. If the polynomials reach further into
     * the future than what is left in the travel range, we eliminate the trail
     * of the polynomials by using a 'true' a the last argument to keep it
     * in the travel range of the motor. RJM 2017-04-05 */
    Nice::Time tDurat = timeLeft(frequency, _startPositionAbsolute, _unit, true);

    double startPositionInSteps = coordinateTransformationInverse(_startPositionAbsolute, _unit);
    m_positionSequence += startPositionInSteps;

    I_LOG("derotation change requested at    : " << _startDate << " with parameter: ")
    I_LOG("segmentLength                     : " << Nice::Time::seconds(samplesPerSegment/frequency))
    I_LOG("samplesPerSegment                 : " << samplesPerSegment)
    I_LOG("faultTolerance                    : " << faultTolerance)
    I_LOG("frequency                         : " << frequency)
    I_LOG("new derotation will start at      : " << _startDate)
    I_LOG("at position                       : " << m_positionSequence[0] << " Steps")     //_startPositionAbsolute << " " << _unit)
    I_LOG("Local time now is                 : " << Nice::Date::now())
    I_LOG("new derotation duration should be : " << Nice::Time::seconds((m_numberOfPolynomials) / m_samplingRate))

    /* store estimated time when the trajectory will halt/finish/stop */
    setTrajFinDate(_startDate + tDurat);

    std::vector<double>::iterator itDouble;
    std::stringstream log_msg;
    log_msg << "Size : " << m_positionSequence.size();
    for(itDouble = m_positionSequence.begin(); itDouble < m_positionSequence.end(); itDouble++)
    {
        log_msg << tilde << (int)*itDouble;
    }
    U9_LOG(log_msg.str());

    W_LOG("*********************************************************************")
    W_LOG("******************** Implementation Under Way ***********************")
    W_LOG("*********************************************************************")
    m_device->changeExternalProfile(_startDate, m_positionSequence);
}

/** @brief Change the trajectory at some time in the future by a relative amount.
  * @param _startDate The time in the future (when the old trajectory is still alive)
  * @param _startPositionRelative The kink to the trajectory at the time.
  * @param _unit The units to interpret _startPositionRelative .  */
void Sharknir::DrotServiceWorker::changeDerotationWithRelativeOffset(const Nice::Date & _startDate, double _startPositionRelative, std::string _unit)
{
    U6_LOG(__FUNCTION__ << tilde << _startDate << tilde << _startPositionRelative << tilde << _unit);

    U7_LOG(__FUNCTION__ << " ~ Under Development");

    return;

    int faultTolerance = m_faultTolerance;
    int frequency = m_frequency;
    int samplesPerSegment = m_samplesPerSegment;

    double arrivalPosition = 0;
    int index = 0;

    // calculate the arrival position of the old trajectory at the time when the new trajectory starts
    while(index < (int)m_positionSequence.size() && m_startDate + Nice::Time::microSeconds((Nice::I64)(index * 1.0 / (double)frequency * 1000000.0)) < _startDate)
    {
        arrivalPosition = m_positionSequence[index];
        index++;
    }

    createTrajectory(_startDate);

    double startPositionInSteps = coordinateTranslateInverse(_startPositionRelative, _unit);
    m_positionSequence += arrivalPosition + startPositionInSteps;

    /* tDurat is an estimate of the actual duration that is left on the trajectory (seconds);
     * the minimum of (i) the duration of the IIF polynomials and (ii) hitting the limits
     * of the travel range of the motor. If the polynomials reach further into
     * the future than what is left in the travel range, we eliminate the trail
     * of the polynomials by using a 'true' a the last argument to keep it
     * in the travel range of the motor. RJM 2017-04-05  */
    Nice::Time tDurat = timeLeft(frequency, 0., stepsString, true);

    I_LOG("derotation change requested at " << _startDate << " with parameter: ")
    I_LOG("     segmentLength:" << Nice::Time::seconds(samplesPerSegment/frequency))
    I_LOG("     samplesPerSegment:" << samplesPerSegment)
    I_LOG("     faultTolerance:" << faultTolerance)
    I_LOG("     frequency:" << frequency)
    I_LOG("new derotation will start at                 " << _startDate)
    I_LOG("   with a relative offset of                 " << _startPositionRelative << " " << _unit)
    I_LOG("   absolute start position will be           " << m_positionSequence[0] << " Steps")
    I_LOG("Local time now is                            " << Nice::Date::now())
    I_LOG("Requested derotation duration                " << Nice::Time::seconds((m_numberOfPolynomials) / m_samplingRate))

    /* store estimated time when the trajectory will halt/finish/stop */
    setTrajFinDate(_startDate + tDurat);

    std::vector<double>::iterator itDouble;
    std::stringstream log_msg;
    log_msg << "Size : " << m_positionSequence.size();
    for(itDouble = m_positionSequence.begin(); itDouble < m_positionSequence.end(); itDouble++)
    {
        log_msg << tilde << (int)*itDouble;
    }
    U9_LOG(log_msg.str());

    W_LOG("*********************************************************************")
    W_LOG("******************** Implementation Under Way ***********************")
    W_LOG("*********************************************************************")
    m_device->changeExternalProfile(_startDate, m_positionSequence);
}

/*  * @todo use the corresponding IIFServiceWorker instead  */
std::string Sharknir::DrotServiceWorker::getRA()
{
#if 0
    Basda::WorkerMap & s = Basda::Basdard::workers();
    Basda::WorkerMap::iterator it = s.begin();
    while( it != s.end())
    {
        std::cerr <<__FILE__ << " " << __LINE__ << " " << it->first << std::endl;
        it++;
    }
#endif

#if 0
    int k = m_config->Nice::Joint::getAtNode<int>(Ltcs::Iif::Svc::PropertyName::cfgNumberOfPolynomials());
    std::cerr <<__FILE__ << " " << __LINE__ << " pang " << m_iif << " no " << k << std::endl;
#endif

    std::string ra;
    if(m_iif)
    {
        ra = m_iif->getRA();
    }
    else
    {
        I_LOG("No connection to IIF service. Method disabled.")
    }

    return ra;
}



/** @brief convert input position from a unit to another
  * @param _position : double, position to be converted
  * @param _absolute : bool, true if position is to be considered absolute, false otherwise
  * @param _unitFrom : unit from which to convert
  * @param _unitTo   : unit to which position have to be converted  */
double Sharknir::DrotServiceWorker::convertPosition(double _position, bool _absolute, std::string _unitFrom, std::string _unitTo)
{
//    U6_LOG(__FUNCTION__ << " ~ " << _position << " ~ " << _absolute << " ~ " << _unitFrom << " ~ " << _unitTo);
    if(_absolute)
    {
        if(_unitFrom == _unitTo)
        {
            return _position;
        }
        else
        {
            const double absolutePositionInSteps = coordinateTransformationInverse(_position, _unitFrom);
//            U9_LOG("Returning converted position " << (int)absolutePositionInSteps << " " << "STEPS");
            return coordinateConversionFromSteps(absolutePositionInSteps, _unitTo);
        }
    }
    else
    {
        if(_unitFrom == _unitTo)
        {
            return _position;
        }
        else
        {
            const double relativePositionInSteps = coordinateTranslateInverse(_position, _unitFrom);
            return coordinateTranslate(relativePositionInSteps, _unitTo);
        }
    }
}

/** @brief called automatically whenever the transformation property has changed.  */
void Sharknir::DrotServiceWorker::transformationFactorChanged(const Nice::Any & /*_a*/)
{
    U6_LOG(__FUNCTION__);

    Nice::JointPtrMap children = m_device->config()->node(Basda::Mocca::Dev::PropertyName::transformation())->children();
    std::map<std::string, Nice::JointPtr>::iterator it = children.begin();
    m_transformation.clear();
    while(it != children.end())
    {
        m_transformation[boost::to_upper_copy(it->first)] = (it->second)->get<Nice::Point>();
        it++;
    }
}

/** @brief This method converts the given absolute value from steps
  *        into _unit. Thereby it uses dx to add the coordinate system
  *        offset.
  *
  * The given value (in steps) will be converted in _unit based on
  * the specified coordinate transformation.
  *
  * @param _value the position value (in steps) which has to be converted
  * @param _unit  the target unit
  *
  * @return the absolute position in _unit  */
double Sharknir::DrotServiceWorker::coordinateTransformation0(double _value, std::string _unit)
{
    // Transformation to the default unit (currently steps)
    // _value are STEPS
    U6_LOG(__FUNCTION__ << " ~ " << (int)_value << " ~ " << _unit);
    _unit = boost::to_upper_copy(_unit);

    return 0;
#ifdef UNDEFINED
    double absEncPosNow;
    double transformedPos;
    double biasStep;
    double biasEnc = 0;
    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());
    }
    // if_unit is ABSENC, the converted value will have to be adjusted to
    // take into account the adjustment for fullturn operated in coordinateTransformationInverse
    // called in setPosition, also take care of absolute encoder and stepper motors counts in opposite directions
    if(_unit == "ABSENC")
    {

        if((_value >= 0) && (_value <= m_travelRange))
            _value = m_travelRange - _value;
        else
        {
            if(_value > m_travelRange)
                _value = std::floor(_value/m_travelRange)*m_travelRange - _value;
        }

        if(_value < 0)
            _value = m_travelRange + _value;

        absEncPosNow = getAbsoluteEncoderPosition();
        transformedPos = (absEncPosNow -  m_transformation[_unit].yp) /  m_transformation[_unit].xp;

        if(absEncPosNow == m_travelRangeABSENC)
        {
            biasEnc = m_travelRangeABSENC;
        }
        else
        {
            if(absEncPosNow > m_travelRangeABSENC)
            {
                biasStep = std::floor(transformedPos/m_travelRange) * m_travelRange;
                biasEnc = biasStep * m_transformation[_unit].xp + m_transformation[_unit].yp;
            }
            else
            {
                biasEnc = 0;
            }
        }
    }

    // return (absEncPosNow-biasEnc) + (_value * m_transformation[_unit].xp + m_transformation[_unit].yp);
    U8_LOG(__FUNCTION__ << " ~ " << biasEnc << " ~ " << (_value * m_transformation[_unit].xp + m_transformation[_unit].yp));
    return biasEnc + (_value * m_transformation[_unit].xp + m_transformation[_unit].yp);
#endif
    // return _value * m_transformation[_unit].xp + m_transformation[_unit].yp;
}


double Sharknir::DrotServiceWorker::coordinateConversionFromSteps(double _value, std::string _unit)
{
    // Must be the same as coordinateTransformation except for absenc check
    double result = 0;
    double half_step = 0.5/m_absencX;
    double absenc = (_value + half_step)*m_absencX + m_absencY;
    int iabsenc = absenc;
    if(_unit == absencString)
    {
        if(absenc == m_absencY + half_step*m_absencX)iabsenc = 0;
        result = iabsenc;
    }
    else
    {
        double steps = (iabsenc - m_absencY)/m_absencX;
        if(_unit == stepsString)
        {
            result = steps;
        }
        else // DEGREE
        {
            double retDegree = steps*m_transformation[degreeString].xp + m_transformation[degreeString].yp;
            result = retDegree;
        }
    }
    int int_result = result;
    return result;
}
double Sharknir::DrotServiceWorker::coordinateTransformation(double _value, std::string _unit)  // Transformation from STEPS (or maybe CFG.POSITION.UNIT)
{
    //Called by the framework. _value are steps returned by this function in a previous call with "_unit" = STEPS

    double result = 0;
    double half_step = 0.5/m_absencX; // m_absencX negative in shark
    // quando m_absencX è negativo e a m_absencY = 65536
    double absenc = (_value + half_step)*m_absencX + m_absencY;
    double absenc2 = getAbsoluteEncoderPosition();    // Same as MoCon getAEncActualPosition (1 171 5 25 1 or 1 171 5)

    int iabsenc = absenc;
    if(m_ControlFlags.bTraceDebug){U6_LOG(__FUNCTION__ << tilde << (int)_value << tilde << _unit << tilde << (int)((absenc2 - m_absencY)/m_absencX));}
//    U9_LOG((int)_value << tilde << _value - (int)_value << tilde << absenc << tilde << absenc2 << tilde << iabsenc);

//    U9_LOG(absenc << tilde << iabsenc);

    if(_unit == absencString)
    {
        int diff = (int)absenc - absenc2;
        if(diff > 1 || diff < - 1)
        {
    //        double retPos = (absenc2 - m_absencY)/m_absencX;
    //        resetPosition(retPos);
            if(m_ControlFlags.bLogMisalignment)
            {
                U9_LOG("Misalignment : " << diff << " ~ " << (int)_value << " ~ " << (int)((absenc2 - m_absencY)/m_absencX));
        //        (_value + half_step)*m_absencX + m_absencY
            }
        }
//        U9_LOG(__FUNCTION__ << " ~ Absenc are : " << absenc);
        if(absenc == m_absencY + half_step*m_absencX)iabsenc = 0;
        result = iabsenc;
    }
    else
    {
        double steps = (absenc - m_absencY)/m_absencX;
        if(_unit == stepsString)
        {
//            U9_LOG(__FUNCTION__ << " ~ Steps to Steps are : " << (int)_value << "/" << (int)(steps + 0.001));
            result = steps;
        }
        else // DEGREE
        {
            double retDegree = steps*m_transformation[degreeString].xp + m_transformation[degreeString].yp;
//            U9_LOG(__FUNCTION__ << " ~ Degrees are : " << retDegree);
            result = retDegree;
        }
    }
    int int_result = result;
    // U6_LOG(__FUNCTION__ << tilde << (int)_value << tilde << _unit << tilde << int_result << "." << std::setfill('0') << std::setw(4) << (int)(10000*(result - int_result))  << tilde << absenc << tilde << absenc2);
    return result;
}


/** @brief Converts the given absolute value from _unit into steps.
  * Thereby it uses dx to subtract the coordinate system offset.
  * The given value will be converted in steps based on _unit
  * and the specified coordinate transformation.
  * @param _value the position value which has to be converted
  * @param _unit  the current unit of _value
  * @param the absolute position in steps  */
double Sharknir::DrotServiceWorker::coordinateTransformationInverse(double _value, std::string _unit) //Transformation to STEPS (or maybe CFG.POSITION.UNIT)
{// Position is converted in CFG.POSITION.UNIT (currently STEPS)
//    U6_LOG(__FUNCTION__ << " ~ " << (int)_value << " ~ " << _unit);
    double transformedPos;

    if(m_transformation.find(_unit) == m_transformation.end())
    {
        E_THROW(Sharknir::DrotUnitNotKnownException, "Unit " << _unit << " is not known.");
    }

    transformedPos = (_value - m_transformation[_unit].yp) / m_transformation[_unit].xp;

    return transformedPos;
}


/** @brief Convert the given value from steps into _unit. Thereby it ignores the coordinate system offset dx.
  * @param _value the relative position value which has to be converted
  * @param _unit  the unit in which the value has to be converted
  * @return the relative position value in _unit  */
double Sharknir::DrotServiceWorker::coordinateTranslate(double _value, std::string _unit)
{
    U6_LOG(__FUNCTION__ << " ~ " << _value << " ~ " << _unit);
    Nice::MapStringPoint::iterator unitNamePrecIt = m_transformation.find(_unit);
    if(unitNamePrecIt == m_transformation.end())
    {
        E_THROW(Sharknir::DrotUnitNotKnownException, "Unit " << _unit << " is not known.");
    }
    return _value * unitNamePrecIt->second.xp;
}

/** @brief Convert the given value from _unit into steps. Thereby it ignores the coordinate system offset dx.
  * @param _value the relative position value which has to be converted
  * @param _unit  the current unit of _value
  * @return the relative position in steps */
double Sharknir::DrotServiceWorker::coordinateTranslateInverse(double _value, std::string _unit)
{
    U6_LOG(__FUNCTION__ << " ~ " << _value << " ~ " << _unit);
    Nice::MapStringPoint::iterator unitNamePrecIt = m_transformation.find(_unit);
    if(unitNamePrecIt == m_transformation.end())
    {
        E_THROW(Sharknir::DrotUnitNotKnownException, "Unit " << _unit << " is not known.");
    }
    return _value / unitNamePrecIt->second.xp;
}


/** @brief Obtain focal plane coordinates for the stars previously sent with the preset.
 *  The ltcs object has stored the list of stars in its Ltcs::IifServiceWorker::scat object;
 *  so the list does not need to be pushed through additional parameters.
 *  @param backupCat The catalogue to be used ifthe iif server does not have one.
 *  This may for example happen ifthe TO preset the telescope, not the
 *  iif server with a star catalogue, or ifwe restarted the iif server, did not
 *  yet preset the telescope, and it therefore has lost its knowledge of its own older preset.
 *  @return The sequence of points, both coordinates, x and y, in units of mm.
 *  @since 2017-06-26
 *  @author R. J. Mathar */
// Nice::SeqPoint Sharknir::DrotServiceWorker::getKFPCoordinates(const lbto::SeqPos2 backupCat)
// Nice::SeqPoint Sharknir::DrotServiceWorker::getKFPCoordinates(const Lics::LicsStarcat backupCat)
Nice::SeqPoint Sharknir::DrotServiceWorker::getKFPCoordinates()
{
    U6_LOG(__FUNCTION__);
    Nice::SeqPoint kfp;
    if(m_iif == 0)
    {
        E_LOG("No telescope proxy initialized");
        return kfp;
    }
    else
    {
        /* the side (sx or dx) is deduced from the name of this service
         */
        // const Ltcs::IifSide sdx( name()) ;
        const Ltcs::IifSide sdx(Ltcs::SIDE_LEFT);
#if 0
        const lbto::SeqPos2 iifKnowncat = m_iif->getPresetStars();
        if(iifKnowncat.empty())
        {
            /* use the backup stars for the query */
            m_iif->scat = backupCat;
            m_iif->kfphash[0].clear();
            m_iif->kfphash[1].clear();
        }
#endif

        return m_iif->getKFPCoordinates(sdx.toString());
    }
}

/*********************************
 * @brief Obtain the stars previously sent with the preset.
 *  The ltcs object has stored the list of stars in its Ltcs::IifServiceWorker::scat object;
 *  so the list does not need to be pushed through additional parameters.
 * @return The star catalogue
 * @since 2017-06-28
 * @author R. J. Mathar */
lbto::SeqPos2 Sharknir::DrotServiceWorker::getPresetStars()
{
    U6_LOG(__FUNCTION__);
    lbto::SeqPos2 scat;
    if(m_iif == 0)
    {
        E_LOG("No telescope proxy initialized");
        return scat;
    }
    else
        return m_iif->getPresetStars();
}


double Sharknir::DrotServiceWorker::getAbsoluteEncoderPosition()
{
    // Same as MoCon getAEncActualPosition (1 171 5 25 1 or 1 171 5)
    double result = Basda::MoccaNServiceWorkerCore<double, double, Nice::Point, DrotServiceWorkerSkeleton>::getAbsoluteEncoderPosition();
    if(m_ControlFlags.bTraceDebug){U6_LOG(__FUNCTION__ << tilde << result << tilde << m_absencY - result);}
    if(result == 0) // Patch : 0 is currently never reached
    {
//        U9_LOG("result : " << (int)result << "; m_SimulatedAbsoluteEncoder = " << m_SimulatedAbsoluteEncoder);
        result = m_SimulatedAbsoluteEncoder;
    }
    else
    {
//        U9_LOG("result : " << (int)result << "; m_SimulatedAbsoluteEncoder = " << m_SimulatedAbsoluteEncoder);
    }
    if(m_bDerotationRunning && m_ControlFlags.bLogDerotationPositions)
    {
        m_positionTimes.push_back(Nice::Date::now());
        m_derotPositions.push_back(result);
    }
    return result;
}


void Sharknir::DrotServiceWorker::resetPosition()
{
    if(m_ControlFlags.bTraceDebug){ U9_LOG(__FUNCTION__);};
    double dev_enc_pos = Basda::MoccaNServiceWorkerCore<double, double, Nice::Point, DrotServiceWorkerSkeleton>::getDeviceEncoderPosition(stepsString);
    double retPos = getAbsoluteEncoderPosition(); // Same as MoCon getAEncActualPosition (1 171 5 25 1 or 1 171 5)
    double to_be_set = (retPos - m_absencY)/m_absencX;
    if(retPos != dev_enc_pos)
    {
        U7_LOG("Resetting position : " << (int)to_be_set << tilde << (int)dev_enc_pos << tilde << (int)(to_be_set - dev_enc_pos));
        m_device->setPosition(to_be_set);
        getDeviceEncoderPosition(stepsString);
    }
    else
    {
        U7_LOG((int)dev_enc_pos << tilde << (int)retPos);
    }
}


void Sharknir::DrotServiceWorker::resetPosition(double to_be_set)
{
    if(m_ControlFlags.bTraceDebug){ U9_LOG(__FUNCTION__);};
    m_device->setPosition(to_be_set);
}

double Sharknir::DrotServiceWorker::getPosition0(std::string _unit)
{
    return getDeviceEncoderPosition(_unit);
}

double Sharknir::DrotServiceWorker::getPosition(std::string _unit)
{
//    U6_LOG(__FUNCTION__ << " ~ " << _unit);
    if(m_ControlFlags.bTraceDebug){ U6_LOG(__FUNCTION__ << " ~ " << _unit);};
    double retPos;
    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());
    }

    // Basda::ServiceWorker::config()->Nice::Joint::getAtNode<SvcType>(Basda::Mocca::Svc::PropertyName::deviceEncoderPosition() + "." + _unit);

    //nwdouble config_result = Basda::ServiceWorker::config()->getAtNode<double>(Basda::Mocca::Svc::PropertyName::deviceEncoderPosition() + "." + _unit);
    //U9_LOG("Config Result = " << config_result);
    //failconfig_result = m_device->config()->getAtNode<double>(Basda::Mocca::Svc::PropertyName::deviceEncoderPosition() + "." + _unit);
    //U9_LOG("Config Result = " << config_result);


    double currentPositionInSteps = getDeviceEncoderPosition(_unit); // Same as MoCon getCurrentPosition (1 166 5) this causes a triple call to coordinateTransformation and updates MoCon
//    U9_LOG("getDeviceEncoderPosition returned : " << (int)currentPositionInSteps);
    // The next call is necessary to update internally to TaN the position and will cause a triple call to "coordinateTransformation"
    double dev_enc_pos = Basda::MoccaNServiceWorkerCore<double, double, Nice::Point, DrotServiceWorkerSkeleton>::getDeviceEncoderPosition(_unit);
//    U9_LOG("getDeviceEncoderPosition returned : " << (int)dev_enc_pos);
    currentPositionInSteps = getDeviceEncoderPosition(_unit);
//    U9_LOG("getDeviceEncoderPosition returned : " << (int)currentPositionInSteps);

    retPos = getAbsoluteEncoderPosition();    // Same as MoCon getAEncActualPosition (1 171 5 25 1 or 1 171 5)
    // convertPosition is good but slower
    // return convertPosition(retPos, true, "ABSENC", _unit);

    if(_unit != absencString)
    {
        // Converting retPos to STEPS which is the standard unit
        if(m_absencLead)
            retPos = (retPos - m_absencY)/m_absencX;
        else
            retPos = dev_enc_pos;
        if(_unit == stepsString)// STEPS is currently default unit
        {
            {
            //    double posSteps = Basda::MoccaNServiceWorkerCore<double, double, Nice::Point, DrotServiceWorkerSkeleton>::getDeviceEncoderPosition("STEPS");
            //    double currentPositionInSteps = m_config->Nice::Joint::getAtNode<double>(Basda::Mocca::Svc::PropertyName::deviceEncoderPosition() + "." + "STEPS");
                double currentPositionInSteps = getDeviceEncoderPosition("STEPS");
//                U6_LOG("current position in steps = " << (int)dev_enc_pos << tilde << (int)currentPositionInSteps << tilde << (int)retPos);
            }
            if(m_ControlFlags.bTraceDebug){U9_LOG(__FUNCTION__ << " ~ Steps are : " << (int)retPos);}
            return retPos;
        }
        else // DEGREE or other
        {
            unitNamePrecIt = m_transformation.find(_unit);
            retPos = retPos*unitNamePrecIt->second.xp + unitNamePrecIt->second.yp;
//            U9_LOG(__FUNCTION__ << " ~ " << _unit << " are : " << retPos);
        }
    }
    if(m_ControlFlags.bTraceDebug){U8_LOG(__FUNCTION__ << " ~ " << _unit << " = " << (int)retPos);}
    return retPos;
}

/*********************************
 * @brief Sleep until the target is reported as reached by the IIF.
 * The function polls the R_IIFOnSource or S_IIFOnSource keyword of the IIF
 * associted with the side LINC has registered with. If these values are
 * 1, the function returns.
 * @param tout The timeout after which polling the DD is abandoned.
 *   Created in the style of Nice::Time tout = Nice::Time::seconds(5*60) by the caller.
 * @return true ifthe source is acquired, false ifnot.
 * @since 2017-06-29
 * @author R. J. Mathar */
bool Sharknir::DrotServiceWorker::isSlewing()
{
    U6_LOG(__FUNCTION__);
    if(m_iif == 0)
    {
        E_LOG("No telescope proxy initialized");
        return false;
    }
    else
        return m_iif->isSlewing();
}

/*********************************
 * @brief Obtain the side (left, right, both) that is assigend to LINC.
 *  Reads the data dictionary and searches for the LINC instrument.
 *  This is not just the configured side of the LN IIF configuration but
 *  the actual online information as set by the telescope operator.
 * @return The LBT side indicator. "left", "right", "both" or "none".
 * @since 2017-12-06
 * @author R. J. Mathar */
std::string Sharknir::DrotServiceWorker::getSHARKNIRstation()
{
    U6_LOG(__FUNCTION__);
    if(m_iif == 0)
    {
        E_LOG("No telescope proxy initialized");
        return "none";
    }
    else
        return m_iif->getLINCstation();
}


std::string Sharknir::DrotServiceWorker::getMode()
{
    std::string result = m_bDerotationRunning ? "ON" : "OFF";
    return result;
}


/*********************************
 * @brief Ask the telescope for the current parallactic angle.
 *  This is a convenience function for all cases where the motor and
 *  the parallactic angle are needed at the same time (for image rotation estimators).
 * @return parallactic angle in degrees.
 * @since 2017-12-06
 * @author R. J. Mathar */
double Sharknir::DrotServiceWorker::getParallacticAngle()
{
    U6_LOG(__FUNCTION__);
    if(m_iif == 0)
    {
        E_LOG("No telescope proxy initialized");
        return false;
    }
    else
        return m_iif->getParallacticAngle();
}


// virtual std::string checkForNamedPosition()
// {

// }


/*!******************************************
 * @brief Compose the unit transformation.
 * @return The transformation with scale factor of unity and zero offset. */
template<> Nice::Point Basda::MoccaNServiceWorkerCore<double, double, Nice::Point, Sharknir::DrotServiceWorkerSkeleton>::getDefaultTransformation()
{
    return Nice::Point(1, 0);
}


// void Sharknir::DrotServiceWorker::callback_drot_move_absolute(const Nice::Completion& _c, boost::function<bool(void)>& _f)
// {
//     switch (_c.state())
//     {
//     case Nice::OK:
//         W_LOG("call _f: delta correction");
//         _f();
//         break;
//     case Nice::EXEC:
//         break;
//     }
// }
