private void UpdateMovementVectors(Dictionary<JointType, bool> jointDirChange, Dictionary<JointType, float> distanceScale)
        {
            foreach (var change in jointDirChange)
            {
                if (_workingMode == 1)
                {
                    if (change.Value == true) // if a dir is changed
                    {
                        var joint = change.Key;
                        //int currentNumberOfVectors = jointMovementVectors[joint].Count();
                        var lastCoordinates = jointMovementVectors[joint]._endPoint;

                        lastCoordinates = SmoothingLine(smoothingBuffer, lastCoordinates, joint);

                        var newVector = new MovementVector(new Point3D(lastCoordinates.X, lastCoordinates.Y, lastCoordinates.Z), lastPositions[0][joint]);

                        bool CheckIfNewVectorIsNeeded = checkNewVector(newVector, joint);
                        if (CheckIfNewVectorIsNeeded == true)
                        {
                            newVector._distance = newVector._distance * distanceScale[joint];
                            jointMovementVectors[joint] = newVector;
                        }
                        else
                        {
                             var lastPoint = jointMovementVectors[joint];
                            //Batev: var lastPoint = new Point3D();

                            jointMovementVectors[joint]._endPoint = lastPositions[0][joint];
                            var angle = Math3DHelper.AngleBetweenTwoPointsInDegrees(lastPositions[0][joint], lastPoint._startPoint);
                            //Batev: var angle = Math3DHelper.AngleBetweenTwoPointsInDegrees(lastPoint, lastPositions[0][joint]);

                            if (angle < 0)
                            {
                                angle = 360 + angle;
                            }
                            jointMovementVectors[joint]._angle = angle;
                            jointMovementVectors[joint]._distance = Math3DHelper.DistanceBetwenTwoPoints(lastPoint._startPoint, lastPositions[0][joint]);
                            //Batev: jointMovementVectors[joint]._distance = Math3DHelper.DistanceBetwenTwoPoints(lastPoint, lastPositions[0][joint]);
                        }
                    }
                }
                // for working mode 2 - we replace the vector every time. We have to skip several frames in order to have better angle calculation
                else if (_workingMode == 2)
                {
                    if (_calculatedFramesSinceStart % _skippedFramesForMode2 == 0)
                    {
                        var joint = change.Key;
                        var lastCoordinates = jointMovementVectors[joint]._endPoint;
                        var newVector = new MovementVector(new Point3D(lastCoordinates.X, lastCoordinates.Y, lastCoordinates.Z), lastPositions[0][joint]);
                        jointMovementVectors[joint] = newVector;
                    }
                }
            }
        }
        /*Constructor*/
        public WorkoutTracking(List<JointType> pointsOfInterest, int workingMode = 1)
        {
            _workingMode = workingMode;
            //_angleThreshold = angleThresh;
            //_distanceThreshold = distanceThresh;
            jointsOfInterest = new List<JointType>(pointsOfInterest);

            lastPositions = new Dictionary<JointType, Point3D>[numberOfFramesForDirCalculation];
            jointDirs = new Dictionary<JointType, int>();
            jointMovementVectors = new Dictionary<JointType, MovementVector>();

            CoordinatesListHead = new List<Point3D>(); //Smoothing
            CoordinatesListSpineMid = new List<Point3D>(); //Smoothing
            CoordinatesListKneeLeft = new List<Point3D>(); //Smoothing
            CoordinatesListKneeRight = new List<Point3D>(); //Smoothing
            CoordinatesListHandLeft = new List<Point3D>(); //Smoothing
            CoordinatesListHandRight = new List<Point3D>(); //Smoothing

            smoothingBuffer = new Dictionary<JointType, List<Point3D>>(); //Smoothing

            foreach (var joint in jointsOfInterest)
            {
                jointDirs.Add(joint, 0);

                var movementVectors = new MovementVector();

                jointMovementVectors.Add(joint, movementVectors);
            }

            for (int i = 0; i < numberOfFramesForDirCalculation; i++)
            {
                var jointDict = new Dictionary<JointType, Point3D>();
                foreach (var joint in jointsOfInterest)
                {
                    jointDict.Add(joint, new Point3D());
                }

                lastPositions[i] = jointDict;
            }
        }
        private bool checkNewVector(MovementVector newVector, JointType joint)
        {
            bool isNeeded = true;

            if (newVector._distance < MOVEMENT_VECTOR_MIN_DISTANCE_IN_PIX)
            {
                isNeeded = false;
            }
            else
            {
                //int currentNumberOfVectors = jointMovementVectors[joint].Count();
                if (Math.Abs(jointMovementVectors[joint]._angle - newVector._angle) < angleDiff)
                {
                    isNeeded = false;
                }
            }

            return isNeeded;
        }
        private void UpdateMovementVectors(Dictionary<JointType, bool> jointDirChange)
        {
            foreach (var change in jointDirChange)
            {
                if (change.Value == true) // if a dir is changed
                {
                    var joint = change.Key;
                    int currentNumberOfVectors = jointMovementVectors[joint].Count;
                    var lastCoordinates = jointMovementVectors[joint][currentNumberOfVectors - 1]._endPoint;

                    lastCoordinates = SmoothingLine(smoothingBuffer, lastCoordinates, joint);

                    var newVector = new MovementVector(new Point3D(lastCoordinates.X, lastCoordinates.Y, lastCoordinates.Z), lastPositions[0][joint]);

                    bool CheckIfNewVectorIsNeeded = checkNewVector(newVector, joint);
                    if (CheckIfNewVectorIsNeeded == true)
                    {
                        jointMovementVectors[joint].Add(newVector);

                    }
                    else
                    {
                        var lastPoint = jointMovementVectors[joint][currentNumberOfVectors - 1];
                        //Batev: var lastPoint = new Point3D();
                        jointMovementVectors[joint][currentNumberOfVectors - 1]._endPoint = lastPositions[0][joint];
                         var angle = Math3DHelper.AngleBetweenTwoPointsInDegrees(lastPoint._startPoint, lastPositions[0][joint]);
                        //Batev: var angle = Math3DHelper.AngleBetweenTwoPointsInDegrees(lastPoint, lastPositions[0][joint]);
                        jointMovementVectors[joint][currentNumberOfVectors - 1]._angle = angle;
                        jointMovementVectors[joint][currentNumberOfVectors - 1]._distance = Math3DHelper.DistanceBetwenTwoPoints(lastPoint._startPoint, lastPositions[0][joint]);
                        //Batev:  jointMovementVectors[joint][currentNumberOfVectors - 1]._distance = Math3DHelper.DistanceBetwenTwoPoints(lastPoint, lastPositions[0][joint]);
                    }
                }
            }
        }