Imports System
Imports System.Linq
Imports System.Windows
Imports System.Windows.Controls
Imports System.Windows.Media
Imports System.Windows.Media.Imaging

Imports Microsoft.Kinect

''' <summary>
''' Interaction logic for MainWindow.xaml
''' </summary>
''' <remarks></remarks>
Class MainWindow
#Region "メンバー変数"
    Private _KinectDevice As KinectSensor
    Private _DepthImage As WriteableBitmap
    Private _DepthImageRect As Int32Rect
    Private _DepthPixelData() As Short
    Private _DepthImageStride As Integer
    Private _FrameSkeletons() As Skeleton
#End Region


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

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

        ' InitializeComponent() 呼び出しの後で初期化を追加します。
        AddHandler KinectSensor.KinectSensors.StatusChanged, AddressOf KinectSensors_StatusChanged
        Me.KinectDevice = KinectSensor.KinectSensors.FirstOrDefault(Function(x)
                                                                        Return x.Status = KinectStatus.Connected
                                                                    End Function)
    End Sub
#End Region


#Region "メソッド"
    Private Sub KinectSensors_StatusChanged(sender As Object, e As StatusChangedEventArgs)
        Select Case e.Status
            Case KinectStatus.Initializing,
                KinectStatus.Connected,
                KinectStatus.NotPowered,
                KinectStatus.NotReady,
                KinectStatus.DeviceNotGenuine
                Me.KinectDevice = e.Sensor
            Case KinectStatus.Disconnected
                'TODO: Give the user feedback to plug-in a Kinect device.                    
                Me.KinectDevice = Nothing
            Case Else
                'TODO: Show an error state
        End Select
    End Sub


    Private Sub KinectDevice_AllFramesReady(sender As Object, e As AllFramesReadyEventArgs)
        Using depthFrame As DepthImageFrame = e.OpenDepthImageFrame()
            If depthFrame IsNot Nothing Then
                Using _skeletonFrame As SkeletonFrame = e.OpenSkeletonFrame()
                    If _skeletonFrame IsNot Nothing Then
                        ProcessDepthFrame(depthFrame)
                        ProcessSkeletonFrame(_skeletonFrame)
                    End If
                End Using
            End If
        End Using
    End Sub

    Private Sub ProcessDepthFrame(depthFrame As DepthImageFrame)
        Dim depth As Integer
        Dim gray As Integer
        Const bytesPerPixel As Integer = 4
        Dim enhPixelData(depthFrame.Width * depthFrame.Height * bytesPerPixel - 1) As Byte

        depthFrame.CopyPixelDataTo(Me._DepthPixelData)

        For i As Integer = 0 To Me._DepthPixelData.Length - 1
            Dim j As Integer = i * bytesPerPixel
            depth = Me._DepthPixelData(i) >> DepthImageFrame.PlayerIndexBitmaskWidth

            If depth = 0 Then
                gray = &HFF
            Else
                gray = (255 * depth / &HFFF)
            End If
            enhPixelData(j + 0) = CType(gray, Byte)
            enhPixelData(j + 1) = CType(gray, Byte)
            enhPixelData(j + 2) = CType(gray, Byte)
        Next

        Me._DepthImage.WritePixels(Me._DepthImageRect,
                                   enhPixelData,
                                   Me._DepthImageStride,
                                   0)
    End Sub

    Private Sub ProcessSkeletonFrame(_skeletonFrame As SkeletonFrame)
        _skeletonFrame.CopySkeletonDataTo(Me._FrameSkeletons)
        Dim _skeleton As Skeleton = GetPrimarySkeleton(Me._FrameSkeletons)

        If _skeleton IsNot Nothing Then
            TrackHand(_skeleton.Joints(JointType.HandLeft),
                      LeftHandElement,
                      LeftHandScaleTransform,
                      LayoutRoot,
                      True)
            TrackHand(_skeleton.Joints(JointType.HandRight),
                      RightHandElement,
                      RightHandScaleTransform,
                      LayoutRoot,
                      False)
        End If
    End Sub


    ' リスト 5-11
    Private Sub TrackHand(hand As Joint,
                          cursorElement As FrameworkElement,
                          cursorScale As ScaleTransform,
                          container As FrameworkElement,
                          isLeft As Integer)
        If hand.TrackingState <> JointTrackingState.NotTracked Then
            Dim z As Double = hand.Position.Z
            cursorElement.Visibility = Visibility.Visible
            Dim jointPoint As Point = GetJointPoint(Me.KinectDevice,
                                                    hand,
                                                    container.RenderSize,
                                                    New Point(cursorElement.ActualWidth / 2.0,
                                                              cursorElement.ActualHeight / 2.0))
            Canvas.SetLeft(cursorElement, jointPoint.X)
            Canvas.SetTop(cursorElement, jointPoint.Y)
            Canvas.SetZIndex(cursorElement, Fix(400 - (z * 100)))

            cursorScale.ScaleX = 4 / z * (CType(IIf(isLeft, -1, 1), Integer))
            cursorScale.ScaleY = 4 / z

            If hand.JointType = JointType.HandLeft Then
                DebugLeftHand.Text = String.Format("Left Hand: {0:0.00}", z)
            Else
                DebugRightHand.Text = String.Format("Right Hand: {0:0.00}", z)
            End If
        Else
            DebugLeftHand.Text = String.Empty
            DebugRightHand.Text = String.Empty
        End If
    End Sub

    Private Shared Function GetJointPoint(kinectDevice As KinectSensor,
                                          _joint As Joint,
                                          containerSize As Size,
                                          offset As Point) As Point
        Dim _point As DepthImagePoint = kinectDevice.MapSkeletonPointToDepth(_joint.Position,
                                                                             kinectDevice.DepthStream.Format)

        _point.X = Fix((_point.X * containerSize.Width / kinectDevice.DepthStream.FrameWidth) - offset.X + 0.5F)
        _point.Y = Fix((_point.Y * containerSize.Height / kinectDevice.DepthStream.FrameHeight) - offset.Y + 0.5F)

        Return New Point(_point.X, _point.Y)
    End Function


    Private Shared Function GetPrimarySkeleton(skeletons() As Skeleton) As Skeleton
        Dim _skeleton As Skeleton = Nothing

        If skeletons IsNot Nothing Then
            'Find the closest skeleton       
            For i As Integer = 0 To skeletons.Length - 1
                If skeletons(i).TrackingState = SkeletonTrackingState.Tracked Then
                    If _skeleton Is Nothing Then
                        _skeleton = skeletons(i)
                    Else
                        If _skeleton.Position.Z > skeletons(i).Position.Z Then
                            _skeleton = skeletons(i)
                        End If
                    End If
                End If
            Next
        End If
        Return _skeleton
    End Function
