Imports System.Net
Imports System.Net.Sockets
Imports System.Text
Imports Microsoft.Kinect
Imports System.Drawing
Imports System.IO.Ports

Imports Emgu.CV.UI
Imports Emgu.CV
Imports Emgu.CV.Structure
Imports Emgu.CV.CvEnum




Public Module modPublicDeclarations


    Public Structure typeImageCoordinate
        Dim X As Integer
        Dim Y As Integer
    End Structure


    Public Structure typeMapCoordinate
        Dim X As Integer
        Dim Y As Integer
    End Structure

    Public Structure typeMapOffset
        Dim X As Single
        Dim Y As Single
    End Structure



    '==============  Map  ==================
    Public bmpMap As Bitmap = System.Drawing.Image.FromFile("GotABot_Map.bmp")
    Public intMapHeight As Integer = bmpMap.Height
    Public intMapWidth As Integer = bmpMap.Width
    Public byteNavigationArray(intMapWidth, intMapHeight) As Byte
    Public intDistanceFromWallSquaredArray(32, intMapWidth, intMapHeight) As Integer '  
    Public boolOccupiedCell(32, intMapWidth, intMapHeight) As Boolean
    Public boolGreySpace(intMapWidth, intMapHeight) As Boolean
    Public boolNoGo(intMapWidth, intMapHeight) As Boolean
    Public sngZoom As Single = 1

    '==============  Map Path   ==================
    Public intPathArray(intMapWidth, intMapHeight) As Integer  '
    Public tmcDestination As typeMapCoordinate
    Public Const constDestinationCost As Integer = 4096
    Public Const constMaxPathCost As Integer = 65535
    Public intSquareDistanceArray(22, 22) As Integer


    '==============  Bot   ==================
    Public boolER1Selected As Boolean = True
    Public boolHeroSelected As Boolean = True
    Public sngBotDiameter As Single = 17
    Public sngBotRadius As Single = sngBotDiameter / 2
    Public sngBotRadiusSquared As Single = sngBotRadius * sngBotRadius
    Public intBotRadiusZoom As Integer = (sngBotDiameter * sngZoom) \ 2
    Public intBotDiameterZoom As Integer = sngBotDiameter * sngZoom
    Public sngCameraOffset As Single = -2
    Public ticCameraResolution As typeImageCoordinate
    Public sngCameraFOV As Single = 60
    Public sngCameraOffsetDegrees As Single = 0
    Public boolEnableBumperSwitches_ER1 As Boolean = True
    Public intHeadAngle As Integer = 0
    Public intPowerMoving As Integer = 60
    Public intPowerStopped As Integer = 60
    Public sngBotWheelSeparation As Single = 38
    'Public comBot As SerialPort

    '==============  Bot Corrections  ==================
    Public sngSteeringBiasOffsetCorrection As Integer = 0
    Public sngDistanceCorrectionCoeff_x2 As Single = -0.0059
    Public sngDistanceCorrectionCoeff_x1 As Single = 0.8986
    Public sngDistanceCorrectionCoeff_x0 As Single = 0.0924
    Public intScanOffsetDegrees As Integer = 0  ' Compensates for when Scanner is not pointed directly ahead




    '==============  Bot Dead Reckoning  ==================
    Public sngBotLinearVelocity As Single = 20    'cm /second
    Public sngBotLinearVelocityMax As Single = sngBotLinearVelocity    'cm /second
    Public sngBotLinearAcceleration As Single = 70  'cm /second /second 
    Public sngBotLinearAccelerationTime As Single = sngBotLinearVelocity / sngBotLinearAcceleration
    Public sngBotLinearAccelerationDistance As Single = ftnCmToInches(sngBotLinearAcceleration * sngBotLinearAccelerationTime * sngBotLinearAccelerationTime / 2)
 
    Public sngBotAngularVelocity As Single = 30 'degrees /second
    Public sngBotAngularAcceleration As Single = 40  'cm /second /second 
    Public sngBotAngularAccelerationTime As Single = sngBotAngularVelocity / sngBotAngularAcceleration
    Public sngBotAngularAccelerationDistance As Single = (sngBotAngularAcceleration * sngBotAngularAccelerationTime * sngBotAngularAccelerationTime / 2)

    Public sngBotMovementAccuracy As Single = 0.65
    Public sngMinCurveDistance As Single = 5

    '==============  Move Info  ==================
    Public tbpTargetPosePlanned As typeBotPose
    Public tomTypeOfMovePlanned As TypeOfMove
    Public tbpStartingPosePlanned As typeBotPose
    Public Enum TypeOfMove
        HALT = 0
        FWD = 1
        TURN = 2
        CURVE = 3
        NONE = 4
    End Enum
    Public Structure typeMoveInfo
        Dim tomTypeOfMove As TypeOfMove
        Dim boolBotIsMoving As Boolean
        Dim timeMoveCommandSent As DateTime
        Dim tbpStartingPose As typeBotPose ' 
        Dim tbpTargetPose As typeBotPose ' 
        Dim boolCourseCorrection As Boolean
        Dim boolMoveCommandInProgress As Boolean
    End Structure
    Public tmiMoveInfo As typeMoveInfo
    Public boolSetMoveInfoBusy As Boolean = False
    Public boolGetMoveInfoBusy As Boolean = False


    '==============  Bot Pose  ==================
    Public Structure typeBotPose
        Dim Location As typeMapCoordinate
        Dim LocationUncertainty As typeMapCoordinate
        Dim Orientation As Single
        Dim OrientationUncertainty As Integer
    End Structure
    Public tbpBotPose As typeBotPose



    '==============  Driving Control  ==================
    Public boolStop As Boolean = False
    Public boolPause As Boolean = False
    Public boolSimulatedMovement As Boolean = False
    Public boolDrivingToDestination As Boolean = False
    Public intDrivingSequenceNumber As Integer
    Public boolCollisionDetectionEnable As Boolean = True
    Public sngMaxTravel As Single = 111
    Public sngCcDelayS As Single = 0.25  ' Course Correction - Delay from Start of timeLocalizationStart to time of new move issued  


    '==============  Visual LandMarks  ==================
    Public Structure typeLandMark
        Dim LandMarkName As String
        Dim LandMarkLocation As typeMapCoordinate
        Dim intLandMarkHeight As Integer
        Dim LandMarkValidRegion As Rectangle
        Dim LandMarkRecognized As Integer
        Dim LandMarkValid As Integer
    End Structure
    Public tlmLandMarkArray(128) As typeLandMark

    Public Structure typeImageRecognized
        Dim Name As String
        Dim intNumber As Integer  ' 
        Dim intPixelX As Integer
        Dim intPixelY As Integer
        Dim sngRange As Single ' NOTE - range from center of Bot when camera in normal position
        Dim sngYRotation As Single
    End Structure

    '==============  SOCKETS  ==================

    Public Class typeSocketClass
        Public workSocket As Socket = Nothing ' Client  socket.
        Public Const BufferSize As Integer = 4096 ' Size of subReceive byteReceiveBuffer.
        Public byteReceiveBuffer(BufferSize) As Byte ' subReceive byteReceiveBuffer
        Public sbReceivedData As New StringBuilder ' Received data string.
    End Class '

    Public Class typeSocketClass_SmallBuffer
        Public workSocket As Socket = Nothing ' Client  socket.
        Public Const BufferSize As Integer = 64 ' Size of subReceive byteReceiveBuffer.
        Public byteReceiveBuffer(BufferSize) As Byte ' subReceive byteReceiveBuffer
        Public sbReceivedData As New StringBuilder ' Received data string.
    End Class '

 


    '==============  Movement and Localization Control  ==================
    Public boolLocalizationInProgress As Boolean = False
    Public timeDrivingStarted As DateTime

    Public Const constMaxTurnsWithLocalization As Integer = 3  '  prevents Bot from turning back and forth repeatedly
    Public intMovementsSinceLocalization As Integer = 0   ' used to force localization at least once every x moves


    '==============  API Variables  ==================
    Public strApiPassword As String = "GotABot"

    '==============   Recognizer Variables  ==================
    Public sngLocalizationTime As Single = 10
    Public boolEnableRcc As Boolean = False
    Public boolEnableFaceRecognition As Boolean = True



    '==============   RBPF Variables  ==================
    Public intNumberOfParticles As Integer = 32
    Public tbpParticlePose(intNumberOfParticles) As typeBotPose
    Public sngRMS_Error(intNumberOfParticles) As Single
    Public sngRMS_ErrorAvg(intNumberOfParticles) As Single
    Public boolEnableRBPF As Boolean = False
    Public intBestParticle As Integer = 0



    '==============  SLAM Variables  ==================
    Public boolSlam As Boolean = False



    '==============  Misc Variables  ==================
    Public boolViewLM As Boolean = True
    Public boolViewDebug As Boolean = True
    Public boolSettingsChanged As Boolean = False
    Public boolMute As Boolean = False
    Public boolShutDown As Boolean = False
    Public boolMapUpdatedWithObstacle As Boolean = False

    '==============  Communication Status  ==================
    Public boolBotConnected As Boolean = False
    Public booRccConnected As Boolean = False


    '==============   Status Line  ==================
    Public strStatusDriving As String = String.Format("{0, -35}", "Waiting for command:     ")
    Public strStatusCommunications As String = String.Format("{0, -35}", "Coms:     ")
    Public strStatusBotVoltage As String = String.Format("{0, -15}", "   --   ")
    Public strStatusLaptopVoltage As String = String.Format("{0, -20}", "   --   ")
    Public strStatusMouseCoordinates As String = String.Format("{0, -15}", "    x,y    ")
    Public strStatusAPIMessage As String = String.Format("{0, -45}", "API: Not connected          ")
    Public strStatusMapContents As String = String.Format("{0, -15}", "   --   ")


    '==============  Docking  ==================
    Public boolDock As Boolean = False
    Public boolDocked As Boolean = False
    Public tmcDock As typeMapCoordinate
    Public intDockOrientation As Integer = 0  '  Orientation of Bot when docked
    Public boolDockBackIn As Boolean = False
    Public intDockTriggerPC As Integer = 30
    Public sngDockTriggerBot As Single = 11
    Public sngBotBatteryVoltage As Single = 2000


    Public Structure typeRangeVector
        Dim sngAngle As Single
        Dim sngRange As Single
    End Structure

    '================== Kinect   ========================
    Public K_Sensor As KinectSensor = Nothing
    Public boolKinectConnected As Boolean = False
    Public intDefaultElevation As Integer = 10

    '================== URG04   ========================
    Public boolURGConnected As Boolean = False
    Public boolUrgLaserOn As Boolean = False

    '================== Scan Vectors (from Kinect, URG, API or whatever)   ========================
    Public Const constMaxScannerBeams As Integer = 640
    Public intNumberOfBeams As Integer = 32
    Public sngScannerCalCoefficients(constMaxScannerBeams) As Single
    Public sngScannerCalibrate As Single = 1
    Public sngScannerCurveAdjust As Single = 0
    Public tmcScanVectorOffset As typeMapCoordinate
    Public trvScanVectors(constMaxScannerBeams) As typeRangeVector

    Public sngScanFOV As Single = 58.5
    Public intScanVectorsMaxRangeInches As Integer = 177
    Public intScanVectorsMinRangeInches As Integer = 20
    Public boolShowGreenVectors As Boolean = False
    Public Event evntVectorsReceived()



    '================== Precalculated Distances  ========================
    Public sngOneSigma As Single = 5
    Public sngOneSigmaSquared As Single = sngOneSigma * sngOneSigma
    Public sngOneDotFiveSigma As Single = 1.5 * sngOneSigma
    Public intOneDotFiveSigmaSquared As Integer = sngOneDotFiveSigma * sngOneDotFiveSigma
    Public intTotallySafeDistanceFromWallSquared As Integer = (sngBotRadius + 0.5 + 8) ^ 2
    Public intSafeDistanceFromWallSquared As Integer = (sngBotRadius + 0.5 + 3) ^ 2
    Public intSaferDistanceFromWallSquared As Integer = (sngBotRadius + 0.5 + 2) ^ 2
    Public sngUnacceptableError As Single = 4
    Public sngOkError As Single = 3
    Public sngLowError As Single = 2


 
    '================== Debug Log File  ========================
    Public boolDebugDrive As Boolean = False   ' 
    Public boolDebugCC As Boolean = False   '
    Public boolDebugObstacle As Boolean = False ' 
    Public boolDebugER1IF As Boolean = False   ' 
    Public boolDebugHero As Boolean = False   ' 
    Public boolDebugAPI As Boolean = False   ' 
    Public boolDebugOdometry As Boolean = False   ' 
    Public boolDebugLocalize2D As Boolean = False   ' 
    Public boolDebugKinect As Boolean = False   ' 
    Public boolDebugDock As Boolean = False    ' 
    Public boolDebugPath As Boolean = False
    Public boolDebugCollision As Boolean = False
    Public boolDebugSlam As Boolean = False
    Public boolDebugRBPF As Boolean = False
    Public boolDebugURG As Boolean = False
    Public boolDebugDR As Boolean = False   ' 
    Public boolDebugArm As Boolean = False





    '================== Video / Face Recognition  ========================
    Public boolEnableVideo As Boolean = False
    Public intVideoSource As Integer = 1
    Public boolVideoStarted As Boolean = False
    Public boolTrained As Boolean = False
    Public strPersonName As String = ""



    '================== Facial Recognition   ========================
  

    Public Structure typeFaceClassifier
        Dim strFaceName As String
        Dim X As Integer
        Dim Y As Integer
        Dim intSize As Integer
    End Structure


    '================== Debug Log File   ========================
    Public queueLogEntries As New Queue(150)
    Public boolLogFileEnable As Boolean = True


End Module
