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

Imports Microsoft.Kinect

Class MainWindow
#Region "メンバー変数"
    Private _KinectDevice As KinectSensor
    Private _GreenScreenImage As WriteableBitmap
    Private _GreenScreenImageRect As Int32Rect
    Private _GreenScreenImageStride As Integer
    Private _DepthPixelData() As Short
    Private _ColorPixelData() As Byte
    Private _DoUsePolling As Boolean
#End Region


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

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

        ' InitializeComponent() 呼び出しの後で初期化を追加します。
        Me._DoUsePolling = True

        If Me._DoUsePolling Then
            AddHandler CompositionTarget.Rendering, AddressOf CompositionTarget_Rendering
        Else
            AddHandler KinectSensor.KinectSensors.StatusChanged, AddressOf KinectSensors_StatusChanged
            Me.KinectDevice = KinectSensor.KinectSensors.FirstOrDefault(Function(x)
                                                                            Return x.Status = KinectStatus.Connected
                                                                        End Function)
        End If
    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 colorFrame As ColorImageFrame = e.OpenColorImageFrame()
            Using depthFrame As DepthImageFrame = e.OpenDepthImageFrame()
                RenderGreenScreen(Me._KinectDevice, colorFrame, depthFrame)
            End Using
        End Using
    End Sub

    Private Sub CompositionTarget_Rendering(sender As Object, e As EventArgs)
        DiscoverKinect()


        If Me.KinectDevice IsNot Nothing Then
            Try
                Using colorFrame As ColorImageFrame = Me.KinectDevice.ColorStream.OpenNextFrame(100)
                    Using depthFrame As DepthImageFrame = Me.KinectDevice.DepthStream.OpenNextFrame(100)
                        RenderGreenScreen(Me.KinectDevice, colorFrame, depthFrame)
                    End Using
                End Using
            Catch
                '必要に応じて例外をハンドルする     
            End Try
        End If
    End Sub

    Private Sub RenderGreenScreen(kinectDevice As KinectSensor, colorFrame As ColorImageFrame, depthFrame As DepthImageFrame)
        If kinectDevice IsNot Nothing AndAlso depthFrame IsNot Nothing AndAlso colorFrame IsNot Nothing Then
            Dim depthPixelIndex As Integer
            Dim playerIndex As Integer
            Dim colorPixelIndex As Integer
            Dim colorPoint As ColorImagePoint
            Dim colorStride As Integer = colorFrame.BytesPerPixel * colorFrame.Width
            Const bytesPerPixel As Integer = 4
            Dim playerImage(depthFrame.Height * Me._GreenScreenImageStride - 1) As Byte
            Dim playerImageIndex As Integer = 0


            depthFrame.CopyPixelDataTo(Me._DepthPixelData)
            colorFrame.CopyPixelDataTo(Me._ColorPixelData)


            For depthY As Integer = 0 To depthFrame.Height - 1
                For depthX As Integer = 0 To depthFrame.Width - 1
                    playerImageIndex += bytesPerPixel
                    depthPixelIndex = depthX + (depthY * depthFrame.Width)
                    playerIndex = Me._DepthPixelData(depthPixelIndex) And DepthImageFrame.PlayerIndexBitmask

                    If playerIndex <> 0 Then
                        colorPoint = kinectDevice.MapDepthToColorImagePoint(depthFrame.Format,
                                                                            depthX,
                                                                            depthY,
                                                                            Me._DepthPixelData(depthPixelIndex),
                                                                            colorFrame.Format)
                        colorPixelIndex = (colorPoint.X * colorFrame.BytesPerPixel) + (colorPoint.Y * colorStride)

                        playerImage(playerImageIndex + 0) = Me._ColorPixelData(colorPixelIndex + 0)         'Blue    
                        playerImage(playerImageIndex + 1) = Me._ColorPixelData(colorPixelIndex + 1)     'Green
                        playerImage(playerImageIndex + 2) = Me._ColorPixelData(colorPixelIndex + 2)     'Red
                        playerImage(playerImageIndex + 3) = &HFF                                          'Alpha
                    End If
                Next
            Next

            Me._GreenScreenImage.WritePixels(Me._GreenScreenImageRect,
                                             playerImage,
                                             Me._GreenScreenImageStride,
                                             0)
        End If
    End Sub

    Private Sub DiscoverKinect()
        If Me._KinectDevice IsNot Nothing AndAlso Me._KinectDevice.Status <> KinectStatus.Connected Then
            UninitializeKinectSensor(Me._KinectDevice)
            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
                InitializeKinectSensor(Me._KinectDevice)
            End If
        End If
    End Sub


    Private Sub InitializeKinectSensor(sensor As KinectSensor)
        If sensor IsNot Nothing Then
            sensor.DepthStream.Range = DepthRange.Default

            sensor.SkeletonStream.Enable()
            sensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30)
            sensor.ColorStream.Enable(ColorImageFormat.RgbResolution1280x960Fps12)



            Dim depthStream As DepthImageStream = sensor.DepthStream
            Me._GreenScreenImage = New WriteableBitmap(depthStream.FrameWidth,
                                                       depthStream.FrameHeight,
                                                       96,
                                                       96,
                                                       PixelFormats.Bgra32,
                                                       Nothing)
            Me._GreenScreenImageRect = New Int32Rect(0,
                                                     0,
                                                     Math.Ceiling(Me._GreenScreenImage.Width),
                                                     Math.Ceiling(Me._GreenScreenImage.Height))
            Me._GreenScreenImageStride = depthStream.FrameWidth * 4
            Me.GreenScreen_Image.Source = Me._GreenScreenImage

            ReDim Me._DepthPixelData(Me._KinectDevice.DepthStream.FramePixelDataLength - 1)
            ReDim Me._ColorPixelData(Me._KinectDevice.ColorStream.FramePixelDataLength - 1)

            If Not Me._DoUsePolling Then
                AddHandler sensor.AllFramesReady, AddressOf KinectDevice_AllFramesReady
            End If

            sensor.Start()
        End If
    End Sub


    Private Sub UninitializeKinectSensor(sensor As KinectSensor)
        If sensor IsNot Nothing Then
            sensor.Stop()
            sensor.ColorStream.Disable()
            sensor.DepthStream.Disable()
            sensor.SkeletonStream.Disable()
            RemoveHandler sensor.AllFramesReady, AddressOf KinectDevice_AllFramesReady
        End If
    End Sub
#End Region


#Region "プロパティ"
    Public Property KinectDevice As KinectSensor
        Get
            Return Me._KinectDevice
        End Get
        Set(value As KinectSensor)
            If Me._KinectDevice IsNot value Then
                'Uninitialize
                If Me._KinectDevice IsNot Nothing Then
                    UninitializeKinectSensor(Me._KinectDevice)
                    Me._KinectDevice = value
                End If

                Me._KinectDevice = value

                'Initialize
                If Me._KinectDevice IsNot Nothing Then
                    If Me._KinectDevice.Status = KinectStatus.Connected Then
                        InitializeKinectSensor(Me._KinectDevice)
                    End If
                End If
            End If
        End Set
    End Property
#End Region
End Class
