cinosmovepath.h

Go to the documentation of this file.

//******************************************************************************
//******************************************************************************
#ifndef INC_CINOSMOVEPATH_H
#define INC_CINOSMOVEPATH_H
//------------------------------------------------------------------------------
// defines
//------------------------------------------------------------------------------
//
// --- general -----------------------------------------------------------------
//
#define DF_INOS_MOVEPATH_ID_DEFAULT         0xffffffff
#define DF_INOS_MOVEPATH_TOLERANCE_DEFAULT  -1.0
#define DF_INOS_MOVEPATH_TOLERANCE_MIN      1.0e-4
#define DF_INOS_MOVEPATH_VELOCITY_DEFAULT   -1.0
#define DF_INOS_MOVEPATH_FLAG_NO                0x00000000
#define DF_INOS_MOVEPATH_FLAG_ABS               0x00000001
#define DF_INOS_MOVEPATH_FLAG_INC               0x00000002
#define DF_INOS_MOVEPATH_FLAG_NLG               0x00000004
#define DF_INOS_MOVEPATH_FLAG_RAW               0x00000008
#define DF_INOS_MOVEPATH_THE_END            1
 
//------------------------------------------------------------------------------
// Curve flags
 
#define DF_INOS_MOVEPATH_FLAG_CURVE_OWNED           0x01000000
#define DF_INOS_MOVEPATH_FLAG_CONTINUATION              0x02000000
#define DF_INOS_MOVEPATH_FLAG_INCOMPLETE                0x04000000
#define DF_INOS_MOVEPATH_FLAG_OFFSET_ZERO       0x40000000
#define DF_INOS_MOVEPATH_FLAG_RELOCATABLE       0x80000000
//
 
//------------------------------------------------------------------------------
// Segment flags
#define DF_INOS_MOVEPATH_FLAG_SHORT                 0x10000000  // segment is too short
#define DF_INOS_MOVEPATH_FLAG_PURE_LINEAR           0x20000000  // segment is pure linear
#define DF_INOS_MOVEPATH_FLAG_PURE_RAPID            0x40000000  // segment is pure rapid
 
#define DF_INOS_MOVEPATH_OPTION_NONE                uint32(0x00000000)
#define DF_INOS_MOVEPATH_OPTION_EXCEPTION_SUPPORT   uint32(0x00000001)
#define DF_INOS_MOVEPATH_OPTION_REGISTER_INCO       uint32(0x00000004)
#define DF_INOS_MOVEPATH_OPTION_ALL \
    (DF_INOS_MOVEPATH_OPTION_EXCEPTION_SUPPORT| \
    DF_INOS_MOVEPATH_OPTION_REGISTER_INCO)
 
#define DF_INOS_MOVEPATH_OPTION_DEFAULT DF_INOS_MOVEPATH_OPTION_NONE
 
// --- helper ------------------------------------------------------------------
//
#define DF_INOS_MOVEPATH_HELPER_CORE_NAME                   "Helper.Core"
#define DF_INOS_MOVEPATH_HELPER_CORE_DEFAULT                -1
//
#define DF_INOS_MOVEPATH_HELPER_PRIORITY_NAME               "Helper.Priority"
#define DF_INOS_MOVEPATH_HELPER_PRIORITY_DEFAULT            DF_INOS_TASK_PRIO_REALTIME_LOWEST
 
