//>>> _using
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using SharpDX;
using SharpDX.Direct3D11;
using SharpDX.Windows;
//<<< _using
using System.Diagnostics;
using Leap;
using Framefield.Core;

namespace Framefield.Core.ID7f786337_46dd_4931_9366_26d111eda65b
{
    public class Class_LeapData : OperatorPart.Function, Framefield.Core.OperatorPartTraits.ITimeAccessor
    {
        //>>> _outputids
        private enum OutputId
        {
            PosX = 0,
            PosY = 1,
            PosZ = 2,
            RotPitch = 3,
            RotYaw = 4,
            RotRoll = 5,
            ParamX = 6,
            ParamY = 7,
            ParamZ = 8,
            Quality = 9
        }
        //<<< _outputids

        //>>> _inputids
        private enum InputId
        {
            Running = 0,
            WorkspaceCenterX = 1,
            WorkspaceCenterY = 2,
            WorkspaceCenterZ = 3,
            WorkspaceRadius = 4,
            ParameterCenterX = 5,
            ParameterCenterY = 6,
            ParameterCenterZ = 7,
            ParameterRangeX = 8,
            ParameterRangeY = 9,
            ParameterRangeZ = 10,
            CameraFriction = 11,
            ParameterFriction = 12
        }
        //<<< _inputids

        public Class_LeapData()
        {
            _controller = new Controller();
            _leapHandler = new LeapHandler();
            _handlerConnected = _controller.AddListener(_leapHandler);
             if (!_handlerConnected)
                 Logger.Error(this, "error connecting to leap controller");
        }

        public override void Dispose()
        {
            _controller.RemoveListener(_leapHandler);
            Utilities.DisposeObj(ref _controller);
            Utilities.DisposeObj(ref _leapHandler);
            _handlerConnected = false;
            base.Dispose();
        }

        public override OperatorPartContext Eval(OperatorPartContext context, List<OperatorPart> inputs, int outputIdx)
        {
            if (!_controller.IsConnected || !_handlerConnected)
            {
                if(!_warningPrinted) {
                    Logger.Error(this, "leap device not found or not connected");
                    _warningPrinted=true;
                }
                
                context.Value = 0.0f;
                return context;
            }
            if(_warningPrinted) {
                Logger.Info(this, "Leap device connected");
                _warningPrinted= false;
            }
            
            

            //>>> _params
            var Running = inputs[(int)InputId.Running].Eval(context).Value;
            var WorkspaceCenterX = inputs[(int)InputId.WorkspaceCenterX].Eval(context).Value;
            var WorkspaceCenterY = inputs[(int)InputId.WorkspaceCenterY].Eval(context).Value;
            var WorkspaceCenterZ = inputs[(int)InputId.WorkspaceCenterZ].Eval(context).Value;
            var WorkspaceCenter = new Vector3(WorkspaceCenterX, WorkspaceCenterY, WorkspaceCenterZ);
            var WorkspaceRadius = inputs[(int)InputId.WorkspaceRadius].Eval(context).Value;
            var ParameterCenterX = inputs[(int)InputId.ParameterCenterX].Eval(context).Value;
            var ParameterCenterY = inputs[(int)InputId.ParameterCenterY].Eval(context).Value;
            var ParameterCenterZ = inputs[(int)InputId.ParameterCenterZ].Eval(context).Value;
            var ParameterCenter = new Vector3(ParameterCenterX, ParameterCenterY, ParameterCenterZ);
            var ParameterRangeX = inputs[(int)InputId.ParameterRangeX].Eval(context).Value;
            var ParameterRangeY = inputs[(int)InputId.ParameterRangeY].Eval(context).Value;
            var ParameterRangeZ = inputs[(int)InputId.ParameterRangeZ].Eval(context).Value;
            var ParameterRange = new Vector3(ParameterRangeX, ParameterRangeY, ParameterRangeZ);
            var CameraFriction = inputs[(int)InputId.CameraFriction].Eval(context).Value;
            var ParameterFriction = inputs[(int)InputId.ParameterFriction].Eval(context).Value;
            //<<< _params

            if (Running > 0.5 && !_leapHandler.IsRunning)
            {
                _leapHandler.Start();
            }

            if (Running < 0.5 && _leapHandler.IsRunning)
            {
                _leapHandler.Stop();
            }

            _leapHandler.WorkspaceCenter = new Vector(WorkspaceCenterX, WorkspaceCenterY, WorkspaceCenterZ);
            _leapHandler.WorkspaceRadius = WorkspaceRadius;
            _leapHandler.ParameterCenter = new Vector(ParameterCenterX, ParameterCenterY, ParameterCenterZ);
            _leapHandler.TrackingRange = 20;
            _leapHandler.CameraFriction = CameraFriction;
            _leapHandler.ParameterFriction = ParameterFriction;

            switch (outputIdx)
            {
                case (int)OutputId.PosX: context.Value = _leapHandler.Position.X; break;
                case (int)OutputId.PosY: context.Value = _leapHandler.Position.Y; break;
                case (int)OutputId.PosZ: context.Value = _leapHandler.Position.Z; break;
                case (int)OutputId.RotPitch: context.Value = _leapHandler.Rotation.Y; break;
                case (int)OutputId.RotYaw: context.Value = _leapHandler.Rotation.X; break;
                case (int)OutputId.RotRoll: context.Value = _leapHandler.Rotation.Z; break;
                case (int)OutputId.ParamX: context.Value = MapInput(_leapHandler.Parameter.X, -1, 0, 1, ParameterCenterX - ParameterRangeX, ParameterCenterX, ParameterCenterX + ParameterRangeX, 0); break;
                case (int)OutputId.ParamY: context.Value = MapInput(_leapHandler.Parameter.Y, -1, 0, 1, ParameterCenterY - ParameterRangeY, ParameterCenterY, ParameterCenterY + ParameterRangeY, 0); break;
                case (int)OutputId.ParamZ: context.Value = MapInput(_leapHandler.Parameter.Z, -1, 0, 1, ParameterCenterZ + ParameterRangeZ, ParameterCenterZ, ParameterCenterZ - ParameterRangeZ, 0); break;
                case (int)OutputId.Quality: context.Value = _leapHandler.Quality; break;
            }
            return context;
        }

