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


Public Class frmDock

    Private Sub subDockLoad(ByVal sender As Object, ByVal e As System.EventArgs) Handles Me.Load
        On Error Resume Next
        UpDnLocX.Value = tmcDock.X
        UpDnLocY.Value = tmcDock.Y
        updnOrientation.Value = intDockOrientation
        UpDownPCTrigger.Value = intDockTriggerPC
        txtBotBatteryTrigger.Text = sngDockTriggerBot.ToString
        chkBackIn.Checked = boolDockBackIn
    End Sub



    Private Sub UpDnLocX_ValueChanged(sender As System.Object, e As System.EventArgs) Handles UpDnLocX.ValueChanged
        On Error Resume Next
        boolSettingsChanged = True
        tmcDock.X = UpDnLocX.Value
        tmcDock = ftnCheckMapArrayLimits(tmcDock)
        UpDnLocX.Value = tmcDock.X
    End Sub


    Private Sub UpDnLocY_ValueChanged(sender As System.Object, e As System.EventArgs) Handles UpDnLocY.ValueChanged
        On Error Resume Next
        tmcDock.Y = UpDnLocY.Value
        tmcDock = ftnCheckMapArrayLimits(tmcDock)
        UpDnLocY.Value = tmcDock.Y
        boolSettingsChanged = True
    End Sub


    Private Sub updnOrientation_ValueChanged(sender As System.Object, e As System.EventArgs) Handles updnOrientation.ValueChanged
        On Error Resume Next
        intDockOrientation = updnOrientation.Value
        boolSettingsChanged = True
    End Sub


    Private Sub ChkBackIn_CheckedChanged(sender As System.Object, e As System.EventArgs) Handles chkBackIn.CheckedChanged
        On Error Resume Next
        boolDockBackIn = chkBackIn.Checked()
        boolSettingsChanged = True
    End Sub

    Private Sub UpDownPCTrigger_ValueChanged(sender As System.Object, e As System.EventArgs) Handles UpDownPCTrigger.ValueChanged
        On Error Resume Next
        intDockTriggerPC = UpDownPCTrigger.Value
        boolSettingsChanged = True
    End Sub

    Private Sub txtBotBatteryTrigger_TextChanged(sender As System.Object, e As System.EventArgs) Handles txtBotBatteryTrigger.TextChanged
        On Error Resume Next
        If (txtBotBatteryTrigger.Text <> "") Then sngDockTriggerBot = Val(txtBotBatteryTrigger.Text)
