// $Header: https://oberon.oapd.inaf.it/shins/trunk/shins/src/motion_ctrl/niceCallback.cpp 3940 2022-05-05 18:36:54Z depa $

#include "niceCallback.h"

niceCallback::niceCallback()
{
    //
    // boost::function<void(const Nice::Completion&)> m_cbFnTest;
    // m_cbFnTest = boost::bind(&niceCallback::callback_motion_complete_count, this, _1, boost::ref(m_counter));

}


niceCallback::~niceCallback()
{
    //
}

void niceCallback::sm_dev_in_position(boost::shared_ptr<osfsm::OSFsm>& _ptrStateMachine,
                                      IceUtil::Mutex& _mtxStateMachine)
{
    IceUtil::Mutex::TryLock lock(_mtxStateMachine);
    // if (!lock.acquired())
    //   {
    //     A_LLOG("LOCK NOT ACQUIRED!")
    //     lock.acquire(); //blocks function execution until mutex is available
    //   }
    if (lock.acquired())
    {
        // std::cout << "\n" <<  __FUNCTION__ << " LOCK ACQUIRED" << "\n" << std::endl;
        _ptrStateMachine->process_event(osfsm::dev_in_position);
    }
    else
    {
        // std::cout << "\n" << "ACQUIRING LOCK" << "\n" << std::endl;
        lock.acquire();
    }
    // std::cout << __FUNCTION__ << std::endl;
    // _ptrStateMachine->process_event(osfsm::dev_in_position);
}

void niceCallback::callback_motion_complete_SM(const Nice::Completion& _c,
                                               boost::shared_ptr<osfsm::OSFsm>& _ptrStateMachine,
                                               IceUtil::Mutex& _mtxStateMachine)
{
    U6_LLOG(__FUNCTION__ << " ~ _c.state() = " << _c.state());
    // std::cout << __FUNCTION__ << " " << &_mtxStateMachine << std::endl;
    try
    {
        // std::cout << "Completion State " << _c.state() << std::endl;
//        U9_LLOG("_c.state() : " << _c.state());
        switch (_c.state())
        {
        case Nice::OK:
            sm_dev_in_position(_ptrStateMachine, _mtxStateMachine);
            // _ptrStateMachine->process_event(osfsm::dev_in_position);
            // I_LLOG("completion state OK");
            // std::cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> AH!" << std::endl;
            // std::cout << __FUNCTION__ << " --- " << "completion state OK" << std::endl;
            // I_LLOG("completion state " << _c.state());
            break;
        case Nice::EXEC:
            // U1_LLOG("completion state EXEC");
            // std::cout << __FUNCTION__ << " --- " << "completion state EXEC" << std::endl;
            // I_LLOG("completion state " << _c.state());
            break;
        case Nice::ERROR:
            W_LLOG("State ERROR!@" << _c.except);
            break;
        default:
            W_LLOG("State neither OK, EXEC or ERROR!@");
            // do nothing
            break;
        }
    }catch (std::exception err)
    {
        E_LLOG("Exception");
        U8_LLOG("");
        throw err;
    }
    U8_LLOG("");
}

//void niceCallback::callback_motion_complete_indi(const Basda::ServiceStateMachine& _stateMachine,
//                                                 const std::string& _strPropName,
//                                                 boost::shared_ptr<indiIF>& _ptrIndiIF)
////./basda/src/Interface/Basda/ServiceStateMachine.h
//{
//    // convert Basda status to INDI status
//    // Basda: [SHUTDOWN | OFFLINE | STANDBY | ONLINE | WORKING]
//    // INDI: Idle Ok Busy Alert
//    std::string strState;
//    switch (_stateMachine.now())
//    {
//    case Basda::ONLINE:
//        // T_LLOG("completion state OK");
//        strState = "ONLINE";
//        // _ptrIndiIF->send_new_property(strState);
//        // std::cout << __FUNCTION__ << " --- " << _state;
//        // std::cout << std::endl;
//        break;
//    case Basda::STANDBY:
//        strState = "STANDBY";
//        // T_LLOG("completion state EXEC");
//        // _ptrIndiIF->send_new_property(strState);
//        // std::cout << __FUNCTION__ << " --- " << _state;
//        break;
//    case Basda::WORKING:
//        strState = "WORKING";
//        // T_LLOG("completion state EXEC");
//        // _ptrIndiIF->send_new_property(strState);
//        // std::cout << __FUNCTION__ << " --- " << _state;
//        break;
//    case Basda::SHUTDOWN:
//        strState = "WORKING";
//        break;
//    case Basda::ERROR:
//        strState = "ERROR";
//    case Basda::OFFLINE:
//        strState = "OFFLINE";
//    default:
//        strState = "UNKNOWN";
//        break;
//    }
//    // T_LLOG("state " << strState);
//    // std::cout << ">>>>>>>>>>>>> STATE: " << strState << std::endl;
//    _ptrIndiIF->send_new_property(strState, _strPropName);
//}