        private float MapInput(float input, float minValue, float neutralValue, float maxValue, float minPoint, float neutralPoint, float maxPoint, float neutralWidth)
        {
            float output = neutralValue;
            if (neutralWidth < 0.001)
            {
                float normalizedInput = (input - minPoint)/(maxPoint - minPoint);
                normalizedInput = Utilities.Clamp(normalizedInput, 0.0f, 1.0f);
                output = Utilities.SmoothStep(normalizedInput)*(maxValue - minValue) + minValue;
            }
            else
            {
                if (minPoint > maxPoint)
                {
                    Utilities.Swap(ref minPoint, ref maxPoint);
                    Utilities.Swap(ref minValue, ref maxValue);
                }
                if (input < 0)
                {
                    float normalizedInput = (input + neutralPoint - minPoint)/(neutralPoint - neutralWidth*0.5f - minPoint);
                    normalizedInput = Utilities.Clamp(normalizedInput, 0.0f, 1.0f);
                    output = Utilities.SmoothStep(normalizedInput)*(neutralValue - minValue) + minValue;
                }
                else
                {
                    float normalizedInput = (input + neutralPoint - neutralPoint - neutralWidth*0.5f)/(maxPoint - neutralPoint - neutralWidth*0.5f);
                    normalizedInput = Utilities.Clamp(normalizedInput, 0.0f, 1.0f);
                    output = Utilities.SmoothStep(normalizedInput)*(maxValue - neutralValue) + neutralValue;
                }
            }
            return output;
        }

        Controller _controller;
        bool _handlerConnected;
        LeapHandler _leapHandler;
        bool _warningPrinted = false;
    }


    class LeapHandler : Leap.Listener
    {
        public bool IsInitialized { get; private set; }
        public bool IsRunning { get; private set; }

        public float MaxGapTime { get; set; }

        public Vector WorkspaceCenter { get; set; }
        public float WorkspaceRadius { get; set; }
        public Vector ParameterCenter { get; set; }
        public float TrackingRange { get; set; }

        public float CameraFriction { get; set; }
        public float ParameterFriction { get; set; }

        public int Reference { get; set; }

        public SharpDX.Vector3 Position
        {
            get
            {
                if (!IsRunning)
                    return Vector3.Zero;

                Vector v;
                lock (_thisLock)
                {
                    v = _smoothedPosition - WorkspaceCenter;
                }
                return new Vector3(v.x, v.y, v.z);
            }
        }

