'ReceiveSurface.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 system and receive Surface data 

Imports System.Threading
Imports System.Runtime.InteropServices

Module ReceiveSurface
    Public Const IP As String = "192.168.1.10"
    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

    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 GoMode
        GO_MODE_UNKONWN = -1
        GO_MODE_VIDEO = 0
        GO_MODE_RANGE = 1
        GO_MODE_PROFILE = 2
        GO_MODE_SURFACE = 3
    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 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 GoSurfaceMsg_RowAt(ByVal msg As IntPtr, ByVal index As UInt32) As IntPtr
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSurfaceMsg_Width(ByVal msg As IntPtr) As UInt32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSurfaceMsg_Length(ByVal msg As IntPtr) As UInt32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSurfaceMsg_XResolution(ByVal msg As IntPtr) As UInt32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSurfaceMsg_YResolution(ByVal msg As IntPtr) As UInt32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSurfaceMsg_ZResolution(ByVal msg As IntPtr) As UInt32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSurfaceMsg_XOffset(ByVal msg As IntPtr) As Int32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSurfaceMsg_YOffset(ByVal msg As IntPtr) As Int32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSurfaceMsg_ZOffset(ByVal msg As IntPtr) As Int32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSurfaceIntensityMsg_RowAt(ByVal msg As IntPtr, ByVal index As UInt32) As IntPtr
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSurfaceIntensityMsg_Width(ByVal msg As IntPtr) As UInt32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSurfaceIntensityMsg_Length(ByVal msg As IntPtr) As UInt32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSurfaceIntensityMsg_XResolution(ByVal msg As IntPtr) As UInt32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSurfaceIntensityMsg_YResolution(ByVal msg As IntPtr) As UInt32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSurfaceIntensityMsg_XOffset(ByVal msg As IntPtr) As Int32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSurfaceIntensityMsg_YOffset(ByVal msg As IntPtr) As Int32
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoDataMsg_Type(ByVal msg As IntPtr) As GoDataMessageTypes
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSensor_Setup(ByVal sensor As IntPtr) As IntPtr
    End Function
    <DllImport(Constants.GODLLPATH)> _
    Public Function GoSetup_SetScanMode(ByVal setup As IntPtr, ByVal mode As GoMode) As Int32
    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 setup As IntPtr = IntPtr.Zero
        Dim dataset As IntPtr = IntPtr.Zero
        Dim dataObj As IntPtr = IntPtr.Zero
        Dim stampMsg As IntPtr = IntPtr.Zero
        Dim stampPtr As IntPtr = IntPtr.Zero
        Dim surfaceMsg As IntPtr = IntPtr.Zero
        Dim surfaceIntMsg As IntPtr = IntPtr.Zero
        Dim stamp As New GoStamp
        Dim addr As New address
        Dim addrPtr As IntPtr = IntPtr.Zero

        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

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

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

        setup = GoSensor_Setup(sensor)

        status = GoSetup_SetScanMode(setup, GoMode.GO_MODE_SURFACE)
        If status <> 1 Then
            Console.WriteLine("GoSetup_SetScanMode 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

        Console.WriteLine("Waiting for Whole Part Data...")

        While GoSystem_ReceiveData(system, dataset, 2000000) <> 1
        End While
        If GoDataSet_Count(dataset) = 0 Then
            Console.WriteLine("Data received, but with zero items. No Ethernet output enabled!")
        End If
        ' each result can have multiple data items
        ' loop through all items in result message
        If GoDataSet_Count(dataset) <> 0 Then
            For i = 0 To (GoDataSet_Count(dataset) - 1)
                dataObj = GoDataSet_At(dataset, i)
                ' retrieve GoStamp message
                If GoDataMsg_Type(dataObj) = GoDataMessageTypes.GO_DATA_MESSAGE_TYPE_STAMP Then
                    stampMsg = dataObj
                    For j = 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 surface data
                If GoDataMsg_Type(dataObj) = GoDataMessageTypes.GO_DATA_MESSAGE_TYPE_SURFACE Then
                    surfaceMsg = dataObj
                    Dim width As UInteger = GoSurfaceMsg_Width(surfaceMsg)
                    Dim height As UInteger = GoSurfaceMsg_Length(surfaceMsg)
                    Dim itemSize = height * width '16 bit values, size in byte
                    Dim rawData(itemSize) As Short
                    Dim surfacePtr As IntPtr = GoSurfaceMsg_RowAt(surfaceMsg, 0)

                    Console.WriteLine("Whole Part Height Map received:")
                    Console.WriteLine(" Buffer width: {0}", width)
                    Console.WriteLine(" Buffer height: {0}", height)
                    Marshal.Copy(surfacePtr, rawData, 0, rawData.Length)
                End If
                ' retreieve surface intensity
                If GoDataMsg_Type(dataObj) = GoDataMessageTypes.GO_DATA_MESSAGE_TYPE_SURFACE_INTENSITY Then
                    surfaceIntMsg = dataObj
                    Dim width As UInteger = GoSurfaceIntensityMsg_Width(surfaceIntMsg)
                    Dim height As UInteger = GoSurfaceIntensityMsg_Length(surfaceIntMsg)
                    Dim itemSize = height * width '8 bit values, size in byte
                    Dim rawData(itemSize) As Byte
                    Dim surfaceIntPtr As IntPtr = GoSurfaceIntensityMsg_RowAt(surfaceIntMsg, 0)
                    Console.WriteLine("Whole Part Intensity Image received:")
                    Console.WriteLine(" Buffer width: {0}", width)
                    Console.WriteLine(" Buffer height: {0}", height)
                    Marshal.Copy(surfaceIntPtr, rawData, 0, rawData.Length)
                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

        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
