/* $Header: https://svn.mpia.de/gulli/TwiceAsNice/branch/unstable/TwiceAsNice/basda-mocca/src/device/pi/BasdaMoccaPiNDevice.cc 14704 2017-09-19 15:13:35Z briegel $
*/

#include <BasdaMoccaPiNDevice.h>
#include <Basda/DeviceFactory.h>
//#include <Nice/Property/Operators.h>
#include <boost/algorithm/string.hpp>
#include <Nice/Time/Timer.h>
#include <math.h>
#include <Nice/Property/Joint.h>

DEVICE_CREATE(Basda::MoccaPiNDevice);

#undef LOG_NAME
#define LOG_NAME m_name

Basda::MoccaPiNDevice::MoccaPiNDevice(const Nice::JointPtr &_config)
        : Basda::MoccaNDevice(_config)
{
    Nice::Guard guard(m_mutex);
    m_config->getWithDefault<std::string>(cfg_connectstring(), "unknown");
    m_config->setAtNode<bool>("IS_REACHABLE", false);
    m_isReachable = false;
    m_tcpConnectionTimeout = Nice::Time::seconds(3600);
    Nice::Application::distribute(m_config->node(cfg_root()));
}

Basda::MoccaPiNDevice::~MoccaPiNDevice()
{

}

void Basda::MoccaPiNDevice::Check4Error(int _rc)
{
//    for(int i = 0; i < m_chat.size() - 2; i++)
//    {
//      U9_LOG(m_chat[i]);
//    }
    if (_rc >= 2)
    {
        long iError = atoi(m_chat[_rc - 2].c_str());
        if (iError != 0)
        {
            E_LOG("Pi: #" << iError << " - Unknown");
        }
//    PI_TranslateError(iError, m_szErrorMesage, 1024);
    }
    else
    {
        E_THROW(Basda::Pi::TimeOutException, "Pi: Connection timeout");
    }
}

void Basda::MoccaPiNDevice::read(const char * _cmd, Nice::SeqString & _val)
{
    Nice::Guard guard(m_mutex);

    std::string c = _cmd;
    Check4Error(m_chat.chat("Physik*", (c + "?\nERR?\nIDN?").c_str()));
    _val.clear();
    for (unsigned int i = 0; i < m_chat.size() - 2; i++)
    {
        std::string s = m_chat[i];
        boost::trim(s);
        _val.push_back(s);
    }
}

template<typename Type>
void Basda::MoccaPiNDevice::read(const char * _cmd, std::vector<Type> & _val)
{
    Nice::Guard guard(m_mutex);

    std::string c = _cmd;
    Check4Error(m_chat.chat("Physik*", (c + "?\nERR?\nIDN?").c_str()));
    _val.clear();
    for (unsigned int i = 0; i < m_chat.size() - 2; i++)
    {
        std::stringstream is(m_chat[i]);
        Type v;
        size_t n;
        char ch;
        is >> n >> ch >> v;
        _val.push_back(v);
    }
}

template<typename Type>
void Basda::MoccaPiNDevice::exec(const char * _cmd, const Type & _val)
{
    Nice::Guard guard(m_mutex);

    std::stringstream os;
    os << _cmd;
    for (size_t i = 0; i < m_axes.size(); i++)
    {
        os << " " << m_axes[i] << " " << _val;
    }
    os << "\nERR?\nIDN?";
    Check4Error(m_chat.chat("Physik*", os.str().c_str()));
}

template<typename Type>
void Basda::MoccaPiNDevice::exec(const char * _cmd,
                                 const std::vector<Type> & _val)
{
    Nice::Guard guard(m_mutex);

    std::stringstream os;
    os << _cmd;
    for (size_t i = 0; i < _val.size(); i++)
    {
        os << " " << m_axes[i] << " " << _val[i];
    }
    os << "\nERR?\nIDN?";
    Check4Error(m_chat.chat("Physik*", os.str().c_str()));
}

bool Basda::MoccaPiNDevice::waitOnTarget(double maxdist,
                                         Basda::DeviceAsync & _asynch)
{
    return true;
    m_timer.start();
    m_time = Nice::Time::seconds(fabs(maxdist) * 0.12 + 0.2);

    Nice::SeqDouble seqDbl1;
    Nice::BitSet moving;
    while (isMoving(moving))
    {
//      read(pi_position(), seqDbl1);
//      std::cout << "pos: " << seqDbl1 << std::endl;

        int percent = (int)getProgress().percent();
        if (percent < 100)
        {
//          U5_LOG(percent);
            _asynch.asyncStatus("Moving", percent);
        }
        else
        {
            _asynch.asyncStatus("Moving", -1);
        }
    }
//    U6_LOG(timer.now());
    return true;
}