        public SharpDX.Vector3 Rotation// yaw, pitch, roll in degree
        {
            get
            {
                if (!IsRunning)
                    return Vector3.Zero;

                Vector v;
                lock (_thisLock)
                {
                    v = _smoothedRotation;
                }
                return new Vector3(v.x, v.y, v.z);
            }
        }

        public SharpDX.Vector3 Parameter
        {
            get
            {
                if (!IsRunning)
                    return Vector3.Zero;

                Vector v;
                lock (_thisLock)
                {
                    v = _smoothedParameter;
                }
                return new Vector3(v.x, v.y, v.z);
            }
        }

        public float Quality
        {
            get
            {
                if (!IsRunning)
                    return 0.0f;

                float quality = 0;
                lock (_thisLock)
                {
                    float fadeOutTime = 1.0f;
                    quality = (float)_timeFromParameterPointableBecomesInvisible.ElapsedTicks/Stopwatch.Frequency < fadeOutTime ? 1.0f : 0.0f;
                }
                return quality;
            }
        }

        public LeapHandler()
        {
            MaxGapTime = 1.0f;
            WorkspaceCenter = new Vector(0, 250, 0);
            WorkspaceRadius = 250;
            ParameterCenter = new Vector(-50, 200, 0);
            TrackingRange = 50;
            _timeFromCameraPointableBecomesInvisible.Restart();
            _timeFromParameterPointableBecomesInvisible.Restart();
        }

        public bool Start()
        {
            if (!IsInitialized)
                return false;
            IsRunning = true;
            _currentPosition = Leap.Vector.Zero;
            _lastPosition = _currentPosition;

            _currentRotation = Leap.Vector.Zero;
            _lastRotation = _currentRotation;

            _currentParameter = Leap.Vector.Zero;
            _lastParameter = _currentParameter;

            lock (_thisLock)
            {
                _smoothedPosition = _currentPosition;
                _smoothedRotation = _currentRotation;
                _smoothedParameter = _currentParameter;
            }

            _timeFromCameraPointableBecomesInvisible.Restart();
            _timeFromParameterPointableBecomesInvisible.Restart();
            _frameTimer.Start();
            _lastUsedCameraPointableID = -1;
            _lastUsedParameterPointableID = -1;
            _cameraFriction = 0.9f;
            _parameterFriction = 0.9f;
            _smoothedCameraFriction = _cameraFriction;
            _smoothedParameterFriction = _parameterFriction;
            return true;
        }

        public void Stop()
        {
            IsRunning = false;
            _timeFromCameraPointableBecomesInvisible.Reset();
            _timeFromParameterPointableBecomesInvisible.Reset();
            _frameTimer.Stop();
        }

        public override void OnInit(Controller controller)
        {
            IsRunning = false;
            IsInitialized = false;
        }

        public override void OnConnect(Controller controller)
        {
            IsInitialized = true;
        }

        public override void OnDisconnect(Controller controller)
        {
            IsInitialized = false;
            IsRunning = false;
        }

        public override void OnExit(Controller controller)
        {
            IsInitialized = false;
            IsRunning = false;
        }

