Imports System.Collections.Generic
Imports System.Windows
Imports System
Imports Beginning.Kinect.Framework.Input
Imports Microsoft.Kinect

''' <summary>
''' Cursor Manager coordinates hand tracking.
''' </summary>
Public Class KinectCursorManager
    ' local members for managing kinect and hand tracking objects
    Private _kinectSensor As KinectSensor
    Private _cursorAdorner As CursorAdorner
    Private ReadOnly _window As Window
    Private _lastElementOver As UIElement
    Private _isHandTrackingActivated As Boolean
    Private Shared _isInitialized As Boolean
    Private Shared _instance As KinectCursorManager
    Private _gesturePoints As List(Of GesturePoint)
    Private _gesturePointTrackingEnabled As Boolean
    Private _swipeLength As Double, _swipeDeviation As Double
    Private _swipeTime As Integer
    Private _hasHandThreshold As Boolean = True

    Private _xOutOfBoundsLength As Double
    Private Shared _initialSwipeX As Double

    ''' <summary>
    ''' Occurs when [swipe detected].
    ''' </summary>
    Public Event SwipeDetected As KinectCursorEventHandler
    ''' <summary>
    ''' Occurs when [swipe out of bounds detected].
    ''' </summary>
    Public Event SwipeOutOfBoundsDetected As KinectCursorEventHandler

    ''' <summary>
    ''' Creates the specified window.
    ''' </summary>
    ''' <param name="_window">The window.</param>
    Public Shared Sub Create(_window As Window)
        If (Not _isInitialized) Then
            _instance = New KinectCursorManager(_window)
            _isInitialized = True
        End If
    End Sub

    ''' <summary>
    ''' Creates the specified window.
    ''' </summary>
    ''' <param name="_window">The window.</param>
    ''' <param name="cursor">The cursor.</param>
    Public Shared Sub Create(_window As Window, cursor As FrameworkElement)
        If (Not _isInitialized) Then
            _instance = New KinectCursorManager(_window, cursor)
            _isInitialized = True
        End If
    End Sub

    ''' <summary>
    ''' Creates the specified window.
    ''' </summary>
    ''' <param name="_window">The window.</param>
    ''' <param name="sensor">The sensor.</param>
    Public Shared Sub Create(_window As Window, sensor As KinectSensor)
        If (Not _isInitialized) Then
            _instance = New KinectCursorManager(_window, sensor)
            _isInitialized = True
        End If
    End Sub

    ''' <summary>
    ''' Creates the specified window.
    ''' </summary>
    ''' <param name="_window">The window.</param>
    ''' <param name="sensor">The sensor.</param>
    ''' <param name="cursor">The cursor.</param>
    Public Shared Sub Create(_window As Window, sensor As KinectSensor, cursor As FrameworkElement)
        If (Not _isInitialized) Then
            _instance = New KinectCursorManager(_window, sensor, cursor)
            _isInitialized = True
        End If
    End Sub

    ''' <summary>
    ''' Gets the instance.
    ''' </summary>
    Public Shared ReadOnly Property Instance As KinectCursorManager
        Get
            Return _instance
        End Get
    End Property

    ''' <summary>
    ''' Prevents a default instance of the <see cref="KinectCursorManager"/> class from being created.
    ''' </summary>
    ''' <param name="_window">The window.</param>
    Private Sub New(_window As Window)
        Me.New(_window, KinectSensor.KinectSensors(0))
    End Sub

    ''' <summary>
    ''' Prevents a default instance of the <see cref="KinectCursorManager"/> class from being created.
    ''' </summary>
    ''' <param name="_window">The window.</param>
    ''' <param name="cursor">The cursor.</param>
    Private Sub New(_window As Window, cursor As FrameworkElement)
        Me.New(_window, KinectSensor.KinectSensors(0), cursor)
    End Sub

    ''' <summary>
    ''' Prevents a default instance of the <see cref="KinectCursorManager"/> class from being created.
    ''' </summary>
    ''' <param name="_window">The window.</param>
    ''' <param name="sensor">The sensor.</param>
    Private Sub New(_window As Window, sensor As KinectSensor)
        Me.new(_window, sensor, Nothing)
    End Sub

    ''' <summary>
    ''' Prevents a default instance of the <see cref="KinectCursorManager"/> class from being created.
    ''' </summary>
    ''' <param name="_window">The window.</param>
    ''' <param name="sensor">The sensor.</param>
    ''' <param name="cursor">The cursor.</param>
    Private Sub New(_window As Window, sensor As KinectSensor, cursor As FrameworkElement)
        Me._window = _window
        Me._gesturePoints = New List(Of GesturePoint)()
        ' ensure kinects are present
        If (KinectSensor.KinectSensors.Count > 0) Then
            AddHandler _window.Unloaded, Sub()
                                             If (Me._kinectSensor.SkeletonStream.IsEnabled) Then
                                                 Me._kinectSensor.SkeletonStream.Disable()
                                             End If
                                             _kinectSensor.Stop()
                                         End Sub

            AddHandler _window.Loaded, Sub()
                                           If (cursor Is Nothing) Then
                                               _cursorAdorner = New CursorAdorner(CType(_window.Content, FrameworkElement))
                                           Else
                                               _cursorAdorner = New CursorAdorner(CType(_window.Content, FrameworkElement), cursor)
                                           End If

                                           Me._kinectSensor = sensor

                                           AddHandler Me._kinectSensor.SkeletonFrameReady, AddressOf SkeletonFrameReady
                                           Me._kinectSensor.SkeletonStream.Enable(New TransformSmoothParameters() With {.Correction = 0.5F,
                                                                                                                        .JitterRadius = 0.05F,
                                                                                                                        .MaxDeviationRadius = 0.04F,
                                                                                                                        .Smoothing = 0.5F})
                                           Me._kinectSensor.Start()
                                       End Sub

        End If
    End Sub

    ''' <summary>
    ''' Gets or sets a value indicating whether me instance has hand threshold.
    ''' </summary>
    ''' <value>
    ''' 	<c>true</c> if me instance has hand threshold otherwise, <c>false</c>.
    ''' </value>
    Public Property HasHandThreshold As Boolean
        Get
            Return _hasHandThreshold
        End Get
        Set(value As Boolean)
            _hasHandThreshold = value
        End Set
    End Property

    ''' <summary>
    ''' Skeletons the frame ready.
    ''' </summary>
    ''' <param name="sender">The sender.</param>
    ''' <param name="e">The <see cref="Microsoft.Kinect.SkeletonFrameReadyEventArgs"/> instance containing the event data.</param>
    Private Sub SkeletonFrameReady(sender As Object, e As SkeletonFrameReadyEventArgs)
        Using frame As SkeletonFrame = e.OpenSkeletonFrame()
            If (frame Is Nothing OrElse frame.SkeletonArrayLength = 0) Then
                Exit Sub
            End If

            Dim skeletons(frame.SkeletonArrayLength - 1) As Skeleton
            frame.CopySkeletonDataTo(skeletons)
            Dim _skeleton As Skeleton = GetPrimarySkeleton(skeletons)

            If (_skeleton Is Nothing) Then
                SetHandTrackingDeactivated()
            Else
                Dim primaryHand As Joint? = GetPrimaryHand(_skeleton)
                If (primaryHand.HasValue) Then
                    UpdateCursor(primaryHand.Value)
                Else
                    SetHandTrackingDeactivated()
                End If
            End If
        End Using
    End Sub

    ''' <summary>
    ''' Updates the cursor.
    ''' </summary>
    ''' <param name="hand">The hand.</param>
    Private Sub UpdateCursor(hand As Joint)
        Dim _point As DepthImagePoint = _kinectSensor.MapSkeletonPointToDepth(hand.Position, _kinectSensor.DepthStream.Format)
        Dim x As Single = _point.X
        Dim y As Single = _point.Y
        Dim z As Single = _point.Depth

        SetHandTrackingActivated()
        x = (x * _window.ActualWidth / _kinectSensor.DepthStream.FrameWidth)
        y = (y * _window.ActualHeight / _kinectSensor.DepthStream.FrameHeight)
        Dim cursorPoint As Point = New Point(x, y)
        HandleGestureTracking(x, y, z)
        HandleCursorEvents(cursorPoint, z, hand)
        _cursorAdorner.UpdateCursor(cursorPoint)
    End Sub

    ''' <summary>
    ''' Sets the hand tracking to activated state.
    ''' </summary>
    Private Sub SetHandTrackingActivated()
        _cursorAdorner.SetVisibility(True)
        If (_lastElementOver IsNot Nothing AndAlso
            _isHandTrackingActivated = False) Then
            _lastElementOver.RaiseEvent(New RoutedEventArgs(KinectInput.KinectCursorActivatedEvent))
        End If
        _isHandTrackingActivated = True
    End Sub

    ''' <summary>
    ''' Sets the hand tracking to deactivated state.
    ''' </summary>
    Private Sub SetHandTrackingDeactivated()
        _cursorAdorner.SetVisibility(False)
        If (_lastElementOver IsNot Nothing AndAlso
            _isHandTrackingActivated = True) Then
            _lastElementOver.RaiseEvent(New RoutedEventArgs(KinectInput.KinectCursorDeactivatedEvent))
        End If
        _isHandTrackingActivated = False
    End Sub

    ''' <summary>
    ''' Gets the primary skeleton.
    ''' </summary>
    ''' <param name="skeletons">The skeletons.</param>
    ''' <returns></returns>
    Private Shared Function GetPrimarySkeleton(skeletons As IEnumerable(Of Skeleton)) As Skeleton
        Dim primarySkeleton As Skeleton = Nothing
        For Each _skeleton As Skeleton In skeletons
            If (_skeleton.TrackingState <> SkeletonTrackingState.Tracked) Then
                Continue For
            End If

            If (primarySkeleton Is Nothing) Then
                primarySkeleton = _skeleton
            ElseIf (primarySkeleton.Position.Z > _skeleton.Position.Z) Then
                primarySkeleton = _skeleton
            End If
        Next
        Return primarySkeleton
    End Function

    ''' <summary>
    ''' Gets the primary hand.
    ''' </summary>
    ''' <param name="_skeleton">The skeleton.</param>
    ''' <returns></returns>
    Private Function GetPrimaryHand(_skeleton As Skeleton) As Joint?
        Dim leftHand As Joint = _skeleton.Joints(JointType.HandLeft)
        Dim rightHand As Joint = _skeleton.Joints(JointType.HandRight)

        If (rightHand.TrackingState = JointTrackingState.Tracked) Then
            If (leftHand.TrackingState <> JointTrackingState.Tracked) Then
                Return rightHand
            ElseIf (leftHand.Position.Z > rightHand.Position.Z) Then
                Return rightHand
            Else
                Return leftHand
            End If
        End If
        If (leftHand.TrackingState = JointTrackingState.Tracked) Then
            Return leftHand
        Else
            Return Nothing
        End If
    End Function

    ''' <summary>
    ''' Handles the gesture tracking.
    ''' </summary>
    ''' <param name="x">The x.</param>
    ''' <param name="y">The y.</param>
    ''' <param name="z">The z.</param>
    Private Sub HandleGestureTracking(x As Single, y As Single, z As Single)
        If (Not _gesturePointTrackingEnabled) Then
            Exit Sub
        End If
        ' check to see if xOutOfBounds is being used
        If (_xOutOfBoundsLength <> 0 AndAlso _initialSwipeX = 0) Then
            _initialSwipeX = x
        End If

        Dim newPoint As New GesturePoint() With {.x = x, .y = y, .z = z, .T = DateTime.Now}
        GesturePoints.Add(newPoint)

        Dim startPoint As GesturePoint = GesturePoints(0)
        Dim _point As New Point(x, y)


        'check for deviation
        If (Math.Abs(newPoint.Y - startPoint.Y) > _swipeDeviation) Then
            'Debug.WriteLine("Y out of bounds")
            RaiseEvent SwipeOutOfBoundsDetected(Me, New KinectCursorEventArgs(_point) With {.Z = z, .Cursor = _cursorAdorner})
            ResetGesturePoint(GesturePoints.Count)
            Exit Sub
        End If
        If ((newPoint.T - startPoint.T).Milliseconds > _swipeTime) Then 'check time
            GesturePoints.RemoveAt(0)
            startPoint = GesturePoints(0)
        End If
        If ((_swipeLength < 0 AndAlso newPoint.X - startPoint.X < _swipeLength) _
            OrElse (_swipeLength > 0 AndAlso newPoint.X - startPoint.X > _swipeLength)) Then
            GesturePoints.Clear()

            'throw local event
            RaiseEvent SwipeDetected(Me, New KinectCursorEventArgs(_point) With {.Z = z, .Cursor = _cursorAdorner})
            Exit Sub
        End If
        If (_xOutOfBoundsLength <> 0 AndAlso
            ((_xOutOfBoundsLength < 0 AndAlso newPoint.X - _initialSwipeX < _xOutOfBoundsLength) _
            OrElse (_xOutOfBoundsLength > 0 AndAlso newPoint.X - _initialSwipeX > _xOutOfBoundsLength))
            ) Then
            RaiseEvent SwipeOutOfBoundsDetected(Me, New KinectCursorEventArgs(_point) With {.Z = z, .Cursor = _cursorAdorner})
        End If
    End Sub

    ''' <summary>
    ''' Handles the cursor events.
    ''' </summary>
    ''' <param name="_point">The point.</param>
    ''' <param name="z">The z.</param>
    ''' <param name="_joint">The joint.</param>
    Private Sub HandleCursorEvents(_point As Point, z As Double, _joint As Joint)
        Dim element As UIElement = GetElementAtScreenPoint(_point, _window)
        If (element IsNot Nothing) Then
            element.RaiseEvent(New KinectCursorEventArgs(KinectInput.KinectCursorMoveEvent, _point, z) With {.Cursor = _cursorAdorner})
            If (element IsNot _lastElementOver) Then
                If (_lastElementOver IsNot Nothing) Then
                    _lastElementOver.RaiseEvent(New KinectCursorEventArgs(KinectInput.KinectCursorLeaveEvent, _point, z) With {.Cursor = _cursorAdorner})
                End If

                element.RaiseEvent(New KinectCursorEventArgs(KinectInput.KinectCursorEnterEvent, _point, z) With {.Cursor = _cursorAdorner})
            End If
        End If
        _lastElementOver = element
    End Sub

    ''' <summary>
    ''' Retrieve the input element at the given screen point.
    ''' </summary>
    ''' <param name="_point">The point for hit testing.</param>
    ''' <param name="_window">The TouchWindow for hit testing.</param>
    ''' <returns>UIElement result of the hit test. Nothing if no elements were located.</returns>
    Private Shared Function GetElementAtScreenPoint(_point As Point, _window As Window) As UIElement
        If (Not _window.IsVisible) Then
            Return Nothing
        End If

        Dim windowPoint As Point = _window.PointFromScreen(_point)

        Dim element As IInputElement = _window.InputHitTest(windowPoint)
        If (TypeOf element Is UIElement) Then
            Return CType(element, UIElement)
        Else
            Return Nothing
        End If
    End Function

    ''' <summary>
    ''' Gets the gesture points.
    ''' </summary>
    Public ReadOnly Property GesturePoints As IList(Of GesturePoint)
        Get
            Return _gesturePoints
        End Get
    End Property

    ''' <summary>
    ''' Gestures the point tracking initialize.
    ''' </summary>
    ''' <param name="swipeLength">Length of the swipe.</param>
    ''' <param name="swipeDeviation">The swipe deviation.</param>
    ''' <param name="swipeTime">The swipe time.</param>
    ''' <param name="xOutOfBounds">The x out of bounds.</param>
    Public Sub GesturePointTrackingInitialize(swipeLength As Double, swipeDeviation As Double, swipeTime As Integer, xOutOfBounds As Double)
        _swipeLength = swipeLength
        _swipeDeviation = swipeDeviation
        _swipeTime = swipeTime
        _xOutOfBoundsLength = xOutOfBounds
    End Sub

    ''' <summary>
    ''' Gestures the point tracking start.
    ''' </summary>
    Public Sub GesturePointTrackingStart()
        If (_swipeLength = 0 OrElse _swipeDeviation = 0 OrElse _swipeTime = 0) Then
            Throw (New InvalidOperationException("Swipe detection not initialized."))
        End If
        _gesturePointTrackingEnabled = True
    End Sub

    ''' <summary>
    ''' Gestures the point tracking stop.
    ''' </summary>
    Public Sub GesturePointTrackingStop()
        _xOutOfBoundsLength = 0
        _gesturePointTrackingEnabled = False
        _gesturePoints.Clear()
    End Sub


    ''' <summary>
    ''' Gets a value indicating whether [gesture point tracking enabled].
    ''' </summary>
    ''' <value>
    ''' 	<c>true</c> if [gesture point tracking enabled] otherwise, <c>false</c>.
    ''' </value>
    Public ReadOnly Property GesturePointTrackingEnabled As Boolean
        Get
            Return _gesturePointTrackingEnabled
        End Get
    End Property

    ''' <summary>
    ''' Resets the gesture point.
    ''' </summary>
    ''' <param name="_point">The point.</param>
    Private Sub ResetGesturePoint(_point As GesturePoint)
        Dim startRemoving As Boolean = False
        For i As Integer = GesturePoints.Count - 1 To 0 Step -1
            If (startRemoving) Then
                GesturePoints.RemoveAt(i)
            Else
                If (GesturePoints(i).Equals(_point)) Then
                    startRemoving = True
                End If
            End If
        Next
    End Sub

    ''' <summary>
    ''' Resets the gesture point.
    ''' </summary>
    ''' <param name="_point">The point.</param>
    Private Sub ResetGesturePoint(_point As Integer)
        If (_point < 1) Then
            Exit Sub
        End If
        For i As Integer = _point - 1 To 0 Step -1
            GesturePoints.RemoveAt(i)
        Next
    End Sub
End Class
