Imports System
Imports System.ComponentModel
Imports System.Linq
Imports System.Windows
Imports System.Windows.Media
Imports System.Windows.Media.Imaging
Imports Microsoft.Kinect

''' <summary>
''' MainWindow.xaml の相互作用ロジック
''' </summary>  
Class MainWindow
#Region "メンバー変数"
    Private KinectDevice As KinectSensor
    Private ColorImageBitmap As WriteableBitmap
    Private ColorImageBitmapRect As Int32Rect
    Private ColorImageStride As Integer
    Private ColorImagePixelData() As Byte
    Private Worker As BackgroundWorker
#End Region

#Region "コンストラクタ"
    Public Sub New()

        ' この呼び出しはデザイナーで必要です。
        InitializeComponent()

        ' InitializeComponent() 呼び出しの後で初期化を追加します。
        Me.Worker = New BackgroundWorker()
        AddHandler Me.Worker.DoWork, AddressOf Worker_DoWork
        Me.Worker.RunWorkerAsync()

        AddHandler Me.Unloaded, Sub(s, e)
                                    Me.Worker.CancelAsync()
                                End Sub
    End Sub
#End Region

    Private Sub DiscoverKinectSensor()
        If (Me.KinectDevice IsNot Nothing AndAlso Me.KinectDevice.Status <> KinectStatus.Connected) Then
            Me.KinectDevice = Nothing
        End If

        If (Me.KinectDevice Is Nothing) Then
            Me.KinectDevice = KinectSensor.KinectSensors.FirstOrDefault(Function(x)
                                                                            Return x.Status = KinectStatus.Connected
                                                                        End Function)

            If (Me.KinectDevice IsNot Nothing) Then
                Me.KinectDevice.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30)
                Me.KinectDevice.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30)
                Me.KinectDevice.SkeletonStream.Enable()
                Me.KinectDevice.Start()

                Dim colorStream As ColorImageStream = Me.KinectDevice.ColorStream

                Me.ColorImageStride = colorStream.FrameWidth * colorStream.FrameBytesPerPixel
                ReDim Me.ColorImagePixelData(colorStream.FramePixelDataLength - 1)
                Me.Rgb_Image.Dispatcher.BeginInvoke(New Action(Sub()
                                                                   Me.ColorImageBitmap = New WriteableBitmap(colorStream.FrameWidth,
                                                                                                             colorStream.FrameHeight,
                                                                                                             96,
                                                                                                             96,
                                                                                                             PixelFormats.Bgr32,
                                                                                                             Nothing)
                                                                   Me.ColorImageBitmapRect = New Int32Rect(0,
                                                                                                           0,
                                                                                                           colorStream.FrameWidth,
                                                                                                           colorStream.FrameHeight)
                                                                   Me.Rgb_Image.Source = Me.ColorImageBitmap
                                                               End Sub))
            End If
        End If
    End Sub


    Private Sub PollColorImageStream()
        If (Me.KinectDevice Is Nothing) Then
            'TODO:使用可能なセンサーが存在しないことを通知する
        Else
            Try
                Using frame As colorimageframe = Me.KinectDevice.ColorStream.OpenNextFrame(100)
                    If (frame IsNot Nothing) Then
                        frame.CopyPixelDataTo(Me.ColorImagePixelData)

                        Me.Rgb_Image.Dispatcher.BeginInvoke(New Action(Sub()
                                                                           Me.ColorImageBitmap.WritePixels(Me.ColorImageBitmapRect,
                                                                                                             Me.ColorImagePixelData,
                                                                                                             Me.ColorImageStride,
                                                                                                             0)
                                                                       End Sub))
                    End If
                End Using
                Using frame As SkeletonFrame = Me.KinectDevice.SkeletonStream.OpenNextFrame(100)
                    If (frame IsNot Nothing) Then
                        DrawSkeleton(frame)
                    End If
                End Using
            Catch
            End Try
        End If
    End Sub

    Private Sub DrawSkeleton(frame As SkeletonFrame)
        Dim skeletons(frame.SkeletonArrayLength - 1) As Skeleton
        frame.CopySkeletonDataTo(skeletons)
        Me.Skeleton_Canvas.Dispatcher.BeginInvoke(New Action(Sub()
                                                                 Me.Skeleton_Canvas.Children.Clear()
                                                                 For Each _skeleton As Skeleton In skeletons
                                                                     If (_skeleton.TrackingState = SkeletonTrackingState.Tracked) Then
                                                                         For Each _joint As Joint In _skeleton.Joints
                                                                             If (_joint.TrackingState = JointTrackingState.Tracked) Then
                                                                                 Dim position As ColorImagePoint =
                                                                                         KinectDevice.MapSkeletonPointToColor(_joint.Position,
                                                                                                                              KinectDevice.ColorStream.Format)
                                                                                 Me.Skeleton_Canvas.Children.Add(
                                                                                     New System.Windows.Shapes.Ellipse() With {
                                                                                         .Margin = New Thickness(position.X - 5,
                                                                                                                position.Y - 5,
                                                                                                                0,
                                                                                                                0),
                                                                                         .Fill = New SolidColorBrush(Colors.Red),
                                                                                         .Width = 10,
                                                                                         .Height = 10
                                                                                     })
                                                                             End If
                                                                         Next
                                                                     End If
                                                                 Next
                                                             End Sub))
    End Sub

    Private Sub Worker_DoWork(sender As Object, e As DoWorkEventArgs)
        Dim worker As BackgroundWorker = CType(sender, BackgroundWorker)

        If (worker IsNot Nothing) Then
            Do While (Not worker.CancellationPending)
                DiscoverKinectSensor()
                PollColorImageStream()
            Loop
        End If
    End Sub

    Private Sub Mode_ComboBox_SelectionChanged(sender As Object,
                                               e As System.Windows.Controls.SelectionChangedEventArgs)
        If (Me.KinectDevice IsNot Nothing AndAlso Me.KinectDevice.SkeletonStream IsNot Nothing) Then
            Dim tagValue As String = CType(CType(Me.Mode_ComboBox.SelectedItem, FrameworkElement).Tag, String)
            Select Case tagValue
                Case "1"
                    Me.KinectDevice.SkeletonStream.TrackingMode = SkeletonTrackingMode.Default
                Case "2"
                    Me.KinectDevice.SkeletonStream.TrackingMode = SkeletonTrackingMode.Seated
            End Select
        End If
    End Sub
End Class
