#pragma once

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


#include <Basda/Mocca/NServiceWorkerCore.h>
#include <Basda/Mocca/WheelServiceWorkerSkeleton.h>
#include <Basda/Mocca/WheelServiceWorkerInternalData.h>

#include <Nice/Application.h>
#include <Nice/Property.h>
#include <Nice/Time.h>
#include <Nice/Map/Point.h>
#include <Nice/Map/NPoint.h>
#include <Nice/Map/WMatrix.h>

namespace Basda
{
    /**
     * @brief This class is the implementation of the generic X interface.
     */

    class MoccaWheelServiceWorker : public MoccaNServiceWorkerCore<double, Nice::Int,
            Nice::Point, MoccaWheelServiceWorkerSkeleton>
    {
        protected:
            /**
             * @brief This is the implementation of the call back object to
             *        update the completion status.
             */
            struct WheelDeviceAsyncImpl : public Basda::DeviceAsync
            {
                    /**
                     * @brief asynchronous wait
                     */
                    virtual int asyncWait(const Nice::Time & _t);

                    /**
                     * @brief asynchronous status
                     */
                    virtual int asyncStatus(const std::string & _s, int _d);

                    /**
                     * @brief member to store the worker object in order to call
                     *        status methods
                     */
                    MoccaWheelServiceWorker * m_service;

                    /**
                     * @brief member to keep the last command name
                     *
                     * Some commands like the upload external profile don't need
                     * to update the motor status information. In that case the
                     * command name of the asyncStatus will be NOSTATUS. But for
                     * the completion bar a command name will be displayed which
                     * is saved in that member before.
                     */
                    std::string m_s;

                    int m_d;
            };

            /**
             * @brief This struct is the implementation of the asynchronous
             *        command status method.
             */
            struct WheelDeviceCmdAsyncImpl : public WheelDeviceAsyncImpl
            {
                    /**
                     * @brief asynchronous status
                     */
                    virtual int asyncStatus(const std::string & _s, int _d);
            };

            WheelDeviceAsyncImpl m_wheelDeviceAsync;
            WheelDeviceCmdAsyncImpl m_wheelDeviceCmdAsync;

        public:
            /**
             * @brief Constructor
             */
            MoccaWheelServiceWorker(const Nice::JointPtr & _config,
                                const MoccaNDevicePtr &_device);

            /**
             * @brief Destructor
             */
            void moveToHome();

            virtual ~MoccaWheelServiceWorker();

            virtual void moveAbsolute(int _target, std::string _unit);

            virtual void moveRelative(int _target, std::string _unit);

            virtual void moveToNamedPosition(int _namedPosition);

            virtual void scanAllReferenceSwitches();

            virtual Nice::Int getPosition();

            void checkFullTurn();

            std::string checkForNamedPosition();

      virtual Nice::Int coordinateTransformation ( double _value, std::string /*_unit*/ )
      {
//                return (double) ((_value + m_fullturn) % m_fullturn);
        return (Nice::Int) _value;
      }

      virtual double coordinateTransformationInverse ( Nice::Int _value, std::string /*_unit*/);
            // {
            //     return (Nice::Int)_value;
            // }

      virtual double coordinateTranslateInverse ( Nice::Int _value, std::string /*_unit*/);
            // {
            //     return (Nice::Int)_value;
            // }

            // for wheelIntegrationTest
            Nice::Int getPositionOffset();

            void scanPositionSwitches();

            void validateScannedPositionSwitches();

      void transformationFactorChanged(const Nice::Any & /*_a*/);

        protected:
            virtual void status();

        private:
      Nice::MapStringPoint m_transformation;

            int searchForNamedPosition(int _namedPosition, int width, std::string _unit);

            double optimizeAbsolutePosition(int _requestedPosition, std::string _unit);

//            void move(int _absolutePosition, std::string _unit);

//            double m_velocity;

            std::vector<Nice::BitSet> m_namedSwitches;
            Nice::SeqInt m_namedPositions;

            std::vector<Nice::BitSet> m_scannedSwitches;
            std::vector<Nice::BitSet> m_brokenSwitches;
            Nice::SeqInt m_scannedPositions;

            Nice::Int m_startPosition;

//            Nice::Int m_positionSwitchWidth;

            Nice::Int m_positionOffset;

            bool m_scanAllReferenceSwitches;

            bool m_homeReferenceSwitchFound;
    };

    template<> Nice::Point Basda::MoccaNServiceWorkerCore<double, Nice::Int,
    Nice::Point, Basda::MoccaWheelServiceWorkerSkeleton>::getDefaultTransformation();

    typedef IceUtil::Handle<MoccaWheelServiceWorker> MoccaWheelServiceWorkerPtr;

}
;