void niceCallback::decrement_counter(int& _count, IceUtil::Mutex& _mtxCount)
{
    IceUtil::Mutex::TryLock lock(_mtxCount);
    if (lock.acquired())
    {
        if(_count == 0)
        {
            U9_LLOG(__FUNCTION__ << " ~ " << "Decrementing null variable. _count == 0");
        }
        _count -= 1;
    }
    else
    {
        lock.acquire();
        _count -= 1;
    }
}

void niceCallback::increment_counter(int& _count, IceUtil::Mutex& _mtxCount)
{
    IceUtil::Mutex::TryLock lock(_mtxCount);
    if (lock.acquired())
    {
        _count += 1;
    }
    else
    {
        lock.acquire();
        _count += 1;
    }
}

bool niceCallback::is_counter_zero(int& _count, IceUtil::Mutex& _mtxCount)
{
    bool res = false;
    IceUtil::Mutex::TryLock lock(_mtxCount);
    // N_LLOG(__FUNCTION__)
    if (lock.acquired())
    {
        if (_count == 0)
            res = true;
    }
    else
    {
        lock.acquire();
        if (_count == 0)
            res = true;
    }
    return res;
}

void niceCallback::callback_drot_move_absolute(const Nice::Completion& _c,
                                               boost::function<bool(void)>& _f)
{
    switch (_c.state())
    {
    case Nice::OK:
        W_LLOG("call _f: delta correction");
        _f();
        break;
    case Nice::EXEC:
        break;
    default:
        W_LLOG(__FUNCTION__ << " ~ unhandled state");
        break;
    }
}

void niceCallback::callback_motion_complete(const Nice::Completion & _c,
                                            const std::string & id)
{
    U6_LLOG(__FUNCTION__ << " ~ " << id << " ~ " << _c.state());
    switch (_c.state())
    {
    case Nice::OK:
        break;
    case Nice::EXEC:
        break;
    default:
        break;
    }
    U8_LLOG("");
}

void niceCallback::callback_motion_complete_count(const Nice::Completion& _c, int& _count,
                                                  IceUtil::Mutex& _mtxCount, boost::shared_ptr<IceUtil::Monitor<IceUtil::Mutex> >& _ptrMonSeq)
{
    U6_LLOG(__FUNCTION__ << " ~ " << _count << " ~ " << &_count);
    switch (_c.state())
    {
    case Nice::OK:
        // T_LLOG("completion state OK");
        // std::cout << __FUNCTION__ << " --- " << "completion state OK" << std::endl
        //           << __FUNCTION__ << " --- counter = " << _count << std::endl;
        decrement_counter(_count, _mtxCount);
        // I_LLOG("Decremented counter");
        // std::cout << __FUNCTION__ << " --- " << "-------------------" << std::endl
        //           << __FUNCTION__ << " --- counter = " << _count << std::endl;
        if (is_counter_zero(_count, _mtxCount) == true)
        {
            // IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*(_ptrMonSeq));
//            A_LLOG("Counter is ZERO");
            // A_LLOG("Notifying monitor");
            // _ptrMonSeq->notify();
//            W_LLOG("Lock released!");
        }
        break;
    case Nice::EXEC:
        //increment_counter(_count, _mtxCount);
        // _count += 1;
        // T_LLOG("completion state EXEC");
        // std::cout << __FUNCTION__ << " --- " << "completion state EXEC" << std::endl
        //           << __FUNCTION__ << " --- " << _count << std::endl;
        break;
    default:
        // do nothing
        break;
    }
    U8_LLOG("_c.state = " << _c.state());
}

void niceCallback::callback_device_move_to_reference(const Nice::Completion& _c,
                                                     const Basda::MoccaWheelServiceWorkerInterfacePtr _device)
{
    switch (_c.state())
    {
    case Nice::OK:
        _device->moveToHomeStart();
        break;
    default:
        break;
    }
}

void niceCallback::callback_absolute_encoder_position(const double& _pos)//,
//const Sharknir::DrotServiceWorkerInterfacePtr _drotDevice)
{
    Nice::Date now = Nice::Date::now();
    std::string fileName = "drot_abs_enc_readings_callback.txt";
    std::ofstream output(fileName.c_str(), std::ofstream::app);
    std::string sep = "    ";

    U9_LLOG("absolute_encoder_position : " << _pos);

    // std::cout << now.time().toString() << sep
    //           << now.time().toMilliSeconds()
    //           << "    " << std::fixed << _pos
    //           << std::endl;



    output << now.time().toMilliSeconds()
           << "    " << std::fixed << _pos
           << std::endl;
    output.close();


    // W_LOG(_pos);
}