Nice::Completion Basda::MoccaPiNDevice::getProgress()
{
    Nice::Completion c;
    c.setPercent(Nice::Int((100.0 * m_timer.now().toSecondsDouble() / m_time.toSecondsDouble())));
    return c;
}


void Basda::MoccaPiNDevice::checkPowerdOnFor60Sec()
{
     try
     {
        std::string con = config()->getAtNode<std::string>(cfg_connectstring());
        con[con.size()-1] = '1';
//        U9_LOG(con << " " << m_connectionTimer.isValid() << " " << m_connectionTimer.elapsed());

        m_chat.open(con.c_str());
        
     }
     
     catch ( ::Nice::Chat::TimeOutException & )
     {
        m_connectionTimer = Nice::Timer();
//        U9_LOG("offline " << m_connectionTimer.isValid());
        E_THROW(::Nice::Chat::TimeOutException, "Device switched off");
     }
     
     catch ( const ::Nice::Chat::ConnectionRefusedException & )
     {
//        U9_LOG("online " << m_connectionTimer.elapsed() << " " << m_connectionTimer.elapsed() <<  " " << m_connectionTimer.now().toSeconds());
        if(!m_connectionTimer.isValid())
        {
            m_connectionTimer.start( Nice::Time::seconds(60) );
            E_THROW(::Nice::Chat::ConnectionRefusedException, "Wait for device boot " << m_connectionTimer.now().toSeconds() << " seconds");
        }
        if(!m_connectionTimer.elapsed())
        {
            E_THROW(::Nice::Chat::ConnectionRefusedException, "Wait for device boot " << m_connectionTimer.now().toSeconds() << " seconds");
        }
     }
}



void Basda::MoccaPiNDevice::open()
{
    Basda::MoccaPiNDevice::close();
//    checkPowerdOnFor60Sec();
    m_chat.open(config()->getAtNode<std::string>(cfg_connectstring()).c_str());
    m_chat.setLineEndSend("\n");
    m_chat.setTimeout(Nice::Time::seconds(2.0));
    version();
    setIsReachable(true);
}

void Basda::MoccaPiNDevice::close()
{
    m_chat.close();
    setIsReachable(false);
}

void Basda::MoccaPiNDevice::reset(DeviceAsync &)
{
}

void Basda::MoccaPiNDevice::version()
{
    m_chat.chat("Physik*", "ERR?\nIDN?");
    U9_LOG(m_chat[0]);
}

void Basda::MoccaPiNDevice::setIsReachable(const bool state)
{
    Nice::Guard guard(m_mutex);
    m_config->setAtNode<bool>("IS_REACHABLE", state);
    m_isReachable = state;
}

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

void Basda::MoccaPiNDevice::init(DeviceAsync &/*_asynch*/)
{
    read(pi_listOfAxisIds(), m_axes);
    U9_LOG("Axes: #" << m_axes.size());

    read(pi_position(), m_lastPosition);
//    std::cout << "pos: " << m_lastPosition << std::endl;

    read(pi_negLimit(), m_minPosition);
    read(pi_posLimit(), m_maxPosition);
    for (size_t i = 0; i < m_axes.size(); i++)
    {
         U9_LOG(" minPos/maxPos Ax# " << m_axes[i] << ":"
                  << m_minPosition[i] << "<" << m_maxPosition[i]);
    }

    m_homePosition = Nice::NPoint(m_axes.size(), 0.0);

    read(pi_velocity(), m_velocity);
    U9_LOG("Velocity: " << m_velocity);
}

void Basda::MoccaPiNDevice::deinit(DeviceAsync &)
{
}

void Basda::MoccaPiNDevice::activate(DeviceAsync &/*_asynch*/)
{
    exec(pi_servoState(), 1);
}

void Basda::MoccaPiNDevice::deactivate(DeviceAsync &/*_asynch*/)
{
    exec(pi_servoState(), 0);
}

void Basda::MoccaPiNDevice::chat(std::string /*_commandID*/,
                                 std::string /*_cardNo*/,
                                 std::string /*_moduleNo*/,
                                 std::string /*_selectID*/,
                                 std::string /*_parameter*/,
                                 std::string /*_numberOfLines*/,
                                 Basda::DeviceAsync & /*_async*/)
{
    W_LOG("Depreciated method!");
    D_LOG("Not supported");
}

