Imports System
Imports System.Linq
Imports System.Text
Imports System.Windows
Imports System.Windows.Controls
Imports System.Windows.Threading


Imports Microsoft.Kinect

Public Enum GamePhase
    GameOver = 0
    SimonInstructing = 1
    PlayerPerforming = 2
End Enum

Class MainWindow
#Region "メンバー変数"
    Private _KinectDevice As KinectSensor
    Private _FrameSkeletons() As Skeleton
    Private _CurrentPhase As GamePhase
    Private _InstructionSequence() As Integer
    Private _InstructionPosition As Integer
    Private _CurrentLevel As Integer
    Private rnd As Random = New Random()
    Private _PoseLibrary() As Pose
    Private _StartPose As Pose
    Private _PoseTimer As DispatcherTimer
#End Region


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

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

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

        Me._PoseTimer = New DispatcherTimer()
        Me._PoseTimer.Interval = TimeSpan.FromSeconds(10)
        AddHandler Me._PoseTimer.Tick, Sub(s, e)
                                           ChangePhase(GamePhase.GameOver)
                                       End Sub
        Me._PoseTimer.Stop()

        PopulatePoseLibrary()
        ChangePhase(GamePhase.GameOver)

        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_SkeletonFrameReady(sender As Object, e As SkeletonFrameReadyEventArgs)
        Using frame As SkeletonFrame = e.OpenSkeletonFrame()
            If frame IsNot Nothing Then
                frame.CopySkeletonDataTo(Me._FrameSkeletons)
                Dim _skeleton As Skeleton = GetPrimarySkeleton(Me._FrameSkeletons)

                If _skeleton Is Nothing Then
                    ChangePhase(GamePhase.GameOver)
                Else
                    If Me._CurrentPhase = GamePhase.SimonInstructing Then
                        LeftHandElement.Visibility = System.Windows.Visibility.Collapsed
                        RightHandElement.Visibility = System.Windows.Visibility.Collapsed
                    Else
                        TrackHand(_skeleton.Joints(JointType.HandLeft), LeftHandElement, LayoutRoot)
                        TrackHand(_skeleton.Joints(JointType.HandRight), RightHandElement, LayoutRoot)

                        Select Case Me._CurrentPhase
                            Case GamePhase.GameOver
                                ProcessGameOver(_skeleton)

                            Case GamePhase.PlayerPerforming
                                ProcessPlayerPerforming(_skeleton)
                        End Select
                    End If
                End If
            End If
        End Using
    End Sub

    Private Sub TrackHand(hand As Joint, cursorElement As FrameworkElement, container As FrameworkElement)
        If hand.TrackingState = JointTrackingState.NotTracked Then
            cursorElement.Visibility = Visibility.Collapsed
        Else
            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)
        End If
    End Sub
 

    Private Sub ProcessGameOver(_skeleton As Skeleton)
        If IsPose(_skeleton, Me._StartPose) Then
            ChangePhase(GamePhase.SimonInstructing)
        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)
        _point.Y = Fix((_point.Y * containerSize.Height / kinectDevice.DepthStream.FrameHeight) - offset.Y)

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

    ' リスト 5-15
    Private Function GetJointAngle(centerJoint As Joint, angleJoint As Joint) As Double
        Dim primaryPoint As Point = GetJointPoint(Me.KinectDevice,
                                                  centerJoint,
                                                  Me.LayoutRoot.RenderSize,
                                                  New Point())
        Dim anglePoint As Point = GetJointPoint(Me.KinectDevice,
                                                angleJoint,
                                                Me.LayoutRoot.RenderSize,
                                                New Point())
        Dim x As Point = New Point(primaryPoint.X + anglePoint.X,
                                   primaryPoint.Y)
        Dim a As Double
        Dim b As Double
        Dim c As Double

        a = Math.Sqrt(Math.Pow(primaryPoint.X - anglePoint.X, 2) + Math.Pow(primaryPoint.Y - anglePoint.Y, 2))
        b = anglePoint.X
        c = Math.Sqrt(Math.Pow(anglePoint.X - x.X, 2) + Math.Pow(anglePoint.Y - x.Y, 2))

        Dim angleRad As Double = Math.Acos((a * a + b * b - c * c) / (2 * a * b))
        Dim angleDeg As Double = angleRad * 180 / Math.PI

        If primaryPoint.Y < anglePoint.Y Then
            angleDeg = 360 - angleDeg
        End If

        Return angleDeg
    End Function

    ' リスト 5-13
    Private Sub PopulatePoseLibrary()
        ReDim Me._PoseLibrary(3)


        'Start Pose - Arms Extended
        Me._StartPose = New Pose()
        Me._StartPose.Title = "T字ポーズ"
        ReDim Me._StartPose.Angles(3)
        Me._StartPose.Angles(0) = New PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, 180, 20)
        Me._StartPose.Angles(1) = New PoseAngle(JointType.ElbowLeft, JointType.WristLeft, 180, 20)
        Me._StartPose.Angles(2) = New PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, 0, 20)
        Me._StartPose.Angles(3) = New PoseAngle(JointType.ElbowRight, JointType.WristRight, 0, 20)


        'Pose 1 - Both Hands Up
        Me._PoseLibrary(0) = New Pose()
        Me._PoseLibrary(0).Title = "両腕アップ"
        ReDim Me._PoseLibrary(0).Angles(3)
        Me._PoseLibrary(0).Angles(0) = New PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, 180, 20)
        Me._PoseLibrary(0).Angles(1) = New PoseAngle(JointType.ElbowLeft, JointType.WristLeft, 90, 20)
        Me._PoseLibrary(0).Angles(2) = New PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, 0, 20)
        Me._PoseLibrary(0).Angles(3) = New PoseAngle(JointType.ElbowRight, JointType.WristRight, 90, 20)


        'Pose 2 - Both Hands Down
        Me._PoseLibrary(1) = New Pose()
        Me._PoseLibrary(1).Title = "両腕ダウン"
        ReDim Me._PoseLibrary(1).Angles(3)
        Me._PoseLibrary(1).Angles(0) = New PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, 180, 20)
        Me._PoseLibrary(1).Angles(1) = New PoseAngle(JointType.ElbowLeft, JointType.WristLeft, 270, 20)
        Me._PoseLibrary(1).Angles(2) = New PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, 0, 20)
        Me._PoseLibrary(1).Angles(3) = New PoseAngle(JointType.ElbowRight, JointType.WristRight, 270, 20)


        'Pose 3 - Left Up and Right Down
        Me._PoseLibrary(2) = New Pose()
        Me._PoseLibrary(2).Title = "左を上げて、右を下げて"
        ReDim Me._PoseLibrary(2).Angles(3)
        Me._PoseLibrary(2).Angles(0) = New PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, 180, 20)
        Me._PoseLibrary(2).Angles(1) = New PoseAngle(JointType.ElbowLeft, JointType.WristLeft, 90, 20)
        Me._PoseLibrary(2).Angles(2) = New PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, 0, 20)
        Me._PoseLibrary(2).Angles(3) = New PoseAngle(JointType.ElbowRight, JointType.WristRight, 270, 20)


        'Pose 4 - Right Up and Left Down
        Me._PoseLibrary(3) = New Pose()
        Me._PoseLibrary(3).Title = "右を上げて、左を下げて"
        ReDim Me._PoseLibrary(3).Angles(3)
        Me._PoseLibrary(3).Angles(0) = New PoseAngle(JointType.ShoulderLeft, JointType.ElbowLeft, 180, 20)
        Me._PoseLibrary(3).Angles(1) = New PoseAngle(JointType.ElbowLeft, JointType.WristLeft, 270, 20)
        Me._PoseLibrary(3).Angles(2) = New PoseAngle(JointType.ShoulderRight, JointType.ElbowRight, 0, 20)
        Me._PoseLibrary(3).Angles(3) = New PoseAngle(JointType.ElbowRight, JointType.WristRight, 90, 20)
    End Sub

    ' リスト 5-14
    Private Function IsPose(_skeleton As Skeleton, _pose As Pose) As Boolean
        Dim _isPose As Boolean = True
        Dim angle As Double
        Dim poseAngle As Double
        Dim poseThreshold As Double
        Dim loAngle As Double
        Dim hiAngle As Double

        For i As Integer = 0 To _pose.Angles.Length - 1
            If Not _isPose Then
                Exit For
            End If
            poseAngle = _pose.Angles(i).Angle
            poseThreshold = _pose.Angles(i).Threshold
            angle = GetJointAngle(_skeleton.Joints(_pose.Angles(i).CenterJoint),
                                  _skeleton.Joints(_pose.Angles(i).AngleJoint))

            hiAngle = poseAngle + poseThreshold
            loAngle = poseAngle - poseThreshold

            If hiAngle >= 360 OrElse loAngle < 0 Then
                loAngle = IIf(loAngle < 0, 360 + loAngle, loAngle)
                hiAngle = hiAngle Mod 360

                _isPose = Not (loAngle > angle AndAlso angle > hiAngle)
            Else
                _isPose = (loAngle <= angle AndAlso hiAngle >= angle)
            End If
        Next

        Return _isPose
    End Function

    ' リスト 5-17
    Private Sub ProcessPlayerPerforming(_skeleton As Skeleton)
        Dim instructionSeq As Integer = Me._InstructionSequence(Me._InstructionPosition)

        If IsPose(_skeleton, Me._PoseLibrary(instructionSeq)) Then
            Me._PoseTimer.Stop()
            Me._InstructionPosition += 1

            If Me._InstructionPosition >= Me._InstructionSequence.Length Then
                ChangePhase(GamePhase.SimonInstructing)
            Else
                'TODO: Notify the user of correct pose
                Me._PoseTimer.Start()
            End If
        End If
    End Sub


    Private Sub ChangePhase(newPhase As GamePhase)
        If newPhase <> Me._CurrentPhase Then
            Me._CurrentPhase = newPhase
            Me._PoseTimer.Stop()

            Select Me._CurrentPhase
                Case GamePhase.GameOver
                    Me._CurrentLevel = 0
                    GameStateElement.Text = "GAME OVER!"
                    GameInstructionsElement.Text = "ターゲットに手をかざすと新しいゲームを開始します。"

                Case GamePhase.SimonInstructing
                    Me._CurrentLevel += 1
                    GameStateElement.Text = String.Format("Level {0}", Me._CurrentLevel)
                    GameInstructionsElement.Text = "サイモンの指示通りに動きましょう。"
                    GenerateInstructions()
                    DisplayInstructions()

                Case GamePhase.PlayerPerforming
                    Me._PoseTimer.Start()
                    Me._InstructionPosition = 0
            End Select
        End If
    End Sub


    Private Sub GenerateInstructions()
        ReDim Me._InstructionSequence(Me._CurrentLevel - 1)

        For i As Integer = 0 To Me._CurrentLevel - 1
            Me._InstructionSequence(i) = rnd.Next(0, Me._PoseLibrary.Length - 1)
        Next
    End Sub


    Private Sub DisplayInstructions()
        GameInstructionsElement.Text = String.Empty
        Dim text As StringBuilder = New StringBuilder()
        Dim instructionsSeq As Integer

        For i As Integer = 0 To Me._InstructionSequence.Length - 1
            instructionsSeq = Me._InstructionSequence(i)
            text.AppendFormat("{0}, ", Me._PoseLibrary(instructionsSeq).Title)
        Next

        GameInstructionsElement.Text = text.ToString()
        ChangePhase(GamePhase.PlayerPerforming)
    End Sub


    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()
                    RemoveHandler Me._KinectDevice.SkeletonFrameReady, AddressOf KinectDevice_SkeletonFrameReady
                    Me._KinectDevice.SkeletonStream.Disable()
                    SkeletonViewerElement.KinectDevice = Nothing
                    Me._FrameSkeletons = Nothing
                End If

                Me._KinectDevice = value

                '初期化する
                If Me._KinectDevice IsNot Nothing Then
                    If Me._KinectDevice.Status = KinectStatus.Connected Then
                        Me._KinectDevice.SkeletonStream.Enable()
                        ReDim Me._FrameSkeletons(Me._KinectDevice.SkeletonStream.FrameSkeletonArrayLength - 1)
                        Me._KinectDevice.Start()

                        SkeletonViewerElement.KinectDevice = Me.KinectDevice
                        AddHandler Me.KinectDevice.SkeletonFrameReady, AddressOf KinectDevice_SkeletonFrameReady
                    End If
                End If
            End If
        End Set
    End Property
#End Region
End Class