        public override void OnFrame(Controller controller)
        {
            if (!IsRunning)
                return;

            float maxTipVelocityForNewPointables = 40.0f;
            float minDistanceBetweenNewPointableToCameraAndParameterPointables = 150.0f;
            float fadeOutTime = 0.2f;

            Pointable cameraPointable = Pointable.Invalid;
            Pointable parameterPointable = Pointable.Invalid;

            using (Frame frame = controller.Frame())
            {
                if (frame.IsValid)
                {
                    cameraPointable = frame.Pointable(_lastUsedCameraPointableID);
                    parameterPointable = frame.Pointable(_lastUsedParameterPointableID);

                    if (!cameraPointable.IsValid)
                    {
                        float minZ = 99999;
                        Pointable frontMostPointable = Pointable.Invalid;
                        float minD = 99999;
                        Pointable closestPointableToLastCameraPosition = Pointable.Invalid;
                        foreach (var p in frame.Pointables)
                        {
                            bool pointableEqualsParameterPointable = p.Equals(parameterPointable);
                            bool pointableIsOnParameterHand = p.Hand.Equals(parameterPointable.Hand);
                            if (p.IsValid && !pointableEqualsParameterPointable && !pointableIsOnParameterHand)
                            {
                                bool isInWorkspace = (WorkspaceCenter - p.TipPosition).Magnitude < WorkspaceRadius;
                                bool pointableIsNearParameterPointable = (p.TipPosition - parameterPointable.TipPosition).Magnitude < minDistanceBetweenNewPointableToCameraAndParameterPointables;

                                if (isInWorkspace && p.TipPosition.z < minZ && !pointableIsNearParameterPointable)
                                {
                                    minZ = p.TipPosition.z;
                                    frontMostPointable = p;
                                }

                                var pos = p.TipPosition - p.Direction*90.0f;
                                float distanceToLastPosition = (_currentPosition - (pos - WorkspaceCenter)).Magnitude;
                                if (isInWorkspace && distanceToLastPosition < minD && !pointableIsNearParameterPointable)
                                {
                                    minD = distanceToLastPosition;
                                    closestPointableToLastCameraPosition = p;
                                }
                            }
                        }

                        if (closestPointableToLastCameraPosition.IsValid && minD < TrackingRange)
                        {
                            cameraPointable = closestPointableToLastCameraPosition;
                            Logger.Debug("get new camera pointable near to last known pointable: {0}", cameraPointable.Id);
                        }
                        else if (frontMostPointable.IsValid)
                        {
                            cameraPointable = frontMostPointable;
                            Logger.Debug("get new camera pointable from front most pointable: {0}", cameraPointable.Id);
                        }
                    }

                    if (!parameterPointable.IsValid && cameraPointable.IsValid)
                    {
                        float maxZ = -99999;
                        Pointable backMostPointable = Pointable.Invalid;
                        float minD = 99999;
                        Pointable closestPointableToLastParameterPosition = Pointable.Invalid;
                        foreach (var p in frame.Pointables)
                        {
                            bool pointableEqualsCameraPointable = p.Equals(cameraPointable);
                            bool pointableIsOnCameraHand = p.Hand.Equals(cameraPointable.Hand);
                            if (p.IsValid && !pointableEqualsCameraPointable && !pointableIsOnCameraHand)
                            {
                                bool isInWorkspace = (WorkspaceCenter - p.TipPosition).Magnitude < WorkspaceRadius;
                                bool pointableIsNearCameraPointable = (p.TipPosition - cameraPointable.TipPosition).Magnitude < minDistanceBetweenNewPointableToCameraAndParameterPointables;

                                if (isInWorkspace && p.TipPosition.z > maxZ && !pointableIsNearCameraPointable)
                                {
                                    maxZ = p.TipPosition.z;
                                    backMostPointable = p;
                                }

                                float distanceToLastPosition = (_currentParameter + ParameterCenter - p.TipPosition).Magnitude;
                                if (isInWorkspace && distanceToLastPosition < minD && !pointableIsNearCameraPointable)
                                {
                                    minD = distanceToLastPosition;
                                    closestPointableToLastParameterPosition = p;
                                }
                            }
                        }

                        if (closestPointableToLastParameterPosition.IsValid && minD < TrackingRange)
                        {
                            parameterPointable = closestPointableToLastParameterPosition;
                            Logger.Debug("get new parameter pointable near to last known pointable: {0}", parameterPointable.Id);
                        }
                        else if (backMostPointable.IsValid)
                        {
                            parameterPointable = backMostPointable;
                            Logger.Debug("get new parameter pointable from back most pointable: {0}", parameterPointable.Id);
                        }
                    }
                }
            }

            bool cameraPointableDetectedInFrame = cameraPointable.IsValid;
            bool cameraPointableDetectionTriggerDownFlank = !cameraPointableDetectedInFrame && _cameraPointableDetectedInLastFrame;
            _cameraPointableDetectedInLastFrame = cameraPointableDetectedInFrame;

            bool parameterPointableDetectedInFrame = parameterPointable.IsValid;
            bool parameterPointableDetectionTriggerDownFlank = !parameterPointableDetectedInFrame && _parameterPointableDetectedInLastFrame;
            _parameterPointableDetectedInLastFrame = parameterPointableDetectedInFrame;

            if (cameraPointableDetectionTriggerDownFlank)
            {
                _timeFromCameraPointableBecomesInvisible.Restart();
            }

            if (parameterPointableDetectionTriggerDownFlank)
            {
                _timeFromParameterPointableBecomesInvisible.Restart();
            }

            if (cameraPointableDetectedInFrame)
            {
                using (cameraPointable)
                {
                    var pos = cameraPointable.TipPosition - cameraPointable.Direction*90.0f;
                    _currentPosition = pos - WorkspaceCenter;
                    _currentRotation = new Leap.Vector(cameraPointable.Direction.Yaw * 180.0f / (float)Math.PI,
                                                       -cameraPointable.Direction.Pitch * 180.0f / (float)Math.PI,
                                                       0);
                    _lastUsedCameraPointableID = cameraPointable.Id;
                    _timeFromCameraPointableBecomesInvisible.Reset();
                }
            }
            else if (_lastUsedCameraPointableID >= 0 && (double)_timeFromCameraPointableBecomesInvisible.ElapsedTicks/Stopwatch.Frequency > fadeOutTime)
            {
                Logger.Info("camera pointable lost: {0}", _lastUsedCameraPointableID);
                _currentPosition = Leap.Vector.Zero;
                _currentRotation = Leap.Vector.Zero;
                _lastUsedCameraPointableID = -1;
            }


            if (parameterPointableDetectedInFrame)
            {
                using (parameterPointable)
                {
                    _currentParameter = parameterPointable.TipPosition - ParameterCenter;
                    _timeFromParameterPointableBecomesInvisible.Reset();
                    _lastUsedParameterPointableID = parameterPointable.Id;
                }
            }
            else if (_lastUsedParameterPointableID >= 0 && (double)_timeFromParameterPointableBecomesInvisible.ElapsedTicks/Stopwatch.Frequency > fadeOutTime)
            {
                Logger.Info("parameter pointable lost: {0}", _lastUsedParameterPointableID);
                _currentParameter = Leap.Vector.Zero;
                _lastUsedParameterPointableID = -1;
            }

            float elapsed = (float)(_frameTimer.ElapsedTicks/Stopwatch.Frequency);
            _frameTimer.Restart();

            if ((_lastPosition - _currentPosition).Magnitude > 100.0f)
            {
                _smoothedCameraFriction = 0.9999f;
            }

            if ((_lastParameter - _currentParameter).Magnitude > 100.0f)
            {
                _smoothedParameterFriction = 0.9999f;
            }

            float frictionFriction = 0.99f;
            _smoothedCameraFriction = frictionFriction*_smoothedCameraFriction + (1.0f - frictionFriction)*_cameraFriction;
            _smoothedParameterFriction = frictionFriction*_smoothedParameterFriction + (1.0f - frictionFriction)*_parameterFriction;

            lock (_thisLock)
            {
                _smoothedPosition = _smoothedCameraFriction*_smoothedPosition + (1.0f - _smoothedCameraFriction)*(WorkspaceCenter + _currentPosition);
                _smoothedRotation = _smoothedCameraFriction*_smoothedRotation + (1.0f - _smoothedCameraFriction)*_currentRotation;
                _smoothedParameter = _smoothedParameterFriction*_smoothedParameter + (1.0f - _smoothedParameterFriction)*(ParameterCenter + _currentParameter);
            }

            _lastPosition = _currentPosition;
            _lastRotation = _currentRotation;
            _lastParameter = _currentParameter;
        }

