Imports System
Imports System.Collections.Generic
Imports System.Diagnostics
Imports System.Windows
Imports System.Windows.Controls
Imports System.Windows.Media
Imports System.Windows.Media.Imaging
Imports Microsoft.Kinect
Imports Microsoft.Kinect.Toolkit.FaceTracking

''' <summary>
''' Class that uses the Face Tracking SDK to display a face mask for
''' tracked skeletons
''' </summary>
Partial Public Class FaceTrackingViewer
    Inherits UserControl
    Implements IDisposable

    Public Shared ReadOnly KinectProperty As DependencyProperty = DependencyProperty.Register(
        "Kinect",
        GetType(KinectSensor),
        GetType(FaceTrackingViewer),
        New PropertyMetadata(
            Nothing, Sub(o, args)
                         CType(o, FaceTrackingViewer).OnSensorChanged(CType(args.OldValue, KinectSensor),
                                                                      CType(args.NewValue, KinectSensor))
                     End Sub))

    Private Const MaxMissedFrames As UInteger = 100

    Private ReadOnly trackedSkeletons As New Dictionary(Of Integer, SkeletonFaceTracker)()

    Private colorImage() As Byte

    Private _colorImageFormat As ColorImageFormat = ColorImageFormat.Undefined

    Private depthImage() As Short

    Private _depthImageFormat As DepthImageFormat = DepthImageFormat.Undefined

    Private skeletonData() As Skeleton

    Protected Overrides Sub Finalize()
        Me.Dispose(False)
    End Sub

    Public Property Kinect() As KinectSensor
        Get
            Return CType(Me.GetValue(KinectProperty), KinectSensor)
        End Get
        Set(value As KinectSensor)
            Me.SetValue(KinectProperty, value)
        End Set
    End Property

#Region "IDisposable Support"
    Private disposedValue As Boolean ' 重複する呼び出しを検出するには

    ' IDisposable
    Protected Overridable Sub Dispose(disposing As Boolean)
        If Not Me.disposedValue Then
            Me.ResetFaceTracking()
            If disposing Then
                ' TODO: マネージ状態を破棄します (マネージ オブジェクト)。
            End If

            ' TODO: アンマネージ リソース (アンマネージ オブジェクト) を解放し、下の Finalize() をオーバーライドします。
            ' TODO: 大きなフィールドを null に設定します。
        End If
        Me.disposedValue = True
    End Sub

    ' TODO: 上の Dispose(ByVal disposing As Boolean) にアンマネージ リソースを解放するコードがある場合にのみ、Finalize() をオーバーライドします。
    'Protected Overrides Sub Finalize()
    '    ' このコードを変更しないでください。クリーンアップ コードを上の Dispose(ByVal disposing As Boolean) に記述します。
    '    Dispose(False)
    '    MyBase.Finalize()
    'End Sub

    ' このコードは、破棄可能なパターンを正しく実装できるように Visual Basic によって追加されました。
    Public Sub Dispose() Implements IDisposable.Dispose
        ' このコードを変更しないでください。クリーンアップ コードを上の Dispose(ByVal disposing As Boolean) に記述します。
        Dispose(True)
        GC.SuppressFinalize(Me)
    End Sub