std::string Basda::MoccaPiNDevice::dontParse(Basda::DeviceAsync & /*_async*/)
{
    W_LOG("Depreciated method! dontParse");
    D_LOG("Not supported");
    return "";
}

std::string Basda::MoccaPiNDevice::receiveDataRaw(std::string /*_commandID*/,
                                                  std::string /*_cardNo*/,
                                                  std::string /*_moduleNo*/,
                                                  std::string /*_numberOfLines*/,
                                                  Basda::DeviceAsync & /*_async*/)
{
    W_LOG("Depreciated method! receiveData");
    D_LOG("Not supported");
    return "";
}

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

void Basda::MoccaPiNDevice::startMoveSync()
{
    C_LOG("Not implemented yet!")
}

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

void Basda::MoccaPiNDevice::moveToHome(Basda::DeviceAsync &_asynch)
{
    moveToHomeStart(_asynch);
    moveToHomeWait(_asynch);
}

void Basda::MoccaPiNDevice::moveToHomeStart(Basda::DeviceAsync &/*_asynch*/)
{
    exec(pi_moveAbs(), 0.0);
}

void Basda::MoccaPiNDevice::moveToHomeWait(Basda::DeviceAsync &_asynch)
{
    waitOnTarget(m_lastPosition.absmax(), _asynch);
}

void Basda::MoccaPiNDevice::moveToHomeSingle(int /*_motor*/, Basda::DeviceAsync &_asynch)
{
    moveToHome(_asynch);
}

void Basda::MoccaPiNDevice::moveToHomeSingleStart(int /*_motor*/, Basda::DeviceAsync &_asynch)
{
    moveToHomeStart(_asynch);
}

void Basda::MoccaPiNDevice::moveToHomeSingleWait(Basda::DeviceAsync &_asynch)
{
    moveToHomeWait(_asynch);
}

void Basda::MoccaPiNDevice::moveToPositiveLimit(Basda::DeviceAsync &_asynch)
{
    moveToPositiveLimitStart(_asynch);
    moveToPositiveLimitWait(_asynch);
}

void Basda::MoccaPiNDevice::moveToPositiveLimitStart(Basda::DeviceAsync &/*_asynch*/)
{
    exec(pi_moveAbs(), (Nice::SeqDouble) m_maxPosition);
}

void Basda::MoccaPiNDevice::moveToPositiveLimitWait(Basda::DeviceAsync &_asynch)
{
    waitOnTarget(m_lastPosition.absmax(), _asynch);
}

void Basda::MoccaPiNDevice::moveToNegativeLimit(Basda::DeviceAsync &_asynch)
{
    moveToNegativeLimitStart(_asynch);
    moveToNegativeLimitWait(_asynch);
}

void Basda::MoccaPiNDevice::moveToNegativeLimitStart(Basda::DeviceAsync &/*_asynch*/)
{
    exec(pi_moveAbs(), (Nice::SeqDouble) m_minPosition);
}

void Basda::MoccaPiNDevice::moveToNegativeLimitWait(Basda::DeviceAsync &_asynch)
{
    waitOnTarget(m_lastPosition.absmax(), _asynch);
}

void Basda::MoccaPiNDevice::moveToLimit(const Nice::NPoint &_limit,
                                        Basda::DeviceAsync &_asynch)
{
    moveToLimitStart(_limit, _asynch);
    moveToLimitWait(_asynch);
}

void Basda::MoccaPiNDevice::moveToLimitStart(const Nice::NPoint &_limit,
                                        Basda::DeviceAsync &/*_asynch*/)
{
    Nice::SeqDouble toLimits = m_lastPosition;
    for (size_t i = 0; i < m_axes.size(); i++)
    {
        if (_limit[i] < 0.001)
        {
            toLimits[i] = m_minPosition[i];
        }
        else if (_limit[i] > 0.001)
        {
            toLimits[i] = m_maxPosition[i];
        }
    }

    m_distance = (m_lastPosition - toLimits).absmax();
    exec(pi_moveAbs(), m_distance);
}

void Basda::MoccaPiNDevice::moveToLimitWait(Basda::DeviceAsync &_asynch)
{
    waitOnTarget(m_distance, _asynch);
}

void Basda::MoccaPiNDevice::moveAbsolute(const Nice::NPoint &_absolutePositions,
                                         Basda::DeviceAsync &_asynch)
{
    moveAbsoluteStart(_absolutePositions, _asynch);
    moveAbsoluteWait(_asynch);
}