#End Region


#Region "プロパティ"
    Public Property KinectDevice As KinectSensor
        Get
            Return Me._KinectDevice
        End Get
        Set(value As KinectSensor)
            If Me._KinectDevice IsNot value Then
                'リソースを解放する
                If Me._KinectDevice IsNot Nothing Then
                    Me._KinectDevice.Stop()
                    Me._KinectDevice.DepthStream.Disable()
                    Me._KinectDevice.SkeletonStream.Disable()
                    RemoveHandler Me._KinectDevice.AllFramesReady, AddressOf KinectDevice_AllFramesReady

                    Me.Depth_Image.Source = Nothing
                    Me._DepthImage = Nothing
                    Me._DepthImageStride = 0

                    Me._FrameSkeletons = Nothing
                End If

                Me._KinectDevice = value

                '初期化する
                If Me._KinectDevice IsNot Nothing Then
                    If Me._KinectDevice.Status = KinectStatus.Connected Then
                        AddHandler Me.KinectDevice.AllFramesReady, AddressOf KinectDevice_AllFramesReady
                        Me._KinectDevice.SkeletonStream.Enable()
                        Me._KinectDevice.DepthStream.Enable()

                        Dim depthStream As DepthImageStream = Me._KinectDevice.DepthStream
                        Me._DepthImage = New WriteableBitmap(depthStream.FrameWidth,
                                                             depthStream.FrameHeight,
                                                             96,
                                                             96,
                                                             PixelFormats.Bgr32,
                                                             Nothing)
                        Me._DepthImageRect = New Int32Rect(0,
                                                           0,
                                                           Math.Ceiling(Me._DepthImage.Width),
                                                           Math.Ceiling(Me._DepthImage.Height))
                        Me._DepthImageStride = depthStream.FrameWidth * 4
                        ReDim Me._DepthPixelData(depthStream.FramePixelDataLength - 1)
                        Me.Depth_Image.Source = Me._DepthImage

                        ReDim Me._FrameSkeletons(Me._KinectDevice.SkeletonStream.FrameSkeletonArrayLength - 1)

                        Me._KinectDevice.Start()
                    End If
                End If
            End If
        End Set
    End Property
#End Region
End Class