        bool _cameraPointableDetectedInLastFrame;
        bool _parameterPointableDetectedInLastFrame;
        Stopwatch _timeFromCameraPointableBecomesInvisible = new Stopwatch();
        Stopwatch _timeFromParameterPointableBecomesInvisible = new Stopwatch();
        Stopwatch _frameTimer = new Stopwatch();

        Leap.Vector _currentPosition = Leap.Vector.Zero; //relative to the workspace center coordinate system
        Leap.Vector _lastPosition = Leap.Vector.Zero;
        Leap.Vector _smoothedPosition = Leap.Vector.Zero;

        Leap.Vector _currentRotation = Leap.Vector.Zero;
        Leap.Vector _lastRotation = Leap.Vector.Zero;
        Leap.Vector _smoothedRotation = Leap.Vector.Zero;

        Leap.Vector _currentParameter = Leap.Vector.Zero; //relative to the parameter center coordinate system
        Leap.Vector _lastParameter = Leap.Vector.Zero;
        Leap.Vector _smoothedParameter = Leap.Vector.Zero;

        float _cameraFriction = 0;
        float _parameterFriction = 0;

        float _smoothedCameraFriction = 0;
        float _smoothedParameterFriction = 0;

        int _lastUsedCameraPointableID = -1;
        int _lastUsedParameterPointableID = -1;

        Object _thisLock = new Object();
    }

}