void Basda::MoccaPiNDevice::moveAbsoluteStart(const Nice::NPoint &_absolutePositions,
                                         Basda::DeviceAsync &/*_asynch*/)
{
    exec(pi_moveAbs(), (Nice::SeqDouble) _absolutePositions);
    m_distance = (m_lastPosition - _absolutePositions).absmax();
}

void Basda::MoccaPiNDevice::moveAbsoluteWait(Basda::DeviceAsync &_asynch)
{
    waitOnTarget(m_distance, _asynch);
}

void Basda::MoccaPiNDevice::moveRelative(const Nice::NPoint & _relativePositions,
                                         Basda::DeviceAsync & _asynch)
{
    moveRelativeStart(_relativePositions, _asynch);
    moveRelativeWait(_asynch);
}

void Basda::MoccaPiNDevice::moveRelativeStart(const Nice::NPoint & _relativePositions,
                                         Basda::DeviceAsync & /*_asynch*/)
{
    exec(pi_moveRel(), (Nice::SeqDouble) _relativePositions);
    m_distance = _relativePositions.absmax();
}

void Basda::MoccaPiNDevice::moveRelativeWait(Basda::DeviceAsync & _asynch)
{
    waitOnTarget(m_distance, _asynch);
}

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

void Basda::MoccaPiNDevice::startExternalProfile(const Nice::Date &/*_startDate*/,
                                                    const Nice::NPoint &/*_positionSequence*/,
                                                    unsigned int /*_frequency*/,
                                                    unsigned int /*_samplesPerSegment*/,
                                                    unsigned int /*_faultTolerance*/,
                                                    Basda::DeviceAsync &/*_async*/)
{
    C_LOG("Not implemented yet");
}

void Basda::MoccaPiNDevice::shinsStartExternalProfile(const Nice::Date &/*_startDate*/,
                                                    const Nice::NPoint &/*_positionSequence*/,
                                                    unsigned int /*_frequency*/,
                                                    unsigned int /*_samplesPerSegment*/,
                                                    unsigned int /*_faultTolerance*/,
                                                    Basda::DeviceAsync &/*_async*/)
{
    C_LOG("Not implemented yet");
}
void Basda::MoccaPiNDevice::changeExternalProfile(const Nice::Date &/*_date*/,
                                                     const Nice::NPoint &/*_positionSequence*/)
{
    C_LOG("Not implemted yet")
}

void Basda::MoccaPiNDevice::prepareExternalProfile(const Nice::SeqSeqInt &/*_segmentsX*/,
                                const Nice::SeqSeqInt &/*_segmentsY*/,
                                Basda::DeviceAsync &/*_async*/)
{
    C_LOG("Not implemted yet")
}

void Basda::MoccaPiNDevice::startExternalProfile(Basda::DeviceAsync &/*_async*/)
{
    C_LOG("Not implemted yet")
}

void Basda::MoccaPiNDevice::shinsStartExternalProfile(Basda::DeviceAsync &/*_async*/)
{
    C_LOG("Not implemted yet")
}
#if 0
void Basda::MoccaPiNDevice::setExternalProfile(const Nice::SeqNPoint & _positionSequence,
                                               double _timeResolution,
                                               unsigned int _samplesPerSegment,
                                               unsigned int _faultTolerance,
                                               Basda::DeviceAsync &_asynch)
{
    E_THROW(Pi::NotImplementedException, "not implemented");
}

void Basda::MoccaPiNDevice::startExternalProfile(std::vector<Nice::Date> _date,
                                                 Basda::DeviceAsync &_asynch)
{
    E_THROW(Pi::NotImplementedException, "not implemented");
}

void Basda::MoccaPiNDevice::pauseExternalProfile()
{
    D_LOG("Not implemented yet.")
}

void Basda::MoccaPiNDevice::stopExternalProfile()
{
    D_LOG("Not implemented yet.")
}
#endif /* 0 */

void Basda::MoccaPiNDevice::completion(std::string /*_cmd*/,
                                       const Nice::NPoint &/*_targetPositions*/,
                                       Basda::DeviceAsync &/*_asynch*/)
{
    E_THROW(Pi::NotImplementedException, "not implemented");
}

void Basda::MoccaPiNDevice::abort()
{
    stop();
}

void Basda::MoccaPiNDevice::stop()
{
    Nice::BitSet inMotion;
    if (isMoving(inMotion)) exec(pi_stopMovement(), Nice::SeqInt());
}

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