#define DF_INOS_MOVEPATH_PREPARE_HELPER_CORE_NAME                   "PrepareHelper.Core"
#define DF_INOS_MOVEPATH_PREPARE_HELPER_CORE_DEFAULT                -1
//
#define DF_INOS_MOVEPATH_PREPARE_HELPER_PRIORITY_NAME               "PrepareHelper.Priority"
#define DF_INOS_MOVEPATH_PREPARE_HELPER_PRIORITY_DEFAULT            DF_INOS_TASK_PRIO_BACKGROUND
#define DF_INOS_MOVEPATH_RUN_HELPER_CORE_NAME                   "RunHelper.Core"
#define DF_INOS_MOVEPATH_RUN_HELPER_CORE_DEFAULT                -1
//
#define DF_INOS_MOVEPATH_RUN_HELPER_PRIORITY_NAME               "RunHelper.Priority"
#define DF_INOS_MOVEPATH_RUN_HELPER_PRIORITY_DEFAULT            DF_INOS_TASK_PRIO_REALTIME_LOWEST
//
// --- mcrobot -----------------------------------------------------------------
//
#define DF_INOS_MOVEPATH_MCROBOT_PREPAREPATH_NAME           "McRobot.PreparePath"
#define DF_INOS_MOVEPATH_MCROBOT_PREPAREPATH_DEFAULT        0
#define DF_INOS_MOVEPATH_MCROBOT_PREPAREPATH_NOTHING        0
#define DF_INOS_MOVEPATH_MCROBOT_PREPAREPATH_ERROR          1
//
// --- interpolator ------------------------------------------------------------
//
#define DF_INOS_MOVEPATH_INTERPOLATOR_SHAPE_ON_SHRINK_NAME      "Shape.OnShrink"
#define DF_INOS_MOVEPATH_INTERPOLATOR_SHAPE_ON_SHRINK_DEFAULT   1
#define DF_INOS_MOVEPATH_INTERPOLATOR_SHAPE_ON_SHRINK_LIMIT_AMAX    0
#define DF_INOS_MOVEPATH_INTERPOLATOR_SHAPE_ON_SHRINK_STOP      1
#define DF_INOS_MOVEPATH_INTERPOLATOR_SHAPE_ON_SHRINK_ERROR     2
#define DF_INOS_MOVEPATH_INTERPOLATOR_SHAPE_ON_SHRINK_LIMIT_JMAX    3
#define DF_INOS_MOVEPATH_INTERPOLATOR_NURBS_MAX_ANGLE           "Nurbs.MaxAngle"
#define DF_INOS_MOVEPATH_INTERPOLATOR_NURBS_MAX_ANGLE_DEFAULT   20.0
#define DF_INOS_MOVEPATH_INTERPOLATOR_NURBS_MAX_LENGTH          "Nurbs.MaxLength"
#define DF_INOS_MOVEPATH_INTERPOLATOR_NURBS_MAX_LENGTH_DEFAULT  0.0
#define DF_INOS_MOVEPATH_INTERPOLATOR_NURBS_OVERLAP             "Nurbs.Overlap"
#define DF_INOS_MOVEPATH_INTERPOLATOR_NURBS_OVERLAP_DEFAULT     0.2
#define DF_INOS_MOVEPATH_INTERPOLATOR_NURBS_VECTORIZE_FACTOR    "Nurbs.Vectorize"
#define DF_INOS_MOVEPATH_INTERPOLATOR_NURBS_VECTORIZE_FACTOR_DEFAULT    0.1
#define DF_INOS_MOVEPATH_INTERPOLATOR_NURBS_INTERPOLATION_POINTS "Nurbs.InterpolationPoints"
#define DF_INOS_MOVEPATH_INTERPOLATOR_NURBS_INTERPOLATION_POINTS_DEFAULT    DF_INOS_NURBSCURVE_INTERPOLATION_POINTS
#define DF_INOS_MOVEPATH_INTERPOLATOR_NURBS_D3_FACTOR "Nurbs.D3Factor"
#define DF_INOS_MOVEPATH_INTERPOLATOR_NURBS_D3_FACTOR_DEFAULT 20.0
#define DF_INOS_MOVEPATH_INTERPOLATOR_CLOTHOID_PURE             "Clothoid.Pure"
#define DF_INOS_MOVEPATH_INTERPOLATOR_CLOTHOID_PURE_DEFAULT     1.0
#define DF_INOS_MOVEPATH_INTERPOLATOR_CLOTHOID_REDUCE           "Clothoid.Reduce"
#define DF_INOS_MOVEPATH_INTERPOLATOR_CLOTHOID_REDUCE_DEFAULT   1.0
#define DF_INOS_MOVEPATH_INTERPOLATOR_CLOTHOID_R                "Clothoid.R"
#define DF_INOS_MOVEPATH_INTERPOLATOR_CLOTHOID_L                "Clothoid.L"
#define DF_INOS_MOVEPATH_INTERPOLATOR_CLOTHOID_A                "Clothoid.A"
 
