Imports System.Math


Module modLocalizeVisualLMs



    Public Function ftnLocalizeUsingVisualLMsTop(ByRef tbpEstimatedPose As typeBotPose, ByRef tbpReferencePose As typeBotPose, ByRef boolGoodVisualDLocalization As Boolean) As typeBotPose
        On Error Resume Next
        ftnLocalizeUsingVisualLMsTop = tbpEstimatedPose
        If Not boolStop Then
            Dim intValidRecognitions As Integer = 0
            Dim intTotalRecognitions As Integer = 0
            Dim strStatusDrivingRoot As String = strStatusDriving & ": "
            ftnLocalizeUsingVisualLMsTop = ftnGetPoseUsingLmRecognition(tbpEstimatedPose, tbpReferencePose, intValidRecognitions, intTotalRecognitions)
            ftnLocalizeUsingVisualLMsTop = ftnValidatePose(ftnLocalizeUsingVisualLMsTop, tbpReferencePose, intBestParticle, boolGoodVisualDLocalization, Not tmiMoveInfo.boolBotIsMoving, "Visual LM")
            subEndImageRecognition()
            frmDebug.subWriteLogs("  ")
            frmDebug.subWriteLogs("  ")
            frmDebug.subWriteLogs(" ##################################### ")
            frmDebug.subWriteLogs("  Completed Image Localization.  Valid  = " & intValidRecognitions & ", Total = " & intTotalRecognitions)
            frmDebug.subWritePose(ftnElapsedTimeS() & "       Pose Derived From LM Recognition:", ftnLocalizeUsingVisualLMsTop)
            strStatusDriving = strStatusDrivingRoot & " LMs= " & intValidRecognitions & "/" & intTotalRecognitions
            If boolGoodVisualDLocalization Then boolGoodVisualDLocalization = (intValidRecognitions > 0)
        End If
    End Function



    Private Function ftnGetPoseUsingLmRecognition(ByVal tbpEstimatedPose As typeBotPose, ByVal tbpReferencePose As typeBotPose,
                                                     ByRef intValidRecognitions As Integer, ByRef intTotalRecognitions As Integer) As typeBotPose
        Try
            ftnGetPoseUsingLmRecognition = tbpEstimatedPose
            If ftnVisualRecognizerEnabled() Then
                Dim strObjectRecognized As String = ""
                Dim timeOut, Time_Quit As DateTime
                Dim tbpBotAvgPose As typeBotPose = tbpEstimatedPose
                Dim tbpPoseFromLM As typeBotPose
                Dim boolValidLocation As Boolean = False
                Dim boolTemp As Boolean = False
                Dim intBotOutOfLMValidRegion As Integer = 0
                Dim intDistanceOutOfRange As Integer = 0
                Dim intAngleOutOfRange As Integer = 0
                Dim strStatusDrivingRoot As String = strStatusDriving
                If Not tmiMoveInfo.boolBotIsMoving Then
                    strStatusDriving = strStatusDrivingRoot & "  LMs= Searching ..."
                    timeOut = Now().AddMilliseconds(sngLocalizationTime * 1000)
                    Time_Quit = Now().AddMilliseconds(sngLocalizationTime * 300)
                    While (ftnVisualRecognizerConnected() And (Now() < timeOut) And (intValidRecognitions < 15) And (Not boolStop) And
                          Not boolShutDown And ((Now() < Time_Quit)))
                        boolValidLocation = False
                        strObjectRecognized = ftnGetNextObject()
                        If strObjectRecognized <> "" Then ' Is it a valid recognition?
                            tbpPoseFromLM = ftnFindPoseFromLM(strObjectRecognized, boolValidLocation, tbpReferencePose,
                                                           intTotalRecognitions,
                                                           intBotOutOfLMValidRegion, intDistanceOutOfRange,
                                                           intAngleOutOfRange)
                            If boolValidLocation Then tbpBotAvgPose = ftnFindAvgPose(tbpBotAvgPose, tbpPoseFromLM, intBestParticle, intValidRecognitions)
                            If CBool(InStr(strObjectRecognized, "object")) Then strObjectRecognized = strObjectRecognized.Replace("object", "")
                            strStatusDriving = strStatusDrivingRoot & " LMs= " & "(" & intValidRecognitions & "/" & intTotalRecognitions & ") " & strObjectRecognized
                        End If
                    End While
                    If ftnVisualRecognizerEnabled() Then strStatusDriving = strStatusDrivingRoot & " LMs= " & intValidRecognitions & "/" & intTotalRecognitions & " ; "
                End If
                ftnGetPoseUsingLmRecognition = ftnValidatePose(tbpBotAvgPose, tbpReferencePose, intBestParticle, boolTemp)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnGetPoseUsingLmRecognition :  " & ex.ToString)
        End Try
    End Function



    Private Function ftnFindPoseFromLM(ByRef strImageRecognized As String,
                        ByRef boolValidLocation As Boolean, ByVal tbpReferencePose As typeBotPose,
                        ByRef intTotalRecognitions As Integer,
                        ByRef intBotOutOfLMValidRegion As Integer,
                        ByRef intDistanceOutOfRange As Integer,
                        ByRef intAngleOutOfRange As Integer) As typeBotPose
        ftnFindPoseFromLM = tbpReferencePose
        Try
            Dim tlmLandMark As typeLandMark
            Dim tirLandMark As typeImageRecognized
            Dim sngLMtoBotAngle, sngBottoLMRelativeAngle As Single
            Dim tbpPoseFromLM As typeBotPose
            tirLandMark = ftnDecodeRecognitionString(strImageRecognized, True)    ' Convert the image recognition string to useable units
            tlmLandMark = ftnGetLandMarkDetails(tirLandMark.Name, tirLandMark.intNumber) ' Find the Land Mark in the database
            sngLMtoBotAngle = ftnLMtoBotAngle(tirLandMark, tbpReferencePose.Orientation)
            sngBottoLMRelativeAngle = ftnRelativeLandMarkBearing(tirLandMark)
            tlmLandMarkArray(tirLandMark.intNumber).LandMarkRecognized += 1
            boolValidLocation = False
            frmDebug.subWriteLogs("  Localizing: " & strImageRecognized & ":  " & strImageRecognized)
            frmDebug.subWriteLogs("  Localizing: " & intTotalRecognitions & ":  " & " LM = " & tirLandMark.Name & " Range = " & tirLandMark.sngRange & " x = " & tirLandMark.intPixelX)
            If ((tlmLandMark.LandMarkName = tirLandMark.Name) And (tirLandMark.Name <> "")) Then ' Is it a valid Land Mark recognition?
                intTotalRecognitions += 1
                If Not ftnPoseWithinLMValidRegion(tbpReferencePose, tlmLandMark) Then
                    intBotOutOfLMValidRegion += 1
                    boolValidLocation = False
                    frmDebug.subWriteLogs(" - - - - - - - -  - -   - - - -  Bot location outside of Land Mark Validity Region")
                Else
                    tbpPoseFromLM = ftnPoseFromLM(tlmLandMark, tirLandMark, boolValidLocation, tbpReferencePose)
                    If boolValidLocation Then  ' It is a Valid Landmark detection
                        ftnFindPoseFromLM = tbpPoseFromLM
                        'FIXTHIS subDrawBotToLM(tlmLandMark.LandMarkLocation, Color.Green)   ' confirm the sighting on the map
                        tlmLandMarkArray(tirLandMark.intNumber).LandMarkValid += 1
                        frmDebug.subWritePose("- - - - - - - - Pose From LM: ", tbpPoseFromLM)
                    ElseIf (Abs(ftnDistanceBetweenLocations(tbpReferencePose.Location, tlmLandMark.LandMarkLocation) - tirLandMark.sngRange) _
                            > Sqrt(tbpReferencePose.LocationUncertainty.X ^ 2 + tbpReferencePose.LocationUncertainty.Y ^ 2) ^ 2) Then
                        intDistanceOutOfRange += 1
                        frmDebug.subWriteLogs(" - - - - - - - -  - -   - - - -  Range invalid for this Land Mark")
                    ElseIf Not ftnBotToLMAngleCheck(tlmLandMark, sngBottoLMRelativeAngle) Then
                        intAngleOutOfRange += 1
                        frmDebug.subWriteLogs(" - - - - - - - -  - -   - - - -  Bot orientation invalid for this Land Mark")
                    Else
                        frmDebug.subWriteLogs(" - - - - - - - -  - -   - - - -  No location determined")
                    End If
                End If
            Else
                frmDebug.subWriteLogs(" - - - - - - - -  - -   - - - -  Unknown LM")
            End If  ' (tlmLandMark.LandMarkName = tirLandMark.Name)
            'frmLandMarkEdit.subUpdateLMStats()
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnFindPoseFromLM :  " & ex.ToString)
        End Try
    End Function


    Public Function ftnDecodeRecognitionString(ByVal strImageRecognized As String, boolAddCameraOffsets As Boolean) As typeImageRecognized
        ' IF the Robot recognizes an object then it will report it in the form
        ' object  "object name"   FeaturesMatched   TotalFeatures   PixelX   PixelY   Distance(cm)
        '   0               1                2           3             4        5         6        
        ftnDecodeRecognitionString.Name = "Null"
        Try
            Dim strLineOfText As String()
            Dim strParsedText1 As String()
            Dim strParsedText As String()
            strImageRecognized = strImageRecognized & "  Null" & "  Null" & vbCr
            strLineOfText = strImageRecognized.Split(vbCr)
            If CBool(InStr(strLineOfText(0), """")) Then
                strParsedText1 = strLineOfText(0).Split(""""c) ' strParsedText1(0) = object;  strParsedText1(1) = "object name"  ;   
                '                                                 strParsedText1(2) = " "FeaturesMatched   TotalFeatures   PixelX   PixelY   Distance(cm) 
                '                                                                     0           1            2              3         4       5
                If (UBound(strParsedText1) >= 2) Then
                    strParsedText = strParsedText1(2).Split(" "c) ' 
                    If (UBound(strParsedText) >= 5) Then
                        If (strParsedText1(0) = "object ") Then
                            ftnDecodeRecognitionString.Name = strParsedText1(1)
                            If (Val(strParsedText(3)) >= 0) Then
                                If boolAddCameraOffsets Then
                                    ftnDecodeRecognitionString.intPixelX = Round(Val(strParsedText(3)) + sngCameraOffsetDegrees * ticCameraResolution.X / sngCameraFOV) ' 
                                Else
                                    ftnDecodeRecognitionString.intPixelX = Round(Val(strParsedText(3))) ' 
                                End If
                            Else
                                ftnDecodeRecognitionString.intPixelX = 0
                            End If
                            If (Val(strParsedText(4)) >= 0) Then
                                ftnDecodeRecognitionString.intPixelY = CInt(Int(Val(strParsedText(4)))) ' make sure 'PixelY' is a valid number
                            Else
                                ftnDecodeRecognitionString.intPixelY = 0
                            End If
                            If CBool(InStr(strParsedText(5), "object")) Then strParsedText(5) = strParsedText(5).Replace("object", "  ") ' kludge
                            If (Val(strParsedText(5)) >= 0) And Not CBool(InStr(strParsedText(5), "object")) Then
                                ftnDecodeRecognitionString.sngRange = Convert.ToSingle(Val(strParsedText(5)))
                            Else
                                ftnDecodeRecognitionString.sngRange = 0
                            End If
                            ftnDecodeRecognitionString.sngRange = ftnCmToInches(ftnDecodeRecognitionString.sngRange)
                            If boolAddCameraOffsets And boolLocalizationInProgress Then ftnDecodeRecognitionString.sngRange += sngCameraOffset
                        Else
                            ftnDecodeRecognitionString.Name = "Null"
                        End If
                    End If
                    If (UBound(strParsedText) >= 7) Then
                        If (strParsedText(6) = "Rotation") Then
                            ftnDecodeRecognitionString.sngYRotation = ftnConvertTo360(Convert.ToSingle(Val(strParsedText(7))))
                        Else
                            ftnDecodeRecognitionString.sngYRotation = 9999
                        End If
                    End If
                End If
            Else
                ftnDecodeRecognitionString.Name = "Null"
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnDecodeRecognitionString :  " & ex.ToString)
        End Try
    End Function




    Public Function ftnGetLandMarkDetails(ByRef strLandMarkName As String, ByRef intLandMarkNo As Integer) As typeLandMark
        ftnGetLandMarkDetails.LandMarkLocation.X = -1000
        ftnGetLandMarkDetails.LandMarkLocation.Y = -1000
        ftnGetLandMarkDetails.LandMarkName = ""
        intLandMarkNo = -1
        Dim intHighestLM As Integer = UBound(tlmLandMarkArray)
        Do
            intLandMarkNo = intLandMarkNo + 1
        Loop Until ((intLandMarkNo = intHighestLM) Or (strLandMarkName = tlmLandMarkArray(intLandMarkNo).LandMarkName)) Or boolStop
        ftnGetLandMarkDetails = tlmLandMarkArray(intLandMarkNo)
    End Function




    Public Function ftnLMtoBotAngle(ByVal tirRecognizedLandmark As typeImageRecognized, ByVal sngBotOrientation As Single) As Single
        ' Get the angle of the Bot from the point of view of the Land Mark. 
        Try
            ftnLMtoBotAngle = 180 + (ftnConvertTo360(sngBotOrientation + ftnRelativeLandMarkBearing(tirRecognizedLandmark)))
            ftnLMtoBotAngle = ftnConvertTo360(ftnLMtoBotAngle)
        Catch
            ftnLMtoBotAngle = 0
        End Try
    End Function


    Public Function ftnRelativeLandMarkBearing(ByRef tirRecognizedLandmark As typeImageRecognized) As Single
        ' Get the angle of the LandMark relative to the Bot center line. 
        Try
            ftnRelativeLandMarkBearing = ftnCameraAngleX(tirRecognizedLandmark) ' Angle to Camera
            Dim RelativeLandMarkBearingRadians As Single = ftnDegreeToRadian(ftnRelativeLandMarkBearing)
            Dim sngX As Single = tirRecognizedLandmark.sngRange * Cos(RelativeLandMarkBearingRadians) + sngCameraOffset
            Dim sngY As Single = tirRecognizedLandmark.sngRange * Sin(RelativeLandMarkBearingRadians)
            If (Abs(sngX) < 0.01) Then
                ftnRelativeLandMarkBearing = 90
            Else
                ftnRelativeLandMarkBearing = ftnRadianToDegree(Atan(sngY / sngX)) ' Angle to Bot center
            End If
        Catch ex As Exception
            ftnRelativeLandMarkBearing = 0
            frmDebug.subWriteLogs(" EXCEPTION in ftnRelativeLandMarkBearing :  " & ex.ToString)
        End Try
    End Function



    Function ftnPoseFromLM(ByVal tlmLandMark As typeLandMark,
                               ByVal tirLandMark As typeImageRecognized,
                               ByRef boolValidLocation As Boolean,
                               ByVal tbpReferencePose As typeBotPose) As typeBotPose
        ' Determine the Bot's Pose based upon the Land Mark recognition
        On Error Resume Next
        boolValidLocation = False
        tirLandMark.sngRange = ftnRangeCorrectForHeight(tlmLandMark, tirLandMark)
        ftnPoseFromLM = ftnPoseFromLM_Loop(tlmLandMark, tirLandMark, boolValidLocation, tbpReferencePose)
    End Function


    Public Function ftnPoseFromLM_Loop(ByVal tlmLandMark As typeLandMark,
                               ByVal tirLandMark As typeImageRecognized,
                               ByRef boolValidLocation As Boolean,
                               ByVal tbpReferencePose As typeBotPose) As typeBotPose
        Try
            Dim intMinAngle, intMaxAngle As Integer
            Dim intStartAngle, intEndAngle As Integer
            Dim tmcMinLocation, tmcMaxLocation As typeMapCoordinate ' used to calculate location
            tmcMinLocation.X = tbpReferencePose.Location.X + tbpReferencePose.LocationUncertainty.X
            tmcMinLocation.Y = tbpReferencePose.Location.Y + tbpReferencePose.LocationUncertainty.Y
            tmcMaxLocation.X = 0
            tmcMaxLocation.Y = 0
            Dim tmcCandidateLocation As typeMapCoordinate
            Dim sngLMtoBotAngle As Single
            subFindStartEndAngles(tbpReferencePose, intStartAngle, intEndAngle)
            intMinAngle = intEndAngle
            intMaxAngle = intStartAngle
            For intBotCandidateOrientation As Integer = intStartAngle To intEndAngle  ' Check All Bot Positions over Orientation Uncertainty range to see if its a valid location for this LM sighting 
                sngLMtoBotAngle = ftnLMtoBotAngle(tirLandMark, ftnConvertTo360(intBotCandidateOrientation))
                tmcCandidateLocation = ftnLocationFromLocation(tlmLandMark.LandMarkLocation, sngLMtoBotAngle, tirLandMark.sngRange)
                If ftnLocationOKForLM(tmcCandidateLocation, tlmLandMark, tbpReferencePose) Then  ' check that its within LM valid range, within Bot Pose, not in a wall
                    If (intBotCandidateOrientation < intMinAngle) Then intMinAngle = intBotCandidateOrientation
                    If (intBotCandidateOrientation > intMaxAngle) Then intMaxAngle = intBotCandidateOrientation
                    If (tmcCandidateLocation.X > tmcMaxLocation.X) Then tmcMaxLocation.X = tmcCandidateLocation.X
                    If (tmcCandidateLocation.Y > tmcMaxLocation.Y) Then tmcMaxLocation.Y = tmcCandidateLocation.Y
                    If (tmcCandidateLocation.X < tmcMinLocation.X) Then tmcMinLocation.X = tmcCandidateLocation.X
                    If (tmcCandidateLocation.Y < tmcMinLocation.Y) Then tmcMinLocation.Y = tmcCandidateLocation.Y
                    boolValidLocation = True
                End If
            Next intBotCandidateOrientation

            If boolValidLocation Then      'Calculate the pose
                If ftnOrientationPointsRight(tbpReferencePose.Orientation) Then
                    ftnPoseFromLM_Loop.OrientationUncertainty = Abs(ftnConvertTo180(intMaxAngle) - ftnConvertTo180(intMinAngle)) \ 2
                Else
                    ftnPoseFromLM_Loop.OrientationUncertainty = Abs(intMaxAngle - intMinAngle) \ 2
                End If
                ftnPoseFromLM_Loop.Orientation = ftnConvertTo360(intMinAngle + ftnPoseFromLM_Loop.OrientationUncertainty)
                If ftnPoseFromLM_Loop.OrientationUncertainty > 20 Then ftnPoseFromLM_Loop.OrientationUncertainty = 20
                ftnPoseFromLM_Loop.LocationUncertainty.X = (tmcMaxLocation.X - tmcMinLocation.X) \ 2     ' 
                ftnPoseFromLM_Loop.LocationUncertainty.Y = (tmcMaxLocation.Y - tmcMinLocation.Y) \ 2
                ftnPoseFromLM_Loop = ftnMinPoseUncertainty(ftnPoseFromLM_Loop)

                sngLMtoBotAngle = ftnLMtoBotAngle(tirLandMark, ftnPoseFromLM_Loop.Orientation)
                ftnPoseFromLM_Loop.Location = ftnLocationFromLocation(tlmLandMark.LandMarkLocation, sngLMtoBotAngle, tirLandMark.sngRange)
                ftnPoseFromLM_Loop = ftnValidatePose(ftnPoseFromLM_Loop, tbpReferencePose, intBestParticle, boolValidLocation)
            Else
                ftnPoseFromLM_Loop = tbpReferencePose
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnPoseFromLM_Loop :  " & ex.ToString)
        End Try
    End Function




 

End Module
