/// <summary>
        /// Adds the position measurement.
        /// </summary>
        /// <param name="timestamp">Timestamp.</param>
        /// <param name="position">Position.</param>
        public virtual void AddPositionMeasurement(double timestamp, Vector3 position)
        {
            TrackingDataPosition trackingDataPosition = new TrackingDataPosition(timestamp - positionLatency, position);

            if (!isMainThread())
            {
                lock (dataQueue)
                {
                    dataQueue.PushFront(trackingDataPosition);
                    dataInQueue = true;
                }
            }
            else
            {
                AddPositionMeasurement(trackingDataPosition);
            }
        }
        public void AddAccelerationMeasurement(TrackingDataIMU trackingDataIMU)
        {
            double lastAccTs = GetLastAccelerationTimestamp();

            if (trackingDataIMU.timestamp - lastAccTs < 0.001)
            {
                //Debug.LogError("Acceleration Timestamp too close at " + trackingDataIMU.timestamp.ToString("F4") + " Previous: " + lastAccTs.ToString());
                return;
            }
            int index = InsertByTimestamp(trackingDataIMU);

            if (index + 1 >= trackingDataBuffer.Size)
            {
                //  Debug.LogError("Index bigger than Buffer Size");
                return;
            }
            //if (index != 0)
            // Debug.LogError("Acceleration was not insered at last position  " + trackingDataIMU.timestamp.ToString("0.000"));

            if (trackingDataBuffer.Size < 2)
            {
                return;
            }

            double delaySinceLastUpdate = trackingDataIMU.timestamp - trackingDataBuffer[index + 1].timestamp;

            if (delaySinceLastUpdate > maxDelaySinceLastMeasurement)
            {
                Debug.LogWarning("Too long delay since last update : " + delaySinceLastUpdate.ToString() + "  " + trackingDataIMU.timestamp.ToString("0.000"));
                if (trackingDataBuffer.Size > 0)
                {
                    ResetFilter();
                }
                return;
            }

            double lastpositionts = GetLastPositionTimestamp();

            //        Debug.Log("Last pos ts " + lastpositionts.ToString("0.000"));
            if (lastpositionts < 0 || trackingDataIMU.timestamp - lastpositionts > accelerationOnlyTrackingDelay)
            {
                //  Debug.LogError("No previous position, or too long ago");
                return;
            }

            // Calculate its speed and position using the previous info

            // The previous data is an acceleration
            if (trackingDataBuffer[index + 1].GetType() == typeof(TrackingDataIMU))
            {
                //TODO: handle offset correction ?
                TrackingDataIMU previousAcceleration = ((TrackingDataIMU)trackingDataBuffer[index + 1]);

                // Predict position and speed at this update

                // Clamping to avoid predicting for too long
                Vector3 newSpeed = (trackingDataIMU.timestamp - lastpositionts) < 0.11f ? previousAcceleration.speed + ((TrackingDataIMU)trackingDataBuffer[index]).acceleration * (float)delaySinceLastUpdate : Vector3.Slerp(previousAcceleration.speed + ((TrackingDataIMU)trackingDataBuffer[index]).acceleration * (float)delaySinceLastUpdate, Vector3.zero, (float)(trackingDataIMU.timestamp - lastpositionts) / accelerationOnlyTrackingDelay);
                trackingDataBuffer[index].speed = newSpeed;
                Vector3 newPositionOffset = (trackingDataIMU.timestamp - lastpositionts) < 0.11f ? previousAcceleration.speed * (float)delaySinceLastUpdate + 0.5f * ((TrackingDataIMU)trackingDataBuffer[index]).acceleration * (float)delaySinceLastUpdate * (float)delaySinceLastUpdate : Vector3.Slerp(previousAcceleration.speed * (float)delaySinceLastUpdate + 0.5f * ((TrackingDataIMU)trackingDataBuffer[index]).acceleration * (float)delaySinceLastUpdate * (float)delaySinceLastUpdate, Vector3.zero, (float)(trackingDataIMU.timestamp - lastpositionts) / accelerationOnlyTrackingDelay);
                trackingDataBuffer[index].position = previousAcceleration.position + newPositionOffset;
                //    Debug.Log("     Offset from prev acceleration : " + (trackingDataBuffer[index].position - previousAcceleration.position).magnitude.ToString("0.000"));
            }
            // The previous data is a position
            else if (trackingDataBuffer[index + 1].GetType() == typeof(TrackingDataPosition))
            {
                //TODO: handle offset correction
                TrackingDataPosition previousPosition = ((TrackingDataPosition)trackingDataBuffer[index + 1]);
                // Predict position and speed at this update
                Vector3 newSpeed = delaySinceLastUpdate < 0.03f ? previousPosition.speed + ((TrackingDataIMU)trackingDataBuffer[index]).acceleration * (float)delaySinceLastUpdate : Vector3.Slerp(previousPosition.speed + ((TrackingDataIMU)trackingDataBuffer[index]).acceleration * (float)delaySinceLastUpdate, Vector3.zero, (float)(delaySinceLastUpdate) / accelerationOnlyTrackingDelay);

                trackingDataBuffer[index].speed = newSpeed;
                Vector3 newPositionOffset = delaySinceLastUpdate < 0.03f ? previousPosition.speed * (float)delaySinceLastUpdate + 0.5f * ((TrackingDataIMU)trackingDataBuffer[index]).acceleration * (float)delaySinceLastUpdate * (float)delaySinceLastUpdate : Vector3.Slerp(previousPosition.speed * (float)delaySinceLastUpdate + 0.5f * ((TrackingDataIMU)trackingDataBuffer[index]).acceleration * (float)delaySinceLastUpdate * (float)delaySinceLastUpdate, Vector3.zero, (float)(trackingDataIMU.timestamp - lastpositionts) / accelerationOnlyTrackingDelay);
                trackingDataBuffer[index].position = previousPosition.position + newPositionOffset;
                //Debug.Log("     Offset from prev position : " + (trackingDataBuffer[index].position-previousPosition.position).magnitude.ToString("0.000"));
            }

            lastCalculatedPosition          = trackingDataBuffer[0].position;
            lastCalculatedPositionTimestamp = trackingDataBuffer[0].timestamp;
            // if (lastCalculatedPosition.magnitude > 5)
            //   Debug.LogError("TS " + lastCalculatedPositionTimestamp.ToString("0.000") + "  MAG: " + lastCalculatedPosition.magnitude.ToString());
        }
        public void AddPositionMeasurement(TrackingDataPosition trackingDataPosition)
        {
            double lastPosTs = GetLastPositionTimestamp();

            if (trackingDataPosition.timestamp - lastPosTs < 0.001f)
            {
//                Debug.LogError("Position Timestamp too close at " + trackingDataPosition.timestamp.ToString("F4") + "  last timestamp: " + lastPosTs.ToString("F4"));
                return;
            }

            int index = InsertByTimestamp(trackingDataPosition);

            if (index + 1 >= trackingDataBuffer.Size)
            {
                Debug.LogError("Index bigger than Buffer Size");
                return;
            }

            //   Debug.Log("POS MEASUREMENT AT  " + (timestamp-positionLatency).ToString("0.000") + " POS: " + position.ToString("0.000"));

            // Check there is no position with a more recent timestamp, either a network error or impossible state
            for (int i = index - 1; i >= 0; i--)
            {
                if (trackingDataBuffer[i].GetType() == typeof(TrackingDataPosition))
                {
                    Debug.LogError("The position received is older than other position at " + trackingDataPosition.timestamp.ToString("F4"));
                    //TODO Remove from buffer ?
                    return;
                }
            }

            if (trackingDataBuffer.Size < 2)
            {
                return;
            }

            // Check time since last measurement (of any type : position or acc)
            double delaySinceLastUpdate = trackingDataPosition.timestamp - trackingDataBuffer[index + 1].timestamp;
            // if (delaySinceLastUpdate > maxDelaySinceLastMeasurement)
            //   Debug.LogWarning("Too long delay since last update : " + delaySinceLastUpdate.ToString());

            bool predictSpeedFromAcc = false; // Boolean to che kif we should calculate the speed from the last acceleration (in case of jump, no position etc)

            // Try to find position jumps
            TrackingDataPosition previousPosition = GetPreviousPositionData(index);

            if (previousPosition != null)
            {
                float speedMagnitudeLastPositions = ((trackingDataPosition.position - previousPosition.position) / (float)(trackingDataPosition.timestamp - previousPosition.timestamp)).magnitude;
                if (speedMagnitudeLastPositions > discardSpeed || (trackingDataPosition.position - previousPosition.position).magnitude > discardDistance)
                {
                    ((TrackingDataPosition)trackingDataBuffer[index]).jump = true;
                    predictSpeedFromAcc = true;
                    //  Debug.LogWarning("Jump detected of speed " + speedMagnitudeLastPositions.ToString() + "  at TS " + trackingDataPosition.timestamp.ToString());
                }
            }
            // We couldn't find any previous position in the buffer so we add an offset to smooth from the last calculated position to here
            else
            {
                offsets.Add(new PositionOffset(trackingDataPosition.timestamp, lastCalculatedPosition - trackingDataPosition.position, 0.4f));
                offsets.Add(new PositionOffset(trackingDataPosition.timestamp, lastCalculatedPosition - trackingDataPosition.position, 0.4f));
                // Debug.LogWarning("Jump detected of speed ");
            }

            if (!predictSpeedFromAcc)
            {
                List <TrackingDataPosition> lastPositions = GetPreviousPositionData(index, speedCalculationDelay);
                // There is not last position
                if (lastPositions.Count <= 1)
                {
                    predictSpeedFromAcc = true;
                    //   Debug.LogError("Could not find any previous position " + index.ToString() + "  - " + trackingDataPosition.timestamp.ToString() + "  count " + lastPositions.Count.ToString() + "  TS: " + timestamp.ToString("0.000"));
                }
                else
                {
                    // Check for jump in last positions
                    foreach (TrackingDataPosition pos in lastPositions)
                    {
                        if (pos.jump)
                        {
                            predictSpeedFromAcc = true;
                            break;
                        }
                    }

                    // Calculate speed from last positions
                    if (!predictSpeedFromAcc)
                    {
                        Vector3 speed          = (lastPositions[0].position - lastPositions[lastPositions.Count - 1].position) / (float)(lastPositions[0].timestamp - lastPositions[lastPositions.Count - 1].timestamp);
                        double  speedtimestamp = (lastPositions[0].timestamp + lastPositions[lastPositions.Count - 1].timestamp) / 2.0d;
                        //  Debug.Log("     Calculating speed form pos : " + speed.magnitude.ToString("0.000"));
                        // Propagate with acceleration until this time
                        List <TrackingDataIMU> accelerations = GetPreviousIMUDataUntilTimestamp(index, speedtimestamp);
                        if (accelerations.Count > 0)
                        {
                            for (int i = accelerations.Count - 1; i >= 0; i--)
                            {
                                speed         += accelerations[i].acceleration * (float)(accelerations[i].timestamp - speedtimestamp);
                                speedtimestamp = accelerations[i].timestamp;
                            }
                            // Could be improved here (like by checking if we have another acc jsut after current index)
                            speed += accelerations[0].acceleration * (float)(trackingDataBuffer[index].timestamp - accelerations[0].timestamp);
                            //  Debug.Log("     Predicting speed from acc to ts : " + speed.magnitude.ToString("0.000"));
                        }
                        ((TrackingDataPosition)trackingDataBuffer[index]).speed = speed;
                    }
                }
            }

            // Calculate speed from previous acceleration
            if (predictSpeedFromAcc)
            {
                // The previous data is an acceleration
                if (trackingDataBuffer[index + 1].GetType() == typeof(TrackingDataIMU))
                {
                    //TODO: handle offset correction ?
                    TrackingDataIMU previousAcceleration = ((TrackingDataIMU)trackingDataBuffer[index + 1]);

                    // Predict position and speed at this update
                    trackingDataBuffer[index].speed = previousAcceleration.speed + previousAcceleration.acceleration * (float)delaySinceLastUpdate;
                    //  Debug.Log("     Predicting speed from acc : " + trackingDataBuffer[index].speed.magnitude.ToString("0.000"));
                }
                // The previous data is a position
                else if (trackingDataBuffer[index + 1].GetType() == typeof(TrackingDataPosition))
                {
                    //TODO: handle offset correction
                    // No prediction : use speed of last position
                    ((TrackingDataPosition)trackingDataBuffer[index + 1]).speed = previousPosition.speed;
                    //    Debug.Log("     Using previous speed : " + previousPosition.speed.magnitude.ToString("0.000"));
                }
            }

            // Correct following positions and speeds
            // Keep latest position before propagation to calculate the offset
            Vector3 latestPosition = trackingDataBuffer[0].position;

            for (int i = index - 1; i >= 0; i--)
            {
                // At this point we can only have IMU data up to the end of the vector
                double deltaTime = trackingDataBuffer[i].timestamp - trackingDataBuffer[i + 1].timestamp;

                trackingDataBuffer[i].speed    = trackingDataBuffer[i + 1].speed + ((TrackingDataIMU)trackingDataBuffer[i]).acceleration * (float)deltaTime;
                trackingDataBuffer[i].position = trackingDataBuffer[i + 1].position + trackingDataBuffer[i + 1].speed * (float)deltaTime + 0.5f * ((TrackingDataIMU)trackingDataBuffer[i]).acceleration * (float)deltaTime * (float)deltaTime;
            }

            Vector3 positionOffsetAfterPropagation = trackingDataBuffer[0].position - latestPosition;

            if (positionOffsetAfterPropagation.magnitude > 0.04f && previousPosition != null)
            {
                offsets.Add(new PositionOffset(trackingDataPosition.timestamp, previousPosition.position - trackingDataPosition.position));
                //      Debug.LogWarning("     Offset : " + positionOffsetAfterPropagation.magnitude.ToString("0.000") + "   at " + trackingDataBuffer[0].timestamp.ToString("0.000"));
            }
            lastCalculatedPosition          = trackingDataBuffer[0].position;
            lastCalculatedPositionTimestamp = trackingDataBuffer[0].timestamp;
            //  if (lastCalculatedPosition.magnitude > 5)
            //    Debug.LogError("TS " + lastCalculatedPositionTimestamp.ToString("0.000") + "  MAG: " + lastCalculatedPosition.magnitude.ToString());
        }