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

Imports Microsoft.Kinect

''' <summary>
''' This project covers code リスト 3-1, 3-2, 3-4, 3-5, 3-6, 3-7, 3-8, 3-9.
''' </summary>
''' <remarks></remarks>
Class MainWindow
#Region "メンバー変数"
    Private _KinectDevice As KinectSensor
    Private _RawDepthImage As WriteableBitmap
    Private _RawDepthImageRect As Int32Rect
    Private _RawDepthImageStride As Integer
    Private _DepthImagePixelData() As Short
    Private _TotalFrames As Integer
    Private _StartFrameTime As DateTime
    Private _LastDepthFrame As DepthImageFrame
#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)

        AddHandler DepthImage.MouseLeftButtonUp, AddressOf DepthImage_MouseLeftButtonUp
    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


    ' リスト 3-2
    'Private Sub KinectDevice_DepthFrameReady(sender As Object, e As DepthImageFrameReadyEventArgs)
    '    Using frame As DepthImageFrame = e.OpenDepthImageFrame
    '        If frame IsNot Nothing Then
    '            Dim pixelData(frame.PixelDataLength - 1) As Short
    '            frame.CopyPixelDataTo(pixelData)
    '            Dim stride As Integer = frame.Width * frame.BytesPerPixel
    '            DepthImage.Source = BitmapSource.Create(frame.Width,
    '                                                    frame.Height,
    '                                                    96,
    '                                                    96,
    '                                                    PixelFormats.Gray16,
    '                                                    Nothing,
    '                                                    pixelData,
    '                                                    stride)
    '        End If
    '    End Using
    'End Sub
    '

    Private Sub KinectDevice_DepthFrameReady(sender As Object, e As DepthImageFrameReadyEventArgs)
        If Me._LastDepthFrame IsNot Nothing Then
            Me._LastDepthFrame.Dispose()
            Me._LastDepthFrame = Nothing
        End If

        Me._LastDepthFrame = e.OpenDepthImageFrame()

        If Me._LastDepthFrame IsNot Nothing Then
            Me._LastDepthFrame.CopyPixelDataTo(Me._DepthImagePixelData)

            Dim element As FrameworkElement = CType(ImageTreatmentSelector.SelectedItem, FrameworkElement)

            If element IsNot Nothing Then
                Dim tagValue As String = CType(element.Tag, String)

                Select Case tagValue
                    Case "1"
                        CreateLighterShadesOfGray(Me._LastDepthFrame,
                                                  Me._DepthImagePixelData)
                    Case "2"
                        CreateBetterShadesOfGray(Me._LastDepthFrame,
                                                 Me._DepthImagePixelData)
                    Case "3"
                        CreateColorDepthImage(Me._LastDepthFrame,
                                              Me._DepthImagePixelData)
                End Select
            End If

            Me._RawDepthImage.WritePixels(Me._RawDepthImageRect,
                                          Me._DepthImagePixelData,
                                          Me._RawDepthImageStride,
                                          0)
        End If

        Me._TotalFrames += 1
        FramesPerSecondElement.Text = String.Format("{0:0} fps", (Me._TotalFrames / DateTime.Now.Subtract(Me._StartFrameTime).TotalSeconds))
    End Sub

    ' リスト 3-5
    Private Sub DepthImage_MouseLeftButtonUp(sender As Object, e As MouseButtonEventArgs)
        Dim p As Point = e.GetPosition(DepthImage)

        If Me._DepthImagePixelData IsNot Nothing AndAlso Me._DepthImagePixelData.Length > 0 Then
            Dim width As Integer = Me._LastDepthFrame.Width
            Dim pixelIndex As Integer = Fix(p.X + (Fix(p.Y) * width))
            Dim depth As Integer = Me._DepthImagePixelData(pixelIndex) >> DepthImageFrame.PlayerIndexBitmaskWidth
            PixelDepth.Text = String.Format("{0}mm", depth)
        End If
    End Sub

    ' リスト 3-7  
    Private Sub CreateLighterShadesOfGray(depthFrame As DepthImageFrame, pixelData() As Short)
        Dim depth As Integer
        Const loThreshold As Integer = 1200
        Const hiThreshold As Integer = 3800
        Dim enhPixelData(depthFrame.Width * depthFrame.Height - 1) As Short

        For i As Integer = 0 To pixelData.Length - 1
            depth = pixelData(i) >> DepthImageFrame.PlayerIndexBitmaskWidth

            If depth < loThreshold OrElse depth > hiThreshold Then
                enhPixelData(i) = &HFF
            Else
                enhPixelData(i) = CType(Not pixelData(i), Short)
            End If
        Next

        EnhancedDepthImage.Source = BitmapSource.Create(depthFrame.Width,
                                                        depthFrame.Height,
                                                        96,
                                                        96,
                                                        PixelFormats.Gray16,
                                                        Nothing,
                                                        enhPixelData,
                                                        depthFrame.Width * depthFrame.BytesPerPixel)
    End Sub

    ' リスト 3-8
    Private Sub CreateBetterShadesOfGray(depthFrame As DepthImageFrame, pixelData() As Short)
        Dim depth As Integer
        Dim gray As Integer
        Const loThreshold As Integer = 1200
        Const hiThreshold As Integer = 3800
        Const bytesPerPixel As Integer = 4
        Dim enhPixelData(depthFrame.Width * depthFrame.Height * bytesPerPixel - 1) As Byte

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

            If depth < loThreshold OrElse depth > hiThreshold 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

        EnhancedDepthImage.Source = BitmapSource.Create(depthFrame.Width,
                                                        depthFrame.Height,
                                                        96,
                                                        96,
                                                        PixelFormats.Bgr32,
                                                        Nothing,
                                                        enhPixelData,
                                                        depthFrame.Width * bytesPerPixel)
    End Sub

    ' リスト 3-9 
    Private Sub CreateColorDepthImage(depthFrame As DepthImageFrame, pixelData() As Short)
        Dim depth As Integer
        Dim hue As Double
        Const loThreshold As Integer = 1200
        Const hiThreshold As Integer = 3800
        Const bytesPerPixel As Integer = 4
        Dim rgb(3 - 1) As Byte
        Dim enhPixelData(depthFrame.Width * depthFrame.Height * bytesPerPixel - 1) As Byte

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

            If depth < loThreshold OrElse depth > hiThreshold Then
                enhPixelData(j + 0) = &H0
                enhPixelData(j + 1) = &H0
                enhPixelData(j + 2) = &H0
            Else
                hue = ((360 * depth / &HFFF) + loThreshold)
                ConvertHslToRgb(hue, 100, 100, rgb)
                enhPixelData(j + 0) = rgb(2)  'Blue
                enhPixelData(j + 1) = rgb(1)  'Green
                enhPixelData(j + 2) = rgb(0)  'Red
            End If
        Next

        EnhancedDepthImage.Source = BitmapSource.Create(depthFrame.Width,
                                                        depthFrame.Height,
                                                        96,
                                                        96,
                                                        PixelFormats.Bgr32,
                                                        Nothing,
                                                        enhPixelData,
                                                        depthFrame.Width * bytesPerPixel)
    End Sub

    Public Sub ConvertHslToRgb(hue As Double, saturation As Double, lightness As Double, rgb() As Byte)
        Dim red As Double = 0.0
        Dim green As Double = 0.0
        Dim blue As Double = 0.0
        hue = hue Mod 360.0
        saturation = saturation / 100.0
        lightness = lightness / 100.0

        If saturation = 0.0 Then
            red = lightness
            green = lightness
            blue = lightness
        Else
            Dim huePrime As Double = hue / 60.0
            Dim x As Integer = Fix(huePrime)
            Dim xPrime As Double = huePrime - CType(x, Double)
            Dim L0 As Double = lightness * (1.0 - saturation)
            Dim L1 As Double = lightness * (1.0 - (saturation * xPrime))
            Dim L2 As Double = lightness * (1.0 - (saturation * (1.0 - xPrime)))

            Select Case x
                Case 0
                    red = lightness
                    green = L2
                    blue = L0
                Case 1
                    red = L1
                    green = lightness
                    blue = L0
                Case 2
                    red = L0
                    green = lightness
                    blue = L2
                Case 3
                    red = L0
                    green = L1
                    blue = lightness
                Case 4
                    red = L2
                    green = L0
                    blue = lightness
                Case 5
                    red = lightness
                    green = L0
                    blue = L1
            End Select
        End If

        rgb(0) = CType(255.0 * red, Byte)
        rgb(1) = CType(255.0 * green, Byte)
        rgb(2) = CType(255.0 * blue, Byte)
    End Sub

    Private Sub InitializeRawDepthImage(depthStream As DepthImageStream)
        If depthStream Is Nothing Then
            Me._RawDepthImage = Nothing
            Me._RawDepthImageRect = New Int32Rect()
            Me._RawDepthImageStride = 0
            Me._DepthImagePixelData = Nothing
        Else
            Me._RawDepthImage = New WriteableBitmap(depthStream.FrameWidth,
                                                    depthStream.FrameHeight,
                                                    96,
                                                    96,
                                                    PixelFormats.Gray16,
                                                    Nothing)
            Me._RawDepthImageRect = New Int32Rect(0,
                                                  0,
                                                  depthStream.FrameWidth,
                                                  depthStream.FrameHeight)
            Me._RawDepthImageStride = depthStream.FrameBytesPerPixel * depthStream.FrameWidth
            ReDim Me._DepthImagePixelData(depthStream.FramePixelDataLength - 1)
        End If

        Me.DepthImage.Source = Me._RawDepthImage
    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
                    Me._KinectDevice.Stop()
                    RemoveHandler Me._KinectDevice.DepthFrameReady, AddressOf KinectDevice_DepthFrameReady
                    Me._KinectDevice.DepthStream.Disable()
                    InitializeRawDepthImage(Nothing)

                    Me.DepthImage.Source = Nothing
                    Me._RawDepthImage = Nothing
                    Me.EnhancedDepthImage.Source = Nothing
                End If

                Me._KinectDevice = value

                'Initialize
                If Me._KinectDevice IsNot Nothing Then
                    If Me._KinectDevice.Status = KinectStatus.Connected Then
                        Me._KinectDevice.DepthStream.Enable()
                        InitializeRawDepthImage(Me._KinectDevice.DepthStream)
                        AddHandler Me._KinectDevice.DepthFrameReady, AddressOf KinectDevice_DepthFrameReady
                        Me._KinectDevice.Start()

                        Me._StartFrameTime = DateTime.Now
                    End If
                End If
            End If
        End Set
    End Property
#End Region
End Class