Nice::NPoint Basda::MoccaPiNDevice::getDeviceEncoderPosition()
{
    read(pi_position(), m_lastPosition);

    m_lastPosition = (m_lastPosition * 1000.0).round() / 1000.0;

    return m_lastPosition;
}

Nice::NPoint Basda::MoccaPiNDevice::getAbsoluteEncoderPosition()
{
    return getPosition();
}

Nice::NPoint Basda::MoccaPiNDevice::getIncrementalEncoderPosition()
{
    return getPosition();
}

void Basda::MoccaPiNDevice::setPosition(const Nice::NPoint &/*_position*/)
{
    E_THROW(Pi::NotImplementedException, "not implemented");
}

Nice::NPoint Basda::MoccaPiNDevice::getVelocity()
{
//    Nice::NPoint  velocity(m_axes.size(), 0.0);
//    E_THROW(Pi::NotImplementedException, "not implemented");
    return m_velocity;
}

Nice::SeqSeqDouble Basda::MoccaPiNDevice::getBasicSettings()
{
    E_THROW(Pi::NotImplementedException, "not implemented");
}

void Basda::MoccaPiNDevice::setVelocitySetting(const Nice::NPoint &/*_velocity*/)
{
    E_THROW(Pi::NotImplementedException, "not implemented");
}

void Basda::MoccaPiNDevice::setAccelerationSetting(const Nice::NPoint &/*_acceleration*/)
{
    E_THROW(Pi::NotImplementedException, "not implemented");
}

void Basda::MoccaPiNDevice::setDecelerationSetting(const Nice::NPoint &/*_deceleration*/)
{
    E_THROW(Pi::NotImplementedException, "not implemented");
}

void Basda::MoccaPiNDevice::setJerkSetting(const Nice::NPoint &/*_jerk*/)
{
    E_THROW(Pi::NotImplementedException, "not implemented");
}

bool Basda::MoccaPiNDevice::isMoving(Nice::BitSet &_motion)
{
    Nice::SeqInt seqONT;
    bool inMotion = false;
    read(pi_onTarget(), seqONT);
    _motion.set(seqONT.size(), false);
    for (size_t i = 0; i < seqONT.size(); i++)
    {
        if (seqONT[i] == 0)
        {
            _motion[i] = true;
            inMotion = true;
        }
    }
    return inMotion;
//    return (std::find_if (seqONT.begin(), seqONT.end(), std::bind2nd (std::equal_to<int>(), 1)) == seqONT.end());
}

bool Basda::MoccaPiNDevice::isAtLimit(Nice::NPoint &_limit)
{
    size_t s = m_axes.size();
    _limit.resize(s);
    bool status = false;

    for (size_t i = 0; i < s; i++)
    {
        if (m_minPosition.isNull() || m_maxPosition.isNull())
        {
            _limit[i] = 0.0;
        }
        else if (Nice::Helper::fuzzyCompare(m_lastPosition[i],
                                            m_minPosition[i]))
        {
            _limit[i] = -1.0;
            status = true;
        }
        else if (Nice::Helper::fuzzyCompare(m_lastPosition[i],
                                            m_maxPosition[i]))
        {
            _limit[i] = 1.0;
            status = true;
        }
        else
        {
            _limit[i] = 0.0;
        }
    }
    return status;

}

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

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

bool Basda::MoccaPiNDevice::isAtHome(Nice::BitSet &_atHome)
{
    size_t s = m_axes.size();
    _atHome = Nice::BitSet(Nice::U32(s), true);
    bool atHome;
    for (size_t i = 0; i < s; i++)
    {
        if (!Nice::Helper::fuzzyCompare(m_lastPosition[i], m_homePosition[i]))
        {
            _atHome[i] = false;
            atHome = false;
        }
    }
    return atHome;
}

Nice::NPoint Basda::MoccaPiNDevice::getReferenceTime()
{
    // E_THROW(Pi::NotImplementedException, "not implemented");
    return Nice::NPoint();
}

Nice::NPoint Basda::MoccaPiNDevice::getCurrentTime()
{
    // E_THROW(Pi::NotImplementedException, "not implemented");
    return Nice::NPoint();
}

Nice::SeqBitSet Basda::MoccaPiNDevice::getPositionSwitchStatus()
{
    // E_THROW(Pi::NotImplementedException, "not implemented");
    return Nice::SeqBitSet();
}

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

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