Imports System.Threading
Imports System.IO.Ports
Imports System.Drawing
Imports System.Collections
Imports System.Windows.Forms


'.  Pilot Motion Processor command
'            1st byte: address
'            2nd byte: checksum
'            3nd byte: axis (always 00)
'            4th byte: name of command (command code)
'            5th byte+: parameters of the command. (higher order bytes send first)
'



Public Structure PWDReply
    Dim data() As Byte  'data[PWMREPLYSIZE]
    Dim dwSize As Integer
    Dim Status() As Integer ' return 0(valid) if the valus is correct
    Dim CheckChecksum() As Integer
End Structure


Module modIF_ER1

    Friend WithEvents comBot As New SerialPort

    '==================================  Serial Interface  =====================================================
    Public Function ftnER1_Config() As Boolean
        ' returns true if comBot is successfully opened and configured
        ftnER1_Config = False
        Try
            If Not boolShutDown And Not boolBotConnected Then
                strStatusDriving = "Detecting ER1."
                For i = 1 To 2
                    Application.DoEvents()
                    If Not boolBotConnected And Not boolShutDown Then
                        For Each sp As String In My.Computer.Ports.SerialPortNames
                            Dim strURGComPort As String = ""
                            If (comURG IsNot Nothing) Then strURGComPort = comURG.PortName
                            If Not boolBotConnected And Not boolShutDown And Not (sp = strURGComPort) _
                                And Not ((sp = ("COM" & frmArm.intArmComPort.ToString)) And frmArm.boolEnableArm) Then
                                frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnER1_Config: Checking " & sp)
                                If comBot IsNot Nothing Then ftnER1_Close()
                                With comBot
                                    .PortName = sp
                                    .BaudRate = 250000
                                    .DataBits = 8
                                    .Parity = Parity.None
                                    .StopBits = StopBits.One
                                    .Handshake = Handshake.None
                                    .WriteTimeout = 64
                                    .ReadTimeout = 64
                                End With
                                ftnER1_SerialOpen()
                                boolBotConnected = ftnER1_CheckConnection()
                                If Not boolBotConnected And (comBot IsNot Nothing) Then
                                    comBot.Close()
                                    comBot.PortName = "999"
                                End If
                            End If
                        Next sp
                    End If
                Next i
                If boolBotConnected And Not boolShutDown Then
                    subResetWheels(PLProfile.Trapezoidal, ER1Default.Acceleration * sngBotAngularAcceleration / 100, ER1Default.Acceleration * sngBotAngularAcceleration / 100)
                    frmDebug.subWriteLogs(" ftnER1_Config: Connected " & comBot.PortName)
                    strStatusDriving = "ER1 Connected."
                Else
                    strStatusDriving = "ER1 not Connected."
                End If
            ElseIf boolShutDown Then
                ftnER1_Close()
            End If
            If boolDebugER1IF Then frmDebug.subWriteLogs(" #####################   ftnER1_Config: boolBotConnected = " & boolBotConnected)
            ftnER1_Config = boolBotConnected
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnER1_Config :  " & ex.ToString)
            strStatusDriving = "ERROR: ER1 not detected."
            ftnER1_Config = False
            ftnER1_Close()
        End Try
    End Function

    Public Function ftnER1_SerialOpen() As Boolean
        ' returns true if comBot is successfully opened
        Try
            ftnER1_SerialOpen = False
            If comBot IsNot Nothing And Not boolShutDown Then
                If Not boolBotConnected Then
                    If Not comBot.IsOpen Then comBot.Open()
                End If
            ElseIf boolShutDown Then
                ftnER1_Close()
            End If
            ftnER1_SerialOpen = comBot.IsOpen
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnER1_SerialOpen :  " & ex.ToString)
            ftnER1_SerialOpen = False
            ftnER1_Close()
        End Try
    End Function

    Function ftnER1_CheckConnection() As Boolean
        Try
            If comBot IsNot Nothing And Not boolShutDown Then
                If comBot.IsOpen Then  ' Check connection
                    If boolDebugER1IF Then frmDebug.subWriteLogs(" ##################### ftnER1_CheckConnection: Confirm connection to RCM ")
                    boolBotConnected = True
                    comBot.DiscardInBuffer()
                    boolBotConnected = ftnSendER1_CommandTop(4, 1, PilotSet.GetChecksum, 0, 0, 0, 0, 0, 0, 4) = &H12345678
                    If boolBotConnected Then _
                        boolBotConnected = ftnSendER1_CommandTop(4, 0, PilotSet.GetChecksum, 0, 0, 0, 0, 0, 0, 4) = &H12345678
                End If
            ElseIf boolShutDown Then
                ftnER1_Close()
            End If
            ftnER1_CheckConnection = boolBotConnected
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnER1_CheckConnection :  " & ex.ToString)
            ftnER1_CheckConnection = False
            ftnER1_Close()
        End Try
    End Function

    Public Function ftnER1_Close() As Boolean
        Try
            If comBot IsNot Nothing Then _
                If comBot.IsOpen Then comBot.Close()
            If boolER1Selected Then boolBotConnected = False
            ftnER1_Close = True
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnER1_Close :  " & ex.ToString)
            ftnER1_Close = False
        End Try
    End Function

    Dim queueMessageThread As New Queue()
    Public Function ftnSendER1_CommandTop(ByRef intBytesToSend As Integer, ByRef byteAddress As Byte, ByRef byteOpCode As Byte, ByRef byte3 As Byte _
                            , ByRef byte4 As Byte, ByRef byte5 As Byte, ByRef byte6 As Byte, ByRef byte7 As Byte, ByRef byte8 As Byte,
                            ByRef intBytesToReceive As Integer) As UInt32
        ftnSendER1_CommandTop = 0
        Try
            If comBot IsNot Nothing And Not boolShutDown Then
                If boolBotConnected Then
                    queueMessageThread.Enqueue(Thread.CurrentThread.Name)
                    Dim timeOut As DateTime = Now().AddMilliseconds(2500)
                    While (queueMessageThread.Peek() IsNot Thread.CurrentThread.Name) And (Now() < timeOut)
                        Thread.Sleep(15)
                    End While
                    If (Now() > timeOut) And boolDebugER1IF Then frmDebug.subWriteLogs(ftnElapsedTimeS() & "  #####################    ftnSendER1_CommandTop: ERROR   Thread Name timeOut")
                    Dim i As Integer = 0
                    Do
                        boolBotConnected = True
                        If (i > 0) And boolDebugER1IF Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " #####################    ftnSendER1_CommandTop: Retry ftnSendER1_Command " & i)
                        ftnSendER1_CommandTop = ftnSendER1_Command(intBytesToSend, byteAddress, byteOpCode, byte3, byte4, byte5, byte6, byte7, byte8, intBytesToReceive)
                        i += 1
                    Loop Until (i = 2) Or boolBotConnected Or boolShutDown
                    queueMessageThread.Dequeue()
                ElseIf boolShutDown Then
                    If comBot.IsOpen Then comBot.Close()
                    boolBotConnected = False
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnSendER1_CommandTop :  " & ex.ToString)
        End Try
    End Function

    Function ftnSendER1_Command(ByRef intBytesToSend As Integer, ByRef byteAddress As Byte, ByRef byteOpCode As Byte, ByRef byte3 As Byte _
                            , ByRef byte4 As Byte, ByRef byte5 As Byte, ByRef byte6 As Byte, ByRef byte7 As Byte, ByRef byte8 As Byte,
                            ByRef intBytesToReceive As Integer) As UInt32
        Try
            If comBot IsNot Nothing And Not boolShutDown Then
                If boolBotConnected Then
                    comBot.DiscardInBuffer()
                    'Thread.Sleep(5)   ' 
                    Dim byteBuffer As Byte() = {
                                             byteAddress,
                                             Convert.ToByte(((Not (byteAddress + byteOpCode + byte3 + byte4 + byte5 + byte6 + byte7 + byte8) And &HFF) + 1) And &HFF),
                                             0,
                                             byteOpCode,
                                             byte3,
                                             byte4,
                                             byte5,
                                             byte6,
                                             byte7,
                                             byte8
                                             }
                    If boolDebugER1IF Then subSendDebug(byteBuffer, intBytesToReceive)
                    comBot.Write(byteBuffer, 0, intBytesToSend)
                    Dim boolReceived As Boolean = False
                    ftnSendER1_Command = ftnGetResponse(intBytesToReceive, boolReceived)
                    If Not boolReceived Then
                        Thread.Sleep(30)   ' 
                        If boolDebugER1IF Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " #####################    ftnSendER1_Command: Retry ftnGetResponse")
                        If boolDebugER1IF Then subSendDebug(byteBuffer, intBytesToReceive)
                        ftnSendER1_Command = ftnGetResponse(intBytesToReceive, boolReceived) ' Retry
                    End If
                    boolBotConnected = boolReceived
                    If boolDebugER1IF And (intBytesToReceive > 0) Then frmDebug.subWriteLogs(ftnElapsedTimeS() & "  ftnSendER1_CommandTop:  Received = " & Hex(ftnSendER1_Command))
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnSendER1_Command :  " & ex.ToString)
        End Try
    End Function


    Sub subSendDebug(ByRef byteBuffer As Byte(), ByRef intBytesToSend As Integer)
        Try
            Dim strSent As String = " }}}}}}}}}}====     Sent To ER1 Serial IF: "
            For i = 0 To intBytesToSend - 1
                strSent &= " " & Hex(byteBuffer(i))
            Next
            frmDebug.subWriteLogs(strSent)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subSendDebug :  " & ex.ToString)
        End Try
    End Sub

    Function ftnGetResponse(ByRef intBytesToReceive As Integer, ByRef boolReceived As Boolean) As UInt32  '
        ftnGetResponse = 0
        Try
            boolReceived = False
            If comBot IsNot Nothing And Not boolShutDown Then
                If comBot.IsOpen And boolBotConnected Then
                    Dim intBytesReceived As Byte = 0
                    Dim timeOut As DateTime = Now().AddMilliseconds(500)
                    Do
                        If comBot.BytesToRead > 0 Then ' read and discard status and checksum
                            ftnGetResponse = (ftnGetResponse << 8) + comBot.ReadByte
                            intBytesReceived += 1
                        End If
                    Loop Until (Now() > timeOut) Or (intBytesReceived = 2) Or boolShutDown
                    If (Now() > timeOut) Then
                        If boolDebugER1IF Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " #####################    ftnGetResponse: ERROR:  status and checksum not received")
                        ftnGetResponse = 0
                    ElseIf (ftnGetResponse > 511) Then
                        If boolDebugER1IF Then frmDebug.subWriteLogs(ftnElapsedTimeS() & "#####################  ftnGetResponse:  COM ERROR:  status / checksum = " & Hex(ftnGetResponse))
                        ftnGetResponse = 0
                    ElseIf (intBytesToReceive > 0) Then
                        intBytesReceived = 0
                        ftnGetResponse = 0
                        Do
                            If comBot.BytesToRead > 0 Then
                                ftnGetResponse = (ftnGetResponse << 8) + comBot.ReadByte
                                intBytesReceived += 1
                            End If
                        Loop Until (Now() > timeOut) Or (intBytesToReceive = intBytesReceived) Or boolShutDown
                        If (intBytesReceived <> intBytesToReceive) Then
                            If boolDebugER1IF Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " #####################    ftnGetResponse: ERROR:  only " & intBytesReceived & " of " & intBytesToReceive & " Bytes received")
                            ftnGetResponse = 0
                        Else
                            boolReceived = True
                        End If
                    Else
                        boolReceived = True
                    End If
                    comBot.DiscardInBuffer()
                Else
                    If boolDebugER1IF Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " #####################    ftnGetResponse: ERROR:  comBot is not open or not connected")
                    boolBotConnected = False
                End If
            ElseIf boolShutDown Then
                ftnER1_Close()
            Else
                If boolDebugER1IF Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " #####################    ftnGetResponse: ERROR:  comBot is nothing")
                boolBotConnected = False
            End If
        Catch ex As Exception
            ftnGetResponse = 0
            boolReceived = False
            boolBotConnected = False
            frmDebug.subWriteLogs(" EXCEPTION in ftnGetResponse :  " & ex.ToString)
        End Try
    End Function






End Module