// --- trajectory --------------------------------------------------------------
//
#define DF_INOS_MOVEPATH_TRAJECTORY_MAIN_AXIS_NAME          "Trajectory.MainAxis"
#define DF_INOS_MOVEPATH_TRAJECTORY_MAIN_AXIS_DEFAULT       0
#define DF_INOS_MOVEPATH_TRAJECTORY_LOGGING_MASK            "Trajectory.Logging.Mask"
#define DF_INOS_MOVEPATH_TRAJECTORY_LOGGING_MASK_DEFAULT    0x00000001
#define DF_INOS_MOVEPATH_TRAJECTORY_LOGGING_MASK_NONE       0.0
#define DF_INOS_MOVEPATH_TRAJECTORY_LOGGING_MASK_PATHS      0x00000001
#define DF_INOS_MOVEPATH_TRAJECTORY_BUFFER_TYPE_NAME        "Trajectory.Buffer.Type"
#define DF_INOS_MOVEPATH_TRAJECTORY_BUFFER_TYPE_DEFAULT     "active"
#define DF_INOS_MOVEPATH_TRAJECTORY_BUFFER_TYPE_NONE        "none"
#define DF_INOS_MOVEPATH_TRAJECTORY_BUFFER_TYPE_ACTIVE      "active"
#define DF_INOS_MOVEPATH_TRAJECTORY_BUFFER_TYPE_PASSIVE     "passive"
#define DF_INOS_MOVEPATH_TRAJECTORY_BUFFER_SIZE_NAME        "Trajectory.Buffer.Size"
#define DF_INOS_MOVEPATH_TRAJECTORY_BUFFER_SIZE_DEFAULT     256
#define DF_INOS_MOVEPATH_TRAJECTORY_BUFFER_READY_NAME       "Trajectory.Buffer.Ready"
#define DF_INOS_MOVEPATH_TRAJECTORY_BUFFER_READY_DEFAULT    16
#define DF_INOS_MOVEPATH_TRAJECTORY_BUFFER_READY_FULL_PATH  0xffffffff
#define DF_INOS_MOVEPATH_TRAJECTORY_BUFFER_LOW_NAME         "Trajectory.Buffer.Low"
#define DF_INOS_MOVEPATH_TRAJECTORY_BUFFER_LOW_DEFAULT      192
#define DF_INOS_MOVEPATH_TRAJECTORY_FILTER_LENGTH_NAME      "Trajectory.Filter.Length"
#define DF_INOS_MOVEPATH_TRAJECTORY_FILTER_LENGTH_DEFAULT   0
#define DF_INOS_MOVEPATH_TRAJECTORY_FOLLOW_ENABLE_NAME      "Trajectory.Follow.Enable"
#define DF_INOS_MOVEPATH_TRAJECTORY_FOLLOW_ENABLE_DEFAULT   0
#define DF_INOS_MOVEPATH_TRAJECTORY_FOLLOW_INDEX_NAME       "Trajectory.Follow.Index"
#define DF_INOS_MOVEPATH_TRAJECTORY_FOLLOW_INDEX_DEFAULT    3
#define DF_INOS_MOVEPATH_TRAJECTORY_FOLLOW_UNIT_NAME        "Trajectory.Follow.Unit"
#define DF_INOS_MOVEPATH_TRAJECTORY_FOLLOW_UNIT_DEFAULT     0
#define DF_INOS_MOVEPATH_TRAJECTORY_CONSTANT_CHECK_VELOCITY "Trajectory.Constant.CheckVelocity"
#define DF_INOS_MOVEPATH_TRAJECTORY_CONSTANT_CHECK_VELOCITY_NONE    0.0
#define DF_INOS_MOVEPATH_TRAJECTORY_CONSTANT_CHECK_VELOCITY_DEFAULT 0.0
#define DF_INOS_MOVEPATH_TRAJECTORY_CONSTANT_CHECK_VELOCITY_REDUCE  2.0
#define DF_INOS_MOVEPATH_TRAJECTORY_CONSTANT_CHECK_VELOCITY_ONLY    1.0
#define DF_INOS_MOVEPATH_TRAJECTORY_CONSTANT_CHECK_VELOCITY_OVERSAMPLING    "Trajectory.Constant.CheckVelocity.Oversampling"
#define DF_INOS_MOVEPATH_TRAJECTORY_CONSTANT_CHECK_VELOCITY_OVERSAMPLING_DEFAULT    1.0
#define DF_INOS_MOVEPATH_TRAJECTORY_SEGMENT_CUBIC_REDUCTION_NAME    "Trajectory.Segment.CubicReduction"
#define DF_INOS_MOVEPATH_TRAJECTORY_SEGMENT_CUBIC_REDUCTION_DEFAULT 1.0
//
//------------------------------------------------------------------------------
// param names
//------------------------------------------------------------------------------
//
#define DF_INOS_MOVEPATH_PARAM_SPEEDSET     "SpeedSet"
#define DF_INOS_MOVEPATH_PARAM_SLOWMOTION   "SlowMotion"
#define DF_INOS_MOVEPATH_PARAM_TOLERANCE    "Tolerance"
#define DF_INOS_MOVEPATH_PARAM_VMAX         "Vmax"
#define DF_INOS_MOVEPATH_PARAM_VTRQ         "Vtrq"
#define DF_INOS_MOVEPATH_PARAM_AMAX         "Amax"
#define DF_INOS_MOVEPATH_PARAM_ATRQ         "Atrq"
#define DF_INOS_MOVEPATH_PARAM_BMAX         "Bmax"
#define DF_INOS_MOVEPATH_PARAM_BTRQ         "Btrq"
#define DF_INOS_MOVEPATH_PARAM_JMAX         "Jmax"
//
#define DF_INOS_MOVEPATH_APPROACH_NAME      "Approach"
#define DF_INOS_MOVEPATH_APPROACH_DEFAULT   0
#define DF_INOS_MOVEPATH_APPROACH_DISTANCE_NAME     "Approach.Distance"
#define DF_INOS_MOVEPATH_APPROACH_DISTANCE_DEFAULT  5.0
#define DF_INOS_MOVEPATH_APPROACH_VELOCITY_NAME     "Approach.Velocity"
#define DF_INOS_MOVEPATH_APPROACH_VELOCITY_DEFAULT  1.0
 
#define DF_INOS_MOVEPATH_RAPID_GATE_INVERT_NAME     "RapidGateInverted"
#define DF_INOS_MOVEPATH_RAPID_GATE_INVERT_DEFAULT  0.0
 
 
//------------------------------------------------------------------------------
// includes
//------------------------------------------------------------------------------
//
// system
#include <inos.h>
#include <cinostaskexdef.h>
#include <cinosvector.h>
#include <cinosmotionframe.h>
 
//------------------------------------------------------------------------------
// type definitions
//------------------------------------------------------------------------------
//
typedef TINOSVector<DF_INOS_MOVEPATH_MAX_AXES> TINOSMovePathVec;
 
 
#include <cinosmovepathparam.h>
#include <cinosmovepathevent.h>
#include <cinosmovepathsegment.h>
#include <cinosmovepathsegmentbase.h>
#include <cinosmovepathsegmentnurbs.h>
#include <cinosmovepathresource.h>
#include <cinosmovepathresourceposaxis.h>
#include <cinosmovepathresourcespeedaxis.h>
#include <cinosmovepathresourcerapidgate.h>
#include <cinosmovepathpart.h>
#include <cinosmovepathpartlinear.h>
#include <cinosmovepathpartlinearbay.h>
#include <cinosmovepathpartcircle.h>
#include <cinosmovepathpartclothoid.h>
#include <cinosmovepathcurve.h>
#include <cinosmovepathinterpolator.h>
#include <cinosmovepathinterpolatorsegment.h>
#include <cinosmovepathtrajectory.h>
#include <cinosmovepathtrajectoryconstant.h>
#include <cinosmovepathtrajectorysegment.h>
#include <cinosmovepathtrajectorylookahead.h>
 
#ifdef INOS_MOVEPATH_SKYWRITING_SUPPORT
#include <cinosmovepathsegmentrapid.h>
#include <cinosmovepathpartrapid.h>
#include <cinosmovepathtrajectoryskywriting.h>
#endif
 
