Imports System.Math
Imports System.Drawing
Imports System.Threading
Imports System.Windows.Forms



Module modMapDraw

    '=================================================

    '       Map Draw MODULE
    '  This module is responsible for:
    '    - Drawing on the map (Bot, Destination, Path, etc)
    '

    '=================================================
    Public Sub subDrawBot(ByRef tbpBotPose As typeBotPose, ByRef tmcOldBotLocation As typeMapCoordinate)    ' Draw the robot icon on the map.  The icon is a green circle with a yellow line to indicate orientation
        Try
            Dim intZoomBotX As Integer = Round(tmcOldBotLocation.X * sngZoom) - intBotRadiusZoom
            Dim intZoomBotY As Integer = Round(tmcOldBotLocation.Y * sngZoom) - intBotRadiusZoom
            If Not ftnPosesEqual(tbpBotPose, ftnConvertToPose(tmcOldBotLocation, 0)) Then
                frmGotABotTop.myMap.CreateGraphics.DrawArc(Pens.White, intZoomBotX, intZoomBotY, intBotDiameterZoom, intBotDiameterZoom, 0, 360) ' Erase the previous Bot Location
                frmGotABotTop.myMap.CreateGraphics.FillEllipse(Brushes.White, intZoomBotX, intZoomBotY, intBotDiameterZoom, intBotDiameterZoom)
            End If
            intZoomBotX = Round(tbpBotPose.Location.X * sngZoom)
            intZoomBotY = Round(tbpBotPose.Location.Y * sngZoom)
            Dim sngOrientationRadians As Single = tbpBotPose.Orientation * 0.017453
            frmGotABotTop.myMap.CreateGraphics.DrawArc(Pens.Green, (intZoomBotX - intBotRadiusZoom), (intZoomBotY - intBotRadiusZoom), intBotDiameterZoom, intBotDiameterZoom, 0, 360)
            frmGotABotTop.myMap.CreateGraphics.FillEllipse(Brushes.Green, (intZoomBotX - intBotRadiusZoom), (intZoomBotY - intBotRadiusZoom), intBotDiameterZoom, intBotDiameterZoom)
            frmGotABotTop.myMap.CreateGraphics.DrawLine(Pens.Yellow, intZoomBotX, intZoomBotY, CInt(intZoomBotX + intBotRadiusZoom * Math.Cos(sngOrientationRadians)), CInt(intZoomBotY - intBotRadiusZoom * Math.Sin(sngOrientationRadians)))
            If Not tmiMoveInfo.boolBotIsMoving Then subDrawBotUncertaintyOutline()
            If (intHeadAngle <> 0) Then subDrawVectorFromBot(tbpBotPose, sngBotRadius, intHeadAngle, Color.White)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subDrawBot :  " & ex.ToString)
        End Try
    End Sub




    Sub subDrawBotUncertaintyOutline()
        Try
            Dim intZoomBotX As Integer = Int(tbpBotPose.Location.X * sngZoom)
            Dim intZoomBotY As Integer = Int(tbpBotPose.Location.Y * sngZoom)
            Dim LocX, LocY As Integer
            Dim LocY2, LocX2 As Integer
            Dim sngLeftUncertaintyRadians As Single = ftnDegreeToRadian(tbpBotPose.Orientation + tbpBotPose.OrientationUncertainty)
            Dim sngRightUncertaintyRadians As Single = ftnDegreeToRadian(tbpBotPose.Orientation - tbpBotPose.OrientationUncertainty)
            LocX = (tbpBotPose.Location.X - tbpBotPose.LocationUncertainty.X)
            LocY = (tbpBotPose.Location.Y - tbpBotPose.LocationUncertainty.Y)
            subCheckMapArrayLimits(LocX, LocY)
            LocX2 = (tbpBotPose.Location.X + tbpBotPose.LocationUncertainty.X)
            LocY2 = (tbpBotPose.Location.Y + tbpBotPose.LocationUncertainty.Y)
            subCheckMapArrayLimits(LocX2, LocY2)

            frmGotABotTop.myMap.CreateGraphics.DrawRectangle(Pens.YellowGreen, LocX * sngZoom, (LocY * sngZoom),
                                                                       ((LocX2 - LocX) * sngZoom), ((LocY2 - LocY) * sngZoom))
            frmGotABotTop.myMap.CreateGraphics.DrawLine(Pens.YellowGreen, intZoomBotX, intZoomBotY,
                                                           CInt(intZoomBotX + intBotRadiusZoom * Math.Cos(sngLeftUncertaintyRadians)),
                                                           CInt(intZoomBotY - intBotRadiusZoom * (Math.Sin(sngLeftUncertaintyRadians))))
            frmGotABotTop.myMap.CreateGraphics.DrawLine(Pens.YellowGreen, intZoomBotX, intZoomBotY,
                                                           CInt(intZoomBotX + intBotRadiusZoom * (Math.Cos(sngRightUncertaintyRadians))),
                                                           CInt(intZoomBotY - intBotRadiusZoom * (Math.Sin(sngRightUncertaintyRadians))))
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subDrawBotUncertaintyOutline :  " & ex.ToString)
        End Try
    End Sub



    Public Sub subDrawDestination(ByRef tmcDestination As typeMapCoordinate)    ' Draw the destination icon on the map.  The icon is a red 'X' (as in "X marks the spot and red indicates STOP here)
        Try
            Dim wideRedPen As New Pen(Brushes.Red)
            tmcDestination = ftnCheckMapArrayLimits(tmcDestination)
            Dim tmcZoomDestinationX As Integer = tmcDestination.X * sngZoom
            Dim tmcZoomDestinationY As Integer = tmcDestination.Y * sngZoom
            wideRedPen.Width = 8.0F
            wideRedPen.LineJoin = Drawing2D.LineJoin.Bevel
            frmGotABotTop.myMap.CreateGraphics.DrawLine(wideRedPen, tmcZoomDestinationX - intBotRadiusZoom, tmcZoomDestinationY - intBotRadiusZoom, tmcZoomDestinationX + intBotRadiusZoom, tmcZoomDestinationY + intBotRadiusZoom)
            frmGotABotTop.myMap.CreateGraphics.DrawLine(wideRedPen, tmcZoomDestinationX - intBotRadiusZoom, tmcZoomDestinationY + intBotRadiusZoom, tmcZoomDestinationX + intBotRadiusZoom, tmcZoomDestinationY - intBotRadiusZoom)
            wideRedPen.Dispose()
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subDrawDestination :  " & ex.ToString)
        End Try
    End Sub

    Public Sub subDrawVectorFromBot(ByRef tbpEstimatedPose As typeBotPose, ByRef sngRange As Single, ByRef sngRelativeAngle As Single, ByRef colVector As Color)    ' Draw a line from the bot to the Ranger supplied vector
        Try
            Dim tmcEndPoint As typeMapCoordinate = ftnLocationFromEstimatedPose(tbpEstimatedPose, sngRelativeAngle, sngRange)
            tmcEndPoint = ftnCheckMapArrayLimits(tmcEndPoint)
            Dim myPen As Pen
            myPen = New Pen(colVector)
            frmGotABotTop.myMap.CreateGraphics.DrawLine(myPen, (tbpBotPose.Location.X * sngZoom), (tbpBotPose.Location.Y * sngZoom), (tmcEndPoint.X * sngZoom), (tmcEndPoint.Y * sngZoom))
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subDrawVectorFromBot")
        End Try
    End Sub


    Public Sub subDrawVector(ByRef tmcStartLocation As typeMapCoordinate, ByRef sngRange As Single, ByRef sngOrientation As Single, ByRef boolGreen As Boolean)    ' Draw a line from the Range Finder
        Try
            Dim tmcEndPoint As typeMapCoordinate = ftnLocationFromLocation(tmcStartLocation, sngOrientation, sngRange)
            If boolGreen Then
                frmGotABotTop.myMap.CreateGraphics.DrawLine(Pens.Lime, (tmcStartLocation.X * sngZoom), (tmcStartLocation.Y * sngZoom), (tmcEndPoint.X * sngZoom), (tmcEndPoint.Y * sngZoom))
                subDrawSmallCircle(tmcEndPoint, Color.Lime)
            Else
                frmGotABotTop.myMap.CreateGraphics.DrawLine(Pens.Red, (tmcStartLocation.X * sngZoom), (tmcStartLocation.Y * sngZoom), (tmcEndPoint.X * sngZoom), (tmcEndPoint.Y * sngZoom))
                subDrawSmallCircle(tmcEndPoint, Color.Red)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subDrawVector :  " & ex.ToString)
        End Try
    End Sub



    Public Sub subDrawLandMark()
        Try
            Dim LocX, LocY As Single
            Dim ValidX, ValidY, ValidHeight, ValidWidth As Single
            LocX = Convert.ToSingle(frmLandMarkEdit.UpDnLocX.Value)
            LocY = Convert.ToSingle(frmLandMarkEdit.UpDnLocY.Value)
            subCheckMapArrayLimits(LocX, LocY)
            ValidX = Convert.ToSingle(frmLandMarkEdit.UpDnValidX.Value)
            ValidY = Convert.ToSingle(frmLandMarkEdit.UpDnValidY.Value)
            subCheckMapArrayLimits(ValidX, ValidY)
            ValidWidth = Convert.ToSingle(frmLandMarkEdit.UpDnValidWidth.Value)
            ValidHeight = Convert.ToSingle(frmLandMarkEdit.UpDnValidHeight.Value)

            Dim sngDotHeight As Single = 4 * sngZoom
            frmGotABotTop.myMap.CreateGraphics.DrawArc(Pens.CornflowerBlue, (LocX * sngZoom - sngDotHeight / 2),
                                                     (LocY * sngZoom - sngDotHeight / 2), sngDotHeight, sngDotHeight, 0, 360)
            frmGotABotTop.myMap.CreateGraphics.FillEllipse(Brushes.CornflowerBlue, (LocX * sngZoom - sngDotHeight / 2),
                                                                LocY * sngZoom - sngDotHeight / 2, sngDotHeight, sngDotHeight)
            frmGotABotTop.myMap.CreateGraphics.DrawRectangle(Pens.CornflowerBlue, ValidX * sngZoom, (ValidY * sngZoom),
                                                                       (ValidWidth * sngZoom), (ValidHeight * sngZoom))
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subDrawLandMark :  " & ex.ToString)
        End Try
    End Sub

    Public Sub subDrawMapPixel(ByRef intPixelX As Integer, ByRef intPixelY As Integer, ByRef colPixel As Color)
        Try
            subCheckMapArrayLimits(intPixelX, intPixelY)
            Dim myBrush As Brush
            myBrush = New SolidBrush(colPixel)
            Dim myPen As Pen
            myPen = New Pen(colPixel)
            Dim intZoomPixelX As Integer = intPixelX * sngZoom - sngZoom
            Dim intZoomPixelY As Integer = intPixelY * sngZoom - sngZoom
            frmGotABotTop.myMap.CreateGraphics.DrawRectangle(myPen, intZoomPixelX, intZoomPixelY, sngZoom, sngZoom)
            frmGotABotTop.myMap.CreateGraphics.FillRectangle(myBrush, intZoomPixelX, intZoomPixelY, sngZoom, sngZoom)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subDrawMapPixel :  " & ex.ToString)
        End Try
    End Sub

    Public Sub subDrawPath(ByVal tmcDestination As typeMapCoordinate)  ' Draw the Bot to Destination path on the map
        Try
            Dim tmcPathStep As typeMapCoordinate = ftnLowestValueNeighbor(tbpBotPose.Location)
            subDrawPathStep(tmcPathStep.X, tmcPathStep.Y)
            Do While Not ftnLocationsEqual(tmcPathStep, tmcDestination)
                tmcPathStep = ftnLowestValueNeighbor(tmcPathStep)
                subDrawPathStep(tmcPathStep.X, tmcPathStep.Y)
            Loop
            subDrawDestination(tmcDestination) ' Restore the destination icon
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subDrawPath :  " & ex.ToString)
        End Try
    End Sub



    Sub subDrawPathStep(ByRef intPathX As Integer, ByRef intPathY As Integer)
        Try
            Dim intX_Left As Integer = intPathX * sngZoom - intBotRadiusZoom  ' 
            Dim intY_Left As Integer = intPathY * sngZoom - intBotRadiusZoom
            frmGotABotTop.myMap.CreateGraphics.DrawArc(Pens.Yellow,
                                intX_Left, intY_Left,
                                intBotDiameterZoom, intBotDiameterZoom, 0, 360)
            frmGotABotTop.myMap.CreateGraphics.FillEllipse(Brushes.Yellow,
                             intX_Left, intY_Left,
                            intBotDiameterZoom, intBotDiameterZoom)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subDrawPathStep :  " & ex.ToString)
        End Try
    End Sub





    Public Sub subDrawSmallCircle(ByVal tmcMapLocation As typeMapCoordinate, ByVal colCircle As Color)    ' Draw a small  circle 
        Try
            Dim myBrush As Brush
            myBrush = New SolidBrush(colCircle)
            Dim myPen As Pen
            myPen = New Pen(colCircle)
            If Not (tmcMapLocation.X = 0 And tmcMapLocation.Y = 0) Then
                tmcMapLocation = ftnCheckMapArrayLimits(tmcMapLocation)
                Dim intX_Location As Integer = (tmcMapLocation.X - 2.5) * sngZoom
                Dim intY_Location As Integer = (tmcMapLocation.Y - 2.5) * sngZoom
                Dim intWidthOrHeight As Integer = 5 * sngZoom
                frmGotABotTop.myMap.CreateGraphics.DrawArc(myPen, intX_Location, intY_Location, intWidthOrHeight, intWidthOrHeight, 0, 360)
                frmGotABotTop.myMap.CreateGraphics.FillEllipse(myBrush, intX_Location, intY_Location, intWidthOrHeight, intWidthOrHeight)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subDrawSmallCircle :  " & ex.ToString)
        End Try
    End Sub



    Public Sub subDrawBumperSwitch(ByVal tbpEstimatedPose As typeBotPose, ByVal sngRelativeAngle As Single)    ' Highlight the bumper switch 
        Try
            Dim tmcBumperSwitch As typeMapCoordinate
            Dim sngDiameter As Single = sngBotRadius / 2 * sngZoom
            tmcBumperSwitch = ftnLocationFromEstimatedPose(tbpEstimatedPose, sngRelativeAngle, sngBotRadius)
            If Not (tmcBumperSwitch.X = 0 And tmcBumperSwitch.Y = 0) Then
                frmGotABotTop.myMap.CreateGraphics.DrawArc(Pens.Red, (tmcBumperSwitch.X * sngZoom - sngBotRadius), (tmcBumperSwitch.Y * sngZoom - sngBotRadius), sngDiameter, sngDiameter, 0, 360)
                frmGotABotTop.myMap.CreateGraphics.FillEllipse(Brushes.Red, (tmcBumperSwitch.X * sngZoom - sngBotRadius), (tmcBumperSwitch.Y * sngZoom - sngBotRadius), sngDiameter, sngDiameter)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subDrawBumperSwitch :  " & ex.ToString)
        End Try
    End Sub


    Public Sub subDrawScanVectors(ByRef tbpPose As typeBotPose, ByRef boolGreen As Boolean)
        Try
            If Not tmiMoveInfo.boolBotIsMoving Then
                If ftnScannerConnected() Then
                    Dim tmcVectorOrigin As typeMapCoordinate = ftnLocationFromEstimatedPose(tbpPose, tmcScanVectorOffset)
                    For intBeamNumber As Integer = 0 To constMaxScannerBeams - 1 Step Max(constMaxScannerBeams \ 32, 2) ' Draw Scan Vectors in Green or Red
                        If (trvScanVectors(intBeamNumber).sngRange > 0) Then _
                                subDrawVector(tmcVectorOrigin, trvScanVectors(intBeamNumber).sngRange, _
                                               ftnConvertTo360(tbpPose.Orientation + trvScanVectors(intBeamNumber).sngAngle), boolGreen) ' Show Scan Vector on map
                    Next intBeamNumber
                End If
                If boolGreen Then subWait(300)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subDrawScanVectors :  " & ex.ToString)
        End Try
    End Sub

 

End Module
