'DualSensor.vb

'Gocator 2000/2300 Visual Basic Sample
'Copyright (C) 2013 by LMI Technologies Inc.

'Licensed under The MIT License.
'Redistributions of files must retain the above copyright notice.

' Purpose: Connect to Gocator buddy system and receive range data in Profile Mode and translate to engineering units (mm). Gocator must be in Profile Mmode.
' Ethernet output for the profile data must be enabled. Gocator buddy is removed at the end.

Imports System.Threading
Imports System.Runtime.InteropServices

Module DualSensor
    Public Const IP As String = "192.168.1.10"
    Public Const BUDDY_IP As String = "192.168.1.11"
    Public Class Constants
#If DEBUG Then
        Public Const GODLLPATH As String = "GoSdk.dll"
        Public Const KAPIDLLPATH As String = "kApi.dll"
#Else
        Public Const GODLLPATH As String = "GoSdk.dll"
        Public Const KAPIDLLPATH As String = "kApi.dll"
#End If
    End Class
    Public Structure address
        Public version As Char
        <MarshalAs(UnmanagedType.ByValArray, SizeConst:=16)> _
        Public IPaddress() As Byte
    End Structure

    Public Structure GoStamp
        Public frameIndex As UInt64
        Public timestamp As UInt64
        Public encoder As Int64
        Public encoderAtZ As Int64
        Public reserved As UInt64
    End Structure

    Public Structure GoPoints
        Public x As Int16
        Public y As Int16
    End Structure

    Public Structure ProfilePoints
        Public x As Double
        Public z As Double
        Public intensity As Byte
    End Structure

    Public Class DataContext
        Public xResolution As Double
        Public zResolution As Double
        Public xOffset As Double
        Public zOffset As Double
        Public serialNumber As UInteger
    End Class

    Enum kStatus
        kERROR_STATE = -1000                                                ' Invalid state.
        kERROR_NOT_FOUND = -999                                             ' Item is not found.
        kERROR_COMMAND = -998                                               ' Command not recognized.
        kERROR_PARAMETER = -997                                             ' Parameter is invalid.
        kERROR_UNIMPLEMENTED = -996                                         ' Feature not implemented.
        kERROR_HANDLE = -995                                                ' Handle is invalid.
        kERROR_MEMORY = -994                                                ' Out of memory.
        kERROR_TIMEOUT = -993                                               ' Action timed out.
        kERROR_INCOMPLETE = -992                                            ' Buffer not large enough for data.
        kERROR_STREAM = -991                                                ' Error in stream.
        kERROR_CLOSED = -990                                                ' Resource is no longer avaiable. 
        kERROR_VERSION = -989                                               ' Invalid version number.
        kERROR_ABORT = -988                                                 ' Operation aborted.
        kERROR_ALREADY_EXISTS = -987                                        ' Conflicts with existing item.
        kERROR_NETWORK = -986                                               ' Network setup/resource error.
        kERROR_HEAP = -985                                                  ' Heap error (leak/double-free).
        kERROR_FORMAT = -984                                                ' Data parsing/formatting error. 
        kERROR_READ_ONLY = -983                                             ' Object is read-only (cannot be written).
        kERROR_WRITE_ONLY = -982                                            ' Object is write-only (cannot be read). 
        kERROR_BUSY = -981                                                  ' Agent is busy (cannot service request).
        kERROR_CONFLICT = -980                                              ' State conflicts with another object.
        kERROR_OS = -979                                                    ' Generic error reported by underlying OS.
        kERROR_DEVICE = -978                                                ' Hardware device error.
        kERROR_FULL = -977                                                  ' Resource is already fully utilized.
        kERROR_IN_PROGRESS = -976                                           ' Operation is in progress, but not yet complete.
        kERROR = 0                                                          ' General error. 
        kOK = 1                                                             ' Operation successful. 
    End Enum

    Enum GoDataMessageTypes
        GO_DATA_MESSAGE_TYPE_UNKNOWN = -1
        GO_DATA_MESSAGE_TYPE_STAMP = 0
        GO_DATA_MESSAGE_TYPE_HEALTH = 1
        GO_DATA_MESSAGE_TYPE_VIDEO = 2
        GO_DATA_MESSAGE_TYPE_RANGE = 3
        GO_DATA_MESSAGE_TYPE_RANGE_INTENSITY = 4
        GO_DATA_MESSAGE_TYPE_PROFILE = 5
        GO_DATA_MESSAGE_TYPE_PROFILE_INTENSITY = 6
        GO_DATA_MESSAGE_TYPE_RESAMPLED_PROFILE = 7
        GO_DATA_MESSAGE_TYPE_SURFACE = 8
        GO_DATA_MESSAGE_TYPE_SURFACE_INTENSITY = 9
        GO_DATA_MESSAGE_TYPE_MEASUREMENT = 10
        GO_DATA_MESSAGE_TYPE_ALIGNMENT = 11
        GO_DATA_MESSAGE_TYPE_EXPOSURE_CAL = 12
    End Enum

    Enum GoRole
        GO_ROLE_MAIN = 0
        GO_ROLE_BUDDY = 1
    End Enum

    ' use DLL import to access GoSdkd.dll
    ' Note: to import the release version of the GoSdk and kApi, use GoSdk.dll and kApi.dll. GoSdkd.dll and kApid.dll are debug versions of the DLL
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSdk_Construct(ByRef assembly As IntPtr) As kStatus
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSystem_Construct(ByRef system As IntPtr, ByVal allocator As IntPtr) As kStatus
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSystem_FindSensorByIpAddress(ByVal system As IntPtr, ByVal addressPointer As IntPtr, ByRef sensor As IntPtr) As kStatus
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSystem_EnableData(ByVal system As IntPtr, ByVal enable As Boolean) As kStatus
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSensor_Connect(ByVal sensor As IntPtr) As kStatus
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSensor_HasBuddy(ByVal sensor As IntPtr) As Boolean
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSensor_AddBuddy(ByVal sensor As IntPtr, ByVal buddy As IntPtr) As kStatus
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSensor_RemoveBuddy(ByVal sensor As IntPtr) As kStatus
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSystem_ReceiveData(ByVal system As IntPtr, ByRef data As IntPtr, ByVal timeout As UInt64) As kStatus
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSystem_Start(ByVal system As IntPtr) As kStatus
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSystem_Stop(ByVal system As IntPtr) As kStatus
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoDataSet_Count(ByVal dataset As IntPtr) As UInt32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoDataSet_At(ByVal dataset As IntPtr, ByVal index As UInt32) As IntPtr
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoStampMsg_Count(ByVal msg As IntPtr) As UInt32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoStampMsg_At(ByVal msg As IntPtr, ByVal index As UInt32) As IntPtr
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoResampledProfileMsg_Count(ByVal msg As IntPtr) As UInt32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoResampledProfileMsg_At(ByVal msg As IntPtr, ByVal index As UInt32) As IntPtr
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoResampledProfileMsg_Width(ByVal msg As IntPtr) As UInt32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoResampledProfileMsg_XResolution(ByVal msg As IntPtr) As UInt32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoResampledProfileMsg_ZResolution(ByVal msg As IntPtr) As UInt32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoResampledProfileMsg_XOffset(ByVal msg As IntPtr) As Int32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoResampledProfileMsg_ZOffset(ByVal msg As IntPtr) As Int32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoProfileMsg_At(ByVal msg As IntPtr, ByVal index As UInt32) As IntPtr
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoProfileMsg_Width(ByVal msg As IntPtr) As UInt32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoProfileMsg_XResolution(ByVal msg As IntPtr) As UInt32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoProfileMsg_ZResolution(ByVal msg As IntPtr) As UInt32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoProfileMsg_XOffset(ByVal msg As IntPtr) As Int32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoProfileMsg_ZOffset(ByVal msg As IntPtr) As Int32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoProfileIntensityMsg_Count(ByVal msg As IntPtr) As UInt32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoProfileIntensityMsg_At(ByVal msg As IntPtr, ByVal index As UInt32) As IntPtr
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoDataMsg_Type(ByVal msg As IntPtr) As GoDataMessageTypes
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSetup_XSpacingCount(ByVal setup As IntPtr, ByVal role As GoRole) As UInt32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoDestroy(ByVal obj As IntPtr) As Int32
    End Function
    <DllImport(Constants.KAPIDLLPATH)> _
    Public Function kIpAddress_Parse(ByVal addressPointer As IntPtr, ByVal text As String) As kStatus
    End Function

    Function Main() As Integer
        Dim status As kStatus
        Dim api As IntPtr = IntPtr.Zero
        Dim system As IntPtr = IntPtr.Zero
        Dim sensor As IntPtr = IntPtr.Zero
        Dim buddy As IntPtr = IntPtr.Zero
        Dim dataset As IntPtr = IntPtr.Zero
        Dim dataObj As IntPtr = IntPtr.Zero
        Dim stampMsg As IntPtr = IntPtr.Zero
        Dim profileMsg As IntPtr = IntPtr.Zero
        Dim stampPtr As IntPtr = IntPtr.Zero
        Dim stamp As New GoStamp
        Dim addr As New address
        Dim addr_buddy As New address
        Dim addrPtr As IntPtr = IntPtr.Zero
        Dim addrPtr_buddy As IntPtr = IntPtr.Zero
        Dim intensityPtr As IntPtr = IntPtr.Zero

        Dim context As New DataContext

        status = GoSdk_Construct(api)
        If status <> 1 Then
            Console.WriteLine("GoSdk_Construct Error:{0}", status)
            Return status
        End If

        status = GoSystem_Construct(system, IntPtr.Zero)
        If status <> 1 Then
            Console.WriteLine("GoSystem_Construct Error:{0}", status)
            Return status
        End If

        addrPtr = Marshal.AllocHGlobal(Marshal.SizeOf(addr))
        Marshal.StructureToPtr(addr, addrPtr, False)

        status = kIpAddress_Parse(addrPtr, IP)
        If status <> 1 Then
            Console.WriteLine("kIpAddress_Parse Error:{0}", status)
            Return status
        End If

        status = GoSystem_FindSensorByIpAddress(system, addrPtr, sensor)
        If status <> 1 Then
            Console.WriteLine("GoSystem_FindSensorByIpAddress Error:{0}", status)
            Return status
        End If

        'NOTE: GoSystem_Connect() can be used on Gocator buddy systems already configured, the GoSystem() will be able to
        'recognize the buddy sensor as a paired buddy and not attempt to obtain a separate connection from the budfdy sensor. 
        'In this example, we are trying to pair two sensors as buddies, hence the need to use GoSensor_Connect() to retrieve sensor
        'status = GoSensor_Connect(sensor)
        If status <> 1 Then
            Console.WriteLine("GoSensor_Connect Error:{0}", status)
            Return status
        End If

        If Not GoSensor_HasBuddy(sensor) Then
            addrPtr_buddy = Marshal.AllocHGlobal(Marshal.SizeOf(addr_buddy))
            Marshal.StructureToPtr(addr_buddy, addrPtr_buddy, False)

            status = kIpAddress_Parse(addrPtr_buddy, BUDDY_IP)
            If status <> 1 Then
                Console.WriteLine("kIpAddress_Parse Error:{0}", status)
                Return status
            End If

            status = GoSystem_FindSensorByIpAddress(system, addrPtr_buddy, buddy)
            If status <> 1 Then
                Console.WriteLine("GoSystem_FindSensorByIpAddress Error:{0}", status)
                Return status
            End If

            status = GoSensor_Connect(buddy)
            If status <> 1 Then
                Console.WriteLine("GoSensor_Connect Error:{0}", status)
                Return status
            End If

            status = GoSensor_AddBuddy(sensor, buddy)
            If status <> 1 Then
                Console.WriteLine("GoSensor_AddBuddy Error:{0}", status)
                Return status
            End If

            Console.WriteLine("Buddy sensor assigned.")
        End If

        status = GoSystem_EnableData(system, True)
        If status <> 1 Then
            Console.WriteLine("GoSystem_EnableData Error:{0}", status)
            Return status
        End If

        status = GoSystem_Start(system)
        If status <> 1 Then
            Console.WriteLine("GoSystem_Start Error:{0}", status)
            Return status
        End If

        ' read data from sensor
        status = GoSystem_ReceiveData(system, dataset, 2000000)
        If status = 1 Then
            Dim itemCount = GoDataSet_Count(dataset)
            ' each result can have multiple data items
            ' loop through all items in result message
            For i As Integer = 0 To itemCount - 1
                dataObj = GoDataSet_At(dataset, i)
                ' retrieve GoStamp message
                If GoDataMsg_Type(dataObj) = GoDataMessageTypes.GO_DATA_MESSAGE_TYPE_STAMP Then
                    stampMsg = dataObj
                    For j As Integer = 0 To GoStampMsg_Count(stampMsg) - 1
                        stampPtr = GoStampMsg_At(stampMsg, j)
                        stamp = Marshal.PtrToStructure(stampPtr, GetType(GoStamp))
                        Console.WriteLine("Frame Index = {0}", stamp.frameIndex)
                        Console.WriteLine("Time Stamp = {0}", stamp.timestamp)
                        Console.WriteLine("Encoder Value = {0}", stamp.encoder)
                    Next
                End If

                ' retrieve resampeld profile data
                If GoDataMsg_Type(dataObj) = GoDataMessageTypes.GO_DATA_MESSAGE_TYPE_RESAMPLED_PROFILE Then
                    profileMsg = dataObj
                    Console.WriteLine("  Resampled Profile Message batch count: {0}", GoResampledProfileMsg_Count(profileMsg))
                    For k As Integer = 0 To GoResampledProfileMsg_Count(profileMsg) - 1
                        Dim validPointCount As Integer = 0
                        Dim profilePointCount = GoResampledProfileMsg_Width(profileMsg)
                        Console.WriteLine("    Item[{0}]: Profile data ({1} points)", k, profilePointCount)

                        context.xResolution = GoResampledProfileMsg_XResolution(profileMsg) / 1000000
                        context.zResolution = GoResampledProfileMsg_ZResolution(profileMsg) / 1000000
                        context.xOffset = GoResampledProfileMsg_XOffset(profileMsg) / 1000
                        context.zOffset = GoResampledProfileMsg_ZOffset(profileMsg) / 1000
                        Dim points(profilePointCount) As Short
                        Dim profileBuffer(profilePointCount) As ProfilePoints
                        Dim pointsPtr As IntPtr = GoResampledProfileMsg_At(profileMsg, k)
                        Marshal.Copy(pointsPtr, points, 0, points.Length)

                        For arrayIndex As Integer = 0 To profilePointCount - 1
                            If (points(arrayIndex) <> -32768) Then
                                profileBuffer(arrayIndex).x = context.xOffset + context.xResolution * arrayIndex
                                profileBuffer(arrayIndex).z = context.xOffset + context.zResolution * points(arrayIndex)
                                validPointCount += 1
                            Else
                                profileBuffer(arrayIndex).x = context.xOffset + context.xResolution * arrayIndex
                                profileBuffer(arrayIndex).z = -32768
                            End If
                        Next
                        Console.WriteLine("Received {0} Range Points", profilePointCount)
                        Console.WriteLine("Valid Points {0}", validPointCount)

                        Dim itemSize = profilePointCount * 2 ' 16 bit values, size in byte
                        Dim rawData(itemSize) As Byte
                        Dim rangePtr As IntPtr = GoResampledProfileMsg_At(profileMsg, k)
                        Marshal.Copy(rangePtr, rawData, 0, itemSize * Len(rawData(0)))

                    Next
                End If

                ' retrieve un-resampeld profile data
                If GoDataMsg_Type(dataObj) = GoDataMessageTypes.GO_DATA_MESSAGE_TYPE_PROFILE Then
                    profileMsg = dataObj
                    Console.WriteLine("  Profile Message batch count: {0}", GoResampledProfileMsg_Count(profileMsg))
                    For k As Integer = 0 To GoResampledProfileMsg_Count(profileMsg) - 1
                        Dim validPointCount As Integer = 0
                        Dim profilePointCount = GoResampledProfileMsg_Width(profileMsg)
                        Console.WriteLine("    Item[{0}]: Profile data ({1} points)", k, profilePointCount)

                        context.xResolution = GoProfileMsg_XResolution(profileMsg) / 1000000
                        context.zResolution = GoProfileMsg_ZResolution(profileMsg) / 1000000
                        context.xOffset = GoProfileMsg_XOffset(profileMsg) / 1000
                        context.zOffset = GoProfileMsg_ZOffset(profileMsg) / 1000
                        Dim points(profilePointCount) As GoPoints
                        Dim profileBuffer(profilePointCount) As ProfilePoints
                        Dim structSize As Integer = Marshal.SizeOf(points(profilePointCount))
                        Dim pointsPtr As IntPtr = GoResampledProfileMsg_At(profileMsg, k)
                        For array As UInt32 = 0 To profilePointCount - 1
                            Dim incPtr As IntPtr = New IntPtr(pointsPtr.ToInt64() + array * structSize)
                            points(array) = Marshal.PtrToStructure(incPtr, GetType(GoPoints))
                        Next

                        For arrayIndex As Integer = 0 To profilePointCount - 1
                            If (points(arrayIndex).x <> -32768) Then
                                profileBuffer(arrayIndex).x = context.xOffset + context.xResolution * points(arrayIndex).x
                                profileBuffer(arrayIndex).z = context.xOffset + context.zResolution * points(arrayIndex).y
                                validPointCount += 1
                            Else
                                profileBuffer(arrayIndex).x = -32768
                                profileBuffer(arrayIndex).z = -32768
                            End If
                        Next
                        Console.WriteLine("Received {0} Range Points", profilePointCount)
                        Console.WriteLine("Valid Points {0}", validPointCount)
                    Next
                End If

                ' retrieve profile intensity data
                If GoDataMsg_Type(dataObj) = GoDataMessageTypes.GO_DATA_MESSAGE_TYPE_PROFILE_INTENSITY Then
                    profileMsg = dataObj
                    Console.WriteLine("  Profile Intensity Message batch count: {0}", GoProfileIntensityMsg_Count(profileMsg))
                    For k As Integer = 0 To GoProfileIntensityMsg_Count(profileMsg) - 1
                        Dim intensity(GoProfileIntensityMsg_Count(profileMsg)) As Byte
                        intensityPtr = GoProfileIntensityMsg_At(profileMsg, k)
                        Marshal.Copy(intensityPtr, intensity, 0, intensity.Length)
                    Next
                End If

            Next

            GoDestroy(dataset)
            Console.WriteLine(Environment.NewLine)
        End If
        status = GoSystem_Stop(system)
        If status <> 1 Then
            Console.WriteLine("GoSystem_Stop Error:{0}", status)
            Return status
        End If

        If GoSensor_HasBuddy(sensor) Then
            status = GoSensor_RemoveBuddy(sensor)
            If status <> 1 Then
                Console.WriteLine("GoSensor_RemoveBuddy Error:{0}", status)
                Return status
            End If

            Console.WriteLine("Buddy sensor removed.")
        End If
        status = GoDestroy(system)
        If status <> 1 Then
            Console.WriteLine("GoDestroy Error:{0}", status)
            Return status
        End If

        status = GoDestroy(api)
        If status <> 1 Then
            Console.WriteLine("GoDestroy Error:{0}", status)
            Return status
        End If

        'wait for ESC key
        Console.WriteLine("Press ESCAPE to stop")
        Do While Not Console.ReadKey(True).Key = ConsoleKey.Escape
            Thread.Sleep(100)
        Loop
        Return status
    End Function

End Module