//
// C++
//
// project
//
//------------------------------------------------------------------------------
// trace macros
//------------------------------------------------------------------------------
//
#define DF_INOS_EVENT_LOGGER_FONT_COLOR_MOVEPATH_CREATE 0x00808000
#define DF_INOS_EVENT_LOGGER_FONT_COLOR_MOVEPATH_RUN    0x00008080
//
#ifdef INOS_MOVEPATH_TRACE_SUPPORT
#define MOVEPATH_TRACE_CREATE( path, apFormat, aArgs... ) \
    if( CINOSEventLogger::Instance().IsLevelActive(DF_INOS_EVENT_LOGGER_LEVEL_MOVEPATH_CREATE) ) {\
        char cBuffer[256];                                                      \
        inos_strncpy(cBuffer, "MOVEPATH[%s] : ", sizeof(cBuffer));              \
        inos_strcat(cBuffer, apFormat, sizeof(cBuffer));                        \
        CINOSEventLogger::Instance().EvtLog(                                    \
            DF_INOS_EVENT_LOGGER_LEVEL_MOVEPATH_CREATE,                         \
            __PRETTY_FUNCTION__,                                                \
            __FILE__,                                                           \
            __LINE__,                                                           \
            DF_INOS_EVENT_LOGGER_FONT_COLOR_MOVEPATH_CREATE,                    \
            cBuffer, path->GetName(),                                           \
            ##aArgs );                                                          \
    }
#define MOVEPATH_TRACE_RUN( path, apFormat, aArgs... ) \
    if( CINOSEventLogger::Instance().IsLevelActive(DF_INOS_EVENT_LOGGER_LEVEL_MOVEPATH_RUN) ) {\
        char cBuffer[256];                                                      \
        inos_strncpy(cBuffer, "MOVEPATH[%s] : ", sizeof(cBuffer));              \
        inos_strcat(cBuffer, apFormat, sizeof(cBuffer));                        \
        CINOSEventLogger::Instance().EvtLog(                                    \
            DF_INOS_EVENT_LOGGER_LEVEL_MOVEPATH_RUN,                            \
            __PRETTY_FUNCTION__,                                                \
            __FILE__,                                                           \
            __LINE__,                                                           \
            DF_INOS_EVENT_LOGGER_FONT_COLOR_MOVEPATH_RUN,                       \
            cBuffer, path->GetName(),                                           \
            ##aArgs );                                                          \
    }
#else
#if defined(_MSC_VER)
#define MOVEPATH_TRACE_CREATE( path, apFormat, ... )
#define MOVEPATH_TRACE_RUN( path, apFormat, ... )
#else
#define MOVEPATH_TRACE_CREATE( path, apFormat, aArgs... )
#define MOVEPATH_TRACE_RUN( path, apFormat, aArgs... )
#endif
#endif
//------------------------------------------------------------------------------
// class definition
//------------------------------------------------------------------------------
//
class CINOSMovePath
{
    //--- user interface ---------------------------------------------------
 
    // public member functions
    public :
        enum EState {
            eStaVirgin      = 0,
            eStaEdit        = 1,
            eStaRaw         = 2,
            eStaReady       = 3,
            eStaRunning     = 4,
            eStaStopping    = 5,
            eStaBreaking    = 6,
            eStaCanceling   = 7,
            eStaSettling    = 8,
            eStaTransient   = 9,
            eStaPaused      = 10,
            eStaDone        = 11,
            eStaLast        = 12
        };
 
        EState GetState() const
        {
            // return actual state
            return m_eState;
        }
        bool IsMoving() const;
 
        uint32 GetError() const
        {
            // return actual error
            return m_uError;
        }
 
        virtual uint32 AcceptError()
        {
            // save pending error
            uint32 uError = m_uError;
            // clear error
            m_uError = 0;
            // return pending error
            return uError;
        }
 
        const char* GetName() const
        {
            // return name
            return &m_cName[0];
        }
 
        uint32 GetOptions() const
        {
            // return options
            return m_uOptions;
        }
 
        // --- creation and destruction ---------------------------------------
 
        static uint32 Create(CINOSMovePath*& apPath,
            uint32 auOptions = DF_INOS_MOVEPATH_OPTION_DEFAULT);
 
        static uint32 Create(CINOSMovePath*& apPath, const char* apName,
            uint32 auOptions = DF_INOS_MOVEPATH_OPTION_DEFAULT);
 
        static uint32 Create(CINOSMovePath*& apPath,
            const char* apCurveType, const char* apInterpolatorType, const char* apTrajectoryType,
            uint32 auOptions = DF_INOS_MOVEPATH_OPTION_DEFAULT);
 
        static uint32 Create(CINOSMovePath*& apPath, const char* apName,
            const char* apCurveType, const char* apInterpolatorType, const char* apTrajectoryType,
            uint32 auOptions = DF_INOS_MOVEPATH_OPTION_DEFAULT);
 
        static uint32 Create(CINOSMovePath*& apPath, const char* apName,
            const char* apCurveType, const char* apInterpolatorType, const char* apTrajectoryType,
            const char* apParam, uint32 auOptions = DF_INOS_MOVEPATH_OPTION_DEFAULT);
 
        static uint32 Destroy(CINOSMovePath* apPath);
 
        // --- static global methods ------------------------------------------
 
        static uint32 glbSetAxis(const char* apName, uint8 auIndex,
            uint32 auFlags = DF_INOS_MOVEPATH_FLAG_NO,
            uint32 auOptions = DF_INOS_MOVEPATH_OPTION_DEFAULT);
 
        static CINOSBaseAxis* glbGetAxis(uint8 auIndex);
 
        static uint32 glbGetActPosition(const char* apName, double& adPosition,
            uint32 auOptions = DF_INOS_MOVEPATH_OPTION_DEFAULT);
 
        // --- edit mode ------------------------------------------------------
 
        virtual uint32 SetAxis(const char* apName, uint8 auIndex,
            uint32 auFlags = DF_INOS_MOVEPATH_FLAG_NO,
            const char* apType = "PosAxis");
 
        virtual uint32 AddResource(const char* apName,
                uint8& auIndex,
                uint32 auFlags = DF_INOS_MOVEPATH_FLAG_NO,
                const char* apType = "PosAxis");
 
        virtual uint32 EditBegin(uint32 auFlags = DF_INOS_MOVEPATH_FLAG_INC,
            double adTolerance = DF_INOS_MOVEPATH_TOLERANCE_DEFAULT,
            double adVelocity = DF_INOS_MOVEPATH_VELOCITY_DEFAULT);
 
        virtual uint32 EditEnd(bool abIncomplete = false);
 
        virtual uint32 SegBegin(uint32 auId = DF_INOS_MOVEPATH_ID_DEFAULT,
            uint32 auFlags = DF_INOS_MOVEPATH_FLAG_NO,
            double adTolerance = DF_INOS_MOVEPATH_TOLERANCE_DEFAULT,
            double adVelocity = DF_INOS_MOVEPATH_VELOCITY_DEFAULT);
 
        virtual uint32 SegEnd();
 
        virtual uint32 Start(uint8 auIndex, double adPosition);
 
        // --- base curve methods ---------------------------------------------
 
        virtual uint32 Linear(uint8 auIndex, double adPosition, uint32 auFlags = 0);
 
        virtual uint32 LinearBay(uint8 auIndex1, uint8 auIndex2, double adPosition1,
            double adPosition2, double adLength, uint32 auFlags = 0);
 
        virtual uint32 CircleCW(uint8 auIndex1, uint8 auIndex2,
            double adEnd1, double adEnd2, double adMid1, double adMid2,
            uint32 auFlags=0);
 
        virtual uint32 CircleCC(uint8 auIndex1, uint8 auIndex2,
            double adEnd1, double adEnd2, double adMid1, double adMid2,
            uint32 auFlags=0);
 
        virtual uint32 CircleCW(uint8 auIndex1, uint8 auIndex2,
            double adMid1, double adMid2, double adAngle, bool abAngleAbsolute,
            uint32 auFlags=0);
 
        virtual uint32 CircleCC(uint8 auIndex1, uint8 auIndex2,
            double adMid1, double adMid2, double adAngle, bool abAngleAbsolute,
            uint32 auFlags=0);
 
        virtual uint32 CircleCW(uint8 auIndex1, uint8 auIndex2,
            double adEnd1, double adEnd2, double adRadius, uint32 auFlags=0);
 
        virtual uint32 CircleCC(uint8 auIndex1, uint8 auIndex2,
            double adEnd1, double adEnd2, double adRadius, uint32 auFlags=0);
 
            // --- rapid segment methods -------------------------------------------
#ifdef INOS_MOVEPATH_SKYWRITING_SUPPORT
 
        virtual uint32 RapidBegin(uint32 auId = DF_INOS_MOVEPATH_ID_DEFAULT,
            uint32 auFlags = DF_INOS_MOVEPATH_FLAG_NO);
 
        virtual uint32 RapidEnd();
 
        virtual uint32 Rapid(uint8 auIndex, double adPosition, uint32 auFlags = 0);
#endif
 
        // --- nurbs curve methods -------------------------------------------
 
        virtual uint32 NrbBegin(uint32 auId = DF_INOS_MOVEPATH_ID_DEFAULT,
            uint32 auFlags = DF_INOS_MOVEPATH_FLAG_NO,
            uint32 auDegree = 3,
            uint32 auInterpolationPoints = DF_INOS_NURBSCURVE_INTERPOLATION_POINTS,
            double adVelocity = DF_INOS_MOVEPATH_VELOCITY_DEFAULT);
 
        virtual uint32 NrbEnd();
 
        virtual uint32 NrbPoint(double adP0, double adP1, double adWeight = 1.0);
 
        #if DF_INOS_MOVEPATH_MAX_AXES > 2
        virtual uint32 NrbPoint(double adP0, double adP1, double adP2, double adWeight);
        #endif
 
        #if DF_INOS_MOVEPATH_MAX_AXES > 3
        virtual uint32 NrbPoint(double adP0, double adP1, double adP2, double adP3,
            double adWeight);
        #endif
 
        #if DF_INOS_MOVEPATH_MAX_AXES > 4
        virtual uint32 NrbPoint(double adP0, double adP1, double adP2, double adP3,
            double adP4, double adWeight);
        #endif
 
        virtual uint32 NrbKnot(double adValue, uint32 auMultiplicity = 1);
 
        // --- general methods -----------------------------------------
 
        virtual uint32 Reset();
 
        // --- general setter/getter -----------------------------------------
 
        virtual uint32 SetParam(const char* apName, double adValue);
 
        virtual uint32 SetParam(uint8 auIndex, const char* apName, double adValue);
 
        virtual uint32 SetParam(const char* apName, const char* apValue);
 
        virtual uint32 SetParam(uint8 auIndex, const char* apName, const char* apValue);
 
        virtual uint32 GetParam(const char* apName, double& adValue, double adDefault = 0.0);
 
        virtual uint32 GetParam(uint8 auIndex, const char* apName, double& adValue, double adDefault = 0.0);
 
        virtual uint32 GetParam(const char* apName, char* apBuffer, uint32 auSize, const char* apDefault = "");
 
        virtual uint32 GetParam(uint8 auIndex, const char* apName, char* apBuffer, uint32 auSize, const char* apDefault = "");
 
        // --- timing --------------------------------------------------------
 
        virtual uint32 GetCycleTime(double& adCycleTime);
 
        virtual uint32 GetCycleData(double& adMainCycleTime, uint8& auMainAxisIndex,
            inos_std::vector<uint8>& auMainCycleDownSampling, uint8& auMainOversampling);
 
        virtual uint32 GetMoveTime(double& adMoveTime, double adDelta = 0.0);
 
        virtual uint32 GetMoveTicks(uint64& adMoveTicks, double arDelta = 0.0);
 
        virtual uint32 GetMoveLength(double& adMoveLength);
 
        virtual uint32 GetMoveRemaining(double& adMoveRemaining);
 
        virtual uint32 GetMaxConstantVelocity(double& adMaxConstantVelocity);
 
        // --- event handling ------------------------------------------------
 
        virtual uint32 AddEvent(const char* apName, double adValue, uint32 auType);
 
        virtual uint32 SetEventDescriptor(struct SINOSMovePathEventDesc* apDesc);
 
        // --- running -------------------------------------------------------
 
        virtual CMcResult Prepare(CINOSSync* apSync = DF_INOS_SYNCHRONOUS);
 
        virtual CMcResult Prepare(const char* apSpeedset, CINOSSync* apSync = DF_INOS_SYNCHRONOUS);
 
        virtual CMcResult Run(CINOSSync* apSync = DF_INOS_SYNCHRONOUS);
 
        virtual CMcResult Run(const char* apSpeedset, CINOSSync* apSync = DF_INOS_SYNCHRONOUS);
 
        virtual uint32 Append(CINOSSync* apSync = DF_INOS_SYNCHRONOUS);
 
        virtual uint32 Stop(CINOSSync* apSync = DF_INOS_SYNCHRONOUS);
 
        virtual uint32 Break(CINOSSync* apSync = DF_INOS_SYNCHRONOUS);
 
        virtual uint32 Continue(CINOSSync* apSync = DF_INOS_SYNCHRONOUS);
 
        virtual CMcResult Cancel(CINOSSync* apSync = DF_INOS_SYNCHRONOUS);
 
        // --- misc ----------------------------------------------------------
 
        virtual uint32 GetData(
            TINOSMovePathVec& vPos,
            TINOSMovePathVec& vVel,
            TINOSMovePathVec& vAcc,
            TINOSMovePathVec& vJrk)
            {
                // FIXME : ensure a valid trajectory generator
                ASSERT_ALWAYS(m_pTrajectory);
                // handle request over to trajectory generator
                return m_pTrajectory->GetData(vPos, vVel, vAcc, vJrk);
            }
 
        virtual uint32 GetData(
            uint32& auSegmentId,
            TINOSMovePathVec& vPos,
            TINOSMovePathVec& vVel,
            TINOSMovePathVec& vAcc,
            TINOSMovePathVec& vJrk)
            {
                // FIXME : ensure a valid trajectory generator
                ASSERT_ALWAYS(m_pTrajectory);
                // handle request over to trajectory generator
                return m_pTrajectory->GetData(auSegmentId, vPos, vVel, vAcc, vJrk);
            }
 
        virtual CINOSMovePathSegment* GetActSegment()
        {
            // FIXME : ensure a valid trajectory generator
            ASSERT_ALWAYS(m_pTrajectory);
            // handle request over to trajectory generator
            return m_pTrajectory->GetActSegment();
        }
        virtual uint32 GetBufferSegment(
                        uint32 auOffset,
                        CINOSMovePathSegment*& opSegment)
        {
            // FIXME : ensure a valid trajectory generator
            ASSERT_ALWAYS(m_pTrajectory);
            // handle request over to trajectory generator
            return m_pTrajectory->GetBufferSegment(auOffset, opSegment);
        }
 
 
 
        virtual uint32 CancelData()
        {
                // FIXME : ensure a valid trajectory generator
                ASSERT_ALWAYS(m_pTrajectory);
                // handle request over to trajectory generator
                return m_pTrajectory->CancelData();
            }
 
        virtual uint32 Fail(uint32 auError);
 
        virtual uint32 GetPositionDelta(TINOSMovePathVec& vDelta);
 
        virtual bool GetPosition(uint32 auIndex, TINOSMovePathVec& ovPos)
            {
                if(m_pMainCurve == nullptr) {
                    return false;   
                }
                // get it from curve
                return m_pMainCurve->GetPosition(auIndex, ovPos);
            }
 
        virtual uint32 GetTorqueLimit()
            {
                return m_uTorqueLimit;
            }
        virtual double GetEpsilon()
            {
                return m_dEpsilon;
            }
 
        virtual double GetTolerance()
            {
                return m_dTolerance;
            }
 
        virtual uint32 GetVmax(TINOSMovePathVec& aVmax)
            {
                aVmax = vVmax;
                return 0;
            }
        virtual uint32 GetVtrq(TINOSMovePathVec& aVtrq)
            {
                aVtrq = vVtrq;
                return 0;
            }
        virtual uint32 GetAmax(TINOSMovePathVec& aAmax)
            {
                aAmax = vAmax;
                return 0;
            }
        virtual uint32 GetAtrq(TINOSMovePathVec& aAtrq)
            {
                aAtrq = vAtrq;
                return 0;
            }
        virtual uint32 GetBmax(TINOSMovePathVec& aBmax)
            {
                aBmax = vBmax;
                return 0;
            }
        virtual uint32 GetBtrq(TINOSMovePathVec& aBtrq)
            {
                aBtrq = vBtrq;
                return 0;
            }
        virtual uint32 GetJmax(TINOSMovePathVec& aJmax)
            {
                aJmax = vJmax;
                return 0;
            }
 
        uint32 SetCurve(CINOSMovePathCurve* apCurve);
 
        CINOSMovePathCurve* GetCurve()
            { return m_pMainCurve; };
 
 
        CINOSMovePathCurve* GetEditCurve()
            { return m_pEditCurve; };
        CINOSMovePathCurve* GetPrepareCurve()
            { return m_pPrepareCurve; };
        CINOSMovePathCurve* GetRunCurve()
            { return m_pRunCurve; };
 
 
        CINOSMovePathTrajectory* GetTrajectory()
            { return m_pTrajectory; };
        void SetTrajectory(CINOSMovePathTrajectory* apTrajectory);
 
        class CINOSMovePathResourceAxis* GetAxis(uint32 auIndex)
            { return m_pAxis[auIndex]; };
 
        class CINOSMovePathResource* GetResource(uint32 auIndex);
 
        void GetActPosition(TINOSMovePathVec& ovAct);
        void GetPositionFirst(TINOSMovePathVec& ovFirst);
        void GetPositionLast(TINOSMovePathVec& ovLast);
 
        bool GetBufferFillLevel(uint32& auBufferFillLevel) const;
 
        uint32 SetMapping(class CINOSMatrix* apMapping);
 
        uint32 GetAxesErrorMask();
 
        typedef std::function<uint32(CINOSMovePath*,uint32,TINOSMovePathVec&,TINOSMovePathVec&,TINOSMovePathVec&,TINOSMovePathVec&)> tpActiveCallback;
 
        void SetActiveCallback(tpActiveCallback aCallback)
        {
            m_ActiveCallback = aCallback;
        }
 
        void GetActual(SINOSRampData& aActual)
            {
                // trajectory available ?
                if (nullptr != m_pTrajectory) {
                    // yes
                    m_pTrajectory->GetActual(aActual);
                }
                else {
                    // not available
                    aActual.m_rS = 0.0;
                    aActual.m_rV = 0.0;
                    aActual.m_rA = 0.0;
                    aActual.m_rJ = 0.0;
                }
            }
 
    //--- internals --------------------------------------------------------
 
    // my friends
    friend class CINOSMovePathHelper;
    friend class CINOSMovePathInterpolator;
    friend class CINOSMovePathInterpolatorBlend;
    friend class CINOSMovePathInterpolatorClothoid;
    friend class CINOSMovePathInterpolatorPolynom;
    friend class CINOSMovePathTrajectory;
    template <typename CINOSMovePath> friend class CINOSTaskExMsgParam::CPtrContainer;
    friend class CINOSMcTargetMovePath;
    friend void _INI_0300_CINOSMovePath();
 
    // testing friends
    friend class CINOSMovePathHelperTest;
 
 
    // protected methods
    protected :
 
        // constructor / destructor
        CINOSMovePath (
            const char* apName,
            const char* apCurveType,
            const char* apInterpolatorType,
            const char* apTrajectoryType,
            uint32 auOptions);
        virtual ~CINOSMovePath ();
 
        virtual uint32 iCircle(bool abCw, uint8 auIndex1, uint8 auIndex2,
            double adEnd1, double adEnd2, double adMid1, double adMid2,
            uint32 auFlags);
        virtual uint32 iCircleAngle(bool abCw, uint8 auIndex1, uint8 auIndex2,
            double adMid1, double adMid2, double adAngle, bool abAngleAbsolute,
            uint32 auFlags);
        virtual uint32 iCircleRadius(bool abCw, uint8 auIndex1, uint8 auIndex2,
            double adEnd1, double adEnd2, double adRadius, uint32 auFlags);
 
        virtual uint32 iPrepare(class CINOSTaskExMsg* apMsg);
        virtual uint32 iPrepare(const char* apSpeedSet);
 
        virtual uint32 iStartRun(class CINOSTaskExMsg* apMsg);
        virtual uint32 iStartRun(const char* apSpeedSet, CINOSSync* apSync);
 
        virtual uint32 iRun(class CINOSTaskExMsg* apMsg);
        virtual uint32 iRun(const char* apSpeedSet, CINOSSync* apSync);
 
        virtual uint32 iPrepareAppend(class CINOSTaskExMsg* apMsg);
        virtual uint32 iPrepareAppend(CINOSSync* apSync);
 
        virtual uint32 iAppend(class CINOSTaskExMsg* apMsg);
        virtual uint32 iAppend(CINOSMovePathCurve* apCurve, CINOSSync* apSync);
 
        virtual uint32 iStop(class CINOSTaskExMsg* apMsg);
        virtual uint32 iBreak(class CINOSTaskExMsg* apMsg);
        virtual uint32 iContinue(class CINOSTaskExMsg* apMsg);
        virtual uint32 iCancel(class CINOSTaskExMsg* apMsg);
        virtual uint32 iCancelData(class CINOSTaskExMsg* apMsg);
        virtual uint32 iSetParam(uint8 auIndex, const char* apName, double adValue);
        virtual uint32 iSetParam(uint8 auIndex, const char* apName, const char* apValue);
        virtual uint32 iGetParam(uint8 auIndex, const char* apName, double* adValue, double adDefault);
        virtual uint32 iGetParam(uint8 auIndex, const char* apName, char* apValue, uint32 auSize, const char* apDefault);
 
 
 
        CMcResult PutMsg(CINOSTaskExMsg* apMsg);
        CMcResult PutCmd(uint32 auCommand, CINOSSync* apSync = DF_INOS_ASYNCHRONOUS);
        uint32 HandleCmd(uint32 auError = INOS_OK);
        uint32 WaitCmd(uint32 auCommand, uint32 auError = INOS_MOVEPATH_ERROR_NO_COMMAND_QUEUE);
 
        CINOSTaskQueue* GetCmdQueue();
        uint32 DispatchCmd(CINOSTaskExMsg* apCmd);
 
 
        uint32 AddPart(CINOSMovePathPart* apPart, uint32 auFlags);
 
        virtual void SetState(EState aeState)
        {
            // set actual state
            m_eState = aeState;
        }
 
        virtual void SetError(uint32 auError)
        {
            // set error
            m_uError = auError;
        }
 
        class TINOSMotionFrame<DF_INOS_MOVEPATH_MAX_AXES>* GetFrame()
        {
            // return frame
            return m_pFrame;
        }
 
        struct SINOSMovePathEventDesc* GetEventDescriptor()
        {
            // return event descriptor
            return m_pEventDesc;
        }
 
        CINCOObject* RequestRegister();
        void ReleaseRegister();
 
    // private methods
    private:
        // ! check validity of speedset values if speedset is defined global
        static uint32 CheckSpeedSetValues(const char* apSpeedSet);
 
    // protected members
    protected :
 
        // commands
        public:
        enum {
            eCmdAttach = 256,
            eCmdDetach = 257,
            // state dependent commands
            eCmdFirst = 258,
            eCmdSetAxis,
            eCmdSetCurve,
            eCmdReset,
            eCmdSetMapping,
            eCmdGetMoveTime,
            eCmdGetMoveTicks,
            eCmdGetMoveLength,
            eCmdGetPositionDelta,
            eCmdGetMaxConstantVelocity,
            eCmdAddEvent,
            eCmdSetEventDescriptor,
            eCmdPrepare,
            eCmdStartRun,
            eCmdRun,
            eCmdPrepareAppend,
            eCmdAppend,
            eCmdBufferLow,
            eCmdBufferEmpty,
            eCmdStop,
            eCmdBreak,
            eCmdContinue,
            eCmdCancel,
            eCmdCancelData,
            eCmdLast
        };
 
        enum {
            eParamIndexGlobalInternal = 0,
            eParamIndexGlobal = 0xff,
            eParamIndexMask = 0x0000ffff,
            eParamTypeNumber = 0x00010000,
            eParamTypeString = 0x00020000,
        };
 
        enum {
            eFlgOwnerInterpolator = 0x00000001,
            eFlgOwnerTrajectory = 0x00000002,
            eFlgHasRapidSegments = 0x00000004,
        };
 
        EState m_eState;
        uint32 m_uFlags;
        uint32 m_uOptions;
        uint32 m_uError;
 
        uint32 m_uActInvolved;
 
        CINOSMutex m_MainMutex;
 
        CINOSMovePathCurve* m_pEditCurve;
        CINOSMovePathCurve* m_pRawCurve;
        CINOSMovePathCurve* m_pAppendCurve;
 
        CINOSMovePathCurve* m_pPrepareCurve;
        CINOSMovePathCurve* m_pRunCurve;
 
        CINOSMovePathCurve* m_pMainCurve;
 
        CINOSMovePathTrajectory* m_pTrajectory;
        class CINOSMovePathHelper* m_pPrepareHelper;
        class CINOSMovePathHelper* m_pRunHelper;
 
        uint32 m_uLengthMask;
 
    private:
 
        uint32 m_uId;
 
        char m_cName[256];
 
        char m_cInterpolatorType[32];
 
        CINOSTaskQueue* m_pCmdQueue;
 
        class TINOSMotionFrame<DF_INOS_MOVEPATH_MAX_AXES>* m_pFrame;
        class TINOSMotionFrame<DF_INOS_MOVEPATH_MAX_AXES>* m_pFrame_1;
 
 
        inos_std::vector<
            TINOSNameBalancedBinaryTree<SINOSMovePathParam>*> m_vParams;
 
        CINOSMovePathResourceAxis* m_pAxis[DF_INOS_MOVEPATH_MAX_AXES];
        inos_std::vector<CINOSMovePathResource*> m_vAdditionalResources;
 
        // misc properties
 
        double m_dTolerance;
        double m_dEpsilon;
 
        // max vectors
 
        uint32 m_uTorqueLimit;
        TINOSMovePathVec vVmax;
        TINOSMovePathVec vVtrq;
        TINOSMovePathVec vAmax;
        TINOSMovePathVec vAtrq;
        TINOSMovePathVec vBmax;
        TINOSMovePathVec vBtrq;
        TINOSMovePathVec vJmax;
 
        // edit helpers
 
        CINOSMovePathSegmentBase* m_pActSegmentBase;
        CINOSMovePathSegmentNurbs* m_pActSegmentNurbs;
        TINOSMovePathVec m_vEditBegPos;
 
        // internals
 
        // used for run helper if core id is -1.
        int8 m_iCallerCore = -1;
 
 
 
        // event handling
 
        struct SINOSMovePathEventDesc* m_pEventDesc;
        CINOSMovePathEvent* m_pFstEvent;
 
        // active callback
 
        tpActiveCallback m_ActiveCallback;
 
        // inco registration
 
        uint32 m_uRegisterCnt = 0;
        CINCOObject* m_pRegister = nullptr;
 
        // static members
 
        static uint32 m_PathId;
        static const char* m_pCurveType;
        static const char* m_pInterpolatorType;
        static const char* m_pTrajectoryType;
        static uint32 m_uGlbLengthMask;
        static class CINOSBaseAxis* m_pGlbAxis[DF_INOS_MOVEPATH_MAX_AXES];
 
    // dynamic
    DECLARE_DYNAMIC(CINOSMovePath);
};
 
//------------------------------------------------------------------------------
// end of file
//------------------------------------------------------------------------------
 
#endif  // INC_CINOSMOVEPATH_H