#End Region

    Protected Overrides Sub OnRender(drawingContext As DrawingContext)
        MyBase.OnRender(drawingContext)
        For Each faceInformation As SkeletonFaceTracker In Me.trackedSkeletons.Values
            faceInformation.DrawFaceModel(drawingContext)
        Next
    End Sub

    Private Sub OnAllFramesReady(sender As Object, e As AllFramesReadyEventArgs)
        Dim _colorImageFrame As ColorImageFrame = Nothing
        Dim _depthImageFrame As DepthImageFrame = Nothing
        Dim _skeletonFrame As SkeletonFrame = Nothing

        Try
            _colorImageFrame = e.OpenColorImageFrame()
            _depthImageFrame = e.OpenDepthImageFrame()
            _skeletonFrame = e.OpenSkeletonFrame()

            If (_colorImageFrame Is Nothing OrElse _depthImageFrame Is Nothing OrElse _skeletonFrame Is Nothing) Then
                Exit Sub
            End If

            ' Check for image format changes.  The FaceTracker doesn't
            ' deal with that so we need to reset.
            If (Me._depthImageFormat <> _depthImageFrame.Format) Then
                Me.ResetFaceTracking()
                Me.depthImage = Nothing
                Me._depthImageFormat = _depthImageFrame.Format
            End If

            If (Me._colorImageFormat <> _colorImageFrame.Format) Then
                Me.ResetFaceTracking()
                Me.colorImage = Nothing
                Me._colorImageFormat = _colorImageFrame.Format
            End If

            ' Create any buffers to store copies of the data we work with
            If (Me.depthImage Is Nothing) Then
                ReDim Me.depthImage(_depthImageFrame.PixelDataLength - 1)
            End If

            If (Me.colorImage Is Nothing) Then
                ReDim Me.colorImage(_colorImageFrame.PixelDataLength - 1)
            End If

            ' Get the skeleton information
            If (Me.skeletonData Is Nothing OrElse Me.skeletonData.Length <> _skeletonFrame.SkeletonArrayLength) Then
                ReDim Me.skeletonData(_skeletonFrame.SkeletonArrayLength - 1)
            End If

            _colorImageFrame.CopyPixelDataTo(Me.colorImage)
            _depthImageFrame.CopyPixelDataTo(Me.depthImage)
            _skeletonFrame.CopySkeletonDataTo(Me.skeletonData)

            ' Update the list of trackers and the trackers with the current frame information
            For Each _skeleton As Skeleton In Me.skeletonData
                If (_skeleton.TrackingState = SkeletonTrackingState.Tracked _
                    OrElse _skeleton.TrackingState = SkeletonTrackingState.PositionOnly) Then
                    ' We want keep a record of any skeleton, tracked or untracked.
                    If (Not Me.trackedSkeletons.ContainsKey(_skeleton.TrackingId)) Then
                        Me.trackedSkeletons.Add(_skeleton.TrackingId, New SkeletonFaceTracker())
                    End If

                    ' Give each tracker the upated frame.
                    Dim _skeletonFaceTracker As SkeletonFaceTracker = Nothing
                    If (Me.trackedSkeletons.TryGetValue(_skeleton.TrackingId, _skeletonFaceTracker)) Then
                        _skeletonFaceTracker.OnFrameReady(Me.Kinect,
                                                          _colorImageFormat,
                                                          colorImage,
                                                          _depthImageFormat,
                                                          depthImage,
                                                          _skeleton)
                        _skeletonFaceTracker.LastTrackedFrame = _skeletonFrame.FrameNumber
                    End If
                End If
            Next

            Me.RemoveOldTrackers(_skeletonFrame.FrameNumber)

            Me.InvalidateVisual()
        Finally
            If (_colorImageFrame IsNot Nothing) Then
                _colorImageFrame.Dispose()
            End If

            If (_depthImageFrame IsNot Nothing) Then
                _depthImageFrame.Dispose()
            End If

            If (_skeletonFrame IsNot Nothing) Then
                _skeletonFrame.Dispose()
            End If
        End Try
    End Sub

    Private Sub OnSensorChanged(oldSensor As KinectSensor, newSensor As KinectSensor)
        If (oldSensor IsNot Nothing) Then
            RemoveHandler oldSensor.AllFramesReady, AddressOf Me.OnAllFramesReady
            Me.ResetFaceTracking()
        End If

        If (newSensor IsNot Nothing) Then
            AddHandler newSensor.AllFramesReady, AddressOf Me.OnAllFramesReady
        End If
    End Sub

    ''' <summary>
    ''' Clear out any trackers for skeletons we haven't heard from for a while
    ''' </summary>
    Private Sub RemoveOldTrackers(currentFrameNumber As Integer)
        Dim trackersToRemove As New List(Of Integer)()

        For Each tracker In Me.trackedSkeletons
            Dim missedFrames As UInteger = CType(currentFrameNumber, UInteger) - CType(tracker.Value.LastTrackedFrame, UInteger)
            If (missedFrames > MaxMissedFrames) Then
                ' There have been too many frames since we last saw me skeleton
                trackersToRemove.Add(tracker.Key)
            End If
        Next

        For Each trackingId As Integer In trackersToRemove
            Me.RemoveTracker(trackingId)
        Next
    End Sub

    Private Sub RemoveTracker(trackingId As Integer)
        Me.trackedSkeletons(trackingId).Dispose()
        Me.trackedSkeletons.Remove(trackingId)
    End Sub

    Private Sub ResetFaceTracking()
        For Each trackingId As Integer In New List(Of Integer)(Me.trackedSkeletons.Keys)
            Me.RemoveTracker(trackingId)
        Next
    End Sub

    Private Class SkeletonFaceTracker
        Implements IDisposable

        Private _faceTracker As FaceTracker

        Private lastFaceTrackSucceeded As Boolean

        Private _skeletonTrackingState As SkeletonTrackingState

        Public Property LastTrackedFrame As Integer

        Public FaceRect As System.Windows.Rect
        Public Ratation As Vector3DF

#Region "IDisposable Support"
        Private disposedValue As Boolean ' 重複する呼び出しを検出するには

        ' IDisposable
        Protected Overridable Sub Dispose(disposing As Boolean)
            If Not Me.disposedValue Then
                If disposing Then
                    ' TODO: マネージ状態を破棄します (マネージ オブジェクト)。
                End If

                ' TODO: アンマネージ リソース (アンマネージ オブジェクト) を解放し、下の Finalize() をオーバーライドします。
                ' TODO: 大きなフィールドを null に設定します。
                If (Me._faceTracker IsNot Nothing) Then
                    Me._faceTracker.Dispose()
                    Me._faceTracker = Nothing
                End If
            End If
            Me.disposedValue = True
        End Sub

        ' TODO: 上の Dispose(ByVal disposing As Boolean) にアンマネージ リソースを解放するコードがある場合にのみ、Finalize() をオーバーライドします。
        'Protected Overrides Sub Finalize()
        '    ' このコードを変更しないでください。クリーンアップ コードを上の Dispose(ByVal disposing As Boolean) に記述します。
        '    Dispose(False)
        '    MyBase.Finalize()
        'End Sub

        ' このコードは、破棄可能なパターンを正しく実装できるように Visual Basic によって追加されました。
        Public Sub Dispose() Implements IDisposable.Dispose
            ' このコードを変更しないでください。クリーンアップ コードを上の Dispose(ByVal disposing As Boolean) に記述します。
            Dispose(True)
            GC.SuppressFinalize(Me)
        End Sub
#End Region
        Public Sub DrawFaceModel(drawingContext As DrawingContext)
            If (Not Me.lastFaceTrackSucceeded OrElse
                Me._skeletonTrackingState <> SkeletonTrackingState.Tracked) Then
                Exit Sub
            End If
            drawingContext.DrawImage(New BitmapImage(New Uri("Images/neko_02.png",
                                                                UriKind.Relative)),
                                                        Me.FaceRect)
        End Sub

        ''' <summary>
        ''' Updates the face tracking information for me skeleton
        ''' </summary>
        Friend Sub OnFrameReady(_kinectSensor As KinectSensor, _colorImageFormat As ColorImageFormat, colorImage() As Byte, _depthImageFormat As DepthImageFormat, depthImage() As Short, skeletonOfInterest As Skeleton)
            Me._skeletonTrackingState = skeletonOfInterest.TrackingState

            If (Me._skeletonTrackingState <> SkeletonTrackingState.Tracked) Then
                ' nothing to do with an untracked skeleton.
                Exit Sub
            End If

            If (Me._faceTracker Is Nothing) Then
                Try
                    Me._faceTracker = New FaceTracker(_kinectSensor)
                Catch ex As InvalidOperationException
                    ' During some shutdown scenarios the FaceTracker
                    ' is unable to be instantiated.  Catch that exception
                    ' and don't track a face.
                    Debug.WriteLine("AllFramesReady - creating a new FaceTracker threw an InvalidOperationException")
                    Me._faceTracker = Nothing
                End Try
            End If

            If (Me._faceTracker IsNot Nothing) Then
                Dim frame As FaceTrackFrame = Me._faceTracker.Track(
                    _colorImageFormat, colorImage, _depthImageFormat, depthImage, skeletonOfInterest)

                Me.lastFaceTrackSucceeded = frame.TrackSuccessful
                If (Me.lastFaceTrackSucceeded) Then
                    Me.FaceRect = New System.Windows.Rect(frame.FaceRect.Left,
                                                          frame.FaceRect.Top,
                                                          frame.FaceRect.Width,
                                                          frame.FaceRect.Height)
                    Me.Ratation = frame.Rotation
                End If
            End If
        End Sub
    End Class
End Class