1:      boolSettingsChanged = True
    End Sub


    Private Sub OK_Button_Click(sender As System.Object, e As System.EventArgs) Handles OK_Button.Click
        Me.DialogResult = System.Windows.Forms.DialogResult.OK
        Me.Visible = False
        Me.Close()
    End Sub

    Private Sub Cancel_Button_Click(sender As System.Object, e As System.EventArgs) Handles Cancel_Button.Click
        Me.DialogResult = System.Windows.Forms.DialogResult.Cancel
        Me.Close()
        frmGotABotTop.mnuLoadSettings_Click(sender, e)
    End Sub


    Private Sub Exit_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Me.FormClosing
        Me.Visible = False
    End Sub


    '*****************************************************************************************************************************


    Public Sub subDock()
        Try
            Thread.CurrentThread.Name = "threadDock"
            If Not ftnCheckForStopMode() Then
                boolDock = True
                ftnSendCommand("play phraseA 'Docking.'")
                Dim boolGoodLocalization As Boolean = False
                tbpBotPose = ftnLocalize_Top(tbpBotPose, intBestParticle, boolGoodLocalization, True)
                subDriveToPreDock()
                tbpBotPose = ftnLocalize_Top(tbpBotPose, intBestParticle, boolGoodLocalization, True)
                Do
                    subPreDockAdjust()  ' Want accurate positioning before attempting Dock contact
                    subCheckForPause()
                    boolDocked = ftnDockMakeContact()
                    If Not boolDocked Then
                        subUnDock(tbpBotPose)
                        tbpBotPose = ftnLocalize_Top(tbpBotPose, intBestParticle, boolGoodLocalization, True)
                        If ftnIAmLost(tbpBotPose) Then tbpBotPose = ftnLostRecoveryTop(tbpBotPose)
                    End If
                Loop Until boolDocked Or boolStop Or ftnIAmLost(tbpBotPose) Or boolShutDown Or Not boolDock
                If boolDocked And Not boolStop And Not boolShutDown Then
                    frmDebug.subWriteFinishedMessage()
                    strStatusDriving = "DOCKED"
                    ftnSendCommand("play phrase 'Docking Completed.'")
                    boolDock = False
                    intMovementsSinceLocalization = 0
                    subInitParticles(tbpBotPose, intNumberOfParticles)
                    boolDocked = True
                    Dim tbpDockedPose As typeBotPose = tbpBotPose
                    tbpDockedPose.Location = tmcDock
                    ftnSetMoveInfo(TypeOfMove.HALT, tbpDockedPose, tbpDockedPose, False)
                    tbpBotPose = tbpDockedPose
                    tbpTargetPosePlanned = tbpDockedPose
                ElseIf ftnIAmLost(tbpBotPose) Then
                    tbpBotPose = ftnLocalize_Top(tbpBotPose, intBestParticle, boolGoodLocalization, True)
                ElseIf boolStop Then
                    boolDocked = False
                    boolDock = False
                Else
                    boolDocked = False
                    strStatusDriving = "DOCKING FAILED"
                    tbpBotPose = ftnLocalize_Top(tbpBotPose, intBestParticle, boolGoodLocalization, True)
                    ftnSendCommand("play phraseA 'Docking Failed.'")
                    boolDock = False
                End If
                ftnSendCommand("set power stopped " & intPowerStopped / 3)
            Else
                boolDock = False
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subDock :  " & ex.ToString)
        End Try
    End Sub

    Public Sub subUnDock(ByRef tbpEstimatedPose As typeBotPose)
        Try
            If Not boolStop And Not boolPause And (ftnLocationsEqual(tbpBotPose.Location, tmcDock) Or boolDocked) And Not boolShutDown Then
                frmDebug.subWriteLogs(vbCrLf & vbCrLf & vbCrLf & Now() & "    --------------------------  UnDocking.  ----------------------- ")
                tbpBotPose = ftnLocalize_Top(tbpBotPose, intBestParticle, False, True)
                strStatusDriving = "UnDocking"
                Dim tbpTargetPose As typeBotPose = tbpEstimatedPose
                If boolDockBackIn Then
                    tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpBotPose, 0, 6)
                Else
                    tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpBotPose, 180, 4 + sngBotRadius)
                End If
                ftnSendCommand("play phrase prompt")
                tbpEstimatedPose = ftnBotFwd(tbpEstimatedPose, tbpTargetPose)  '  
                intMovementsSinceLocalization = 99  'Force Localization
                tbpBotPose = ftnPostMoveLocalizeAndPath(tbpTargetPose, TypeOfMove.FWD, tbpTargetPose)
                boolDocked = False
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subUnDock :  " & ex.ToString)
        End Try
    End Sub

    Function ftnGetBatteryPreDock() As Single
        On Error Resume Next
        ftnGetBatteryPreDock = 0
        strStatusDriving = "Wait For Battery Stabilzation"
        If Not boolSimulatedMovement Then subWait(10000)
        ftnSendCommand("battery")
        For i = 1 To 4  ' Find average of the battery voltage to reduce conversion noise
            ftnSendCommand("battery")
            ftnGetBatteryPreDock += sngBotBatteryVoltage
        Next
        ftnGetBatteryPreDock = ftnGetBatteryPreDock / 4
        If boolDebugDock Then _
            frmDebug.subWriteLogs(Now() & " >DOCKING CHECK: ftnGetBatteryPreDock: " & ftnGetBatteryPreDock)
    End Function

    Sub subDriveToPreDock()
        On Error Resume Next
        tmcDestination = ftnPreDockLocation()
        ftnDriveToDestination_Top(tbpBotPose, tmcDestination)
        tbpBotPose = ftnLocalize_Top(tbpBotPose, intBestParticle, False, True)
    End Sub

    Sub subPreDockAdjust()
        ' Goto preDock Location
        ' Turn towards Dock 
        Try  ' Fine positioning before final docking maneuvers. Called when Bot has arrived near its pre docking position (approx 8-10" in front of dock)
            Dim intPreDockedOientation As Integer = intDockOrientation
            If boolDockBackIn Then intPreDockedOientation = ftnConvertTo360(intPreDockedOientation + 180)
            frmDebug.subWriteLogs(vbCrLf & ftnElapsedTimeS() & "    --------------------------  PreDocking Fine Adjustments  ----------------------- ")
            strStatusDriving = "PreDock Adjustment "
            tbpBotPose = ftnAdjustPose(tbpBotPose, ftnConvertToPose(ftnPreDockLocation(), intPreDockedOientation), 2)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subPreDockAdjust :  " & ex.ToString)
        End Try
    End Sub

 
     Function ftnDockMakeContact() As Boolean  ' returns true if docking successful
        ftnDockMakeContact = False
        Try ' Final docking maneuvers - Line up charging contacts with Dock and then drive to Dock
            If Not boolStop And Not boolShutDown Then
                frmDebug.subWriteLogs(vbCrLf & vbCrLf & vbCrLf & ftnElapsedTimeS() & "    --------  Final Docking Sequence  --------- ")
                Dim intTries As Integer = 0
                While Not ftnDockMakeContact And (intTries < 2) And Not boolStop And Not boolShutDown
                    ftnDockMakeContact = ftnTurnToDockingOrientation()
                    subCheckForPause()
                    intTries += 1
                End While
                If ftnDockMakeContact Then ftnDockMakeContact = ftnDockMakeContactFinalMovement(tbpBotPose)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnDockMakeContact :  " & ex.ToString)
        End Try
    End Function

    Function ftnTurnToDockingOrientation() As Boolean
        Try
            ftnTurnToDockingOrientation = False
            If Not boolStop And Not boolPause And Not boolShutDown Then
                Dim intTries As Integer = 1
                While (Abs(ftnAngleDifference(intDockOrientation, tbpBotPose.Orientation)) > 3) And (intTries < 3) And Not boolStop And Not boolShutDown
                    intDrivingSequenceNumber += 1
                    strStatusDriving = "Turn Towards Dock"
                    frmDebug.subWriteLogs(ftnElapsedTimeS() & "    --------  Turn Charging Contacts Towards Dock  --------- ")
                    tbpBotPose = ftnBotTurn(tbpBotPose, False, ftnConvertToPose(tbpBotPose.Location, intDockOrientation))
                    If ftnScannerConnected() Or Not boolDockBackIn Then tbpBotPose = ftnLocalize_Top(tbpBotPose, intBestParticle, True, True)
                    subCheckForPause()
                    intTries += 1
                End While
                ftnTurnToDockingOrientation = (Abs(ftnAngleDifference(intDockOrientation, tbpBotPose.Orientation)) <= 3)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnTurnToDockingOrientation :  " & ex.ToString)
        End Try
    End Function

    Function ftnDockMakeContactFinalMovement(ByRef tbpEstimatedPose As typeBotPose) As Boolean
        ' Make contact with Dock and determine success by measuring battery voltage
        Try
            ftnDockMakeContactFinalMovement = False
            If Not boolStop And Not boolPause And Not boolShutDown Then
                Dim sngBotLinearVelocityTemp As Single = sngBotLinearVelocity
                Dim sngBotLinearAccelerationTemp As Single = sngBotLinearAcceleration
                Dim intPowerMovingTemp As Integer = intPowerMoving
                Dim intPowerStoppedTemp As Integer = intPowerStopped
                'Dim boolEnableCourseCorrectionTemp As Boolean = boolEnableCourseCorrection
                Dim tbpTargetPose As typeBotPose = tbpEstimatedPose
                sngBotLinearVelocity = Min(sngBotLinearVelocity, 10) ' slower speed so we dont ram the dock
                intPowerMoving = intPowerMoving * 0.6  ' lower power so we dont ram the dock
                intPowerStopped = Max(intPowerStopped, intPowerStopped) ' 
                sngBotLinearAcceleration = Max(sngBotLinearAcceleration, 25)
                'boolEnableCourseCorrection = False
                intDrivingSequenceNumber += 1
                If boolDockBackIn Then  ' Dock contacts are accessed via the rear of the robot
                    If boolURGConnected Then
                        tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpBotPose, 180, ftnDistanceBetweenLocations(tbpBotPose.Location, tmcDock) + 3)
                    Else
                        tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpBotPose, 180, ftnDistanceBetweenLocations(tbpBotPose.Location, tmcDock) + 5)
                    End If
                Else
                    If boolURGConnected Then
                        tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpBotPose, 0, ftnDistanceBetweenLocations(tbpBotPose.Location, tmcDock) + 3)
                    Else
                        tbpTargetPose.Location = ftnLocationFromEstimatedPose(tbpBotPose, 0, ftnDistanceBetweenLocations(tbpBotPose.Location, tmcDock) + 5)
                    End If
                End If
                Dim boolCheckLaptopCharge As Boolean = Not (SystemInformation.PowerStatus.BatteryChargeStatus = BatteryChargeStatus.Charging)
                Dim sngBotBatteryPreDock As Single = ftnGetBatteryPreDock()  '  Bot's Battery voltage used to determine docking success
                frmDebug.subWriteLogs(Now() & "    --------  Make Contact With Dock  --------- ")
                tbpBotPose = ftnBotFwd(tbpEstimatedPose, tbpTargetPose)  ' Make contact
                If boolSimulatedMovement Then
                    ftnDockMakeContactFinalMovement = True
                    tbpBotPose.Location = tbpTargetPose.Location
                ElseIf Not boolStop And Not boolShutDown And Not boolPause Then
                    strStatusDriving = "Wait For Battery To Charge"
                    If Not boolSimulatedMovement Then subWait(4000) ' wait for battery to charge a bit
                    Dim i As Integer = 0
                    While (i < 12) And Not ftnDockMakeContactFinalMovement And Not boolShutDown And Not boolStop And Not boolPause
                        i += 1
                        subWait(1000)
                        ftnDockMakeContactFinalMovement = ftnDocked(sngBotBatteryPreDock, boolCheckLaptopCharge)
                        If boolDebugDock Then
                            frmDebug.subWriteLogs(Now() & " >DOCKING CHECK: sngBotBatteryPreDock: " & sngBotBatteryPreDock & "    sngBotBatteryVoltage: " & sngBotBatteryVoltage)
                            frmDebug.subWriteLogs(Now() & " >            boolCheckLaptopCharge : " & boolCheckLaptopCharge & ";   Status: " & SystemInformation.PowerStatus.BatteryChargeStatus.ToString)
                        End If
                    End While
                    tbpBotPose.Location = tbpTargetPose.Location
                    If ftnDockMakeContactFinalMovement Then
                        frmDebug.subWriteLogs(ftnElapsedTimeS() & " >DOCKED:  sngBotBatteryPreDock: " & sngBotBatteryPreDock & "    sngBotBatteryVoltage: " & sngBotBatteryVoltage)
                        frmDebug.subWriteLogs(ftnElapsedTimeS() & " >            boolCheckLaptopCharge : " & boolCheckLaptopCharge & ";   Status: " & SystemInformation.PowerStatus.BatteryChargeStatus.ToString)
                    Else
                        frmDebug.subWriteLogs(ftnElapsedTimeS() & " >DOCKING FAILED: sngBotBatteryPreDock: " & sngBotBatteryPreDock & "    sngBotBatteryVoltage: " & sngBotBatteryVoltage)
                        frmDebug.subWriteLogs(ftnElapsedTimeS() & " >            boolCheckLaptopCharge : " & boolCheckLaptopCharge & ";   Status: " & SystemInformation.PowerStatus.BatteryChargeStatus.ToString)
                        tbpBotPose.Location = tmcDock
                        tbpBotPose.LocationUncertainty.X += 3
                        tbpBotPose.LocationUncertainty.Y += 3
                        tbpBotPose.OrientationUncertainty += 15
                        subUnDock(tbpBotPose)
                    End If
                End If
                sngBotLinearAcceleration = sngBotLinearAccelerationTemp
                sngBotLinearVelocity = sngBotLinearVelocityTemp
                intPowerMoving = intPowerMovingTemp
                intPowerStopped = intPowerStoppedTemp
                'boolEnableCourseCorrection = boolEnableCourseCorrectionTemp
                If ftnDockMakeContactFinalMovement Then
                    ftnSetMoveInfo(TypeOfMove.HALT, tbpBotPose, tbpBotPose, True)
                    ftnBotStop(tbpBotPose, tbpBotPose)
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnDockMakeContactFinalMovement :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnPreDockLocation() As typeMapCoordinate
        Try
            Dim sngDistanceFromDock As Single
            If boolURGConnected Then
                sngDistanceFromDock = 5  ' A URG has a short minimum distance for measurement and excellent localizing abilities
            ElseIf boolKinectConnected Then
                sngDistanceFromDock = 10 ' Kinect requires a long minimum distance for measurement
            Else
                sngDistanceFromDock = 8
            End If
            If Not boolDockBackIn Then sngDistanceFromDock = -sngDistanceFromDock
            ftnPreDockLocation = ftnLocationFromLocation(tmcDock, intDockOrientation, sngDistanceFromDock)
            If ftnInWall_NoChk(ftnPreDockLocation, intBestParticle) Then ftnPreDockLocation = ftnLocationFromLocation(tmcDock, intDockOrientation, sngDistanceFromDock / 2)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnPreDockLocation")
        End Try
    End Function

    Function ftnDockContactLocation() As typeMapCoordinate
        If boolDockBackIn Then
            ftnDockContactLocation = ftnLocationFromLocation(tmcDock, intDockOrientation, -sngBotRadius)
        Else
            ftnDockContactLocation = ftnLocationFromLocation(tmcDock, intDockOrientation, +sngBotRadius)
        End If
    End Function


    Function ftnDocked(ByVal sngBotBatteryPreDock As Single, ByRef boolCheckLaptopCharge As Boolean) As Boolean  ' True if charging
        Try
            ftnDocked = False
            If boolSimulatedMovement Then
                ftnDocked = True
            ElseIf boolCheckLaptopCharge And (SystemInformation.PowerStatus.BatteryChargeStatus = BatteryChargeStatus.Charging) Or CBool(InStr(SystemInformation.PowerStatus.BatteryChargeStatus.ToString, "Charging")) Then  ' 
                ftnDocked = True
            ElseIf Not boolStop And Not boolShutDown Then
                ftnSendCommand("battery")
                ftnDocked = (sngBotBatteryVoltage > (sngBotBatteryPreDock * 1.015)) ' Bot battery charging
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnDocked :  " & ex.ToString)
        End Try
    End Function


End Class