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

Module modIF_Hero_Top

    Public Function ftnHeroCommand(ByRef strCommand As String) As String
        ftnHeroCommand = ""
        Try
            Dim strParsedText As String() = strCommand.Split(" "c)
            If (Not boolSimulatedMovement) Or (strParsedText(0) = "play") Then ' 
                Select Case strParsedText(0)
                    Case "move" : If Not boolStop Then _
                            ftnHeroCommand = ftnMoveCommand(strParsedText, strCommand)
                    Case "battery" : ftnHeroCommand = ftnGetBattery(strParsedText, strCommand)
                    Case "input" : If (UBound(strParsedText) > 0) Then
                            If (strParsedText(1) = "analog") Then  ' get  voltage
                                ftnHeroCommand = ftnGetAnalog(strParsedText, strCommand)
                            ElseIf (strParsedText(1) = "digital") Then
                                ftnHeroCommand = ftnGetDigital()
                            End If
                        End If
                    Case "play" : ftnHeroCommand = ftnSpeak(strCommand)
                    Case "pose" : ftnHeroCommand = ftnGetPose(strParsedText) ' get odometry data
                    Case "moving" : ftnHeroCommand = ftnMoving() ' get moving status
                    Case "bumper" : ftnHeroCommand = ftnGetBumper(strParsedText, strCommand) ' get bumper switch status
                    Case "set" : ftnHeroCommand = ftnSet(strParsedText, strCommand) ' Set bot parameter
                    Case "stop" : ftnHeroCommand = ftnStop(strParsedText, strCommand) 'stop
                    Case Else : ftnHeroCommand = ftnHero_Communicate(strCommand, ">", 30000)
                End Select
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnHeroCommand :  " & ex.ToString)
        End Try
    End Function

    Function ftnMoving() As String
        Try
            If tmiMoveInfo.boolBotIsMoving Then
                ftnMoving = "True"
            Else
                ftnMoving = "False"
            End If
        Catch ex As Exception
            ftnMoving = "False"
            frmDebug.subWriteLogs(" EXCEPTION in ftnMoving :  " & ex.ToString)
        End Try
    End Function

    '================================ Movement Commands   =========================================

    Private Function ftnMoveCommand(ByRef strParsedText As String(), ByVal strCommand As String) As String
        ftnMoveCommand = "OK"
        Try
            Dim sngMoveDistanceOrAngle As Single
            sngMoveDistanceOrAngle = Convert.ToSingle(Val(strParsedText(1)))

            ftnSetMoveInfo(tomTypeOfMovePlanned, tbpStartingPosePlanned, tbpTargetPosePlanned, True)
            If (tomTypeOfMovePlanned = TypeOfMove.TURN) Then
                ftnMoveCommand = ftnHeroTurn(sngMoveDistanceOrAngle)
            ElseIf (tomTypeOfMovePlanned = TypeOfMove.FWD) Then
                ftnMoveCommand = ftnHeroFwd(sngMoveDistanceOrAngle)
            End If
            ftnSetMoveInfo(tmiMoveInfo.tomTypeOfMove, tmiMoveInfo.tbpStartingPose, tmiMoveInfo.tbpTargetPose, False)
            tmiMoveInfo.boolMoveCommandInProgress = False
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnMoveCommand :  " & ex.ToString)
            For i = 0 To Len(strParsedText) - 1
                frmDebug.subWriteLogs(i & " strParsedText " & strParsedText(i))
            Next
        End Try
    End Function


    Function ftnHeroTurn(ByRef sngTurnAngle As Single) As String
        Try
            ftnHeroTurn = ""
            Dim intDistance As Integer = Round(Abs(sngTurnAngle) * 0.1624)
            If (sngTurnAngle >= 0) Then
                ftnHero_Communicate("GMC3E800", ">", 80000)  'Full Left
                intDistance = intDistance * (1 - sngSteeringBiasOffsetCorrection / 8)
            Else
                ftnHero_Communicate("GMC3EC93", ">", 80000) 'Full Right
                intDistance = intDistance * (1 + sngSteeringBiasOffsetCorrection / 8)
            End If
            Dim strDistanceInHex As String = Hex(intDistance)
            If (Len(strDistanceInHex) < 2) Then strDistanceInHex = "0" & strDistanceInHex
            Dim timeOut As DateTime = Now().AddMilliseconds(10000 + intDistance * 1000)
            ftnHeroTurn = ftnHero_Communicate("GMD308" & strDistanceInHex, ">", 10000 + intDistance * 1000) '
            While Not CBool(InStr(ftnHero_Communicate("GV", ">", 1000), "GotAHero")) And (Now() < timeOut) ' Glitch recovery
                Thread.Sleep(200)
                If boolDebugHero Then frmDebug.subWriteLogs(" Glitch ")
            End While
            If boolDebugHero Then frmDebug.subWriteLogs(" ftnHeroTurn End ")
        Catch ex As Exception
            ftnHeroTurn = " COM ERROR"
            frmDebug.subWriteLogs(" EXCEPTION in ftnHeroTurn :  " & ex.ToString)
        End Try
    End Function


    Function ftnHeroFwd(ByVal sngFwdDistance As Single) As String
        ftnHeroFwd = ""
        Try
            sngFwdDistance = (sngFwdDistance)
            sngFwdDistance = sngFwdDistance * ftnFwdCorrectionFactor(sngFwdDistance)

            Dim strDistanceInHex As String = Hex(Round(Abs(sngFwdDistance) * 1.07))
            If (Len(strDistanceInHex) < 2) Then strDistanceInHex = "0" & strDistanceInHex
            Dim intPresentSteeringPosition As Integer = Convert.ToInt32(ftnGetHeroPosition(6), 16) ' 
            Dim intCorrectedCenter As Integer = &H49 + sngSteeringBiasOffsetCorrection
            Dim strCorrectedCenter As String = Hex(intCorrectedCenter)
            If (intPresentSteeringPosition < intCorrectedCenter) Then
                ftnHero_Communicate("GMC3EC" & strCorrectedCenter, ">", 40000) ' Steer straight ahead
            ElseIf (intPresentSteeringPosition > intCorrectedCenter) Then
                ftnHero_Communicate("GMC3E8" & strCorrectedCenter, ">", 40000) ' Steer straight ahead
            End If
            Dim timeOut As DateTime = Now().AddMilliseconds(10000 + Abs(sngFwdDistance) * 1000)
            If (sngFwdDistance >= 0) Then
                ftnHeroFwd = ftnHero_Communicate("GMD308" & strDistanceInHex, ">", 10000 + sngFwdDistance * 1000) ' FWD 
            Else
                ftnHeroFwd = ftnHero_Communicate("GMD30C" & strDistanceInHex, ">", 10000 + Abs(sngFwdDistance) * 1000) ' BACK
            End If
            While Not CBool(InStr(ftnHero_Communicate("GV", ">", 1000), "GotAHero")) And (Now() < timeOut) ' Glitch recovery
                Thread.Sleep(200)
                If boolDebugHero Then frmDebug.subWriteLogs(" Glitch ")
            End While
            subWait(100)
            If boolDebugHero Then ftnGetHeroPosition(8)
            If boolDebugHero Then frmDebug.subWriteLogs(" ftnHeroFwd End ")
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnHeroFwd :  " & ex.ToString)
            frmDebug.subWriteLogs("    sngFwdDistance =  " & sngFwdDistance)
            frmDebug.subWriteLogs("    ftnHeroFwd =  " & ftnHeroFwd)
            ftnHeroFwd = " COM ERROR"
        End Try
    End Function

    Function ftnFwdCorrectionFactor(ByRef sngDistance As Single) As Single
        ' Heathkit Heros are notoriously bad at driving the correct distance
        ' This is a crude attempt to correct the distance commanded in order to drive the distance desired
        Try
            ftnFwdCorrectionFactor = sngDistance /
                                   (sngDistanceCorrectionCoeff_x2 * sngDistance ^ 2 _
                                   + sngDistanceCorrectionCoeff_x1 * sngDistance _
                                   + sngDistanceCorrectionCoeff_x0)
        Catch ex As Exception
            ftnFwdCorrectionFactor = 1
            frmDebug.subWriteLogs(" EXCEPTION in ftnFwdCorrectionFactor :  " & ex.ToString)
        End Try
    End Function


    Private Function ftnStop(ByVal strParsedText As String(), ByVal strCommand As String) As String
        Try
            ftnSetMoveInfo(TypeOfMove.HALT, tbpBotPose, tbpBotPose, True)
            ftnHero_Communicate("GH", ">", 1000)
            subWait(500)
            ftnStop = "OK"
            ftnSetMoveInfo(TypeOfMove.HALT, tbpBotPose, tbpBotPose, False)
            tmiMoveInfo.boolMoveCommandInProgress = False
        Catch ex As Exception
            ftnStop = " COM ERROR "
            frmDebug.subWriteLogs(" EXCEPTION in ftnStop :  " & ex.ToString)
        End Try
    End Function




    Function ftnGetHeroPosition(ByVal intMotorNumber As Integer) As String
        'Return motor position (in Hex) for Hero
        ftnGetHeroPosition = "ERROR :"
        Try
            ftnGetHeroPosition = ftnHero_Communicate("GP" & intMotorNumber.ToString, ">", 1000)
            If boolDebugHero Then frmDebug.subWriteLogs(" Position for Motor " & intMotorNumber & " = " & ftnGetHeroPosition)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnGetHeroPosition :  " & ex.ToString)
            ftnGetHeroPosition = "ERROR :"
        End Try
    End Function



    Private Function ftnGetPose(ByVal strParsedText As String()) As String
        ftnGetPose = "ERROR :"
        Try
            Dim tbpHeroPose As typeBotPose = ftnLocalizeUsingDeadReckoning(tbpBotPose, 0, True)
            ftnGetPose = "OK " & tbpHeroPose.Location.X.ToString & " " & tbpHeroPose.Location.Y.ToString & " " & tbpHeroPose.Orientation
            If boolDebugOdometry Then frmDebug.subWriteLogs("     ---------   ftnGetPose:  = " & ftnGetPose)
        Catch ex As Exception
            ftnGetPose = "OK " & tbpBotPose.Location.X.ToString & " " & tbpBotPose.Location.Y.ToString & " " & tbpBotPose.Orientation
            frmDebug.subWriteLogs(" EXCEPTION in ftnGetPose :  " & ex.ToString)
        End Try
    End Function


    Private Function ftnGetBumper(ByRef strParsedText As String(), ByRef strCommand As String) As String
        ftnGetBumper = "7"
        Try
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnGetBumper :  " & ex.ToString)
        End Try
    End Function




    Private Function ftnSet(ByRef strParsedText As String(), ByRef strCommand As String) As String
        Try
            ftnSet = ""
        Catch ex As Exception
            ftnSet = ""
            frmDebug.subWriteLogs(" EXCEPTION in ftnSet :  " & ex.ToString)
        End Try
    End Function


    Private Function ftnGetBattery(ByVal strParsedText As String(), ByRef strCommand As String) As String
        Try
            Dim intBatteryPercentage As Integer = 0
            Dim strRecv As String = ""
            sngBotBatteryVoltage = 12
            strStatusBotVoltage = "Bot: 12.0V"
            ftnGetBattery = strStatusBotVoltage
            If (ftnGetBattery = "   ??   ") And boolBotConnected Then ftnHeroCommand("play phrase 'No power'")
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnGetBattery :  " & ex.ToString)
            ftnGetBattery = ""
        End Try
    End Function


    Private Function ftnGetAnalog(ByVal strParsedText As String(), ByRef strCommand As String) As String
        ftnGetAnalog = ""
        Try
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnGetAnalog :  " & ex.ToString)
        End Try
    End Function

    Private Function ftnGetDigital() As Byte
        Try
            ftnGetDigital = "&Hff"
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnGetDigital :  " & ex.ToString)
        End Try
    End Function

    Private Function ftnSpeak(ByVal strCommand As String) As String
        Try
            ftnSpeak = "OK"
            Dim strParsedText As String() = strCommand.Split(" "c)
            If boolMute Then
                ftnSpeak = "OK"
            Else
                subSpeak(strCommand)
            End If
        Catch ex As Exception
            ftnSpeak = ""
            frmDebug.subWriteLogs(" EXCEPTION in ftnSpeak :  " & ex.ToString)
        End Try
    End Function



    '================================  Hero S19 Download  =========================================

    Public Sub subHeroS19Download()
        ' http://www.homeandlearn.co.uk/NET/nets8p2.html
        Try
            If (Dir("GOTAHERO.S19") <> "") Then
                strStatusDriving = "Hero Downloading... "
                Dim intLinesRead As Integer = 0
                Dim intLinesInFile As Integer = ftnLineCount("GOTAHERO.S19")
                Dim strTextLine As String = ""
                Dim objReader As New System.IO.StreamReader("GOTAHERO.S19")
                frmGotABotTop.pgbProgressBar1.Visible = True
                frmGotABotTop.pgbProgressBar1.Value = 0
                Do While objReader.Peek() <> -1
                    strTextLine = objReader.ReadLine() & vbNewLine
                    If boolDebugHero Then frmDebug.subWriteLogs(strTextLine)
                    intLinesRead += 1
                    frmGotABotTop.pgbProgressBar1.Value = intLinesRead / intLinesInFile * 100
                    ftnHero_Communicate(strTextLine, "", 500)
                    subWait(80)
                Loop
                objReader.Close()
                frmGotABotTop.pgbProgressBar1.Visible = False
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subHeroS19Download :  " & ex.ToString)
        End Try
    End Sub

    Function ftnLineCount(ByVal strFileName As String) As Integer
        Try
            ftnLineCount = 0
            Dim obj As New System.IO.StreamReader(strFileName)
            Do Until obj.ReadLine Is Nothing
                ftnLineCount += 1
            Loop
            obj.Close()
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnLineCount :  " & ex.ToString)
        End Try
    End Function


    '================================  Scan Vectors   =========================================
    Function ftnGetHeroScanVectors() As Boolean
        ' Rotate Hero's Head while taking sonar range measurements 
        Try
            strStatusDriving = "Measuring Scan Vectors... "
            If classScanner.chkContinuous.Checked Then ' don't rotate - 
                intHeadAngle = 0 + intScanOffsetDegrees
            Else
                intHeadAngle = trvScanVectors(0).sngAngle
                ftnHero_Communicate("GMC3C800H", ">", 90000) ' Move Head fully CCW
                Dim intHeadPosition As Integer
                Dim strHeadPositionHex As String
                For intBeamNumber As Integer = 0 To constMaxScannerBeams - 1 Step constMaxScannerBeams \ intNumberOfBeams
                    trvScanVectors(intBeamNumber).sngRange = 0
                Next
                For intBeamNumber As Integer = 0 To constMaxScannerBeams - 1 Step constMaxScannerBeams \ intNumberOfBeams
                    subCheckForPause()
                    strStatusDriving = "Measuring Scan Vectors: " & intBeamNumber & " / " & intNumberOfBeams
                    If boolStop Then Exit For
                    If (intBeamNumber <> 0) Then intHeadPosition = intBeamNumber * 194 / (constMaxScannerBeams - 1)
                    strHeadPositionHex = Hex(intHeadPosition).ToString
                    If (Len(strHeadPositionHex) = 1) Then strHeadPositionHex = "0" & strHeadPositionHex
                    ftnHero_Communicate("GMC3CC" & strHeadPositionHex, ">", 3000) ' Move Head to intHeadPosition
                    intHeadAngle = trvScanVectors(intBeamNumber).sngAngle
                    trvScanVectors(intBeamNumber).sngRange = ftnGetHeroSonarRange()
                Next intBeamNumber
                If Not boolStop Then
                    intHeadAngle = 0 + intScanOffsetDegrees
                    ftnHero_Communicate("GMC3C861H", ">", 60000) ' Move Head CCW to center
                End If
            End If
            ftnGetHeroScanVectors = True
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnGetHeroScanVectors - :  " & ex.ToString)
        End Try
    End Function


    Function ftnConvertHexStringToInteger(ByRef strHex As String, ByRef intDigits As Integer) As Integer
        Try
            ftnConvertHexStringToInteger = 0
            Dim intCharValue As Integer
            strHex &= "0000"
            While (ftnConvertHexCharToInteger(Left(strHex, 1)) < 0) And (Len(strHex) > intDigits)
                strHex = Replace(strHex, Left(strHex, 1), "", 1, 1) ' Get rid of any leading non Hex characters
            End While
            For i = 1 To intDigits
                ftnConvertHexStringToInteger = ftnConvertHexStringToInteger * 16
                intCharValue = ftnConvertHexCharToInteger(Mid(strHex, i, 1))
                ftnConvertHexStringToInteger += ftnConvertHexCharToInteger(Mid(strHex, i, 1))
            Next
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnConvertHexStringToInteger - :  " & ex.ToString)
        End Try
    End Function

    Function ftnConvertHexCharToInteger(ByRef strHex As String) As Integer
        ' returns -1 if non valid Hex chracter, else 
        Try
            ftnConvertHexCharToInteger = Asc(Left(strHex, 1)) - 48
            If (ftnConvertHexCharToInteger < 0) Or (ftnConvertHexCharToInteger > 22) Or ((ftnConvertHexCharToInteger > 9) And (ftnConvertHexCharToInteger < 17)) Then
                ftnConvertHexCharToInteger = -1
            ElseIf (ftnConvertHexCharToInteger > 9) Then
                ftnConvertHexCharToInteger -= 7
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnConvertHexCharToInteger - :  " & ex.ToString)
        End Try
    End Function




    Private Function ftnGetHeroSonarRange() As Single
        'Return scan range in Inches relative to center of Bot
        ftnGetHeroSonarRange = 0
        Try
            If boolHeroSelected Then
                Dim strRange As String = ftnHero_Communicate("GS", ">", 1000)
                ftnGetHeroSonarRange = ftnConvertHexStringToInteger(strRange, 2)
                If (ftnGetHeroSonarRange = 255) Then ftnGetHeroSonarRange = 0
                If (ftnGetHeroSonarRange > 0) Then _
                    ftnGetHeroSonarRange = ftnGetHeroSonarRange * 0.408 + 3 ' 
                If boolDebugHero Then frmDebug.subWriteLogs(" Range = " & ftnGetHeroSonarRange & " inches")
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnGetHeroSonarRange :  " & ex.ToString)
        End Try
    End Function




End Module
