Imports System.Math
Imports System.Drawing
Imports Microsoft.Kinect



Module modScanVectors

    Class classGetScanVectors
        Shared Event evntVectorsReceived()

        Shared Sub subGetKinectScanVectors()
            Try
                Dim depthPixels(640 * 480 - 1) As DepthImagePixel
                Const intMiddleDepthSample As Integer = 640 * 321 - 2
                Const constBeamsFactor As Integer = 640 / constMaxScannerBeams
                ReDim depthPixels(640 * 480 - 1)
                While Not boolShutDown
                    If boolKinectConnected Then
                        If boolDebugKinect Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " subGetKinectScanVectors:  START")
                        Using depthFrame As DepthImageFrame = K_Sensor.DepthStream.OpenNextFrame(100)  ' many System.InvalidCastException errors during movement and Localize While Moving - first noticed during 3.4 development
                            If depthFrame IsNot Nothing Then
                                depthFrame.CopyDepthImagePixelDataTo(depthPixels) ' Copy the pixel data from the image to a temporary array
                                Dim intBeamStep As Integer = constMaxScannerBeams / intNumberOfBeams
                                If boolSlam Then intBeamStep = 1
                                For intBeamNumber As Integer = 0 To constMaxScannerBeams - 1 Step intBeamStep
                                    trvScanVectors(intBeamNumber).sngRange = depthPixels(intMiddleDepthSample - constBeamsFactor * intBeamNumber).Depth
                                    trvScanVectors(intBeamNumber).sngRange = ftnCmToInches(trvScanVectors(intBeamNumber).sngRange * sngScannerCalCoefficients(intBeamNumber))
                                Next intBeamNumber
                                RaiseEvent evntVectorsReceived()
                            End If
                        End Using
                    Else
                        subWait(60000)
                        If Not boolShutDown And Not boolURGConnected And Not boolDrivingToDestination And Not boolLocalizationInProgress Then _
                                boolKinectConnected = ftnKinectInitializeTop()
                    End If
                    'If Not tmiMoveInfo.boolBotIsMoving Then 
                    subWait(100)
                End While
            Catch ex As Exception
                frmDebug.subWriteLogs(" EXCEPTION in subGetKinectScanVectors - :  " & ex.ToString)
                subSetBeamsToMax()
            End Try
        End Sub


        Shared Sub subGetURGScanVectors()
            Dim listData As New List(Of Integer)
            Try
                While Not boolShutDown
                    If boolDebugURG Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ==================================== subGetURGScanVectors: START  ")
                    If boolURGConnected Then
                        If comURG.IsOpen Then
                            subURGInitialize()
                            Dim strUrgDataReceived As String = ftnUrgXmit("GD0044072501", 2134, 300) ' get scan data
                            If (strUrgDataReceived <> "") Then
                                Dim strTemp() As String = strUrgDataReceived.Split(vbLf)
                                Dim strLine As String = ""
                                listData.Clear()
                                For i As Integer = 3 To strTemp.Length - 1
                                    If (strTemp(i).Length > 0) Then strLine &= strTemp(i).Substring(0, strTemp(i).Length - 1).ToString()
                                Next
                                listData.AddRange(ftnDecodeStr(strLine))
                                Dim intBeamStep As Integer = constMaxScannerBeams / intNumberOfBeams
                                If boolSlam Then intBeamStep = 1
                                For intBeamNumber As Integer = 0 To constMaxScannerBeams - 1 Step intBeamStep ' Convert to trvScanVectors and apply correction coefficients
                                    trvScanVectors(intBeamNumber).sngRange = listData(660 - intBeamNumber)
                                    trvScanVectors(intBeamNumber).sngRange = ftnCmToInches(trvScanVectors(intBeamNumber).sngRange) * sngScannerCalCoefficients(intBeamNumber)
                                    If boolDebugURG Then frmDebug.subWriteVector(" subGetURGScanVectors: trvScanVectors(" & intBeamNumber & ") ", trvScanVectors(intBeamNumber))
                                Next intBeamNumber
                                RaiseEvent evntVectorsReceived()
                            Else
                                frmDebug.subWriteLogs(ftnElapsedTimeS() & " subGetURGScanVectors: ERROR - ftnUrgXmit = failed ")
                                boolURGConnected = (ftnUrgXmit("BM", 8, 500) <> "")  ' Laser on
                            End If
                        Else
                            frmDebug.subWriteLogs(" subGetURGScanVectors: ERROR - comURG is not open ")
                            ftnUrgClose()
                        End If
                    Else
                        subWait(60000)
                        If Not boolShutDown And Not boolDrivingToDestination And Not boolLocalizationInProgress And Not boolKinectConnected Then _
                            boolURGConnected = ftnURGInitializeTop()
                    End If
                End While
            Catch ex As Exception
                frmDebug.subWriteLogs(" EXCEPTION in subGetURGScanVectors :  " & ex.ToString)
                ftnUrgClose()
                subSetBeamsToMax()
            End Try
        End Sub

        Shared Sub subStartCourseCorrection() Handles Me.evntVectorsReceived
            subCourseCorrection_Top()
        End Sub

        Shared Sub subStartCollisionPredictAndDetect() Handles Me.evntVectorsReceived
            subCollisionPredictAndDetect()
        End Sub



        Shared Function ftnDecodeStr(strUrgDataReceivedLine As String) As List(Of Integer)
            Dim listDecode As New List(Of Integer)
            Try
                Dim strTemp As String = ""
                For i As Integer = 0 To strUrgDataReceivedLine.Length Step 3
                    If i < strUrgDataReceivedLine.Length - 3 Then
                        strTemp = strUrgDataReceivedLine.Substring(i, 3)
                    Else
                        strTemp = strUrgDataReceivedLine.Substring(i)
                    End If
                    listDecode.Add(ftnDecode(strTemp))
                Next
                If (strUrgDataReceivedLine.Length < (660 * 3)) Then ' ERROR- all data not received
                    frmDebug.subWriteLogs(" ftnDecodeStr: ERROR ERROR ERROR Incomplete URG data received " & strUrgDataReceivedLine.Length)
                    For i As Integer = strUrgDataReceivedLine.Length To 663 Step 3
                        listDecode.Add(0)
                    Next
                End If
            Catch ex As Exception
                frmDebug.subWriteLogs(" EXCEPTION in ftnDecodeStr :  " & ex.ToString)
            End Try
            Return listDecode
        End Function

        Shared Function ftnDecode(strSixBitEncode As String) As Integer
            Dim intDecode As Integer = 0
            Try
                For Each c As Char In strSixBitEncode
                    intDecode <<= 6
                    intDecode = intDecode And (Not 63)
                    intDecode = intDecode Or (Asc(c) - 48)
                Next
            Catch ex As Exception
                frmDebug.subWriteLogs(" EXCEPTION in ftnDecode :  " & ex.ToString)
            End Try
            Return intDecode
        End Function
    End Class








    Public Sub subCalculateScanBeamAngles()
        Try
            Dim sngBeamOrientationFactor As Single = sngScanFOV / constMaxScannerBeams
            For intBeamNumber As Integer = 0 To constMaxScannerBeams - 1
                trvScanVectors(intBeamNumber).sngAngle = (sngScanFOV / 2 - sngBeamOrientationFactor * intBeamNumber) + intScanOffsetDegrees
            Next intBeamNumber
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subCalculateScanBeamAngles :  " & ex.ToString)
        End Try
    End Sub

    Public Sub subCalculateScanCalCoefficients()
        Try
            Dim sngScannerCalibrateCM As Single = sngScannerCalibrate / 10
            Dim sngScannerCurveAdjusted As Single = sngScannerCurveAdjust / ((sngScanFOV / 2) ^ 1.4)
            For intBeamNumber As Integer = 0 To constMaxScannerBeams - 1
                sngScannerCalCoefficients(intBeamNumber) = sngScannerCalibrateCM * (1 + sngScannerCurveAdjusted * Abs(trvScanVectors(intBeamNumber).sngAngle) ^ 1.4)
            Next intBeamNumber
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subCalculateScanCalCoefficients :  " & ex.ToString)
        End Try
    End Sub

    Sub subSetBeamsToMax()
        Try
            For intBeamNumber As Integer = 0 To constMaxScannerBeams - 1
                trvScanVectors(intBeamNumber).sngRange = intScanVectorsMaxRangeInches
            Next intBeamNumber
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subSetBeamsToMax :  " & ex.ToString)
        End Try
    End Sub



End Module
