'Various checks used to verify a localization result.
Imports System.Math
Imports System.Drawing

Module modLocalizeChecks

    Function ftnLocationValidForLM(ByVal tmcPossibleLocation As typeMapCoordinate, ByRef LandMark As typeLandMark, ByRef tbpReferencePose As typeBotPose) As Boolean
        tmcPossibleLocation = ftnCheckMapArrayLimits(tmcPossibleLocation)
        If ftnLocationWithinPose(tmcPossibleLocation, tbpReferencePose) And
            Not ftnInWall_NoChk(tmcPossibleLocation, intBestParticle) And
           ftnLocationWithinLMValidRegion(tmcPossibleLocation, LandMark) Then
            ftnLocationValidForLM = True
        Else
            ftnLocationValidForLM = False
        End If
    End Function

    Function ftnLocationOKForLM(ByVal tmcPossibleLocation As typeMapCoordinate, ByRef tlmLandMark As typeLandMark, ByRef tbpReferencePose As typeBotPose) As Boolean
        tmcPossibleLocation = ftnCheckMapArrayLimits(tmcPossibleLocation)
        If ftnLocationWithinPose(tmcPossibleLocation, tbpReferencePose) And _
           Not boolOccupiedCell(0, tmcPossibleLocation.X, tmcPossibleLocation.Y) And _
           ftnLocationWithinLMValidRegion(tmcPossibleLocation, tlmLandMark) Then
            ftnLocationOKForLM = True
        Else
            ftnLocationOKForLM = False
        End If
    End Function



    Public Function ftnValidatePose(ByRef tbpCandidatePose As typeBotPose, ByRef tbpReferencePose As typeBotPose, ByRef intParticleNumber As Integer, ByRef boolValidLocation As Boolean, ByRef boolLogDetails As Boolean, ByRef strlogdetails As String) As typeBotPose
        Try
            If Not ftnLocationsEqual(ftnCheckMapArrayLimits(tbpCandidatePose.Location), tbpCandidatePose.Location) Then
                boolValidLocation = False
                If boolLogDetails Then frmDebug.subWriteLogs(strlogdetails & " Localization failed - Location not within Map ")
                ftnValidatePose = tbpReferencePose
            ElseIf ftnInWall_NoChk(tbpCandidatePose.Location, intParticleNumber) Then ' 
                boolValidLocation = False
                If boolLogDetails Then frmDebug.subWriteLogs(strlogdetails & " Localization failed - Location within wall ")
                ftnValidatePose = tbpReferencePose
            ElseIf (Not ftnLocationWithinPose(tbpCandidatePose.Location, tbpReferencePose)) Then
                boolValidLocation = False
                If boolLogDetails Then frmDebug.subWriteLogs(strlogdetails & " Localization failed - Location not within reference Pose ")
                ftnValidatePose = tbpReferencePose
            ElseIf Not ftnAnglesWithinUncertainty(tbpCandidatePose.Orientation, tbpReferencePose.Orientation, tbpReferencePose.OrientationUncertainty) Then
                boolValidLocation = False
                If boolLogDetails Then frmDebug.subWriteLogs(strlogdetails & " Localization failed - Orientation outside limits ")
                ftnValidatePose = tbpReferencePose
            Else
                boolValidLocation = True
                ftnValidatePose = tbpCandidatePose
            End If
            If boolLogDetails And Not boolValidLocation Then
                frmDebug.subWritePose(strlogdetails & "       tbpReferencePose = ", tbpReferencePose)
                frmDebug.subWritePose(strlogdetails & "       Invalid Pose =     ", tbpCandidatePose)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnValidatePose :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnValidatePose(ByRef tbpCandidatePose As typeBotPose, ByRef tbpReferencePose As typeBotPose, ByRef intParticleNumber As Integer, ByRef boolValidLocation As Boolean) As typeBotPose
        Try
            If Not ftnLocationsEqual(ftnCheckMapArrayLimits(tbpCandidatePose.Location), tbpCandidatePose.Location) Then
                boolValidLocation = False
                ftnValidatePose = tbpReferencePose
            ElseIf ftnInWall_NoChk(tbpCandidatePose.Location, intParticleNumber) Then
                boolValidLocation = False
                ftnValidatePose = tbpReferencePose
            ElseIf (Not ftnLocationWithinPose(tbpCandidatePose.Location, tbpReferencePose)) Then
                boolValidLocation = False
                ftnValidatePose = tbpReferencePose
            ElseIf Not ftnAnglesWithinUncertainty(tbpCandidatePose.Orientation, tbpReferencePose.Orientation, tbpReferencePose.OrientationUncertainty) Then
                boolValidLocation = False
                ftnValidatePose = tbpReferencePose
            Else
                boolValidLocation = True
                ftnValidatePose = tbpCandidatePose
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnValidatePose :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnBotWithinLMValidRegion(ByVal LandMark As typeLandMark) As Boolean
        If (((tbpBotPose.Location.X + tbpBotPose.LocationUncertainty.X) >= LandMark.LandMarkValidRegion.X) And
            ((tbpBotPose.Location.X - tbpBotPose.LocationUncertainty.X) <= (LandMark.LandMarkValidRegion.X + LandMark.LandMarkValidRegion.Width)) And
            ((tbpBotPose.Location.Y + tbpBotPose.LocationUncertainty.Y) >= LandMark.LandMarkValidRegion.Y) And
            ((tbpBotPose.Location.Y - tbpBotPose.LocationUncertainty.Y) <= (LandMark.LandMarkValidRegion.Y + LandMark.LandMarkValidRegion.Height))) Then
            ftnBotWithinLMValidRegion = True
        Else
            ftnBotWithinLMValidRegion = False
        End If
    End Function


    Public Function ftnLocationWithinPose(ByRef tmcPossibleLocation As typeMapCoordinate, ByRef tbpReferencePose As typeBotPose) As Boolean
        Try
            If (Abs(tmcPossibleLocation.X - tbpReferencePose.Location.X) <= tbpReferencePose.LocationUncertainty.X) And
               (Abs(tmcPossibleLocation.Y - tbpReferencePose.Location.Y) <= tbpReferencePose.LocationUncertainty.Y) Then
                ftnLocationWithinPose = True
            Else
                ftnLocationWithinPose = False
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnLocationWithinPose :  " & ex.ToString)
        End Try
    End Function


    Function ftnLocationWithinLMValidRegion(ByRef tmcPossibleLocation As typeMapCoordinate, ByRef LandMark As typeLandMark) As Boolean
        If ((tmcPossibleLocation.X >= LandMark.LandMarkValidRegion.X) And _
            (tmcPossibleLocation.X <= (LandMark.LandMarkValidRegion.X + LandMark.LandMarkValidRegion.Width)) And _
            (tmcPossibleLocation.Y >= LandMark.LandMarkValidRegion.Y) And _
            (tmcPossibleLocation.Y <= (LandMark.LandMarkValidRegion.Y + LandMark.LandMarkValidRegion.Height))) Then
            ftnLocationWithinLMValidRegion = True
        Else
            ftnLocationWithinLMValidRegion = False
        End If
    End Function

    Function ftnPoseWithinLMValidRegion(ByVal tmcPossiblePose As typeBotPose, ByVal LandMark As typeLandMark) As Boolean
        If (((tmcPossiblePose.Location.X + tmcPossiblePose.LocationUncertainty.X) >= LandMark.LandMarkValidRegion.X) And _
            ((tmcPossiblePose.Location.X - tmcPossiblePose.LocationUncertainty.X) <= (LandMark.LandMarkValidRegion.X + LandMark.LandMarkValidRegion.Width)) And _
            ((tmcPossiblePose.Location.Y + tmcPossiblePose.LocationUncertainty.Y) >= LandMark.LandMarkValidRegion.Y) And _
            ((tmcPossiblePose.Location.Y - tmcPossiblePose.LocationUncertainty.Y) <= (LandMark.LandMarkValidRegion.Y + LandMark.LandMarkValidRegion.Height))) Then
            ftnPoseWithinLMValidRegion = True
        Else
            ftnPoseWithinLMValidRegion = False
        End If
    End Function

    Public Function ftnBotToLMAngleCheck(ByVal LandMarkDetails As typeLandMark, ByVal sngBottoLMRelativeAngle As Single) As Boolean
        Dim sngBotToLMAngleAsMeasured, sngBotToLMAngleOnMap As Single

        sngBotToLMAngleOnMap = ftnConvertTo360(ftnBotToLMAngle(LandMarkDetails))
        sngBotToLMAngleAsMeasured = ftnConvertTo360(tbpBotPose.Orientation + sngBottoLMRelativeAngle)
        ftnBotToLMAngleCheck = ftnAnglesWithinUncertainty(Convert.ToInt16(sngBotToLMAngleOnMap), Convert.ToInt16(sngBotToLMAngleAsMeasured), tbpBotPose.OrientationUncertainty)
    End Function



    Public Function ftnBotToLMAngle(ByVal tlmLandMarkDetails As typeLandMark) As Single
        ftnBotToLMAngle = ftnP1ToP2Angle(tbpBotPose.Location, tlmLandMarkDetails.LandMarkLocation)
    End Function





End Module
