Streamingle_URP/Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackStreamingClient.cs

/*
Copyright (c) 2016 NaturalPoint Inc.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

using System;
using System.Collections.Generic;
using System.Net;
using System.Net.Sockets;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading;
using UnityEngine;
using NaturalPoint;
using NaturalPoint.NatNetLib;

/// <summary>Skeleton naming conventions supported by OptiTrack Motive.</summary>
public enum OptitrackBoneNameConvention
{
    Motive,
    FBX,
    BVH,
}

public enum StreamingCoordinatesValues
{
    Local,
    Global
}

/// <summary>Describes the position and orientation of a streamed tracked object.</summary>
public class OptitrackPose
{
    public Vector3 Position;
    public Quaternion Orientation;
}

/// <summary>Represents the state of a streamed marker.</summary>
public class OptitrackMarkerState
{
    public string Name;
    public Vector3 Position;
    public float Size;
    public bool Labeled;
    public Int32 Id;
    public bool IsActive;
}

/// <summary>Represents the state of a streamed rigid body at an instant in time.</summary>
public class OptitrackRigidBodyState
{
    public OptitrackHiResTimer.Timestamp DeliveryTimestamp;
    public OptitrackPose Pose;
    public bool IsTracked;
}

/// <summary>Represents the state of a streamed skeleton at an instant in time.</summary>
public class OptitrackSkeletonState
{
    /// <summary>Maps from OptiTrack bone IDs to their corresponding bone poses.</summary>
    public Dictionary<Int32, OptitrackPose> BonePoses;
    public Dictionary<Int32, OptitrackPose> LocalBonePoses;
    /// <summary>Timestamp for received NatNet frame data.</summary>
    public OptitrackHiResTimer.Timestamp DeliveryTimestamp;
}

/// <summary>Represents the state of a streamed trained markerset at an instant in time.</summary>
public class OptitrackTMarkersetState // trained markerset added
{
    /// <summary>Maps from OptiTrack bone IDs to their corresponding bone poses.</summary>
    public Dictionary<Int32, OptitrackPose> BonePoses;
    public Dictionary<Int32, OptitrackPose> LocalBonePoses;
}

public class OptitrackRigidBodyDefinition
{
    public class MarkerDefinition
    {
        public Vector3 Position;
        public Int32 RequiredLabel;
    }

    public Int32 Id;
    public string Name;
    public List<MarkerDefinition> Markers;
}

/// <summary>Describes the hierarchy and neutral pose of a streamed skeleton.</summary>
public class OptitrackSkeletonDefinition
{
    public class BoneDefinition
    {
        /// <summary>The ID of this bone within this skeleton.</summary>
        public Int32 Id;

        /// <summary>The ID of this bone's parent bone. A value of 0 means that this is the root bone.</summary>
        public Int32 ParentId;

        /// <summary>The name of this bone.</summary>
        public string Name;

        /// <summary>
        /// This bone's position offset from its parent in the skeleton's neutral pose.
        /// (The neutral orientation is always <see cref="Quaternion.identity"/>.)
        /// </summary>
        public Vector3 Offset;
    }

    /// <summary>Skeleton ID. Used as an argument to <see cref="OptitrackStreamingClient.GetLatestSkeletonState"/>.</summary>
    public Int32 Id;

    /// <summary>Skeleton asset name.</summary>
    public string Name;

    /// <summary>True when the definition was inferred from frame packets and does not contain a Motive asset name.</summary>
    public bool IsSynthetic;

    /// <summary>Bone names, hierarchy, and neutral pose position information.</summary>
    public List<BoneDefinition> Bones;

    /// <summary>Bone hierarchy information</summary>
    public Dictionary<Int32, Int32> BoneIdToParentIdMap;
}

public class OptitrackTMarkersetDefinition // trained markerset added // check where this is coming from
{
    public class BoneDefinition
    {
        /// <summary>The ID of this bone within this trained markerset.</summary>
        public Int32 Id;

        /// <summary>The ID of this bone's parent bone. A value of 0 means that this is the root bone.</summary>
        public Int32 ParentId;

        /// <summary>The name of this bone.</summary>
        public string Name;

        /// <summary>
        /// This bone's position offset from its parent in the skeleton's neutral pose.
        /// (The neutral orientation is always <see cref="Quaternion.identity"/>.)
        /// </summary>
        public Vector3 Offset;
    }

    public class MarkerDefinition
    {
        /// <summary>The name of this marker.</summary>
        public string Name;
        public Vector3 Position;
        public Int32 Id;

        //public float Size;
        //public bool Labeled;
        //public bool IsActive;
    }

    /// <summary>Asset ID. Used as an argument to <see cref="OptitrackStreamingClient.GetLatestTMarkersetState"/>.</summary>
    public Int32 Id;

    /// <summary>Skeleton asset name.</summary>
    public string Name;

    /// <summary>Bone names, hierarchy, and neutral pose position information.</summary>
    public List<BoneDefinition> Bones;

    /// <summary>Bone hierarchy information</summary>
    public Dictionary<Int32, Int32> BoneIdToParentIdMap;

    public List<MarkerDefinition> Markers;
}

public class OptitrackMarkersDefinition
{
    /// <summary>The name of this bone.</summary>
    public string Name;
}

public class OptitrackForcePlateDefinition
{
    /// <summary>The ID of this force plate.</summary>
    public Int32 Id;

    /// <summary>The serial number of this force plate.</summary>
    public string SerialNumber;

    /// <summary>The width of the force plate.</summary>
    public float Width;

    /// <summary>The length of the force plate.</summary>
    public float Length;

    /// <summary>The electrical offset of this force plate.</summary>
    public Vector3 ElectricalOffset;

    /// <summary>The calibration matrix of this force plate.</summary>
    public List<float> CalibrationMatrix;

    /// <summary>The corner locations of this force plate.</summary>
    public List<float> Corners;

    /// <summary>The force plate type.</summary>
    public Int32 PlateType;

    /// <summary>The force plate channel data type.</summary>
    public Int32 ChannelDataType;

    /// <summary>The force plate channel count.</summary>
    public Int32 ChannelCount;

    /// <summary>The channel names for the force plate.</summary>
    public List<string> ChannelNames;
}

public class OptitrackCameraDefinition
{
    /// <summary>The name of this camera.</summary>
    public string Name;

    /// <summary>The name of this camera.</summary>
    public Vector3 Position;

    /// <summary>The name of this camera.</summary>
    public Quaternion Orientation;
}

public static class OptitrackHiResTimer
{
    public struct Timestamp
    {
        internal Int64 m_ticks;

        public float AgeSeconds
        {
            get
            {
                return Now().SecondsSince( this );
            }
        }

        public float SecondsSince( Timestamp reference )
        {
            Int64 deltaTicks = m_ticks - reference.m_ticks;
            return deltaTicks / (float)System.Diagnostics.Stopwatch.Frequency;
        }
    }

    public static Timestamp Now()
    {
        return new Timestamp {
            m_ticks = System.Diagnostics.Stopwatch.GetTimestamp()
        };
    }
}

/// <summary>
/// Connects to a NatNet streaming server and makes the data available in lightweight Unity-friendly representations.
/// </summary>
public class OptitrackStreamingClient : MonoBehaviour
{
    private const int k_DefaultNatNetCommandPort = 1510;
    private const int k_DefaultNatNetDataPort = 1511;
    public enum ClientConnectionType
    {
        Multicast,
        Unicast
    }

    [Header("Connection Settings")]

    [Tooltip("The Streaming IP (Local Interface) in Motive")]
    public string ServerAddress = "127.0.0.1";

    [Tooltip("Must be on the same network as the Streaming IP (Local Interface) in Motive.")]
    [HideInInspector]
    public string LocalAddress = "127.0.0.1";

    [Tooltip("Automatically selects the local IPv4 interface that routes to ServerAddress. LocalAddress is used as a fallback if detection fails.")]
    [HideInInspector]
    public bool AutoDetectLocalAddress = true;

    [Tooltip("The local IPv4 address selected for the active connection.")]
    [HideInInspector]
    public string ResolvedLocalAddress = "";

    [Tooltip("Unicast performs subscription reducing your overall data set in some applications.")]
    public ClientConnectionType ConnectionType;

    [Header("NatNet Endpoint Override")]
    [Tooltip("NatNet command port. Motive/MMRP default = 1510.")]
    [Range(1, 65535)]
    public int CommandPort = k_DefaultNatNetCommandPort;

    [Tooltip("NatNet data port. Motive/MMRP default = 1511.")]
    [Range(1, 65535)]
    public int DataPort = k_DefaultNatNetDataPort;

    [Tooltip("Optional multicast group override. Empty uses the NatNet default group (usually 239.255.42.99). Use a separate group for replay, e.g. 239.255.42.100.")]
    public string MulticastAddress = "";

    [Header("Replay Priority Quick Setup")]
    [Tooltip("Enable Live Motive + MMRP Replay priority switching from this Streaming Client inspector.")]
    public bool EnableReplayPriority = false;

    [Tooltip("MMRP / recording PC IP address. Replay frames from this IP override live frames while fresh.")]
    public string ReplayServerAddress = "127.0.0.1";

    [Tooltip("Replay frames are preferred while newer than this many seconds.")]
    [Range(0.05f, 5.0f)]
    public float ReplayFreshnessSeconds = 0.35f;

    [Tooltip("Controls whether skeleton data is streamed with local or global coordinates.")]
    public StreamingCoordinatesValues SkeletonCoordinates = StreamingCoordinatesValues.Local;

    [Tooltip("Controls whether tmarkerset data is streamed with local or global coordinates.")]
    public StreamingCoordinatesValues TMarkersetCoordinates = StreamingCoordinatesValues.Local;

    [Tooltip("Controls the Bone Naming Convention in the streamed data.")]
    public OptitrackBoneNameConvention BoneNamingConvention = OptitrackBoneNameConvention.Motive;

    [Header("Extra Features")]

    [Tooltip("Draws marker visuals in the viewport for debugging and other uses. Using this will increase the data rate in Unicast mode.")]
    public bool DrawMarkers = false;

    [Tooltip("Draws trained markerset marker visuals in the viewport for debugging and other uses. Using this will increase the data rate in Unicast mode.")]
    public bool DrawTMarkersetMarkers = false; // trained markerset added

    [Tooltip("Draws camera visuals in the viewport for debugging and other uses. Motive 3.0+ only.")]
    public bool DrawCameras = false;

    [Tooltip("Draws force plate visuals in the viewport for debugging and other uses.")]
    public bool DrawForcePlates = false;

    [Tooltip("Motive will record when the Unity project is played.")]
    public bool RecordOnPlay = false;

    [Tooltip("Skips getting data descriptions. Skeletons will not work with this feature turned on, but it will reduce network usage with a large number of rigid bodies.")]
    public bool SkipDataDescriptions = false;

    [Tooltip("Automatically retries the initial connection and reconnects when streaming frames stop.")]
    public bool AutoReconnect = true;

    [Header("Connection Health")]
    [Tooltip("How often connection health is checked. This only reads local timestamps and does not contact Motive.")]
    [Min(0.1f)]
    public float ConnectionHealthCheckIntervalSeconds = 0.25f;

    [Tooltip("Seconds without live/replay frames before the stream is considered stale.")]
    [Min(0.5f)]
    public float StreamingFrameTimeoutSeconds = 1.0f;

    [Tooltip("Delay before the first full reconnect attempt after a stale stream is detected.")]
    [Min(0f)]
    public float FullReconnectInitialDelaySeconds = 0.0f;

    [Tooltip("Seconds to wait for frames after a full reconnect attempt before retrying.")]
    [Min(0.25f)]
    public float FullReconnectFrameWaitSeconds = 1.5f;

    [Tooltip("Delay before subsequent full reconnect attempts.")]
    [Min(0.5f)]
    public float FullReconnectRetryDelaySeconds = 2.0f;

    [Tooltip("Timeout for the direct NatNet server-info command during connect/reconnect, in milliseconds.")]
    [Min(100)]
    public int DirectServerInfoTimeoutMs = 500;

    [Tooltip("Timeout for the direct NatNet MODELDEF command during connect/reconnect, in milliseconds.")]
    [Min(100)]
    public int DirectModelDefTimeoutMs = 1000;

    [Tooltip("Changes to the version of Natnet used by the server")]
    public string ServerNatNetVersion = "";
    public string ClientNatNetVersion = "";

    //[Tooltip("Timecode Provider")]
    //public bool TimecodeProvider = false;

    [Header("Mirror Mode")]
    [Tooltip("Mirrors streamed skeleton bones and retargeted avatar motion left/right.")]
    public bool MirrorMode = false;

    #region Private fields
    //private UInt16 ServerCommandPort = NatNetConstants.DefaultCommandPort;
    //private UInt16 ServerDataPort = NatNetConstants.DefaultDataPort;

    private bool m_doneSubscriptionNotice = false;
    private bool m_receivedFrameSinceConnect = false;
    private bool m_hasDrawnCameras = false;
    private bool m_hasDrawnForcePlates = false;
    private bool m_subscribedToMarkers = false;
    private float m_markerSubscribeRetryCooldown = 0f;
    private const float k_MarkerSubscribeRetryInterval = 10f;

    private OptitrackHiResTimer.Timestamp m_lastFrameDeliveryTimestamp;
    private Coroutine m_connectionHealthCoroutine = null;
    private Coroutine m_replayConnectCoroutine = null;
    private UdpClient m_directFrameUdp = null;
    private Thread m_directFrameThread = null;
    private volatile bool m_directFrameRunning = false;
    private bool m_directFrameLogged = false;
    private volatile bool m_directFrameLogPending = false;
    private volatile bool m_directTimeoutLogPending = false;
    private volatile bool m_directSocketErrorLogPending = false;
    private string m_directSocketErrorMessage = "";
    private int m_directTimeoutCount = 0;
    private bool m_directNatNetConnected = false;
    private bool m_directNoModelDefWarned = false;
    private IPAddress m_directServerAddress = null;
    private IPAddress m_directLocalAddress = null;
    private IPAddress m_directMulticastAddress = null;
    private UInt16 m_directCommandPort = 0;
    private UInt16 m_directDataPort = 0;

    private enum DirectDefinitionSource
    {
        None,
        Live,
        Replay,
    }

    private DirectDefinitionSource m_directDefinitionSource = DirectDefinitionSource.None;

    private NatNetClient m_client;
    private NatNetClient m_replayClient;
    private bool m_replayReceivedFrameSinceConnect = false;
    private bool m_replayConnectWarned = false;
    private OptitrackHiResTimer.Timestamp m_lastReplayFrameDeliveryTimestamp;
    private NatNetClient.DataDescriptions m_dataDescs;
    private List<OptitrackRigidBodyDefinition> m_rigidBodyDefinitions = new List<OptitrackRigidBodyDefinition>();
    private List<OptitrackSkeletonDefinition> m_skeletonDefinitions = new List<OptitrackSkeletonDefinition>();
    private List<OptitrackTMarkersetDefinition> m_tmarkersetDefinitions = new List<OptitrackTMarkersetDefinition>(); // trained markerset added
    private List<OptitrackMarkersDefinition> m_tmarkmarkersDefinitions = new List<OptitrackMarkersDefinition>(); // trained markerset added
    private List<OptitrackMarkersDefinition> m_markersDefinitions = new List<OptitrackMarkersDefinition>();
    private List<OptitrackCameraDefinition> m_cameraDefinitions = new List<OptitrackCameraDefinition>();
    private List<OptitrackForcePlateDefinition> m_forcePlateDefinitions = new List<OptitrackForcePlateDefinition>();

    /// <summary>Maps from a streamed rigid body's ID to its most recent available pose data.</summary>
    private Dictionary<Int32, OptitrackRigidBodyState> m_latestRigidBodyStates = new Dictionary<Int32, OptitrackRigidBodyState>();

    /// <summary>Maps from a streamed skeleton's ID to its most recent available pose data.</summary>
    private Dictionary<Int32, OptitrackSkeletonState> m_latestSkeletonStates = new Dictionary<Int32, OptitrackSkeletonState>();

    /// <summary>Reusable staging buffers used to validate a complete skeleton frame before committing it.</summary>
    private Dictionary<Int32, sRigidBodyData[]> m_skeletonFrameScratch = new Dictionary<Int32, sRigidBodyData[]>();

    /// <summary>Cache for MirrorMode skeleton bone ID mappings.</summary>
    private Dictionary<Int32, Dictionary<Int32, Int32>> m_mirrorBoneIdMaps = new Dictionary<Int32, Dictionary<Int32, Int32>>();

    /// <summary>Maps from a streamed trained markerset's ID to its most recent available pose data.</summary>
    private Dictionary<Int32, OptitrackTMarkersetState> m_latestTMarkersetStates = new Dictionary<Int32, OptitrackTMarkersetState>(); // trained markerset added

    /// <summary>Maps from a streamed marker's ID to its most recent available position.</summary>
    private Dictionary<Int32, OptitrackMarkerState> m_latestMarkerStates = new Dictionary<Int32, OptitrackMarkerState>();

    /// <summary>Maps from a streamed trained markerset marker's ID to its most recent available position.</summary>
    private Dictionary<Int32, OptitrackMarkerState> m_latestTMarkMarkerStates = new Dictionary<Int32, OptitrackMarkerState>(); // trained markerset added

    /// <summary>Maps from a streamed rigid body's ID to its component.</summary>
    private Dictionary<Int32, MonoBehaviour> m_rigidBodies = new Dictionary<Int32, MonoBehaviour>();

    /// <summary>Maps from a streamed skeleton names to its component.</summary>
    private Dictionary<string, MonoBehaviour> m_skeletons = new Dictionary<string, MonoBehaviour>();
    private HashSet<MonoBehaviour> m_registeredSkeletonComponents = new HashSet<MonoBehaviour>();

    /// <summary>Maps from a streamed trained markerset names to its component.</summary>
    private Dictionary<string, MonoBehaviour> m_tmarkersets = new Dictionary<string, MonoBehaviour>(); // trained markerset added

    /// <summary>Maps from a streamed marker's ID to its sphere game object. Used for drawing markers.</summary>
    private Dictionary<Int32, GameObject> m_latestMarkerSpheres = new Dictionary<Int32, GameObject>();

    /// <summary>Maps from a streamed trained markerset marker's ID to its sphere game object. Used for drawing markers.</summary>
    private Dictionary<Int32, GameObject> m_latestTMarkMarkerSpheres = new Dictionary<Int32, GameObject>(); // trained markerset added

    /// <summary>
    /// Lock held during access to fields which are potentially modified by <see cref="OnNatNetFrameReceived"/> (which
    /// executes on a separate thread). Note while the lock is held, any frame updates received are simply dropped.
    /// </summary>
    private object m_frameDataUpdateLock = new object();

    /// <summary>Cache from streamed asset ID to asset name.</summary>
    private Dictionary<Int32, string> m_assetIdToNameCache = new Dictionary<Int32, string>();

    private HashSet<Int32> m_latestFrameRigidBodyIds = new HashSet<Int32>();
    private HashSet<Int32> m_currentFrameRigidBodyIds = new HashSet<Int32>();
    private HashSet<Int32> m_latestFrameSkeletonIds = new HashSet<Int32>();
    private HashSet<Int32> m_currentFrameSkeletonIds = new HashSet<Int32>();
    private bool m_hasFrameRigidBodyTopologySnapshot = false;
    private bool m_hasFrameSkeletonTopologySnapshot = false;

    private volatile bool m_pendingDefinitionRefresh = false;
    private volatile bool m_forceDefinitionRefreshSoon = false;
    private volatile bool m_pendingReplayDefinitionRefresh = false;
    private volatile bool m_forceReplayDefinitionRefreshSoon = false;
    private volatile bool m_pendingSkeletonDefinitionNotify = false;
    private float m_definitionRefreshCooldown = 0f;

    private bool m_isReconnecting = false;

    private bool m_isRecording = false;

    private bool m_hasAppliedServerSettings = false;

    [Header("Definition Refresh")]
    [Tooltip("Motive에서 Actor/RigidBody가 추가되거나 삭제될 때 Unity 재시작 없이 DataDescription을 주기적으로 다시 조회합니다.")]
    public bool AutoRefreshDefinitions = true;

    [Tooltip("DataDescription 자동 재조회 간격(초). 값이 낮을수록 변경 반영이 빠르지만 Motive 명령 요청이 늘어납니다.")]
    [Min(1f)]
    public float DefinitionRefreshInterval = 5f;

    private int m_definitionRefreshFailureCount = 0;
    private float m_nextAutoDefinitionRefreshTime = 0f;
    #endregion Private fields

    /// <summary>
    /// </summary>
    public void RequestDefinitionRefresh()
    {
        m_pendingDefinitionRefresh = true;
    }

    private bool CanRefreshDefinitions()
    {
        return !SkipDataDescriptions &&
               (m_client != null || (ConnectionType == ClientConnectionType.Multicast && m_directNatNetConnected));
    }

    private bool CanRefreshReplayDefinitions()
    {
        return !SkipDataDescriptions &&
               EnableReplayPriority &&
               ConnectionType == ClientConnectionType.Multicast &&
               m_directNatNetConnected &&
               !string.IsNullOrWhiteSpace(ReplayServerAddress);
    }

    private void RefreshDefinitionsForActiveTransport()
    {
        if (m_client != null)
        {
            UpdateDefinitions();
            ResubscribeRegisteredAssets();
            return;
        }

        if (ConnectionType == ClientConnectionType.Multicast && m_directNatNetConnected)
        {
            UpdateDirectDefinitions();
            ResubscribeRegisteredAssets();
            return;
        }

        throw new InvalidOperationException("No active OptiTrack transport is available for DataDescription refresh.");
    }

    private void RefreshReplayDefinitionsForActiveTransport()
    {
        if (ConnectionType == ClientConnectionType.Multicast && m_directNatNetConnected)
        {
            UpdateDirectReplayDefinitions();
            ResubscribeRegisteredAssets();
            return;
        }

        throw new InvalidOperationException("No active MMRP replay transport is available for DataDescription refresh.");
    }

    private void Update()
    {
        if (m_pendingSkeletonDefinitionNotify)
        {
            m_pendingSkeletonDefinitionNotify = false;
            NotifyRegisteredSkeletonDefinitionsChanged();
        }

        if (m_directFrameLogPending)
        {
            m_directFrameLogPending = false;
            Debug.Log(GetType().FullName + ": receiving NatNet frames through direct UDP fallback.", this);
        }
        if (m_directTimeoutLogPending)
        {
            m_directTimeoutLogPending = false;
            Debug.LogWarning(GetType().FullName + ": direct NatNet receiver is joined but has not received multicast packets yet. If Python receives frames on this PC, check Unity Editor firewall permission or multicast routing.", this);
        }
        if (m_directSocketErrorLogPending)
        {
            m_directSocketErrorLogPending = false;
            Debug.LogWarning(GetType().FullName + ": direct NatNet receiver socket warning: " + m_directSocketErrorMessage, this);
        }

        if (EnableReplayPriority && m_replayConnectCoroutine == null && ConnectionType != ClientConnectionType.Multicast)
            m_replayConnectCoroutine = StartCoroutine(ReplayConnectLoop());
        else if (!EnableReplayPriority && m_replayConnectCoroutine != null)
        {
            StopCoroutine(m_replayConnectCoroutine);
            m_replayConnectCoroutine = null;
            CleanupReplayClient();
        }

        if (DrawMarkers)
        {
            if (m_client != null && ConnectionType == ClientConnectionType.Unicast && !m_subscribedToMarkers)
            {
                if (m_markerSubscribeRetryCooldown <= 0f)
                {
                    SubscribeMarkers();
                    if (!m_subscribedToMarkers)
                        m_markerSubscribeRetryCooldown = k_MarkerSubscribeRetryInterval;
                }
                else
                {
                    m_markerSubscribeRetryCooldown -= Time.deltaTime;
                }
            }

            var markerSnapshot = new Dictionary<Int32, (Vector3 pos, float size, string name, bool isActive)>();
            lock (m_frameDataUpdateLock)
            {
                foreach (var markerEntry in m_latestMarkerStates)
                {
                    markerSnapshot[markerEntry.Key] = (
                        markerEntry.Value.Position,
                        markerEntry.Value.Size,
                        markerEntry.Value.Name,
                        markerEntry.Value.IsActive
                    );
                }
            }

            var activeIds = new HashSet<Int32>(markerSnapshot.Count);
            foreach (var kvp in markerSnapshot)
            {
                activeIds.Add(kvp.Key);
                if (m_latestMarkerSpheres.ContainsKey(kvp.Key))
                {
                    m_latestMarkerSpheres[kvp.Key].transform.position = kvp.Value.pos;
                }
                else
                {
                    var sphere = GameObject.CreatePrimitive(PrimitiveType.Cube);
                    sphere.transform.parent = this.transform;
                    sphere.transform.localScale = new Vector3(kvp.Value.size, kvp.Value.size, kvp.Value.size);
                    sphere.transform.position = kvp.Value.pos;
                    sphere.name = kvp.Value.name;
                    if (kvp.Value.isActive)
                        sphere.GetComponent<Renderer>().material.SetColor("_Color", Color.cyan);
                    m_latestMarkerSpheres[kvp.Key] = sphere;
                }
            }
            var staleIds = new List<Int32>();
            foreach (var sphereEntry in m_latestMarkerSpheres)
            {
                if (!activeIds.Contains(sphereEntry.Key))
                    staleIds.Add(sphereEntry.Key);
            }
            foreach (var id in staleIds)
            {
                Destroy(m_latestMarkerSpheres[id]);
                m_latestMarkerSpheres.Remove(id);
            }
        }
        else
        {
            // not drawing markers, remove all marker spheres
            foreach (KeyValuePair<Int32, GameObject> markerSphereEntry in m_latestMarkerSpheres)
            {
                Destroy( m_latestMarkerSpheres[markerSphereEntry.Key] );
            }
            m_latestMarkerSpheres.Clear();
        }

        if (DrawCameras && !m_hasDrawnCameras )
        {
            if (m_client.ServerAppVersion >= new Version(3, 0, 0))
            {
                var cameraGroup = new GameObject("Cameras");
                foreach (OptitrackCameraDefinition camera in m_cameraDefinitions)
                {
                    var geometry = GameObject.CreatePrimitive(PrimitiveType.Cube);
                    geometry.transform.parent = cameraGroup.transform;
                    geometry.transform.localScale = new Vector3(0.1f, 0.1f, 0.1f);
                    geometry.transform.position = camera.Position;
                    geometry.transform.rotation = camera.Orientation;
                    geometry.name = camera.Name;
                    geometry.GetComponent<Renderer>().material.SetColor("_Color", Color.black);
                }
            }
            else
            {
                Debug.LogWarning("Drawing cameras is only supported in Motive 3.0+.");
            }

            m_hasDrawnCameras = true;
        }

        if (DrawForcePlates && !m_hasDrawnForcePlates)
        {
            var forcePlateGroup = new GameObject("Force Plates");
            foreach (OptitrackForcePlateDefinition plate in m_forcePlateDefinitions)
            {
                Vector3 p0 = new Vector3(-plate.Corners[0], plate.Corners[1], plate.Corners[2]);
                Vector3 p1 = new Vector3(-plate.Corners[3], plate.Corners[4], plate.Corners[5]);
                Vector3 p2 = new Vector3(-plate.Corners[6], plate.Corners[7], plate.Corners[8]);
                Vector3 p3 = new Vector3(-plate.Corners[9], plate.Corners[10], plate.Corners[11]);
                Vector3 pAverage = (p0 + p1 + p2 + p3) / 4;

                var geometry = GameObject.CreatePrimitive(PrimitiveType.Cube);
                geometry.transform.parent = forcePlateGroup.transform;
                geometry.transform.localScale = new Vector3(plate.Length * 0.0254f, 0.03f, plate.Width * 0.0254f);
                geometry.transform.position = pAverage;
                geometry.transform.rotation = Quaternion.LookRotation(p2 - p1);
                geometry.name = plate.SerialNumber;
                geometry.GetComponent<Renderer>().material.SetColor("_Color", Color.blue);
            }

            m_hasDrawnForcePlates = true;
        }

        //if (TimecodeProvider) 
        //{
        //    Debug.Log("");
        //}

        // Trained Markerset Markers if requested to draw // trained markerset added
        if (DrawTMarkersetMarkers)
        {
            var tmarkSnapshot = new Dictionary<Int32, (Vector3 pos, float size, string name, bool isActive)>();
            lock (m_frameDataUpdateLock)
            {
                foreach (var markerEntry in m_latestTMarkMarkerStates)
                {
                    tmarkSnapshot[markerEntry.Key] = (
                        markerEntry.Value.Position,
                        markerEntry.Value.Size,
                        markerEntry.Value.Name,
                        markerEntry.Value.IsActive
                    );
                }
            }

            var activeTMarkIds = new HashSet<Int32>(tmarkSnapshot.Count);
            foreach (var kvp in tmarkSnapshot)
            {
                activeTMarkIds.Add(kvp.Key);
                if (m_latestTMarkMarkerSpheres.ContainsKey(kvp.Key))
                {
                    m_latestTMarkMarkerSpheres[kvp.Key].transform.position = kvp.Value.pos;
                }
                else
                {
                    var cube = GameObject.CreatePrimitive(PrimitiveType.Cube);
                    cube.transform.parent = this.transform;
                    cube.transform.localScale = new Vector3(kvp.Value.size, kvp.Value.size, kvp.Value.size);
                    cube.transform.position = kvp.Value.pos;
                    cube.name = kvp.Value.name;
                    if (kvp.Value.isActive)
                        cube.GetComponent<Renderer>().material.SetColor("_Color", Color.cyan);
                    m_latestTMarkMarkerSpheres[kvp.Key] = cube;
                }
            }
            var staleTMarkIds = new List<Int32>();
            foreach (var cubeEntry in m_latestTMarkMarkerSpheres)
            {
                if (!activeTMarkIds.Contains(cubeEntry.Key))
                    staleTMarkIds.Add(cubeEntry.Key);
            }
            foreach (var id in staleTMarkIds)
            {
                Destroy(m_latestTMarkMarkerSpheres[id]);
                m_latestTMarkMarkerSpheres.Remove(id);
            }
        }
        else
        {
            // not drawing markers, remove all marker spheres
            foreach (KeyValuePair<Int32, GameObject> markerCubeEntry in m_latestTMarkMarkerSpheres)
            {
                Destroy(m_latestTMarkMarkerSpheres[markerCubeEntry.Key]);
            }
            m_latestTMarkMarkerSpheres.Clear();
        }

        if (m_forceDefinitionRefreshSoon)
        {
            m_forceDefinitionRefreshSoon = false;
            m_pendingDefinitionRefresh = true;
            m_definitionRefreshCooldown = 0f;
        }
        if (m_forceReplayDefinitionRefreshSoon)
        {
            m_forceReplayDefinitionRefreshSoon = false;
            m_pendingReplayDefinitionRefresh = true;
            m_definitionRefreshCooldown = 0f;
        }

        // Refresh streamed asset definitions when Motive actors or rigid bodies are added/removed.
        if (m_pendingReplayDefinitionRefresh && m_definitionRefreshCooldown <= 0f && CanRefreshReplayDefinitions())
        {
            m_pendingReplayDefinitionRefresh = false;
            try
            {
                RefreshReplayDefinitionsForActiveTransport();
                m_definitionRefreshCooldown = Mathf.Max(DefinitionRefreshInterval, 1f);
                m_nextAutoDefinitionRefreshTime = Time.unscaledTime + Mathf.Max(DefinitionRefreshInterval, 1f);
            }
            catch (Exception ex)
            {
                m_definitionRefreshFailureCount++;
                m_definitionRefreshCooldown = Mathf.Min(60f, 2f * Mathf.Pow(2f, Mathf.Min(m_definitionRefreshFailureCount, 5)));
                m_pendingReplayDefinitionRefresh = true;
                Debug.LogWarning(GetType().FullName + ": MMRP DataDescription refresh failed. Retrying in " + m_definitionRefreshCooldown + " seconds.", this);
                Debug.LogException(ex, this);
            }
        }
        if (m_pendingDefinitionRefresh && m_definitionRefreshCooldown <= 0f && CanRefreshDefinitions())
        {
            m_pendingDefinitionRefresh = false;
            try
            {
                RefreshDefinitionsForActiveTransport();
                m_definitionRefreshCooldown = Mathf.Max(DefinitionRefreshInterval, 1f);
                m_nextAutoDefinitionRefreshTime = Time.unscaledTime + Mathf.Max(DefinitionRefreshInterval, 1f);
            }
            catch (Exception ex)
            {
                m_definitionRefreshFailureCount++;
                m_definitionRefreshCooldown = Mathf.Min(60f, 2f * Mathf.Pow(2f, Mathf.Min(m_definitionRefreshFailureCount, 5)));
                m_pendingDefinitionRefresh = true;
                Debug.LogWarning(GetType().FullName + ": DataDescription refresh failed. Retrying in " + m_definitionRefreshCooldown + " seconds.", this);
                Debug.LogException(ex, this);
            }
        }
        if (m_definitionRefreshCooldown > 0f)
            m_definitionRefreshCooldown -= Time.deltaTime;

        if (AutoRefreshDefinitions &&
            CanRefreshDefinitions() &&
            m_definitionRefreshCooldown <= 0f &&
            Time.unscaledTime >= m_nextAutoDefinitionRefreshTime)
        {
            m_nextAutoDefinitionRefreshTime = Time.unscaledTime + Mathf.Max(DefinitionRefreshInterval, 1f);
            RequestDefinitionRefresh();
        }
    }

    /// <summary>
    /// Returns the first <see cref="OptitrackStreamingClient"/> component located in the scene.
    /// Provides a convenient, sensible default in the common case where only a single client exists.
    /// Issues a warning if more than one such component is found.
    /// </summary>
    /// <returns>An arbitrary OptitrackClient from the scene, or null if none are found.</returns>
    public static OptitrackStreamingClient FindDefaultClient()
    {
        OptitrackStreamingClient[] allClients = FindObjectsByType<OptitrackStreamingClient>(FindObjectsSortMode.None);

        if ( allClients.Length == 0 )
        {
            Debug.LogError( "Unable to locate any " + typeof( OptitrackStreamingClient ).FullName + " components." );
            return null;
        }
        else if ( allClients.Length > 1 )
        {
            Debug.LogWarning( "Multiple " + typeof( OptitrackStreamingClient ).FullName + " components found in scene; defaulting to first available." );
        }

        return allClients[0];
    }

    /// <summary>
    /// Sends a message to Motive to start recording
    /// </summary>
    /// <returns>A boolean indicating if message was successful.</returns>
    public bool StartRecording()
    {
        if(m_client != null)
        {
            bool result = m_client.RequestCommand("StartRecording");
            if (result) m_isRecording = true;
            return result;
        }
        return false;
    }

    /// <summary>
    /// Sends a message to Motive to stop recording
    /// </summary>
    /// <returns>A boolean indicating if message was successful.</returns>
    public bool StopRecording()
    {
        if (m_client != null)
        {
            bool result = m_client.RequestCommand("StopRecording");
            if (result) m_isRecording = false;
            return result;
        }
        return false;
    }

    /// <summary>
    /// Reconnects to the OptiTrack streaming server by disconnecting and reconnecting.
    /// </summary>
    public void Reconnect()
    {
        if (m_isReconnecting)
        {
            Debug.LogWarning(GetType().FullName + ": Reconnect is already in progress.", this);
            return;
        }

        Debug.Log("OptiTrack: full reconnect requested.");

        OnDisable();

        StartCoroutine(ReconnectCoroutine());
    }

    /// <summary>
    /// Coroutine for handling the reconnection process with a retry loop.
    /// Attempts up to 5 times using the configurable full reconnect timing values.
    /// Last known pose is preserved during reconnect (m_latestSkeletonStates not cleared).
    /// </summary>
    private System.Collections.IEnumerator ReconnectCoroutine()
    {
        m_isReconnecting = true;
        const int maxAttempts = 5;

        for (int attempt = 1; attempt <= maxAttempts; attempt++)
        {
            m_receivedFrameSinceConnect = false;
            float delay = (attempt == 1)
                ? Mathf.Max(FullReconnectInitialDelaySeconds, 0f)
                : Mathf.Max(FullReconnectRetryDelaySeconds, 0.5f);
            Debug.Log(string.Format("{0}: reconnect attempt {1}/{2} in {3} seconds...", GetType().FullName, attempt, maxAttempts, delay), this);
            if (delay > 0f)
                yield return new WaitForSeconds(delay);

            yield return StartCoroutine(ConnectCoroutine());

            float deadline = Time.realtimeSinceStartup + Mathf.Max(FullReconnectFrameWaitSeconds, 0.25f);
            while (!m_receivedFrameSinceConnect && Time.realtimeSinceStartup < deadline)
                yield return null;

            if (m_receivedFrameSinceConnect)
            {
                Debug.Log(GetType().FullName + ": reconnect succeeded.", this);
                if (RecordOnPlay)
                    StartRecording();
                m_isReconnecting = false;
                yield break;
            }

            Debug.LogWarning(string.Format("{0}: reconnect attempt {1}/{2} failed; no streaming frames received.", GetType().FullName, attempt, maxAttempts), this);

            if (m_connectionHealthCoroutine != null)
            {
                StopCoroutine(m_connectionHealthCoroutine);
                m_connectionHealthCoroutine = null;
            }
            if (m_client != null || m_directNatNetConnected)
                CleanupClient();
        }

        Debug.LogError(string.Format("{0}: all {1} reconnect attempts failed. Reconnect manually from the Inspector.", GetType().FullName, maxAttempts), this);
        m_isReconnecting = false;
    }

    /// <summary>
    /// Gets the current connection status of the client.
    /// </summary>
    /// <returns>True if connected and receiving data, false otherwise.</returns>
    public bool IsConnected()
    {
        return ((m_client != null || m_directNatNetConnected) && m_receivedFrameSinceConnect) || IsReplayFrameFresh();
    }

    /// <summary>
    /// Gets the connection status as a readable string.
    /// </summary>
    /// <returns>Connection status description.</returns>
    public string GetConnectionStatus()
    {
        if (IsReplayFrameFresh())
        {
            return "Replay active (MMRP)";
        }

        if (m_client == null && !m_directNatNetConnected)
        {
            return "Disconnected";
        }
        else if (!m_receivedFrameSinceConnect)
        {
            return "Connected, waiting for data";
        }
        else
        {
            float lastFrameAge = m_lastFrameDeliveryTimestamp.AgeSeconds;
            if (lastFrameAge < 5.0f)
            {
                return "Connected, receiving data";
            }
            else
            {
                return "Connected, data interrupted";
            }
        }
    }

    #region Recording Control Functions
    /// <summary>
    /// Starts recording with enhanced logging.
    /// </summary>
    public void StartRecordingWithLog()
    {
        bool result = StartRecording();
        if (result)
        {
            Debug.Log("OptiTrack: recording started.");
        }
        else
        {
            Debug.LogWarning("OptiTrack: failed to start recording.");
        }
    }

    /// <summary>
    /// Stops recording with enhanced logging.
    /// </summary>
    public void StopRecordingWithLog()
    {
        bool result = StopRecording();
        if (result)
        {
            Debug.Log("OptiTrack: recording stopped.");
        }
        else
        {
            Debug.LogWarning("OptiTrack: failed to stop recording.");
        }
    }

    /// <summary>
    /// Toggles recording state on this client.
    /// </summary>
    public void ToggleRecording()
    {
        if (m_client != null)
        {
            if (m_isRecording)
            {
                if (StopRecording())
                    Debug.Log("OptiTrack: recording stopped.");
                else
                    Debug.LogWarning("OptiTrack: failed to stop recording.");
            }
            else
            {
                if (StartRecording())
                    Debug.Log("OptiTrack: recording started.");
                else
                    Debug.LogWarning("OptiTrack: failed to start recording.");
            }
        }
        else
        {
            Debug.LogError("OptiTrack: client is not connected.");
        }
    }

    public void ToggleDrawMarkers()
    {
        DrawMarkers = !DrawMarkers;
        Debug.Log($"OptiTrack: DrawMarkers = {DrawMarkers}");
    }
    #endregion

    /// <summary>Get the most recently received state for the specified rigid body.</summary>
    /// <param name="rigidBodyId">Corresponds to the "User ID" field in Motive.</param>
    /// <returns>The most recent available state, or null if none available.</returns>
    public OptitrackRigidBodyState GetLatestRigidBodyState( Int32 rigidBodyId, bool networkCompensation = true )
    {
        OptitrackRigidBodyState rbState;

        if ( ! networkCompensation || m_client == null )
        {
            lock ( m_frameDataUpdateLock )
            {
                m_latestRigidBodyStates.TryGetValue( rigidBodyId, out rbState );
            }
        }
        else
        {
            sRigidBodyData rbData;
            m_client.GetPredictedRigidBodyPose( rigidBodyId, out rbData, 0.0 );

            rbState = new OptitrackRigidBodyState();
            RigidBodyDataToState( rbData, OptitrackHiResTimer.Now(), rbState );
        }

        if ( MirrorMode && rbState != null )
        {
            rbState = new OptitrackRigidBodyState
            {
                DeliveryTimestamp = rbState.DeliveryTimestamp,
                IsTracked = rbState.IsTracked,
                Pose = new OptitrackPose
                {
                    Position    = MirrorPosition( rbState.Pose.Position ),
                    Orientation = MirrorOrientation( rbState.Pose.Orientation ),
                }
            };
        }

        return rbState;
    }

    /// <summary>Get the most recently received state for the specified skeleton.</summary>
    /// <param name="skeletonId">
    /// Taken from the corresponding <see cref="OptitrackSkeletonDefinition.Id"/> field.
    /// To find the appropriate skeleton definition, use <see cref="GetSkeletonDefinitionByName"/>.
    /// </param>
    /// <returns>The most recent available state, or null if none available.</returns>
    public OptitrackSkeletonState GetLatestSkeletonState( Int32 skeletonId )
    {
        OptitrackSkeletonState skelState;

        lock ( m_frameDataUpdateLock )
        {
            m_latestSkeletonStates.TryGetValue( skeletonId, out skelState );
        }

        return skelState;
    }

    /// <summary>
    /// </summary>
    /// <returns>True when the requested data exists; otherwise false.</returns>
    public bool FillBoneSnapshot( Int32 skeletonId,
        Dictionary<Int32, Vector3> posOut,
        Dictionary<Int32, Quaternion> oriOut,
        out OptitrackHiResTimer.Timestamp deliveryTimestamp )
    {
        deliveryTimestamp = default( OptitrackHiResTimer.Timestamp );
        lock ( m_frameDataUpdateLock )
        {
            OptitrackSkeletonState state;
            if ( !m_latestSkeletonStates.TryGetValue( skeletonId, out state ) || state == null )
                return false;

            deliveryTimestamp = state.DeliveryTimestamp;
            posOut.Clear();
            oriOut.Clear();

            if ( MirrorMode )
            {
                Dictionary<Int32, Int32> mirrorMap = GetOrBuildMirrorBoneIdMap( skeletonId );
                foreach ( var kvp in state.BonePoses )
                {
                    Int32 targetId = mirrorMap != null && mirrorMap.TryGetValue( kvp.Key, out Int32 mid ) ? mid : kvp.Key;

                    if ( targetId == kvp.Key )
                    {
                        posOut[kvp.Key] = MirrorPosition( kvp.Value.Position );
                        oriOut[kvp.Key] = MirrorOrientation( kvp.Value.Orientation );
                    }
                    else
                    {
                        posOut[kvp.Key]  = kvp.Value.Position;
                        oriOut[targetId] = MirrorOrientation( kvp.Value.Orientation );
                    }
                }
            }
            else
            {
                foreach ( var kvp in state.BonePoses )
                {
                    posOut[kvp.Key] = kvp.Value.Position;
                    oriOut[kvp.Key] = kvp.Value.Orientation;
                }
            }
            return true;
        }
    }

    /// <summary>
    /// Returns the left/right counterpart for a Motive bone name, or null for center bones.
    /// For example, LUArm maps to RUArm, RShin maps to LShin, and Hip maps to null.
    /// </summary>
    private static string GetMirrorBoneName( string name )
    {
        if ( string.IsNullOrEmpty( name ) || name.Length < 2 ) return null;
        if ( name[0] == 'L' && char.IsUpper( name[1] ) ) return "R" + name.Substring( 1 );
        if ( name[0] == 'R' && char.IsUpper( name[1] ) ) return "L" + name.Substring( 1 );
        return null;
    }

    /// <summary>
    /// </summary>
    private Dictionary<Int32, Int32> GetOrBuildMirrorBoneIdMap( Int32 skeletonId )
    {
        if ( m_mirrorBoneIdMaps.TryGetValue( skeletonId, out var cached ) )
            return cached;

        OptitrackSkeletonDefinition skelDef = GetSkeletonDefinitionById( skeletonId );
        if ( skelDef == null ) return null;

        var nameToId = new Dictionary<string, Int32>( skelDef.Bones.Count );
        foreach ( var bone in skelDef.Bones )
            nameToId[bone.Name] = bone.Id;

        var map = new Dictionary<Int32, Int32>( skelDef.Bones.Count );
        foreach ( var bone in skelDef.Bones )
        {
            string fullName  = bone.Name;
            int    sep       = fullName.IndexOf( '_' );
            string prefix    = sep >= 0 ? fullName.Substring( 0, sep + 1 ) : "";   // "Skeleton1_"
            string shortName = sep >= 0 ? fullName.Substring( sep + 1 )    : fullName; // "LUArm"

            string mirrorShort = GetMirrorBoneName( shortName );
            string mirrorFull  = mirrorShort != null ? prefix + mirrorShort : null;

            if ( mirrorFull != null && nameToId.TryGetValue( mirrorFull, out Int32 mirrorId ) )
                map[bone.Id] = mirrorId;
            else
                map[bone.Id] = bone.Id;
        }

        m_mirrorBoneIdMaps[skeletonId] = map;
        return map;
    }

    /// <summary>Mirror helper for YZ-plane coordinate conversion.</summary>
    private static Quaternion MirrorOrientation( Quaternion q )
        => new Quaternion( q.x, -q.y, -q.z, q.w );

    /// <summary>Mirror helper for YZ-plane coordinate conversion.</summary>
    private static Vector3 MirrorPosition( Vector3 pos )
        => new Vector3( -pos.x, pos.y, pos.z );

    /// <summary>Get the most recently received state for the specified trained markerset.</summary>
    /// <param name="tmarkersetId">
    /// Taken from the corresponding <see cref="OptitrackTMarkersetDefinition.Id"/> field.
    /// To find the appropriate skeleton definition, use <see cref="GetTMarkersetDefinitionByName"/>.
    /// </param>
    /// <returns>The most recent available state, or null if none available.</returns>
    public OptitrackTMarkersetState GetLatestTMarkersetState(Int32 tmarkersetId)
    {
        lock (m_frameDataUpdateLock)
        {
            if (!m_latestTMarkersetStates.TryGetValue(tmarkersetId, out var source) || source == null)
                return null;

            var snapshot = new OptitrackTMarkersetState
            {
                BonePoses = CopyPoseDictionary(source.BonePoses, MirrorMode),
                LocalBonePoses = CopyPoseDictionary(source.LocalBonePoses, MirrorMode),
            };
            return snapshot;
        }
    }

    private static Dictionary<Int32, OptitrackPose> CopyPoseDictionary(
        Dictionary<Int32, OptitrackPose> source, bool mirror)
    {
        var snapshot = new Dictionary<Int32, OptitrackPose>(source != null ? source.Count : 0);
        if (source == null)
            return snapshot;

        foreach (var kvp in source)
        {
            snapshot[kvp.Key] = new OptitrackPose
            {
                Position = mirror ? MirrorPosition(kvp.Value.Position) : kvp.Value.Position,
                Orientation = mirror ? MirrorOrientation(kvp.Value.Orientation) : kvp.Value.Orientation,
            };
        }
        return snapshot;
    }

    /// <summary>
    /// Copies trained markerset poses while holding the NatNet frame lock.
    /// </summary>
    public bool FillTMarkersetSnapshot(Int32 tmarkersetId, bool useLocalBonePoses,
        Dictionary<Int32, Vector3> posOut, Dictionary<Int32, Quaternion> oriOut)
    {
        lock (m_frameDataUpdateLock)
        {
            if (!m_latestTMarkersetStates.TryGetValue(tmarkersetId, out var state) || state == null)
                return false;

            var source = useLocalBonePoses ? state.LocalBonePoses : state.BonePoses;
            if (source == null)
                return false;

            posOut.Clear();
            oriOut.Clear();
            foreach (var kvp in source)
            {
                posOut[kvp.Key] = MirrorMode ? MirrorPosition(kvp.Value.Position) : kvp.Value.Position;
                oriOut[kvp.Key] = MirrorMode ? MirrorOrientation(kvp.Value.Orientation) : kvp.Value.Orientation;
            }
            return true;
        }
    }

    /// <summary>Get the most recently received state for streamed markers.</summary>
    /// <returns>The most recent available marker states, or null if none available.</returns>
    public List<OptitrackMarkerState> GetLatestMarkerStates()
    {
        List<OptitrackMarkerState> markerStates = new List<OptitrackMarkerState>();

        lock (m_frameDataUpdateLock)
        {
            foreach (KeyValuePair<Int32, OptitrackMarkerState> markerEntry in m_latestMarkerStates)
            {
                OptitrackMarkerState newMarkerState = new OptitrackMarkerState
                {
                    Position = MirrorMode ? MirrorPosition( markerEntry.Value.Position ) : markerEntry.Value.Position,
                    Labeled = markerEntry.Value.Labeled,
                    Size = markerEntry.Value.Size,
                    Id = markerEntry.Value.Id
                };
                markerStates.Add( newMarkerState );
            }
        }

        return markerStates;
    }

    /// <summary>Retrieves the definition of the rigid body with the specified streaming ID.</summary>
    /// <param name="rigidBodyId"></param>
    /// <returns>The specified rigid body definition, or null if not found.</returns>
    public OptitrackRigidBodyDefinition GetRigidBodyDefinitionById( Int32 rigidBodyId )
    {
        return m_rigidBodyDefinitions.Find( def => def.Id == rigidBodyId );
    }

    /// <summary>
    /// Gets the rigid body ID by its name.
    /// </summary>
    /// <param name="rigidBodyName">The name of the rigid body to find.</param>
    /// <returns>The ID of the rigid body if found, -1 otherwise.</returns>
    public int GetRigidBodyIdByName(string rigidBodyName)
    {
        var definition = m_rigidBodyDefinitions.Find(def => def.Name == rigidBodyName);
        return definition != null ? definition.Id : -1;
    }

    /// <summary>Retrieves the definition of the skeleton with the specified asset name.</summary>
    /// <param name="skeletonAssetName">The name of the skeleton for which to retrieve the definition.</param>
    /// <returns>The specified skeleton definition, or null if not found.</returns>
    public OptitrackSkeletonDefinition GetSkeletonDefinitionByName( string skeletonAssetName )
    {
        for ( int i = 0; i < m_skeletonDefinitions.Count; ++i )
        {
            OptitrackSkeletonDefinition skelDef = m_skeletonDefinitions[i];

            if (skelDef.IsSynthetic)
                continue;

            if ( skelDef.Name.Equals( skeletonAssetName, StringComparison.InvariantCultureIgnoreCase ) )
            {
                return skelDef;
            }
        }

        return null;
    }

    /// <summary>Retrieves the definition of the skeleton with the specified skeleton id.</summary>
    /// <param name="skeletonId">The id of the skeleton for which to retrieve the definition.</param>
    /// <returns>The specified skeleton definition, or null if not found.</returns>
    public OptitrackSkeletonDefinition GetSkeletonDefinitionById( Int32 skeletonId )
    {
        for (int i = 0; i < m_skeletonDefinitions.Count; ++i)
        {
            OptitrackSkeletonDefinition skelDef = m_skeletonDefinitions[i];

            if (skelDef.Id == skeletonId)
            {
                return skelDef;
            }
        }

        return null;
    }

    /// <summary>Retrieves the definition of the tmarkerset with the specified asset name.</summary>
    /// <param name="TMarkersetName">The name of the tmarkerset for which to retrieve the definition.</param>
    /// <returns>The specified tmarkerset definition, or null if not found.</returns> // trained markerset added
    public OptitrackTMarkersetDefinition GetTMarkersetDefinitionByName(string TMarkersetName)
    {
        for (int i = 0; i < m_tmarkersetDefinitions.Count; i++)
        {
            OptitrackTMarkersetDefinition tmarDef = m_tmarkersetDefinitions[i];

            if (tmarDef.Name.Equals(TMarkersetName, StringComparison.InvariantCultureIgnoreCase))
            {
                return tmarDef;
            }
        }

        return null;
    }

    /// <summary>Retrieves the definition of the tmarkerset with the specified tmarkerset id.</summary>
    /// <param name="tmarkersetId">The id of the tmarkerset for which to retrieve the definition.</param>
    /// <returns>The specified tmarkerset definition, or null if not found.</returns> // trained markerset added
    public OptitrackTMarkersetDefinition GetTMarkersetDefinitionById(Int32 tmarkersetId)
    {
        for (int i = 0; i < m_tmarkersetDefinitions.Count; ++i)
        {
            OptitrackTMarkersetDefinition tmarDef = m_tmarkersetDefinitions[i];

            if (tmarDef.Id == tmarkersetId)
            {
                return tmarDef;
            }
        }

        return null;
    }

    /// <summary>Get the most recently received state for streamed trained markerset markers.</summary>
    /// <returns>The most recent available trained markerset marker states, or null if none available.</returns>
    public List<OptitrackMarkerState> GetLatestTMarkMarkerStates() // trained markerset added
    {
        List<OptitrackMarkerState> tmarkmarkerStates = new List<OptitrackMarkerState>();

        lock (m_frameDataUpdateLock)
        {
            foreach (KeyValuePair<Int32, OptitrackMarkerState> markerEntry in m_latestTMarkMarkerStates)
            {
                OptitrackMarkerState newMarkerState = new OptitrackMarkerState
                {
                    Position = MirrorMode ? MirrorPosition( markerEntry.Value.Position ) : markerEntry.Value.Position,
                    Labeled = markerEntry.Value.Labeled,
                    Size = markerEntry.Value.Size,
                    Id = markerEntry.Value.Id
                };
                tmarkmarkerStates.Add(newMarkerState);
            }
        }

        return tmarkmarkerStates;
    }

    /// <summary>Request data descriptions from the host, then update our definitions.</summary>
    /// <exception cref="NatNetException">
    /// Thrown by <see cref="NatNetClient.GetDataDescriptions"/> if the request to the server fails.
    /// </exception>
    public void UpdateDefinitions()
    {
        // This may throw an exception if the server request times out or otherwise fails.
        UInt32 descriptionTypeMask = 0;
        descriptionTypeMask |= (1 << (int)NatNetDataDescriptionType.NatNetDataDescriptionType_RigidBody);
        descriptionTypeMask |= (1 << (int)NatNetDataDescriptionType.NatNetDataDescriptionType_Skeleton);
        descriptionTypeMask |= (1 << (int)NatNetDataDescriptionType.NatNetDataDescriptionType_Asset); // trained markerset added
        if (DrawMarkers)
        {
            descriptionTypeMask |= (1 << (int)NatNetDataDescriptionType.NatNetDataDescriptionType_MarkerSet);
        }
        if (DrawCameras)
        {
            descriptionTypeMask |= (1 << (int)NatNetDataDescriptionType.NatNetDataDescriptionType_Camera);
        }
        if( DrawForcePlates)
        {
            descriptionTypeMask |= (1 << (int)NatNetDataDescriptionType.NatNetDataDescriptionType_ForcePlate);
        }
        m_dataDescs = m_client.GetDataDescriptions(descriptionTypeMask);

        m_definitionRefreshFailureCount = 0;

        m_rigidBodyDefinitions.Clear();
        m_skeletonDefinitions.Clear();
        m_tmarkersetDefinitions.Clear();
        m_mirrorBoneIdMaps.Clear();

        lock (m_frameDataUpdateLock)
        {
            m_assetIdToNameCache.Clear();
        }

        // ----------------------------------
        // - Translate Rigid Body Definitions
        // ----------------------------------
        for ( int nativeRbDescIdx = 0; nativeRbDescIdx < m_dataDescs.RigidBodyDescriptions.Count; ++nativeRbDescIdx )
        {
            sRigidBodyDescription nativeRb = m_dataDescs.RigidBodyDescriptions[nativeRbDescIdx];

            OptitrackRigidBodyDefinition rbDef = new OptitrackRigidBodyDefinition {
                Id = nativeRb.Id,
                Name = nativeRb.Name,
                Markers = new List<OptitrackRigidBodyDefinition.MarkerDefinition>( nativeRb.MarkerCount ),
            };

            // Populate nested marker definitions.
            for ( int nativeMarkerIdx = 0; nativeMarkerIdx < nativeRb.MarkerCount; ++nativeMarkerIdx )
            {
                int positionOffset = nativeMarkerIdx * Marshal.SizeOf( typeof( MarkerDataVector ) );
                IntPtr positionPtr = new IntPtr( nativeRb.MarkerPositions.ToInt64() + positionOffset );

                int labelOffset = nativeMarkerIdx * Marshal.SizeOf( typeof( Int32 ) );
                IntPtr labelPtr = new IntPtr( nativeRb.MarkerRequiredLabels.ToInt64() + labelOffset );

                MarkerDataVector nativePos =
                    (MarkerDataVector)Marshal.PtrToStructure( positionPtr, typeof( MarkerDataVector ) );

                Int32 nativeLabel = Marshal.ReadInt32( labelPtr );

                OptitrackRigidBodyDefinition.MarkerDefinition markerDef =
                    new OptitrackRigidBodyDefinition.MarkerDefinition {
                        Position = new Vector3( -nativePos.Values[0], nativePos.Values[1], nativePos.Values[2] ),
                        RequiredLabel = nativeLabel,
                    };

                rbDef.Markers.Add( markerDef );
            }

            m_rigidBodyDefinitions.Add( rbDef );
        }

        // ----------------------------------
        // - Translate Skeleton Definitions
        // ----------------------------------
        for ( int nativeSkelDescIdx = 0; nativeSkelDescIdx < m_dataDescs.SkeletonDescriptions.Count; ++nativeSkelDescIdx )
        {
            sSkeletonDescription nativeSkel = m_dataDescs.SkeletonDescriptions[nativeSkelDescIdx];

            OptitrackSkeletonDefinition skelDef = new OptitrackSkeletonDefinition {
                Id = nativeSkel.Id,
                Name = nativeSkel.Name,
                Bones = new List<OptitrackSkeletonDefinition.BoneDefinition>(nativeSkel.RigidBodyCount),
                BoneIdToParentIdMap = new Dictionary<int, int>(),
            };

            // Populate nested bone definitions.
            for ( int nativeBoneIdx = 0; nativeBoneIdx < nativeSkel.RigidBodyCount; ++nativeBoneIdx )
            {
                sRigidBodyDescription nativeBone = nativeSkel.RigidBodies[nativeBoneIdx];

                OptitrackSkeletonDefinition.BoneDefinition boneDef =
                    new OptitrackSkeletonDefinition.BoneDefinition {
                        Id = nativeBone.Id,
                        ParentId = nativeBone.ParentId,
                        Name = nativeBone.Name,
                        Offset = new Vector3( -nativeBone.OffsetX, nativeBone.OffsetY, nativeBone.OffsetZ ),
                    };

                skelDef.Bones.Add( boneDef );
                skelDef.BoneIdToParentIdMap[boneDef.Id] = boneDef.ParentId;
            }

            m_skeletonDefinitions.Add( skelDef );
        }

        // --------------------------------------------------------------------
        // - Translate Trained Markerset Definitions // trained markerset added
        // --------------------------------------------------------------------
        for (int nativeTmarkDescIdx = 0; nativeTmarkDescIdx < m_dataDescs.AssetDescriptions.Count; ++nativeTmarkDescIdx)
        {
            sAssetDescription nativeTmark = m_dataDescs.AssetDescriptions[nativeTmarkDescIdx];
            // Debug.Log("#rbs: " + nativeTmark.RigidBodyCount); // correct

            OptitrackTMarkersetDefinition tmarkDef = new OptitrackTMarkersetDefinition
            {
                Id = nativeTmark.AssetID,
                Name = nativeTmark.Name,
                Bones = new List<OptitrackTMarkersetDefinition.BoneDefinition>(nativeTmark.RigidBodyCount),
                BoneIdToParentIdMap = new Dictionary<int, int>(),
                Markers = new List<OptitrackTMarkersetDefinition.MarkerDefinition>(nativeTmark.MarkerCount),
            };

            // Populate nested bone definitions.
            for (int nativeBoneIdx = 0; nativeBoneIdx < nativeTmark.RigidBodyCount; ++nativeBoneIdx)
            {
                sRigidBodyDescription nativeBone = nativeTmark.RigidBodies[nativeBoneIdx];

                OptitrackTMarkersetDefinition.BoneDefinition boneDef =
                    new OptitrackTMarkersetDefinition.BoneDefinition
                    {
                        Id = nativeBone.Id,
                        ParentId = nativeBone.ParentId,
                        Name = nativeBone.Name,
                        Offset = new Vector3(-nativeBone.OffsetX, nativeBone.OffsetY, nativeBone.OffsetZ),
                    };

                tmarkDef.Bones.Add(boneDef);
                tmarkDef.BoneIdToParentIdMap[boneDef.Id] = boneDef.ParentId;
            }

            // Populate nested marker definitions
            for (int nativeMarkerIdx = 0; nativeMarkerIdx < nativeTmark.MarkerCount; ++nativeMarkerIdx)
            {
                sMarkerDescription nativeMarker = nativeTmark.Markers[nativeMarkerIdx];
                //Debug.Log("TMarkerset (X, Y, Z): " + nativeMarker.X + " " + nativeMarker.Y + " " + nativeMarker.Z);
                //Debug.Log(nativeMarker.Id + " " + nativeMarker.Name);

                OptitrackTMarkersetDefinition.MarkerDefinition markerDef =
                    new OptitrackTMarkersetDefinition.MarkerDefinition
                    {
                        Name = nativeMarker.Name,
                        Id = nativeMarker.Id,
                        Position = new Vector3(-nativeMarker.X, nativeMarker.Y, nativeMarker.Z),
                    };
                tmarkDef.Markers.Add(markerDef);

            }

            //foreach (KeyValuePair<int, int> kvp in tmarkDef.BoneIdToParentIdMap)
            //{
            //    Debug.Log("Key: " + kvp.Key + "Value: " + kvp.Value);
            //} // correct

            m_tmarkersetDefinitions.Add(tmarkDef);
        }

        // ----------------------------------
        // - Get Marker Definitions (ToDo)
        // ----------------------------------
        //for (int markersetNumber = 0; markersetNumber < m_dataDescs.MarkerSetDescriptions.Count; ++markersetNumber)
        //{ 
        //    sMarkerSetDescription markerDescription = m_dataDescs.MarkerSetDescriptions[markersetNumber];

        //    if(markerDescription.Name == "all")
        //    {
        //        Int32 nMarkers = markerDescription.MarkerCount;

        //        for( int i = 0; i < nMarkers; ++i)
        //        {
        //            int nameOffset = i * NatNetConstants.MaxNameLength; //Marshal.SizeOf(typeof(Char**));
        //            IntPtr namePtr = new IntPtr(markerDescription.MarkerNames.ToInt64() + nameOffset);

        //            // FIXME: Need to de-construct the char array of names to use for marker naming later. 
        //            // This throws an exception, thus doesn't work. 
        //            // MarkerNames is a char** of size [MarkerCount][MaxNameLength]
        //            //string nativeLabel = Marshal.PtrToStringAnsi(namePtr);
        //        }

        //        OptitrackMarkersDefinition markersDef = new OptitrackMarkersDefinition
        //        {
        //            Name = markerDescription.Name

        //        };

        //        m_markersDefinitions.Add(markersDef);
        //    }
        //}

        // ----------------------------------
        // - Camera Definitions
        // ----------------------------------
        for (int cameraIndex = 0; cameraIndex < m_dataDescs.CameraDescriptions.Count; ++cameraIndex)
        {
            sCameraDescription camera = m_dataDescs.CameraDescriptions[cameraIndex];

            OptitrackCameraDefinition cameraDef = new OptitrackCameraDefinition
            {
                Name = camera.Name,
                //Unity and motive use opposite positive-x directions, so positionX needs to be set to negative
                Position = new Vector3(-camera.PositionX, camera.PositionY, camera.PositionZ),
                Orientation = new Quaternion(camera.RotationX, camera.RotationY, camera.RotationZ, camera.RotationW),
            };

            m_cameraDefinitions.Add(cameraDef);
        }

        // ----------------------------------
        // - Force Plate Definitions
        // ----------------------------------
        for (int plateIndex = 0; plateIndex < m_dataDescs.ForcePlateDescriptions.Count; ++plateIndex)
        {
            sForcePlateDescription plate = m_dataDescs.ForcePlateDescriptions[plateIndex];

            OptitrackForcePlateDefinition forcePlateDef = new OptitrackForcePlateDefinition
            {
                Id = plate.Id,
                SerialNumber = plate.SerialNo,
                Width = plate.Width, // in inches
                Length = plate.Length, // in inches
                ElectricalOffset = new Vector3(plate.OriginX, plate.OriginY, plate.OriginZ),
                CalibrationMatrix = new List<float>(12 * 12),
                Corners = new List<float>(4 * 3),
                PlateType = plate.PlateType,
                ChannelDataType = plate.ChannelDataType,
                ChannelCount = plate.ChannelCount,
                ChannelNames = new List<string>(plate.ChannelCount),
            };

            // Populate corner locations
            for( int i = 0; i < 12; ++i )
            {
                forcePlateDef.Corners.Add( plate.Corners[i] );
            }

            //Populate Channel Names
            for (int i = 0; i < forcePlateDef.ChannelCount; ++i)
            {
                forcePlateDef.ChannelNames.Add(plate.ChannelNames[i]);
            }

            m_forcePlateDefinitions.Add(forcePlateDef);
        }

        PruneStatesMissingFromDefinitions();
        NotifyRegisteredSkeletonDefinitionsChanged();

    }

    private void PruneStatesMissingFromDefinitions()
    {
        var validRigidBodyIds = new HashSet<Int32>();
        foreach (var def in m_rigidBodyDefinitions)
            validRigidBodyIds.Add(def.Id);

        var validSkeletonIds = new HashSet<Int32>();
        foreach (var def in m_skeletonDefinitions)
            validSkeletonIds.Add(def.Id);

        var validTMarkersetIds = new HashSet<Int32>();
        foreach (var def in m_tmarkersetDefinitions)
            validTMarkersetIds.Add(def.Id);

        lock (m_frameDataUpdateLock)
        {
            RemoveMissingKeys(m_latestRigidBodyStates, validRigidBodyIds);
            RemoveMissingKeys(m_latestSkeletonStates, validSkeletonIds);
            RemoveMissingKeys(m_skeletonFrameScratch, validSkeletonIds);
            RemoveMissingKeys(m_latestTMarkersetStates, validTMarkersetIds);
        }
    }

    private static void RemoveMissingKeys<T>(Dictionary<Int32, T> dictionary, HashSet<Int32> validKeys)
    {
        if (dictionary.Count == 0)
            return;

        var staleKeys = new List<Int32>();
        foreach (var kvp in dictionary)
        {
            if (!validKeys.Contains(kvp.Key))
                staleKeys.Add(kvp.Key);
        }

        foreach (var key in staleKeys)
            dictionary.Remove(key);
    }

    private void ResubscribeRegisteredAssets()
    {
        if (m_client == null || ConnectionType != ClientConnectionType.Unicast)
            return;

        m_doneSubscriptionNotice = false;
        ResetStreamingSubscriptions();

        foreach (KeyValuePair<Int32, MonoBehaviour> rb in m_rigidBodies)
            SubscribeRigidBody(rb.Value, rb.Key);
        foreach (KeyValuePair<string, MonoBehaviour> skel in m_skeletons)
            SubscribeSkeleton(skel.Value, skel.Key);
        foreach (KeyValuePair<string, MonoBehaviour> tmark in m_tmarkersets)
            SubscribeTMarkerset(tmark.Value, tmark.Key);
        if (DrawTMarkersetMarkers)
            SubscribeTMarkMarkers();
        if (DrawMarkers)
            SubscribeMarkers();
    }

    private void NotifyRegisteredSkeletonDefinitionsChanged()
    {
        if (m_registeredSkeletonComponents.Count == 0)
            return;

        var registeredSkeletons = new List<MonoBehaviour>(m_registeredSkeletonComponents);
        foreach (MonoBehaviour component in registeredSkeletons)
        {
            if (component == null)
            {
                m_registeredSkeletonComponents.Remove(component);
                continue;
            }

            if (component is OptitrackSkeletonAnimator_Mingle animator && animator.isActiveAndEnabled)
                animator.RefreshSkeletonDefinitionBinding();
        }
    }

    public void RegisterRigidBody( MonoBehaviour component, Int32 rigidBodyId )
    {
        if ( m_rigidBodies.TryGetValue( rigidBodyId, out MonoBehaviour existingComponent ) )
        {
            if (existingComponent == component)
            {
                SubscribeRigidBody(component, rigidBodyId);
                return;
            }

#if false
            MonoBehaviour existingRb = m_rigidBodies[rigidBodyId];
            Debug.LogError( GetType().FullName + ": " + rb.GetType().FullName + " has duplicate rigid body ID " + rigidBodyId, component );
            Debug.LogError( GetType().FullName + ": (Existing " + existingRb.GetType().FullName + " was already registered with that ID)", existingRb );
            rb.enabled = false;
#endif
        }

        m_rigidBodies[rigidBodyId] = component;

        SubscribeRigidBody(component, rigidBodyId);
    }

    public void UnregisterRigidBody( MonoBehaviour component, Int32 rigidBodyId )
    {
        if (m_rigidBodies.TryGetValue(rigidBodyId, out MonoBehaviour existingComponent) &&
            existingComponent == component)
        {
            m_rigidBodies.Remove(rigidBodyId);
        }
    }

    public void RegisterSkeleton(MonoBehaviour component, string name)
    {
        m_registeredSkeletonComponents.Add(component);

        if (m_skeletons.TryGetValue(name, out MonoBehaviour existingComponent))
        {
            if (existingComponent == component)
            {
                SubscribeSkeleton(component, name);
                return;
            }

#if false
            MonoBehaviour existingSkel = m_skeletons[rigidBodyId];
            Debug.LogError( "Duplicate skeleton detected, " + GetType().FullName + ": (Existing " + existingRb.GetType().FullName + " was already registered with that ID)", existingRb );
#endif
        }

        m_skeletons[name] = component;

        SubscribeSkeleton(component, name);
    }

    public void RegisterTMarkerset(MonoBehaviour component, string name) // trained markerset added
    {
        if (m_tmarkersets.TryGetValue(name, out MonoBehaviour existingComponent))
        {
            if (existingComponent == component)
            {
                SubscribeTMarkerset(component, name);
                return;
            }
        }

        m_tmarkersets[name] = component;

        SubscribeTMarkerset(component, name);
    }

    /// <summary>
    /// (Re)initializes <see cref="m_client"/> and connects to the configured streaming server.
    /// </summary>
    void OnEnable()
    {
        m_receivedFrameSinceConnect = false;
        if (AutoReconnect)
            StartCoroutine( InitialConnectWithRetry() );
        else
            StartCoroutine( ConnectCoroutine() );

        if (EnableReplayPriority && m_replayConnectCoroutine == null && ConnectionType != ClientConnectionType.Multicast)
            m_replayConnectCoroutine = StartCoroutine( ReplayConnectLoop() );
    }

    /// <summary>
    /// </summary>
    private System.Collections.IEnumerator InitialConnectWithRetry()
    {
        const int maxAttempts = 10;
        float[] retryDelays = { 1f, 2f, 3f, 5f, 10f };

        for (int attempt = 1; attempt <= maxAttempts; attempt++)
        {
            yield return StartCoroutine(ConnectCoroutine());

            bool transportConnected = m_client != null || m_directNatNetConnected;
            if (transportConnected)
            {
                if (m_directNatNetConnected && m_client == null)
                {
                    if (attempt > 1)
                        Debug.Log(string.Format("{0}: initial direct NatNet receiver started (attempt {1}/{2}).", GetType().FullName, attempt, maxAttempts), this);
                    yield break;
                }

                float deadline = Time.realtimeSinceStartup + 5.0f;
                while (!m_receivedFrameSinceConnect && Time.realtimeSinceStartup < deadline)
                    yield return null;

                if (m_receivedFrameSinceConnect)
                {
                    if (attempt > 1)
                        Debug.Log(string.Format("{0}: initial connection succeeded (attempt {1}/{2}).", GetType().FullName, attempt, maxAttempts), this);
                    yield break;
                }

                Debug.LogWarning(string.Format("{0}: initial connection attempt {1}/{2} connected to {3} but received no frames for 5 seconds. ServerNatNet={4}, ClientNatNet={5}, Local={6}, Connection={7}. Check Motive streaming mode, multicast interface, or NatNet SDK compatibility.", GetType().FullName, attempt, maxAttempts, ServerAddress, ServerNatNetVersion, ClientNatNetVersion, ResolvedLocalAddress, ConnectionType), this);
                CleanupClient();
            }

            if (attempt == maxAttempts)
            {
                Debug.LogError(string.Format("{0}: all {1} initial connection attempts failed. Reconnect manually from the Inspector.", GetType().FullName, maxAttempts), this);
                yield break;
            }

            float delay = retryDelays[Mathf.Min(attempt - 1, retryDelays.Length - 1)];
            Debug.Log(string.Format("{0}: Initial connection attempt {1}/{2} failed; retrying in {3} seconds.", GetType().FullName, attempt, maxAttempts, delay), this);
            yield return new WaitForSeconds(delay);
        }
    }

    /// <summary>
    /// </summary>
    private void CleanupClient()
    {
        StopDirectFrameReceiver();
        m_directNatNetConnected = false;
        if (m_connectionHealthCoroutine != null)
        {
            StopCoroutine(m_connectionHealthCoroutine);
            m_connectionHealthCoroutine = null;
        }
        if (m_client != null)
        {
            try { m_client.NativeFrameReceived -= OnNatNetFrameReceived; } catch (Exception) { }
            try { m_client.Disconnect(); } catch (Exception) { }
            try { m_client.Dispose(); } catch (Exception) { }
            m_client = null;
        }
    }


    private System.Collections.IEnumerator ReplayConnectLoop()
    {
        var retryDelay = new WaitForSeconds(2.0f);
        while (EnableReplayPriority)
        {
            if (m_replayClient == null && !string.IsNullOrWhiteSpace(ReplayServerAddress))
                yield return StartCoroutine(ConnectReplayCoroutine());
            else if (m_replayClient != null &&
                     m_replayReceivedFrameSinceConnect &&
                     m_lastReplayFrameDeliveryTimestamp.AgeSeconds > 5.0f)
            {
                CleanupReplayClient();
            }
            yield return retryDelay;
        }
        CleanupReplayClient();
        m_replayConnectCoroutine = null;
    }

    private System.Collections.IEnumerator ConnectReplayCoroutine()
    {
        IPAddress serverAddr;
        IPAddress localAddr;
        try
        {
            serverAddr = IPAddress.Parse(ReplayServerAddress.Trim());
            localAddr = ResolveLocalAddress(serverAddr);
        }
        catch (Exception ex)
        {
            Debug.LogException(ex, this);
            yield break;
        }

        NatNetClient connectedClient = null;
        Exception connectError = null;
        System.Threading.Tasks.Task connectTask = System.Threading.Tasks.Task.Run(() =>
        {
            NatNetClient c = null;
            try
            {
                c = new NatNetClient();
                c.Connect(
                    NatNetConnectionType.NatNetConnectionType_Multicast,
                    localAddr,
                    serverAddr,
                    k_DefaultNatNetCommandPort,
                    k_DefaultNatNetDataPort,
                    IPAddress.Parse("239.255.42.100"));
                connectedClient = c;
            }
            catch (Exception ex)
            {
                connectError = ex;
                if (c != null) { try { c.Dispose(); } catch { } }
            }
        });

        while (!connectTask.IsCompleted)
            yield return null;

        if (connectError != null || connectedClient == null)
        {
            if (!m_replayConnectWarned)
            {
                Debug.LogWarning(GetType().FullName + ": Replay NatNet is not available yet. Server=" + ReplayServerAddress + ", local=" + localAddr + ", command=1510, data=1511, multicast=239.255.42.100. This is normal while MMRP is not replaying.", this);
                m_replayConnectWarned = true;
            }
            yield break;
        }

        CleanupReplayClient();
        m_replayClient = connectedClient;
        m_replayReceivedFrameSinceConnect = false;
        m_replayConnectWarned = false;
        m_replayClient.NativeFrameReceived += OnNatNetFrameReceived;
        Debug.Log(GetType().FullName + ": Replay NatNet connected to " + ReplayServerAddress + " (239.255.42.100).", this);
    }

    private void CleanupReplayClient()
    {
        if (m_replayClient != null)
        {
            try { m_replayClient.NativeFrameReceived -= OnNatNetFrameReceived; } catch { }
            try { m_replayClient.Disconnect(); } catch { }
            try { m_replayClient.Dispose(); } catch { }
            m_replayClient = null;
        }
        m_replayReceivedFrameSinceConnect = false;
    }

    private bool IsReplayFrameFresh()
    {
        return EnableReplayPriority &&
               m_replayReceivedFrameSinceConnect &&
               m_lastReplayFrameDeliveryTimestamp.AgeSeconds <= ReplayFreshnessSeconds;
    }

    private bool HasRecentStreamingFrame(float thresholdSeconds)
    {
        OptitrackHiResTimer.Timestamp liveTimestamp;
        liveTimestamp.m_ticks = Interlocked.Read(ref m_lastFrameDeliveryTimestamp.m_ticks);
        bool liveFresh = m_receivedFrameSinceConnect && liveTimestamp.AgeSeconds < thresholdSeconds;
        bool replayFresh = IsReplayFrameFresh();
        return liveFresh || replayFresh;
    }

    private bool StartDirectFrameReceiver(string localAddress, string multicastAddress, UInt16 dataPort)
    {
        StopDirectFrameReceiver();

        try
        {
            IPAddress group = IPAddress.Parse(multicastAddress);
            IPAddress local = IPAddress.Parse(localAddress);

            m_directFrameUdp = new UdpClient(AddressFamily.InterNetwork);
            m_directFrameUdp.ExclusiveAddressUse = false;
            m_directFrameUdp.Client.ReceiveBufferSize = 1024 * 1024;
            m_directFrameUdp.Client.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true);
            m_directFrameUdp.Client.Bind(new IPEndPoint(IPAddress.Any, dataPort));
            m_directFrameUdp.Client.SetSocketOption(SocketOptionLevel.IP, SocketOptionName.AddMembership, new MulticastOption(group, local));
            if (EnableReplayPriority)
            {
                try
                {
                    IPAddress replayGroup = IPAddress.Parse("239.255.42.100");
                    IPAddress replayLocal = local;
                    if (!string.IsNullOrWhiteSpace(ReplayServerAddress))
                    {
                        IPAddress replayServer = IPAddress.Parse(ReplayServerAddress.Trim());
                        replayLocal = ResolveLocalAddress(replayServer);
                    }
                    if (!replayGroup.Equals(group) || !replayLocal.Equals(local))
                        m_directFrameUdp.Client.SetSocketOption(SocketOptionLevel.IP, SocketOptionName.AddMembership, new MulticastOption(replayGroup, replayLocal));
                }
                catch (Exception ex)
                {
                    Debug.LogWarning(GetType().FullName + ": direct NatNet replay multicast join failed: " + ex.Message, this);
                }
            }
            m_directFrameUdp.Client.ReceiveTimeout = 1000;

            m_directFrameRunning = true;
            m_directNatNetConnected = true;
            m_directFrameLogged = false;
            m_directTimeoutCount = 0;
            m_directTimeoutLogPending = false;
            m_directSocketErrorLogPending = false;
            m_directFrameThread = new Thread(DirectFrameThreadLoop)
            {
                IsBackground = true,
                Name = "OptitrackStreamingClient.DirectNatNet"
            };
            m_directFrameThread.Start();
            Debug.Log(GetType().FullName + ": direct NatNet frame receiver joined " + multicastAddress + ":" + dataPort + " on " + localAddress + (EnableReplayPriority ? " with replay priority group 239.255.42.100." : "."), this);
            return true;
        }
        catch (Exception ex)
        {
            StopDirectFrameReceiver();
            Debug.LogWarning(GetType().FullName + ": direct NatNet frame receiver failed to start: " + ex.Message, this);
            return false;
        }
    }

    private void StopDirectFrameReceiver()
    {
        m_directFrameRunning = false;
        m_directFrameLogPending = false;
        m_directTimeoutLogPending = false;
        m_directSocketErrorLogPending = false;
        m_directNatNetConnected = false;
        try { m_directFrameUdp?.Close(); } catch { }
        m_directFrameUdp = null;

        if (m_directFrameThread != null && m_directFrameThread.IsAlive)
            m_directFrameThread.Join(500);
        m_directFrameThread = null;
    }

    private void DirectFrameThreadLoop()
    {
        IPEndPoint ep = new IPEndPoint(IPAddress.Any, 0);
        while (m_directFrameRunning)
        {
            try
            {
                byte[] data = m_directFrameUdp.Receive(ref ep);
                m_directTimeoutCount = 0;
                ParseDirectFramePacket(data, ep.Address);
            }
            catch (SocketException se)
            {
                if (!m_directFrameRunning) break;
                if (se.SocketErrorCode == SocketError.TimedOut)
                {
                    int timeoutCount = Interlocked.Increment(ref m_directTimeoutCount);
                    if (timeoutCount == 5 || timeoutCount == 15)
                        m_directTimeoutLogPending = true;
                    continue;
                }
                m_directSocketErrorMessage = se.SocketErrorCode + " / " + se.Message;
                m_directSocketErrorLogPending = true;
            }
            catch (ObjectDisposedException)
            {
                break;
            }
            catch (Exception ex)
            {
                m_directSocketErrorMessage = ex.GetType().Name + " / " + ex.Message;
                m_directSocketErrorLogPending = true;
            }
        }
    }

    private void ParseDirectFramePacket(byte[] data, IPAddress sourceAddress)
    {
        if (data == null || data.Length < 12) return;
        if (BitConverter.ToUInt16(data, 0) != 7) return; // NAT_FRAMEOFDATA

        int o = 4;
        int frameNumber = BitConverter.ToInt32(data, o); o += 4;
        OptitrackHiResTimer.Timestamp frameTimestamp = OptitrackHiResTimer.Now();
        if (!m_directFrameLogged)
        {
            m_directFrameLogged = true;
            m_directFrameLogPending = true;
        }
        bool isReplayFrame = EnableReplayPriority &&
                             !string.IsNullOrWhiteSpace(ReplayServerAddress) &&
                             sourceAddress != null &&
                             sourceAddress.ToString() == ReplayServerAddress.Trim();

        if (!isReplayFrame && IsReplayFrameFresh())
            return;

        QueueDefinitionRefreshForFrameSource(isReplayFrame);

        if (isReplayFrame)
        {
            m_replayReceivedFrameSinceConnect = true;
            Interlocked.Exchange(ref m_lastReplayFrameDeliveryTimestamp.m_ticks, frameTimestamp.m_ticks);
            m_receivedFrameSinceConnect = true;
            Interlocked.Exchange(ref m_lastFrameDeliveryTimestamp.m_ticks, frameTimestamp.m_ticks);
        }
        else
        {
            m_receivedFrameSinceConnect = true;
            Interlocked.Exchange(ref m_lastFrameDeliveryTimestamp.m_ticks, frameTimestamp.m_ticks);
        }

        if (!Monitor.TryEnter(m_frameDataUpdateLock, 1))
            return;

        try
        {
            int rbCount, rbStart, rbEnd;
            if (!ReadDirectSection(data, ref o, out rbCount, out rbStart, out rbEnd)) return; // markerSets
            if (!ReadDirectSection(data, ref o, out rbCount, out rbStart, out rbEnd)) return; // unlabeled
            if (!ReadDirectSection(data, ref o, out rbCount, out rbStart, out rbEnd)) return; // rigidBodies
            ParseDirectRigidBodies(data, rbCount, rbStart, rbEnd, frameTimestamp, isReplayFrame);

            int skelCount, skelStart, skelEnd;
            if (!ReadDirectSection(data, ref o, out skelCount, out skelStart, out skelEnd)) return; // skeletons
            ParseDirectSkeletons(data, skelCount, skelStart, skelEnd, frameTimestamp, isReplayFrame);

            int skipCount, skipStart, skipEnd;
            if (!ReadDirectSection(data, ref o, out skipCount, out skipStart, out skipEnd)) return; // assets
            if (!ReadDirectSection(data, ref o, out skipCount, out skipStart, out skipEnd)) return; // labeled markers
            if (!ReadDirectSection(data, ref o, out skipCount, out skipStart, out skipEnd)) return; // force plates

            if (!ReadDirectSection(data, ref o, out skipCount, out skipStart, out skipEnd)) return; // devices

        }
        finally
        {
            Monitor.Exit(m_frameDataUpdateLock);
        }
    }

    private static bool ReadDirectSection(byte[] data, ref int o, out int count, out int start, out int end)
    {
        count = 0;
        start = end = o;
        if (o + 8 > data.Length) return false;
        count = BitConverter.ToInt32(data, o); o += 4;
        int size = BitConverter.ToInt32(data, o); o += 4;
        if (count < 0 || size < 0 || o + size > data.Length) return false;
        start = o;
        end = o + size;
        o = end;
        return true;
    }

    private static bool ReadDirectRigidBody(byte[] data, ref int o, int end, out sRigidBodyData rb)
    {
        rb = new sRigidBodyData();
        if (o + 38 > end || o + 38 > data.Length) return false;
        rb.Id = BitConverter.ToInt32(data, o); o += 4;
        rb.X = BitConverter.ToSingle(data, o); o += 4;
        rb.Y = BitConverter.ToSingle(data, o); o += 4;
        rb.Z = BitConverter.ToSingle(data, o); o += 4;
        rb.QX = BitConverter.ToSingle(data, o); o += 4;
        rb.QY = BitConverter.ToSingle(data, o); o += 4;
        rb.QZ = BitConverter.ToSingle(data, o); o += 4;
        rb.QW = BitConverter.ToSingle(data, o); o += 4;
        rb.MeanError = BitConverter.ToSingle(data, o); o += 4;
        rb.Params = BitConverter.ToInt16(data, o); o += 2;
        return true;
    }

    private void QueueStreamedTopologyDefinitionRefresh(bool preferReplayDefinitions)
    {
        if (!AutoRefreshDefinitions || SkipDataDescriptions)
            return;

        if (preferReplayDefinitions && EnableReplayPriority && !string.IsNullOrWhiteSpace(ReplayServerAddress))
        {
            if (m_pendingReplayDefinitionRefresh || m_forceReplayDefinitionRefreshSoon)
                return;

            m_pendingReplayDefinitionRefresh = true;
            m_forceReplayDefinitionRefreshSoon = true;
            return;
        }

        if (m_pendingDefinitionRefresh || m_forceDefinitionRefreshSoon)
            return;

        m_pendingDefinitionRefresh = true;
        m_forceDefinitionRefreshSoon = true;
    }

    private void QueueDefinitionRefreshForFrameSource(bool isReplayFrame)
    {
        if (isReplayFrame)
        {
            if (m_directDefinitionSource != DirectDefinitionSource.Replay)
                QueueStreamedTopologyDefinitionRefresh(true);
        }
        else if (m_directDefinitionSource == DirectDefinitionSource.Replay)
        {
            QueueStreamedTopologyDefinitionRefresh(false);
        }
    }

    private static bool UpdateFrameTopologyIds(HashSet<Int32> latestIds, HashSet<Int32> currentIds, ref bool hasSnapshot)
    {
        if (!hasSnapshot)
        {
            latestIds.Clear();
            foreach (Int32 id in currentIds)
                latestIds.Add(id);
            hasSnapshot = true;
            return false;
        }

        if (latestIds.SetEquals(currentIds))
            return false;

        latestIds.Clear();
        foreach (Int32 id in currentIds)
            latestIds.Add(id);
        return true;
    }

    private void ParseDirectRigidBodies(byte[] data, int count, int start, int end, OptitrackHiResTimer.Timestamp frameTimestamp, bool isReplayFrame)
    {
        int o = start;
        m_currentFrameRigidBodyIds.Clear();
        for (int i = 0; i < count; i++)
        {
            sRigidBodyData rbData;
            if (!ReadDirectRigidBody(data, ref o, end, out rbData)) return;
            m_currentFrameRigidBodyIds.Add(rbData.Id);
            OptitrackRigidBodyState rbState = GetOrCreateRigidBodyState(rbData.Id);
            RigidBodyDataToState(rbData, frameTimestamp, rbState);
        }

        if (UpdateFrameTopologyIds(m_latestFrameRigidBodyIds, m_currentFrameRigidBodyIds, ref m_hasFrameRigidBodyTopologySnapshot))
            QueueStreamedTopologyDefinitionRefresh(isReplayFrame);
    }

    private void ParseDirectSkeletons(byte[] data, int count, int start, int end, OptitrackHiResTimer.Timestamp frameTimestamp, bool isReplayFrame)
    {
        int o = start;
        m_currentFrameSkeletonIds.Clear();
        for (int i = 0; i < count; i++)
        {
            if (o + 8 > end) return;
            int skeletonId = BitConverter.ToInt32(data, o); o += 4;
            int boneCount = BitConverter.ToInt32(data, o); o += 4;
            if (boneCount < 0 || boneCount > 4096 || o + boneCount * 38 > end) return;
            m_currentFrameSkeletonIds.Add(skeletonId);

            sRigidBodyData[] stagedBones = GetSkeletonFrameScratch(skeletonId, boneCount);
            for (int b = 0; b < boneCount; b++)
            {
                if (!ReadDirectRigidBody(data, ref o, end, out stagedBones[b])) return;
            }

            CommitDirectSkeletonFrame(skeletonId, stagedBones, boneCount, frameTimestamp, isReplayFrame);
        }

        if (UpdateFrameTopologyIds(m_latestFrameSkeletonIds, m_currentFrameSkeletonIds, ref m_hasFrameSkeletonTopologySnapshot))
            QueueStreamedTopologyDefinitionRefresh(isReplayFrame);
    }

    private void CommitDirectSkeletonFrame(int skeletonId, sRigidBodyData[] stagedBones, int boneCount, OptitrackHiResTimer.Timestamp frameTimestamp, bool isReplayFrame)
    {
        OptitrackSkeletonDefinition skelDef = GetSkeletonDefinitionById(skeletonId);
        if (skelDef == null)
        {
            skelDef = EnsureDirectSyntheticSkeletonDefinition(skeletonId, stagedBones, boneCount);
        }

        if (skelDef == null)
        {
            QueueStreamedTopologyDefinitionRefresh(isReplayFrame);
            return;
        }

        OptitrackSkeletonState skelState = GetOrCreateSkeletonState(skeletonId);

        for (int b = 0; b < boneCount; b++)
        {
            sRigidBodyData boneData = stagedBones[b];
            if (!TryGetDirectCommittableBoneId(boneData, skeletonId, skelDef, out int boneId))
                continue;

            if (!skelState.BonePoses.ContainsKey(boneId))
                skelState.BonePoses[boneId] = new OptitrackPose();
            if (!skelState.LocalBonePoses.ContainsKey(boneId))
                skelState.LocalBonePoses[boneId] = new OptitrackPose();

            skelState.BonePoses[boneId].Position = new Vector3(-boneData.X, boneData.Y, boneData.Z);
            skelState.BonePoses[boneId].Orientation = new Quaternion(-boneData.QX, boneData.QY, boneData.QZ, -boneData.QW);
        }

        for (int b = 0; b < boneCount; b++)
        {
            sRigidBodyData boneData = stagedBones[b];
            if (!TryGetDirectCommittableBoneId(boneData, skeletonId, skelDef, out int boneId))
                continue;

            Vector3 bonePos = skelState.BonePoses[boneId].Position;
            Quaternion boneOri = skelState.BonePoses[boneId].Orientation;
            Vector3 parentBonePos = Vector3.zero;
            Quaternion parentBoneOri = Quaternion.identity;

            Int32 parentId = skelDef.BoneIdToParentIdMap[boneId];
            if (parentId != 0 && skelState.BonePoses.TryGetValue(parentId, out OptitrackPose parentPose))
            {
                parentBonePos = parentPose.Position;
                parentBoneOri = parentPose.Orientation;
            }

            skelState.LocalBonePoses[boneId].Position = bonePos - parentBonePos;
            skelState.LocalBonePoses[boneId].Orientation = Quaternion.Inverse(parentBoneOri) * boneOri;
        }
        skelState.DeliveryTimestamp = frameTimestamp;
    }

    private static void DirectDecodeId(int encodedId, out int assetId, out int memberId)
    {
        assetId = (encodedId >> 16) & 0x7FFF;
        memberId = encodedId & 0xFFFF;
    }

    private static bool TryGetDirectCommittableBoneId(sRigidBodyData boneData, Int32 skeletonId, OptitrackSkeletonDefinition skelDef, out int boneId)
    {
        int boneSkelId;
        DirectDecodeId(boneData.Id, out boneSkelId, out boneId);
        return boneSkelId == skeletonId &&
               skelDef.BoneIdToParentIdMap.ContainsKey(boneId) &&
               IsBoneDataUsable(boneData);
    }

    private OptitrackSkeletonDefinition EnsureDirectSyntheticSkeletonDefinition(int skeletonId, sRigidBodyData[] stagedBones, int boneCount)
    {
        OptitrackSkeletonDefinition existing = GetSkeletonDefinitionById(skeletonId);
        if (existing != null)
            return existing;

        string skeletonName = ChooseSyntheticSkeletonName(skeletonId);
        OptitrackSkeletonDefinition skelDef = new OptitrackSkeletonDefinition
        {
            Id = skeletonId,
            Name = skeletonName,
            IsSynthetic = true,
            Bones = new List<OptitrackSkeletonDefinition.BoneDefinition>(boneCount),
            BoneIdToParentIdMap = new Dictionary<int, int>(),
        };

        for (int i = 0; i < boneCount; i++)
        {
            int boneSkelId, boneId;
            DirectDecodeId(stagedBones[i].Id, out boneSkelId, out boneId);
            if (boneSkelId != skeletonId)
                continue;

            string shortName = DirectMotiveBoneName(boneId);
            int parentId = DirectMotiveParentId(boneId);
            OptitrackSkeletonDefinition.BoneDefinition boneDef = new OptitrackSkeletonDefinition.BoneDefinition
            {
                Id = boneId,
                ParentId = parentId,
                Name = skeletonName + "_" + shortName,
                Offset = Vector3.zero,
            };
            skelDef.Bones.Add(boneDef);
            skelDef.BoneIdToParentIdMap[boneDef.Id] = boneDef.ParentId;
        }

        if (skelDef.Bones.Count == 0)
            return null;

        m_skeletonDefinitions.Add(skelDef);
        m_mirrorBoneIdMaps.Clear();
        m_pendingSkeletonDefinitionNotify = true;
        return skelDef;
    }

    private string ChooseSyntheticSkeletonName(int skeletonId)
    {
        return "Skeleton" + skeletonId;
    }

    private static string DirectMotiveBoneName(int boneId)
    {
        switch (boneId)
        {
            case 1: return "Hip";
            case 2: return "Ab";
            case 52: return "Spine2";
            case 53: return "Spine3";
            case 54: return "Spine4";
            case 3: return "Chest";
            case 4: return "Neck";
            case 58: return "Neck2";
            case 5: return "Head";
            case 6: return "LShoulder";
            case 7: return "LUArm";
            case 8: return "LFArm";
            case 9: return "LHand";
            case 22: return "LThumb1";
            case 23: return "LThumb2";
            case 24: return "LThumb3";
            case 25: return "LIndex1";
            case 26: return "LIndex2";
            case 27: return "LIndex3";
            case 28: return "LMiddle1";
            case 29: return "LMiddle2";
            case 30: return "LMiddle3";
            case 31: return "LRing1";
            case 32: return "LRing2";
            case 33: return "LRing3";
            case 34: return "LPinky1";
            case 35: return "LPinky2";
            case 36: return "LPinky3";
            case 10: return "RShoulder";
            case 11: return "RUArm";
            case 12: return "RFArm";
            case 13: return "RHand";
            case 37: return "RThumb1";
            case 38: return "RThumb2";
            case 39: return "RThumb3";
            case 40: return "RIndex1";
            case 41: return "RIndex2";
            case 42: return "RIndex3";
            case 43: return "RMiddle1";
            case 44: return "RMiddle2";
            case 45: return "RMiddle3";
            case 46: return "RRing1";
            case 47: return "RRing2";
            case 48: return "RRing3";
            case 49: return "RPinky1";
            case 50: return "RPinky2";
            case 51: return "RPinky3";
            case 14: return "LThigh";
            case 15: return "LShin";
            case 16: return "LFoot";
            case 17: return "LToe";
            case 18: return "RThigh";
            case 19: return "RShin";
            case 20: return "RFoot";
            case 21: return "RToe";
            default: return "Bone" + boneId;
        }
    }

    private static int DirectMotiveParentId(int boneId)
    {
        switch (boneId)
        {
            case 1: return 0;
            case 2: return 1;
            case 52: return 2;
            case 53: return 52;
            case 54: return 53;
            case 3: return 54;
            case 4: return 3;
            case 58: return 4;
            case 5: return 58;
            case 6: return 3;
            case 7: return 6;
            case 8: return 7;
            case 9: return 8;
            case 22: return 9;
            case 23: return 22;
            case 24: return 23;
            case 25: return 9;
            case 26: return 25;
            case 27: return 26;
            case 28: return 9;
            case 29: return 28;
            case 30: return 29;
            case 31: return 9;
            case 32: return 31;
            case 33: return 32;
            case 34: return 9;
            case 35: return 34;
            case 36: return 35;
            case 10: return 3;
            case 11: return 10;
            case 12: return 11;
            case 13: return 12;
            case 37: return 13;
            case 38: return 37;
            case 39: return 38;
            case 40: return 13;
            case 41: return 40;
            case 42: return 41;
            case 43: return 13;
            case 44: return 43;
            case 45: return 44;
            case 46: return 13;
            case 47: return 46;
            case 48: return 47;
            case 49: return 13;
            case 50: return 49;
            case 51: return 50;
            case 14: return 1;
            case 15: return 14;
            case 16: return 15;
            case 17: return 16;
            case 18: return 1;
            case 19: return 18;
            case 20: return 19;
            case 21: return 20;
            default: return 0;
        }
    }

    private bool ConnectDirectNatNet(IPAddress serverAddr, IPAddress localAddr, UInt16 commandPort, UInt16 dataPort, IPAddress multicastAddr)
    {
        StopDirectFrameReceiver();

        string hostName;
        byte[] appVersion;
        byte[] natNetVersion;
        byte[] modelDefPacket;
        UInt16 negotiatedDataPort = dataPort;
        IPAddress negotiatedMulticast = multicastAddr;

        bool hasServerInfo = DirectRequestServerInfo(serverAddr, localAddr, commandPort, out hostName, out appVersion, out natNetVersion, out negotiatedDataPort, out negotiatedMulticast);
        if (!hasServerInfo)
        {
            hostName = "NatNet multicast source";
            appVersion = new byte[4];
            natNetVersion = new byte[] { 4, 2, 0, 0 };
            negotiatedDataPort = dataPort;
            negotiatedMulticast = multicastAddr ?? IPAddress.Parse("239.255.42.99");
            if (!m_directNoModelDefWarned)
            {
                Debug.LogWarning(GetType().FullName + ": NatNet command channel did not respond on " + serverAddr + ":" + commandPort + ". Continuing with multicast frames and synthetic skeleton definitions.", this);
                m_directNoModelDefWarned = true;
            }
        }

        if (negotiatedDataPort == 0)
            negotiatedDataPort = dataPort;
        if (negotiatedMulticast == null)
            negotiatedMulticast = multicastAddr ?? IPAddress.Parse("239.255.42.99");

        if (hasServerInfo && DirectRequestModelDef(serverAddr, localAddr, commandPort, out modelDefPacket))
        {
            if (!SkipDataDescriptions)
            {
                if (DirectUpdateDefinitions(modelDefPacket))
                    m_directDefinitionSource = DirectDefinitionSource.Live;
                else
                    Debug.LogWarning(GetType().FullName + ": direct NatNet MODELDEF parse failed. Falling back to synthetic skeleton definitions from incoming frames.", this);
            }
        }
        else if (hasServerInfo && !m_directNoModelDefWarned)
        {
            Debug.LogWarning(GetType().FullName + ": direct NatNet MODELDEF request failed. Falling back to synthetic skeleton definitions from incoming frames.", this);
            m_directNoModelDefWarned = true;
        }

        ServerNatNetVersion = natNetVersion[0] + "." + natNetVersion[1] + "." + natNetVersion[2] + "." + natNetVersion[3];
        ClientNatNetVersion = "Direct UDP";
        if (!StartDirectFrameReceiver(localAddr.ToString(), negotiatedMulticast.ToString(), negotiatedDataPort))
            return false;
        StoreDirectEndpoint(serverAddr, localAddr, commandPort, negotiatedDataPort, negotiatedMulticast);
        m_nextAutoDefinitionRefreshTime = Time.unscaledTime + Mathf.Max(DefinitionRefreshInterval, 1f);
        Debug.Log(GetType().FullName + ": Connected to direct NatNet server. Server=" + serverAddr + ", host=" + hostName + ", local=" + localAddr + ", serverNatNet=" + ServerNatNetVersion + ", multicast=" + negotiatedMulticast + ":" + negotiatedDataPort + ".", this);
        return true;
    }

    private void StoreDirectEndpoint(IPAddress serverAddr, IPAddress localAddr, UInt16 commandPort, UInt16 dataPort, IPAddress multicastAddr)
    {
        m_directServerAddress = serverAddr;
        m_directLocalAddress = localAddr;
        m_directCommandPort = commandPort;
        m_directDataPort = dataPort;
        m_directMulticastAddress = multicastAddr;
    }

    private void UpdateDirectDefinitions()
    {
        IPAddress serverAddr;
        IPAddress localAddr;
        IPAddress multicastAddr = null;
        UInt16 commandPort;
        UInt16 dataPort;

        try
        {
            serverAddr = IPAddress.Parse(ServerAddress);
            commandPort = (UInt16)Mathf.Clamp(CommandPort, 1, 65535);
            dataPort = (UInt16)Mathf.Clamp(DataPort, 1, 65535);
            if (!string.IsNullOrWhiteSpace(MulticastAddress))
                multicastAddr = IPAddress.Parse(MulticastAddress.Trim());
            localAddr = ResolveLocalAddress(serverAddr);
            ResolvedLocalAddress = localAddr.ToString();
        }
        catch (Exception ex)
        {
            throw new InvalidOperationException("Error parsing direct NatNet refresh settings.", ex);
        }

        string hostName;
        byte[] appVersion;
        byte[] natNetVersion;
        byte[] modelDefPacket;
        UInt16 negotiatedDataPort = dataPort;
        IPAddress negotiatedMulticast = multicastAddr ?? IPAddress.Parse("239.255.42.99");

        if (DirectRequestServerInfo(serverAddr, localAddr, commandPort, out hostName, out appVersion, out natNetVersion, out negotiatedDataPort, out negotiatedMulticast))
        {
            if (negotiatedDataPort == 0)
                negotiatedDataPort = dataPort;
            if (negotiatedMulticast == null)
                negotiatedMulticast = multicastAddr ?? IPAddress.Parse("239.255.42.99");

            ServerNatNetVersion = natNetVersion[0] + "." + natNetVersion[1] + "." + natNetVersion[2] + "." + natNetVersion[3];
        }
        else
        {
            negotiatedDataPort = dataPort;
            negotiatedMulticast = multicastAddr ?? IPAddress.Parse("239.255.42.99");
        }

        if (!DirectRequestModelDef(serverAddr, localAddr, commandPort, out modelDefPacket))
            throw new InvalidOperationException("Direct NatNet MODELDEF request failed.");
        if (!DirectUpdateDefinitions(modelDefPacket))
            throw new InvalidOperationException("Direct NatNet MODELDEF parse failed.");
        m_directDefinitionSource = DirectDefinitionSource.Live;

        bool dataEndpointChanged =
            m_directServerAddress == null ||
            m_directLocalAddress == null ||
            m_directMulticastAddress == null ||
            !m_directServerAddress.Equals(serverAddr) ||
            !m_directLocalAddress.Equals(localAddr) ||
            !m_directMulticastAddress.Equals(negotiatedMulticast) ||
            m_directCommandPort != commandPort ||
            m_directDataPort != negotiatedDataPort;

        if (dataEndpointChanged)
        {
            if (!StartDirectFrameReceiver(localAddr.ToString(), negotiatedMulticast.ToString(), negotiatedDataPort))
                throw new InvalidOperationException("Direct NatNet frame receiver failed to restart after endpoint change.");
            Debug.Log(GetType().FullName + ": direct NatNet frame endpoint changed during refresh; receiver restarted on " + negotiatedMulticast + ":" + negotiatedDataPort + ".", this);
        }

        StoreDirectEndpoint(serverAddr, localAddr, commandPort, negotiatedDataPort, negotiatedMulticast);
        ClientNatNetVersion = "Direct UDP";
    }

    private void UpdateDirectReplayDefinitions()
    {
        IPAddress serverAddr;
        IPAddress localAddr;
        UInt16 commandPort;

        try
        {
            serverAddr = IPAddress.Parse(ReplayServerAddress.Trim());
            commandPort = (UInt16)Mathf.Clamp(CommandPort, 1, 65535);
            localAddr = ResolveLocalAddress(serverAddr);
        }
        catch (Exception ex)
        {
            throw new InvalidOperationException("Error parsing MMRP replay NatNet refresh settings.", ex);
        }

        string hostName;
        byte[] appVersion;
        byte[] natNetVersion;
        byte[] modelDefPacket;
        UInt16 negotiatedDataPort;
        IPAddress negotiatedMulticast;

        DirectRequestServerInfo(serverAddr, localAddr, commandPort, out hostName, out appVersion, out natNetVersion, out negotiatedDataPort, out negotiatedMulticast);

        if (!DirectRequestModelDef(serverAddr, localAddr, commandPort, out modelDefPacket))
            throw new InvalidOperationException("MMRP replay NatNet MODELDEF request failed.");
        if (!DirectUpdateDefinitions(modelDefPacket))
            throw new InvalidOperationException("MMRP replay NatNet MODELDEF parse failed.");

        m_directDefinitionSource = DirectDefinitionSource.Replay;
        ClientNatNetVersion = "Direct UDP + MMRP";
    }

    private bool DirectRequestServerInfo(IPAddress serverAddr, IPAddress localAddr, UInt16 commandPort, out string hostName, out byte[] appVersion, out byte[] natNetVersion, out UInt16 dataPort, out IPAddress multicastAddress)
    {
        hostName = "";
        appVersion = new byte[4];
        natNetVersion = new byte[4];
        dataPort = 0;
        multicastAddress = null;

        byte[] response;
        byte[] pingPayload = DirectBuildPingPayload();
        if (!DirectCommandRequest(serverAddr, localAddr, commandPort, 0, pingPayload, Mathf.Max(DirectServerInfoTimeoutMs, 100), out response)) // NAT_PING
            return false;
        if (response.Length < 4 || BitConverter.ToUInt16(response, 0) != 1) // NAT_PINGRESPONSE / server info
            return false;

        int payloadLen = BitConverter.ToUInt16(response, 2);
        if (payloadLen < 268 || response.Length < 4 + payloadLen)
            return false;

        int o = 4;
        hostName = DirectReadFixedString(response, o, 256);
        o += 256;
        Buffer.BlockCopy(response, o, appVersion, 0, 4); o += 4;
        Buffer.BlockCopy(response, o, natNetVersion, 0, 4); o += 4;

        // NatNet 4.x server info may append clock frequency, data port, multicast flag and multicast address.
        if (response.Length >= o + 8)
            o += 8;
        if (response.Length >= o + 2)
        {
            dataPort = BitConverter.ToUInt16(response, o);
            o += 2;
        }
        if (response.Length >= o + 1)
            o += 1; // multicast flag
        if (response.Length >= o + 4)
        {
            byte a = response[o], b = response[o + 1], c = response[o + 2], d = response[o + 3];
            if (a != 0 || b != 0 || c != 0 || d != 0)
                multicastAddress = new IPAddress(new byte[] { a, b, c, d });
        }
        return true;
    }

    private bool DirectRequestModelDef(IPAddress serverAddr, IPAddress localAddr, UInt16 commandPort, out byte[] modelDefPacket)
    {
        modelDefPacket = null;
        byte[] response;
        if (!DirectCommandRequest(serverAddr, localAddr, commandPort, 4, null, Mathf.Max(DirectModelDefTimeoutMs, 100), out response)) // NAT_REQUEST_MODELDEF
            return false;
        if (response.Length < 8 || BitConverter.ToUInt16(response, 0) != 5) // NAT_MODELDEF
            return false;
        modelDefPacket = response;
        return true;
    }

    private bool DirectCommandRequest(IPAddress serverAddr, IPAddress localAddr, UInt16 commandPort, UInt16 messageId, byte[] payload, int timeoutMs, out byte[] response)
    {
        response = null;
        try
        {
            using (UdpClient udp = new UdpClient())
            {
                udp.Client.ReceiveTimeout = timeoutMs;
                udp.Client.Bind(new IPEndPoint(localAddr, 0));
                byte[] packet = DirectBuildPacket(messageId, payload);
                udp.Send(packet, packet.Length, new IPEndPoint(serverAddr, commandPort));
                IPEndPoint remote = new IPEndPoint(IPAddress.Any, 0);
                response = udp.Receive(ref remote);
                return response != null && response.Length >= 4;
            }
        }
        catch (Exception ex)
        {
            Debug.LogWarning(GetType().FullName + ": direct NatNet command " + messageId + " failed against " + serverAddr + ":" + commandPort + " (" + ex.Message + ").", this);
            return false;
        }
    }

    private static byte[] DirectBuildPacket(UInt16 messageId, byte[] payload)
    {
        int payloadLen = payload != null ? payload.Length : 0;
        byte[] packet = new byte[4 + payloadLen];
        Array.Copy(BitConverter.GetBytes(messageId), 0, packet, 0, 2);
        Array.Copy(BitConverter.GetBytes((UInt16)payloadLen), 0, packet, 2, 2);
        if (payloadLen > 0)
            Buffer.BlockCopy(payload, 0, packet, 4, payloadLen);
        return packet;
    }

    private static byte[] DirectBuildPingPayload()
    {
        byte[] payload = new byte[264];
        byte[] name = Encoding.ASCII.GetBytes("UnityDirectNatNet");
        Buffer.BlockCopy(name, 0, payload, 0, Math.Min(name.Length, 255));
        payload[256] = 4;
        payload[257] = 0;
        payload[258] = 0;
        payload[259] = 0;
        payload[260] = 4;
        payload[261] = 2;
        payload[262] = 0;
        payload[263] = 0;
        return payload;
    }

    private bool DirectUpdateDefinitions(byte[] modelDefPacket)
    {
        if (modelDefPacket == null || modelDefPacket.Length < 8)
            return false;

        int payloadLen = BitConverter.ToUInt16(modelDefPacket, 2);
        int end = Math.Min(modelDefPacket.Length, 4 + payloadLen);
        int o = 4;
        if (o + 4 > end)
            return false;

        List<OptitrackRigidBodyDefinition> rigidBodies = new List<OptitrackRigidBodyDefinition>();
        List<OptitrackSkeletonDefinition> skeletons = new List<OptitrackSkeletonDefinition>();

        int dataSetCount = BitConverter.ToInt32(modelDefPacket, o); o += 4;
        for (int i = 0; i < dataSetCount && o + 8 <= end; i++)
        {
            int dataSetType = BitConverter.ToInt32(modelDefPacket, o); o += 4;
            int dataSetSize = BitConverter.ToInt32(modelDefPacket, o); o += 4;
            if (dataSetSize < 0 || o + dataSetSize > end)
                return false;

            int dataSetEnd = o + dataSetSize;
            try
            {
                if (dataSetType == 1)
                {
                    rigidBodies.Add(DirectReadRigidBodyDefinition(modelDefPacket, ref o, dataSetEnd));
                }
                else if (dataSetType == 2)
                {
                    skeletons.Add(DirectReadSkeletonDefinition(modelDefPacket, ref o, dataSetEnd));
                }
            }
            catch
            {
                return false;
            }

            o = dataSetEnd;
        }

        m_definitionRefreshFailureCount = 0;
        m_rigidBodyDefinitions.Clear();
        m_skeletonDefinitions.Clear();
        m_tmarkersetDefinitions.Clear();
        m_mirrorBoneIdMaps.Clear();
        m_rigidBodyDefinitions.AddRange(rigidBodies);
        m_skeletonDefinitions.AddRange(skeletons);

        lock (m_frameDataUpdateLock)
        {
            m_assetIdToNameCache.Clear();
        }

        PruneStatesMissingFromDefinitions();
        NotifyRegisteredSkeletonDefinitionsChanged();

        return true;
    }

    private static OptitrackRigidBodyDefinition DirectReadRigidBodyDefinition(byte[] data, ref int o, int end)
    {
        string name;
        int id, parentId, markerCount;
        Vector3 offset;
        DirectReadRigidBodyDesc(data, ref o, end, out name, out id, out parentId, out offset, out markerCount);

        OptitrackRigidBodyDefinition rbDef = new OptitrackRigidBodyDefinition
        {
            Id = id,
            Name = name,
            Markers = new List<OptitrackRigidBodyDefinition.MarkerDefinition>(Math.Max(0, markerCount)),
        };

        for (int i = 0; i < markerCount; i++)
            rbDef.Markers.Add(new OptitrackRigidBodyDefinition.MarkerDefinition());
        return rbDef;
    }

    private static OptitrackSkeletonDefinition DirectReadSkeletonDefinition(byte[] data, ref int o, int end)
    {
        string name = DirectReadCString(data, ref o, end);
        int id = DirectReadInt32(data, ref o, end);
        int boneCount = DirectReadInt32(data, ref o, end);

        OptitrackSkeletonDefinition skelDef = new OptitrackSkeletonDefinition
        {
            Id = id,
            Name = name,
            Bones = new List<OptitrackSkeletonDefinition.BoneDefinition>(Math.Max(0, boneCount)),
            BoneIdToParentIdMap = new Dictionary<int, int>(),
        };

        for (int i = 0; i < boneCount; i++)
        {
            string boneName;
            int boneId, parentId, markerCount;
            Vector3 offset;
            DirectReadRigidBodyDesc(data, ref o, end, out boneName, out boneId, out parentId, out offset, out markerCount);
            OptitrackSkeletonDefinition.BoneDefinition boneDef = new OptitrackSkeletonDefinition.BoneDefinition
            {
                Id = boneId,
                ParentId = parentId,
                Name = boneName,
                Offset = offset,
            };
            skelDef.Bones.Add(boneDef);
            skelDef.BoneIdToParentIdMap[boneDef.Id] = boneDef.ParentId;
        }

        return skelDef;
    }

    private static void DirectReadRigidBodyDesc(byte[] data, ref int o, int end, out string name, out int id, out int parentId, out Vector3 offset, out int markerCount)
    {
        name = DirectReadCString(data, ref o, end);
        id = DirectReadInt32(data, ref o, end);
        parentId = DirectReadInt32(data, ref o, end);
        float x = DirectReadFloat(data, ref o, end);
        float y = DirectReadFloat(data, ref o, end);
        float z = DirectReadFloat(data, ref o, end);
        offset = new Vector3(-x, y, z);

        // Orientation offset is present in NatNet 4.x descriptions.
        DirectReadFloat(data, ref o, end);
        DirectReadFloat(data, ref o, end);
        DirectReadFloat(data, ref o, end);
        DirectReadFloat(data, ref o, end);

        markerCount = DirectReadInt32(data, ref o, end);
        int markerBytes = markerCount * (12 + 4);
        if (markerCount < 0 || o + markerBytes > end)
            throw new ArgumentOutOfRangeException("markerCount");
        o += markerBytes;
        for (int i = 0; i < markerCount; i++)
            DirectReadCString(data, ref o, end);
    }

    private static int DirectReadInt32(byte[] data, ref int o, int end)
    {
        if (o + 4 > end) throw new ArgumentOutOfRangeException("o");
        int v = BitConverter.ToInt32(data, o);
        o += 4;
        return v;
    }

    private static float DirectReadFloat(byte[] data, ref int o, int end)
    {
        if (o + 4 > end) throw new ArgumentOutOfRangeException("o");
        float v = BitConverter.ToSingle(data, o);
        o += 4;
        return v;
    }

    private static string DirectReadCString(byte[] data, ref int o, int end)
    {
        if (o >= end) return "";
        int start = o;
        while (o < end && data[o] != 0)
            o++;
        string value = Encoding.UTF8.GetString(data, start, o - start);
        if (o < end)
            o++;
        return value;
    }

    private static string DirectReadFixedString(byte[] data, int o, int maxLen)
    {
        int len = 0;
        while (len < maxLen && o + len < data.Length && data[o + len] != 0)
            len++;
        return Encoding.UTF8.GetString(data, o, len);
    }

    private System.Collections.IEnumerator ConnectCoroutine()
    {
        m_receivedFrameSinceConnect = false;

        IPAddress serverAddr;
        IPAddress localAddr;
        IPAddress multicastAddr = null;
        UInt16 commandPort;
        UInt16 dataPort;
        NatNetConnectionType connType;
        try
        {
            serverAddr = IPAddress.Parse( ServerAddress );
            commandPort = (UInt16)Mathf.Clamp( CommandPort, 1, 65535 );
            dataPort = (UInt16)Mathf.Clamp( DataPort, 1, 65535 );
            if ( !string.IsNullOrWhiteSpace( MulticastAddress ) )
                multicastAddr = IPAddress.Parse( MulticastAddress.Trim() );
            localAddr = ResolveLocalAddress( serverAddr );
            ResolvedLocalAddress = localAddr.ToString();
            connType = ConnectionType == ClientConnectionType.Unicast
                ? NatNetConnectionType.NatNetConnectionType_Unicast
                : NatNetConnectionType.NatNetConnectionType_Multicast;
        }
        catch ( Exception ex )
        {
            Debug.LogException( ex, this );
            Debug.LogError( GetType().FullName + ": Error parsing connection settings. Server=" + ServerAddress + ", command=" + CommandPort + ", data=" + DataPort + ", multicast=" + MulticastAddress + ".", this );
            yield break;
        }

        if (ConnectionType == ClientConnectionType.Multicast)
        {
            IPAddress directMulticastAddr = multicastAddr ?? IPAddress.Parse("239.255.42.99");
            if (!ConnectDirectNatNet(serverAddr, localAddr, commandPort, dataPort, directMulticastAddr))
            {
                Debug.LogError(GetType().FullName + ": Error connecting to direct NatNet server. Server=" + ServerAddress + ", local=" + localAddr + ", command=" + commandPort + ", data=" + dataPort + ", multicast=" + directMulticastAddr + ". Check Motive streaming, firewall, and network interface.", this);
                yield break;
            }

            if (m_replayConnectCoroutine != null)
            {
                StopCoroutine(m_replayConnectCoroutine);
                m_replayConnectCoroutine = null;
                CleanupReplayClient();
            }

            if (RecordOnPlay && !m_isReconnecting)
                Debug.LogWarning(GetType().FullName + ": RecordOnPlay requires the NatNet SDK command path and is skipped in direct multicast mode.", this);

            m_connectionHealthCoroutine = StartCoroutine(CheckConnectionHealth());
            yield break;
        }

        Debug.LogError(GetType().FullName + ": Unicast/NatNet SDK connection is disabled. This project now uses the direct UDP multicast NatNet path only.", this);
        yield break;

#pragma warning disable 162
        bool applyServerSettings = !m_hasAppliedServerSettings;
        NatNetClient connectedClient = null;
        Exception connectError = null;

        System.Threading.Tasks.Task connectTask = System.Threading.Tasks.Task.Run( () =>
        {
            NatNetClient c = null;
            try
            {
                c = new NatNetClient();
                if (commandPort == k_DefaultNatNetCommandPort &&
                    dataPort == k_DefaultNatNetDataPort &&
                    multicastAddr == null)
                {
                    c.Connect( connType, localAddr, serverAddr );
                }
                else
                {
                    c.Connect( connType, localAddr, serverAddr, commandPort, dataPort, multicastAddr );
                }

                if ( applyServerSettings )
                {
                    // Remotely change the Skeleton Coordinate property to Global/Local
                    if (SkeletonCoordinates == StreamingCoordinatesValues.Global)
                        c.RequestCommand("SetProperty,,Skeleton Coordinates,false");
                    else
                        c.RequestCommand("SetProperty,,Skeleton Coordinates,true");

                    // Remotely change the Bone Naming Convention to Motive/FBX/BVH
                    if (BoneNamingConvention == OptitrackBoneNameConvention.Motive)
                        c.RequestCommand("SetProperty,,Bone Naming Convention,0");
                    else if (BoneNamingConvention == OptitrackBoneNameConvention.FBX)
                        c.RequestCommand("SetProperty,,Bone Naming Convention,1");
                    else if (BoneNamingConvention == OptitrackBoneNameConvention.BVH)
                        c.RequestCommand("SetProperty,,Bone Naming Convention,2");
                }

                connectedClient = c;
            }
            catch ( Exception ex )
            {
                connectError = ex;
                if ( c != null ) { try { c.Dispose(); } catch { } }
            }
        } );

        while ( !connectTask.IsCompleted )
            yield return null;

        if ( connectError != null || connectedClient == null )
        {
            if ( connectError != null )
                Debug.LogException( connectError, this );
            Debug.LogError( GetType().FullName + ": Error connecting to NatNet server. Server=" + ServerAddress + ", local=" + localAddr + ", command=" + commandPort + ", data=" + dataPort + ", multicast=" + (multicastAddr != null ? multicastAddr.ToString() : "default") + ". Check Motive streaming, firewall, and network interface.", this );
            yield break;
        }

        m_client = connectedClient;
        if ( applyServerSettings )
            m_hasAppliedServerSettings = true;
        else
            Debug.Log(GetType().FullName + ": reconnect skipped SetProperty commands; they were already applied on the first connection.", this);

        if (!m_isReconnecting)
            yield return new UnityEngine.WaitForSeconds( 0.1f );

        try
        {
            if (!SkipDataDescriptions)
            {
                UpdateDefinitions();
                m_nextAutoDefinitionRefreshTime = Time.unscaledTime + Mathf.Max(DefinitionRefreshInterval, 1f);
            }
        }
        catch (Exception ex)
        {
            Debug.LogException(ex, this);
        }

        ResubscribeRegisteredAssets();

        if (RecordOnPlay && !m_isReconnecting)
            StartRecording();

        byte[] NatNetVersion = m_client.ServerDescription.NatNetVersion;
        ServerNatNetVersion = NatNetVersion[0] + "." + NatNetVersion[1] + "." + NatNetVersion[2] + "." + NatNetVersion[3];
        ClientNatNetVersion = "" + NatNetClient.NatNetLibVersion;
        Debug.Log(GetType().FullName + ": Connected to NatNet server. Server=" + ServerAddress + ", local=" + ResolvedLocalAddress + ", serverNatNet=" + ServerNatNetVersion + ", clientNatNet=" + ClientNatNetVersion + ", connection=" + ConnectionType + ".", this);

        m_client.NativeFrameReceived += OnNatNetFrameReceived;
        m_connectionHealthCoroutine = StartCoroutine( CheckConnectionHealth() );
#pragma warning restore 162
    }

    /// <summary>
    /// Disconnects from the streaming server and cleans up <see cref="m_client"/>.
    /// </summary>
    void OnDisable()
    {
        StopAllCoroutines();
        StopDirectFrameReceiver();
        m_connectionHealthCoroutine = null;
        m_replayConnectCoroutine = null;
        m_isReconnecting = false;

        CleanupReplayClient();

        if (m_client != null)
        {
            if (RecordOnPlay)
                StopRecording();

            m_client.NativeFrameReceived -= OnNatNetFrameReceived;
            try { m_client.Disconnect(); } catch { }
            m_client.Dispose();
            m_client = null;
        }
    }

    System.Collections.IEnumerator CheckConnectionHealth()
    {
        float healthCheckIntervalSeconds = Mathf.Max(ConnectionHealthCheckIntervalSeconds, 0.1f);
        float recentFrameThresholdSeconds = Mathf.Max(StreamingFrameTimeoutSeconds, healthCheckIntervalSeconds);

        // The lifespan of these variables is tied to the lifespan of a single connection session.
        // The coroutine is stopped on disconnect and restarted on connect.
        YieldInstruction checkIntervalYield = new WaitForSeconds( healthCheckIntervalSeconds );
        OptitrackHiResTimer.Timestamp connectionInitiatedTimestamp = OptitrackHiResTimer.Now();
        bool wasReceivingFrames = false;
        bool warnedPendingFirstFrame = false;

        while ( true )
        {
            yield return checkIntervalYield;

            if ( m_receivedFrameSinceConnect == false && !IsReplayFrameFresh() )
            {
                // Still waiting for first frame. Warn exactly once if this takes too long.
                if ( connectionInitiatedTimestamp.AgeSeconds > recentFrameThresholdSeconds )
                {
                    if ( warnedPendingFirstFrame == false )
                    {
                        if (m_directNatNetConnected)
                            Debug.LogWarning( GetType().FullName + ": Direct NatNet receiver is running but no frames have arrived in Unity yet. Check Unity Editor firewall permission or multicast routing.", this );
                        else
                            Debug.LogWarning( GetType().FullName + ": No frames received from the server yet. Verify your connection settings are correct and that the server is streaming.", this );
                        warnedPendingFirstFrame = true;

                    }

                    continue;
                }
            }
            else
            {
                // We've received at least one frame, do ongoing checks for changes in connection health.
                bool receivedRecentFrame = HasRecentStreamingFrame(recentFrameThresholdSeconds);

                if ( wasReceivingFrames == false && receivedRecentFrame == true )
                {
                    // Transition: Bad health -> good health.
                    wasReceivingFrames = true;
                    Debug.Log( GetType().FullName + ": Receiving streaming data from the server.", this );
                    continue;
                }
                else if ( wasReceivingFrames == true && receivedRecentFrame == false )
                {
                    // Transition: Good health -> bad health.
                    wasReceivingFrames = false;
                    Debug.LogWarning( GetType().FullName + ": No streaming frames received from the server recently.", this );
                    if ( AutoReconnect )
                    {
                        Debug.Log( GetType().FullName + ": starting automatic reconnect.", this );
                        Reconnect();
                    }
                    continue;
                }
            }
        }
    }

#region Private methods
    /// <summary>
    /// Event handler for NatNet frame delivery. Updates our simplified state representations.
    /// NOTE: This executes in the context of the NatNetLib network service thread!
    /// </summary>
    /// <remarks>
    /// Because the <see cref="sFrameOfMocapData"/> type is expensive to marshal, we instead utilize the
    /// <see cref="NatNetClient.NativeFrameReceivedEventArgs.NativeFramePointer"/>, treating it as as opaque, and
    /// passing it to some helper "accessor" functions to retrieve the subset of data we care about, using only
    /// blittable types which do not cause any garbage to be allocated.
    /// </remarks>
    /// <param name="sender"></param>
    /// <param name="eventArgs"></param>
    private void OnNatNetFrameReceived( object sender, NatNetClient.NativeFrameReceivedEventArgs eventArgs )
    {
        bool isReplayFrame = ReferenceEquals(sender, m_replayClient);
        if (!isReplayFrame && IsReplayFrameFresh())
        {
            return;
        }

        if ( ! Monitor.TryEnter( m_frameDataUpdateLock, 1 ) )
        {
            return;
        }

        try
        {
            // Update health markers.
            OptitrackHiResTimer.Timestamp frameTimestamp = OptitrackHiResTimer.Now();
            if (isReplayFrame)
            {
                m_replayReceivedFrameSinceConnect = true;
                Interlocked.Exchange(ref m_lastReplayFrameDeliveryTimestamp.m_ticks, frameTimestamp.m_ticks);
            }
            m_receivedFrameSinceConnect = true;
            Interlocked.Exchange( ref m_lastFrameDeliveryTimestamp.m_ticks, frameTimestamp.m_ticks );

            // Process received frame.
            IntPtr pFrame = eventArgs.NativeFramePointer;
            NatNetError result = NatNetError.NatNetError_OK;

            // get timestamp
            UInt64 transmitTimestamp;
            result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetTransmitTimestamp(pFrame, out transmitTimestamp);

            // get and decode timecode (if available)
            UInt32 timecode;
            UInt32 timecodeSubframe;
            result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetTimecode(pFrame, out timecode, out timecodeSubframe);
            Int32 hour, minute, second, frameNumber, subframeNumber;
            NaturalPoint.NatNetLib.NativeMethods.NatNet_DecodeTimecode(timecode, timecodeSubframe, out hour, out minute, out second, out frameNumber, out subframeNumber);
            //Debug.Log(hour + "......" + minute + second + frameNumber + subframeNumber);

            // ----------------------
            // - Update rigid bodies
            // ----------------------
            Int32 frameRbCount;
            result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetRigidBodyCount( pFrame, out frameRbCount );
            NatNetException.ThrowIfNotOK( result, "NatNet_Frame_GetRigidBodyCount failed." );

            m_currentFrameRigidBodyIds.Clear();
            for (int rbIdx = 0; rbIdx < frameRbCount; ++rbIdx)
            {
                sRigidBodyData rbData = new sRigidBodyData();
                result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetRigidBody( pFrame, rbIdx, out rbData );
                NatNetException.ThrowIfNotOK( result, "NatNet_Frame_GetRigidBody failed." );

                m_currentFrameRigidBodyIds.Add(rbData.Id);

                // Ensure we have a state corresponding to this rigid body ID.
                OptitrackRigidBodyState rbState = GetOrCreateRigidBodyState( rbData.Id );
                RigidBodyDataToState(rbData, OptitrackHiResTimer.Now(), rbState);
            }
            if (UpdateFrameTopologyIds(m_latestFrameRigidBodyIds, m_currentFrameRigidBodyIds, ref m_hasFrameRigidBodyTopologySnapshot))
                QueueStreamedTopologyDefinitionRefresh(isReplayFrame);

            // ----------------------
            // - Update skeletons
            // ----------------------
            Int32 frameSkeletonCount;
            result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetSkeletonCount( pFrame, out frameSkeletonCount );
            NatNetException.ThrowIfNotOK( result, "NatNet_Frame_GetSkeletonCount failed." );

            m_currentFrameSkeletonIds.Clear();
            for (int skelIdx = 0; skelIdx < frameSkeletonCount; ++skelIdx)
            {
                Int32 skeletonId;
                result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_Skeleton_GetId( pFrame, skelIdx, out skeletonId );
                NatNetException.ThrowIfNotOK( result, "NatNet_Frame_Skeleton_GetId failed." );

                m_currentFrameSkeletonIds.Add(skeletonId);

                // Ensure we have a state corresponding to this skeleton ID.
                OptitrackSkeletonState skelState = GetOrCreateSkeletonState( skeletonId );

                // Enumerate this skeleton's bone rigid bodies.
                Int32 skelRbCount;
                result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_Skeleton_GetRigidBodyCount( pFrame, skelIdx, out skelRbCount );
                NatNetException.ThrowIfNotOK( result, "NatNet_Frame_Skeleton_GetRigidBodyCount failed." );

                OptitrackSkeletonDefinition skelDef = GetSkeletonDefinitionById(skeletonId);
                if (skelDef == null)
                {
                    if (!m_pendingDefinitionRefresh)
                    {
                        Debug.LogWarning(GetType().FullName + ": missing skeleton definition for streamed skeleton ID " + skeletonId + "; scheduling definition refresh.", this);
                    }
                    QueueStreamedTopologyDefinitionRefresh(isReplayFrame);
                    continue;
                }

                sRigidBodyData[] stagedBones = GetSkeletonFrameScratch(skeletonId, skelRbCount);
                for (int boneIdx = 0; boneIdx < skelRbCount; ++boneIdx)
                {
                    result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_Skeleton_GetRigidBody( pFrame, skelIdx, boneIdx, out stagedBones[boneIdx] );
                    NatNetException.ThrowIfNotOK( result, "NatNet_Frame_Skeleton_GetRigidBody failed." );
                }

                for (int boneIdx = 0; boneIdx < skelRbCount; ++boneIdx)
                {
                    sRigidBodyData boneData = stagedBones[boneIdx];
                    if (!TryGetCommittableBoneId(boneData, skeletonId, skelDef, out int boneId))
                        continue;

                    // TODO: Could pre-populate this map when the definitions are retrieved.
                    // Should never allocate after the first frame, at least.
                    if (skelState.BonePoses.ContainsKey( boneId ) == false)
                    {
                        skelState.BonePoses[boneId] = new OptitrackPose();
                    }
                    if (skelState.LocalBonePoses.ContainsKey( boneId ) == false)
                    {
                        skelState.LocalBonePoses[boneId] = new OptitrackPose();
                    }

                    // Flip coordinate handedness from right to left by inverting X and W.
                    Vector3 bonePos = new Vector3(-boneData.X, boneData.Y, boneData.Z);
                    Quaternion boneOri = new Quaternion(-boneData.QX, boneData.QY, boneData.QZ, -boneData.QW);
                    skelState.BonePoses[boneId].Position = bonePos;
                    skelState.BonePoses[boneId].Orientation = boneOri;
                }

                // Derive locals in a second pass so parent order in the payload does not matter.
                for (int boneIdx = 0; boneIdx < skelRbCount; ++boneIdx)
                {
                    sRigidBodyData boneData = stagedBones[boneIdx];
                    if (!TryGetCommittableBoneId(boneData, skeletonId, skelDef, out int boneId))
                        continue;

                    Vector3 bonePos = skelState.BonePoses[boneId].Position;
                    Quaternion boneOri = skelState.BonePoses[boneId].Orientation;
                    Vector3 parentBonePos = new Vector3(0,0,0);
                    Quaternion parentBoneOri = new Quaternion(0,0,0,1);

                    Int32 pId = skelDef.BoneIdToParentIdMap[boneId];
                    if (pId != 0 && skelState.BonePoses.TryGetValue(pId, out OptitrackPose parentPose))
                    {
                        parentBonePos = parentPose.Position;
                        parentBoneOri = parentPose.Orientation;
                    }
                    skelState.LocalBonePoses[boneId].Position = bonePos - parentBonePos;
                    skelState.LocalBonePoses[boneId].Orientation = Quaternion.Inverse(parentBoneOri) * boneOri;
                }

                skelState.DeliveryTimestamp = frameTimestamp;
            }
            if (UpdateFrameTopologyIds(m_latestFrameSkeletonIds, m_currentFrameSkeletonIds, ref m_hasFrameSkeletonTopologySnapshot))
                QueueStreamedTopologyDefinitionRefresh(isReplayFrame);

            // -----------------------------------------------------
            // - Update trained markerset // trained markerset added
            // ----------------------------------------------------
            var dataDescsSnapshot = m_dataDescs;
            m_latestTMarkMarkerStates.Clear();
            if (dataDescsSnapshot != null && dataDescsSnapshot.AssetDescriptions != null)
            for (int tmarkIdx = 0; tmarkIdx < dataDescsSnapshot.AssetDescriptions.Count; ++tmarkIdx)
            {
                Int32 tmarkersetId = dataDescsSnapshot.AssetDescriptions[tmarkIdx].AssetID;

                // Ensure we have a state corresponding to this tmarkerset ID.
                OptitrackTMarkersetState tmarkState = GetOrCreateTMarkersetState(tmarkersetId);

                Int32 tmarkRbCount = dataDescsSnapshot.AssetDescriptions[tmarkIdx].RigidBodyCount;
                OptitrackTMarkersetDefinition tmarkDef = GetTMarkersetDefinitionById(tmarkersetId);
                if (tmarkDef == null)
                {
                    Debug.LogError(GetType().FullName + ": OnNatNetFrameReceived, no corresponding tmarkerset definition for received tmarkerset frame data.", this);
                    continue;
                }

                for (int boneIdx = 0; boneIdx < tmarkRbCount; ++boneIdx)
                {
                    sRigidBodyData boneData = new sRigidBodyData();
                    result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_TMarkerset_GetRigidBody(pFrame, tmarkIdx, boneIdx, out boneData);
                    NatNetException.ThrowIfNotOK(result, "NatNet_Frame_TMarkerset_GetRigidBody failed.");

                    Int32 boneTMarkId, boneId;
                    NaturalPoint.NatNetLib.NativeMethods.NatNet_DecodeID(boneData.Id, out boneTMarkId, out boneId);

                    if (tmarkState.BonePoses.ContainsKey(boneId) == false)
                    {
                        tmarkState.BonePoses[boneId] = new OptitrackPose();
                    }
                    if (tmarkState.LocalBonePoses.ContainsKey(boneId) == false)
                    {
                        tmarkState.LocalBonePoses[boneId] = new OptitrackPose();
                    }

                    // Flip coordinate handedness from right to left by inverting X and W.
                    Vector3 bonePos = new Vector3(-boneData.X, boneData.Y, boneData.Z);
                    Quaternion boneOri = new Quaternion(-boneData.QX, boneData.QY, boneData.QZ, -boneData.QW);
                    tmarkState.BonePoses[boneId].Position = bonePos;
                    tmarkState.BonePoses[boneId].Orientation = boneOri;

                    Vector3 parentBonePos = new Vector3(0, 0, 0);
                    Quaternion parentBoneOri = new Quaternion(0, 0, 0, 1);

                    Int32 pId = tmarkDef.BoneIdToParentIdMap[boneId];
                    if (pId != -1)
                    {
                        parentBonePos = tmarkState.BonePoses[pId].Position;
                        parentBoneOri = tmarkState.BonePoses[pId].Orientation;
                    }
                    tmarkState.LocalBonePoses[boneId].Position = bonePos - parentBonePos;
                    tmarkState.LocalBonePoses[boneId].Orientation = Quaternion.Inverse(parentBoneOri) * boneOri;
                }

                // --------------------------------------------
                // - Update trained markerset markers
                // --------------------------------------------
                Int32 tmarkMarkerCount = dataDescsSnapshot.AssetDescriptions[tmarkIdx].MarkerCount;
                /*result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_TMarkerset_GetMarkerCount(pFrame, tmarkIdx, out tmarkMarkerCount);
                NatNetException.ThrowIfNotOK(result, "NatNet_Frame_TMarkerset_GetMarkerCount failed.");*/
                //Debug.Log("tmark marker count: " + tmarkMarkerCount); // working finally

                // Update Trained Markerset Marker data
                for (int markerIdx = 0; markerIdx < tmarkMarkerCount; ++markerIdx)
                {
                    sMarker tmarker = new sMarker(); // markerData
                    result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_TMarkerset_GetMarker(pFrame, tmarkIdx, markerIdx, out tmarker);
                    NatNetException.ThrowIfNotOK(result, "NatNet_Frame_TMarkerset_GetMarker failed.");
                    //Debug.Log("result: " + result);

                    // Flip coordinate handedness
                    OptitrackMarkerState tmarkerState = GetOrCreateTMarkMarkerState(tmarker.Id);
                    tmarkerState.Name = GetMarkerName(tmarker);
                    tmarkerState.Position = new Vector3(-tmarker.X, tmarker.Y, tmarker.Z);
                    tmarkerState.Size = tmarker.Size;
                    tmarkerState.Labeled = (tmarker.Params & 0x10) == 0;
                    tmarkerState.Id = tmarker.Id;
                    tmarkerState.IsActive = (tmarker.Params & 0x20) != 0;
                }

            }

            // ----------------------
            // - Update markers
            // ----------------------
            Int32 MarkerCount;
            result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetLabeledMarkerCount( pFrame, out MarkerCount );
            NatNetException.ThrowIfNotOK( result, "NatNet_Frame_GetLabeledMarkerCount failed.");

            m_latestMarkerStates.Clear();
            //Debug.Log("marker count: " + MarkerCount);

            for (int markerIdx = 0; markerIdx < MarkerCount; ++markerIdx)
            {
                sMarker marker = new sMarker();
                result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetLabeledMarker( pFrame, markerIdx, out marker );
                NatNetException.ThrowIfNotOK( result, "NatNet_Frame_GetLabeledMarker failed." );

                // Flip coordinate handedness
                OptitrackMarkerState markerState = GetOrCreateMarkerState( marker.Id );
                markerState.Name = GetMarkerName( marker );
                markerState.Position = new Vector3( -marker.X, marker.Y, marker.Z );
                markerState.Size = marker.Size;
                markerState.Labeled = (marker.Params & 0x10) == 0;
                markerState.Id = marker.Id;
                markerState.IsActive = (marker.Params & 0x20) != 0;
            }

        }
        catch (Exception ex)
        {
            Debug.LogError( GetType().FullName + ": OnNatNetFrameReceived encountered an exception.", this );
            Debug.LogException( ex, this );
        }
        finally
        {
            Monitor.Exit( m_frameDataUpdateLock );
        }
    }

    private string GetMarkerName( sMarker marker )
    {
        int assetID = marker.Id >> 16; // high word = Asset ID Number
        int memberID = marker.Id & 0x00ffff; // low word = Member ID Number (constraint number)

        string assetName;
        if (!m_assetIdToNameCache.TryGetValue(assetID, out assetName))
        {
            assetName = "";
            OptitrackRigidBodyDefinition rigidBodyDef = GetRigidBodyDefinitionById( assetID );
            if (rigidBodyDef != null)
                assetName = rigidBodyDef.Name;
            else
            {
                OptitrackSkeletonDefinition skeletonDef = GetSkeletonDefinitionById( assetID );
                if (skeletonDef != null)
                    assetName = skeletonDef.Name;
                else
                {
                    OptitrackTMarkersetDefinition tmarkersetDef = GetTMarkersetDefinitionById( assetID );
                    if (tmarkersetDef != null)
                        assetName = tmarkersetDef.Name;
                }
            }
            m_assetIdToNameCache[assetID] = assetName;
        }

        bool IsLabeled = (marker.Params & 0x10) == 0;
        bool IsActive = (marker.Params & 0x20) != 0;

        if (IsActive && !IsLabeled)
            return "Active " + marker.Id.ToString();
        else if (IsActive && IsLabeled)
            return "Active " + marker.Id.ToString() + " (" + assetName + " Member ID: " + memberID + " )";
        else if (!IsActive && !IsLabeled)
            return "Passive " + marker.Id.ToString();
        else
            return "Passive (" + assetName + " Member ID: " + memberID + ")";
    }

    private void RigidBodyDataToState(sRigidBodyData rbData, OptitrackHiResTimer.Timestamp timestamp, OptitrackRigidBodyState rbState)
    {
        rbState.DeliveryTimestamp = timestamp;
        rbState.Pose = new OptitrackPose
        {
            Position = new Vector3(-rbData.X, rbData.Y, rbData.Z),
            Orientation = new Quaternion(-rbData.QX, rbData.QY, rbData.QZ, -rbData.QW),                  
        };
        rbState.IsTracked = (rbData.Params & 0x01) != 0;
    }

    private void ResetStreamingSubscriptions()
    {
        m_client.RequestCommand( "SubscribeToData" );
    }

    private void SubscribeRigidBody( MonoBehaviour component, Int32 rigidBodyId )
    {
        if ( m_client != null && ConnectionType == ClientConnectionType.Unicast )
        {
            // Try subscribing up to 3 times with a 2000 ms timeout before giving up. 
            bool subscribeSucceeded = m_client.RequestCommand( "SubscribeByID,RigidBody," + rigidBodyId, 2000, 3 );

            // Log a warning on the first failure.
            if ( ! subscribeSucceeded && ! m_doneSubscriptionNotice )
            {
                if ( m_client.ServerDescription.HostApp == "Motive" )
                {
                    // Host app is Motive: If new enough to support subscription, failure is an error.
                    // Otherwise, warn them that they may want to update Motive to reduce bandwidth consumption.
                    if ( m_client.ServerAppVersion >= new Version(2, 2, 0) )
                    {
                        Debug.LogError( "Failed to subscribe to rigid body streaming data for component", component);
                    }
                    else
                    {
                        Debug.LogWarning("Your version of Motive is too old to support NatNet rigid body data subscription; streaming bandwidth consumption may be higher than necessary. This feature works in Motive 2.2.0+.");
                    }
                }
                else
                {
                    // Not Motive, we don't know whether it "should" support this. Warning instead of error.
                    Debug.LogWarning( "Failed to subscribe to rigid body streaming data for component", component );
                }

                m_doneSubscriptionNotice = true;
            }
        }
    }

    private void SubscribeSkeleton(MonoBehaviour component, string name )
    {
        if (m_client != null && ConnectionType == ClientConnectionType.Unicast)
        {
            if (m_client.ServerAppVersion >= new Version(2, 2, 1))
            {
                // Try subscribing up to 3 times with a 2000 ms timeout before giving up. 
                bool subscribeSucceeded = m_client.RequestCommand("SubscribeToData,Skeleton," + name, 2000, 3);

                // Log a warning on the first failure.
                if (!subscribeSucceeded && !m_doneSubscriptionNotice)
                {
                    Debug.LogError("Failed to subscribe to skeleton streaming data for component", component);
                    m_doneSubscriptionNotice = true;
                }
            }
            else if (m_client.ServerAppVersion == new Version(2, 2, 0, 0))
            {
                // Motive 2.2.0 has a bug were Motive says it subscribes successfully, but doesn't.
                // Subscribing to all skeletons still works, so for this version that is done instead. 

                // Try subscribing up to 3 times with a 2000 ms timeout before giving up. 
                bool subscribeSucceeded = m_client.RequestCommand("SubscribeToData,Skeleton,All" + name, 2000, 3);

                if (!subscribeSucceeded && !m_doneSubscriptionNotice)
                {
                    Debug.LogError("Failed to subscribe to all skeletons streaming data some unknown reason.", component);
                    m_doneSubscriptionNotice = true;
                }
            }
            else
            {
                Debug.LogWarning("Your version of Motive is too old to support NatNet skeleton data subscription; streaming bandwidth consumption may be higher than necessary. This feature works in Motive 2.2.1+.");
                m_doneSubscriptionNotice = true;
            }
        }
    }

    private void SubscribeTMarkerset(MonoBehaviour component, string name) // check the version numbers // trained markerset added
    {
        if (m_client != null && ConnectionType == ClientConnectionType.Unicast)
        {
            if (m_client.ServerAppVersion >= new Version(2, 2, 1))
            {
                // Try subscribing up to 3 times with a 2000 ms timeout before giving up. 
                bool subscribeSucceeded = m_client.RequestCommand("SubscribeToData,TrainedMarkersets," + name, 2000, 3);

                // Log a warning on the first failure.
                if (!subscribeSucceeded && !m_doneSubscriptionNotice)
                {
                    Debug.LogError("Failed to subscribe to trained markerset streaming data for component", component);
                    m_doneSubscriptionNotice = true;
                }
            }

            else if (m_client.ServerAppVersion == new Version(2, 2, 0, 0))
            {
                // Motive 2.2.0 has a bug were Motive says it subscribes successfully, but doesn't.
                // Subscribing to all skeletons still works, so for this version that is done instead. 

                // Try subscribing up to 3 times with a 2000 ms timeout before giving up. 
                bool subscribeSucceeded = m_client.RequestCommand("SubscribeToData,TrainedMarkersets,All" + name, 2000, 3);

                if (!subscribeSucceeded && !m_doneSubscriptionNotice)
                {
                    Debug.LogError("Failed to subscribe to all trained markersets streaming data some unknown reason.", component);
                    m_doneSubscriptionNotice = true;
                }
            }

            else
            {
                Debug.LogWarning("Your version of Motive is too old to support NatNet skeleton data subscription; streaming bandwidth consumption may be higher than necessary. This feature works in Motive 2.2.1+.");
                m_doneSubscriptionNotice = true;
            }
        }

    }

    private void SubscribeTMarkMarkers()
    {
        if (m_client != null && ConnectionType == ClientConnectionType.Unicast)
        {
            bool subscribeSucceeded4 = m_client.RequestCommand("SubscribeToData,TrainedMarkersetMarkers,All", 2000, 3);
            //Debug.Log("TMMarkers: " + subscribeSucceeded4);

            // Log a warning on the first failure.
            if (!subscribeSucceeded4 && !m_doneSubscriptionNotice)
            {
                if (m_client.ServerDescription.HostApp == "Motive")
                {
                    // Host app is Motive: If new enough to support subscription, failure is an error.
                    // Otherwise, warn them that they may want to update Motive to reduce bandwidth consumption.
                    if (m_client.ServerAppVersion >= new Version(2, 2, 0))
                    {
                        Debug.LogError("Failed to subscribe to tmark marker streaming data");
                    }
                    else
                    {
                        Debug.LogWarning("Your version of Motive is too old to support NatNet tmark marker data subscription; streaming bandwidth consumption may be higher than necessary. This feature works in Motive 2.2.0+.");
                    }
                }
                else
                {
                    // Not Motive, we don't know whether it "should" support this. Warning instead of error.
                    Debug.LogWarning("Failed to subscribe to tmark marker streaming data");
                }

                m_doneSubscriptionNotice = true;
            }
        }
    }

    private void SubscribeMarkers( )
    {
        if (m_client != null && ConnectionType == ClientConnectionType.Unicast)
        {
            // Try subscribing up to 3 times with a 2000 ms timeout before giving up. 
            bool subscribeSucceeded = m_client.RequestCommand("SubscribeToData,MarkerSetMarkers,All", 2000, 3);
            bool subscribeSucceeded2 = m_client.RequestCommand("SubscribeToData,LabeledMarkers,All", 2000, 3);
            bool subscribeSucceeded3 = m_client.RequestCommand("SubscribeToData,LegacyUnlabeledMarkers,All", 2000, 3);
            //bool subscribeSucceeded4 = m_client.RequestCommand("SubscribeToData, TrainedMarkersetMarkers,All", 2000, 3);
            //bool allSubscribeSucceeded = subscribeSucceeded4;
            bool allSubscribeSucceeded = subscribeSucceeded && subscribeSucceeded2 && subscribeSucceeded3;
            m_subscribedToMarkers = allSubscribeSucceeded;

            // Log a warning on the first failure.
            if (!allSubscribeSucceeded && !m_doneSubscriptionNotice)
            {
                if (m_client.ServerDescription.HostApp == "Motive")
                {
                    // Host app is Motive: If new enough to support subscription, failure is an error.
                    // Otherwise, warn them that they may want to update Motive to reduce bandwidth consumption.
                    if (m_client.ServerAppVersion >= new Version(2, 2, 0))
                    {
                        Debug.LogError("Failed to subscribe to marker streaming data");
                    }
                    else
                    {
                        Debug.LogWarning("Your version of Motive is too old to support NatNet rigid body data subscription; streaming bandwidth consumption may be higher than necessary. This feature works in Motive 2.2.0+.");
                    }
                }
                else
                {
                    // Not Motive, we don't know whether it "should" support this. Warning instead of error.
                    Debug.LogWarning("Failed to subscribe to marker streaming data");
                }

                m_doneSubscriptionNotice = true;
            }
        }
    }

    private IPAddress ResolveLocalAddress(IPAddress serverAddress)
    {
        if (!AutoDetectLocalAddress)
            return IPAddress.Parse(LocalAddress);

        if (IPAddress.IsLoopback(serverAddress))
            return IPAddress.Loopback;

        try
        {
            using (var socket = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp))
            {
                socket.Connect(new IPEndPoint(serverAddress, Mathf.Clamp(CommandPort, 1, 65535)));
                if (socket.LocalEndPoint is IPEndPoint endpoint &&
                    endpoint.Address.AddressFamily == AddressFamily.InterNetwork &&
                    !IPAddress.Any.Equals(endpoint.Address))
                {
                    LocalAddress = endpoint.Address.ToString();
                    return endpoint.Address;
                }
            }
        }
        catch (Exception ex)
        {
            Debug.LogWarning(GetType().FullName + ": failed to auto-detect local address. Falling back to LocalAddress. " + ex.Message, this);
        }

        return IPAddress.Parse(LocalAddress);
    }

    private sRigidBodyData[] GetSkeletonFrameScratch(Int32 skeletonId, Int32 boneCount)
    {
        if (!m_skeletonFrameScratch.TryGetValue(skeletonId, out var scratch) || scratch.Length != boneCount)
        {
            scratch = new sRigidBodyData[boneCount];
            m_skeletonFrameScratch[skeletonId] = scratch;
        }
        return scratch;
    }

    /// <summary>
    /// </summary>
    private static bool IsBoneDataUsable(sRigidBodyData boneData)
    {
        float quaternionMagnitudeSquared =
            boneData.QX * boneData.QX +
            boneData.QY * boneData.QY +
            boneData.QZ * boneData.QZ +
            boneData.QW * boneData.QW;

        return IsFinite(boneData.X) &&
            IsFinite(boneData.Y) &&
            IsFinite(boneData.Z) &&
            IsFinite(boneData.QX) &&
            IsFinite(boneData.QY) &&
            IsFinite(boneData.QZ) &&
            IsFinite(boneData.QW) &&
            quaternionMagnitudeSquared > 0.000001f;
    }

    /// <summary>
    /// </summary>
    private static bool TryGetCommittableBoneId(sRigidBodyData boneData, Int32 skeletonId, OptitrackSkeletonDefinition skelDef, out int boneId)
    {
        Int32 boneSkelId;
        NaturalPoint.NatNetLib.NativeMethods.NatNet_DecodeID( boneData.Id, out boneSkelId, out boneId );
        return boneSkelId == skeletonId
            && skelDef.BoneIdToParentIdMap.ContainsKey(boneId)
            && IsBoneDataUsable(boneData);
    }

    private static bool IsFinite(float value)
    {
        return !float.IsNaN(value) && !float.IsInfinity(value);
    }

    /// <summary>
    /// Returns the <see cref="OptitrackRigidBodyState"/> corresponding to the provided <paramref name="rigidBodyId"/>.
    /// If the requested state object does not exist yet, it will initialize and return a newly-created one.
    /// </summary>
    /// <remarks>Makes the assumption that the lock on <see cref="m_frameDataUpdateLock"/> is already held.</remarks>
    /// <param name="rigidBodyId">The ID of the rigid body for which to retrieve the corresponding state.</param>
    /// <returns>The existing state object, or a newly created one if necessary.</returns>
    private OptitrackRigidBodyState GetOrCreateRigidBodyState( Int32 rigidBodyId )
    {
        OptitrackRigidBodyState returnedState = null;

        if ( m_latestRigidBodyStates.ContainsKey( rigidBodyId ) )
        {
            returnedState = m_latestRigidBodyStates[rigidBodyId];
        }
        else
        {
            OptitrackRigidBodyState newRbState = new OptitrackRigidBodyState {
                Pose = new OptitrackPose(),
            };

            m_latestRigidBodyStates[rigidBodyId] = newRbState;

            returnedState = newRbState;
        }

        return returnedState;
    }

    /// <summary>
    /// Returns the <see cref="OptitrackSkeletonState"/> corresponding to the provided <paramref name="skeletonId"/>.
    /// If the requested state object does not exist yet, it will initialize and return a newly-created one.
    /// </summary>
    /// <remarks>Makes the assumption that the lock on <see cref="m_frameDataUpdateLock"/> is already held.</remarks>
    /// <param name="skeletonId">The ID of the skeleton for which to retrieve the corresponding state.</param>
    /// <returns>The existing state object, or a newly created one if necessary.</returns>
    private OptitrackSkeletonState GetOrCreateSkeletonState( Int32 skeletonId )
    {
        OptitrackSkeletonState returnedState = null;

        if ( m_latestSkeletonStates.ContainsKey( skeletonId ) )
        {
            returnedState = m_latestSkeletonStates[skeletonId];
        }
        else
        {
            OptitrackSkeletonState newSkeletonState = new OptitrackSkeletonState {
                BonePoses = new Dictionary<Int32, OptitrackPose>(),
                LocalBonePoses = new Dictionary<int, OptitrackPose>(),
            };

            m_latestSkeletonStates[skeletonId] = newSkeletonState;

            returnedState = newSkeletonState;
        }

        return returnedState;
    }

    private OptitrackTMarkersetState GetOrCreateTMarkersetState(Int32 tmarkersetId)
    {
        OptitrackTMarkersetState returnedState = null;

        if (m_latestTMarkersetStates.ContainsKey(tmarkersetId))
        {
            returnedState = m_latestTMarkersetStates[tmarkersetId];
        }
        else
        {
            OptitrackTMarkersetState newTMarkersetState = new OptitrackTMarkersetState
            {
                BonePoses = new Dictionary<Int32, OptitrackPose>(),
                LocalBonePoses = new Dictionary<int, OptitrackPose>(),
            };

            m_latestTMarkersetStates[tmarkersetId] = newTMarkersetState;

            returnedState = newTMarkersetState;
        }

        return returnedState;
    }

    /// <summary>
    /// Returns the <see cref="OptitrackMarkerState"/> corresponding to the provided <paramref name="markerId"/>.
    /// If the requested state object does not exist yet, it will initialize and return a newly-created one.
    /// </summary>
    /// <remarks>Makes the assumption that the lock on <see cref="m_frameDataUpdateLock"/> is already held.</remarks>
    /// <param name="markerId">The ID of the bone in trained markerset for which to retrieve the corresponding state.</param>
    /// <returns>The existing state object, or a newly created one if necessary.</returns>
    private OptitrackMarkerState GetOrCreateTMarkMarkerState(Int32 markerId)
    {
        OptitrackMarkerState returnedState = null;

        if (m_latestTMarkMarkerStates.ContainsKey(markerId))
        {
            returnedState = m_latestTMarkMarkerStates[markerId];
        }
        else
        {
            OptitrackMarkerState newMarkerState = new OptitrackMarkerState
            {
                Position = new Vector3(),
            };

            m_latestTMarkMarkerStates[markerId] = newMarkerState;

            returnedState = newMarkerState;
        }

        //Debug.Log(returnedState);
        return returnedState;
    }

    /// <summary>
    /// Returns the <see cref="OptitrackMarkerState"/> corresponding to the provided <paramref name="markerId"/>.
    /// If the requested state object does not exist yet, it will initialize and return a newly-created one.
    /// </summary>
    /// <remarks>Makes the assumption that the lock on <see cref="m_frameDataUpdateLock"/> is already held.</remarks>
    /// <param name="markerId">The ID of the rigid body for which to retrieve the corresponding state.</param>
    /// <returns>The existing state object, or a newly created one if necessary.</returns>
    private OptitrackMarkerState GetOrCreateMarkerState(Int32 markerId)
    {
        OptitrackMarkerState returnedState = null;

        if (m_latestMarkerStates.ContainsKey( markerId ))
        {
            returnedState = m_latestMarkerStates[markerId];
        }
        else
        {
            OptitrackMarkerState newMarkerState = new OptitrackMarkerState
            {
                Position = new Vector3(),
            };

            m_latestMarkerStates[markerId] = newMarkerState;

            returnedState = newMarkerState;
        }

        return returnedState;
    }

    /// <summary>
    /// </summary>
    [System.Obsolete("Use FillBoneSnapshot() instead of direct lock manipulation.")]
    internal void _EnterFrameDataUpdateLock()
    {
        Monitor.Enter( m_frameDataUpdateLock );
    }

    /// <summary>
    /// </summary>
    [System.Obsolete("Use FillBoneSnapshot() instead of direct lock manipulation.")]
    internal void _ExitFrameDataUpdateLock()
    {
        Monitor.Exit( m_frameDataUpdateLock );
    }
#endregion Private methods
}