' ***************************************************************************
'
' File name: MimLocatePeak1d.vb
' Location: See Matrox Example Launcher in the MIL Control Center
' 
'
' Synopsis:  This program finds the peak In Each column Of an input sequence
'            And reconstruct the height of a 3D object using it.
'
' Copyright (C) Matrox Electronic Systems Ltd., 1992-2020.
' All Rights Reserved
' ***************************************************************************
Imports System
Imports System.Text
Imports System.Threading
Imports Matrox.MatroxImagingLibrary

Namespace MImLocatePeak1d
    Friend Class Program
        ' Input sequence specifications.
        Private Const SEQUENCE_FILE As String = MIL.M_IMAGE_PATH & "HandWithLaser.avi"

        '     ^            +
        '     |        +       +
        '     |      + <-Width-> + <------------
        '     |     +             +             | Min contrast
        '     | ++++               ++++++++ <---
        '     |
        '     |
        '     ------------------------------>
        '        Peak intensity profile

        ' Peak detection parameters.
        Private Const LINE_WIDTH_AVERAGE As Integer = 20
        Private Const LINE_WIDTH_DELTA As Integer = 20
        Private Const MIN_CONTRAST As Double = 100.0
        Private Const NB_FIXED_POINT As Integer = 4

        ' M3D display parameters
        Private Const M3D_MESH_SCALING_X As Double = 1.0
        Private Const M3D_MESH_SCALING_Y As Double = 4.0
        Private Const M3D_MESH_SCALING_Z As Double = -0.13

        Shared Sub Main(ByVal args() As String)
            Dim MilApplication As MIL_ID = MIL.M_NULL      ' Application identifier.
            Dim MilSystem As MIL_ID = MIL.M_NULL           ' System identifier.
            Dim MilDisplay As MIL_ID = MIL.M_NULL          ' Display identifier.
            Dim MilDisplayImage As MIL_ID = MIL.M_NULL     ' Image buffer identifier.
            Dim MilGraList As MIL_ID = MIL.M_NULL          ' Graphic list identifier.
            Dim MilImage As MIL_ID = MIL.M_NULL            ' Image buffer identifier.
            Dim MilPosYImage As MIL_ID = MIL.M_NULL        ' Image buffer identifier.
            Dim MilValImage As MIL_ID = MIL.M_NULL         ' Image buffer identifier.
            Dim MilContext As MIL_ID = MIL.M_NULL          ' Processing context identifier.
            Dim MilLocatePeak As MIL_ID = MIL.M_NULL       ' Processing result identifier.
            Dim MilStatContext As MIL_ID = MIL.M_NULL      ' Statistics context identifier.
            Dim MilExtreme As MIL_ID = MIL.M_NULL          ' Result buffer identifier.

            Dim SizeX As MIL_INT = 0
            Dim SizeY As MIL_INT = 0
            Dim NumberOfImages As MIL_INT = 0
            Dim FrameRate As Double = 0.0
            Dim n As MIL_INT = 0
            Dim PreviousTime As Double = 0.0
            Dim StartTime As Double = 0.0
            Dim EndTime As Double = 0.0
            Dim TotalProcessTime As Double = 0.0
            Dim WaitTime As Double = 0.0
            Dim ExtremePosY As MIL_INT() = New MIL_INT() {0, 0}

            ' Allocate defaults.
            MIL.MappAllocDefault(MIL.M_DEFAULT, MilApplication, MilSystem, MilDisplay, CType(MIL.M_NULL, IntPtr), CType(MIL.M_NULL, IntPtr))

            ' Inquire characteristics of the input sequence.
            MIL.MbufDiskInquire(SEQUENCE_FILE, MIL.M_SIZE_X, SizeX)
            MIL.MbufDiskInquire(SEQUENCE_FILE, MIL.M_SIZE_Y, SizeY)
            MIL.MbufDiskInquire(SEQUENCE_FILE, MIL.M_NUMBER_OF_IMAGES, NumberOfImages)
            MIL.MbufDiskInquire(SEQUENCE_FILE, MIL.M_FRAME_RATE, FrameRate)

            ' Allocate buffers to hold images.
            MIL.MbufAlloc2d(MilSystem, SizeX, SizeY, 8 + MIL.M_UNSIGNED, MIL.M_IMAGE + MIL.M_PROC, MilImage)
            MIL.MbufAlloc2d(MilSystem, SizeX, SizeY, 8 + MIL.M_UNSIGNED, MIL.M_IMAGE + MIL.M_DISP, MilDisplayImage)
            MIL.MbufAlloc2d(MilSystem, SizeX, NumberOfImages, 16 + MIL.M_UNSIGNED, MIL.M_IMAGE + MIL.M_PROC, MilPosYImage)
            MIL.MbufAlloc2d(MilSystem, SizeX, NumberOfImages, 8 + MIL.M_UNSIGNED, MIL.M_IMAGE + MIL.M_PROC, MilValImage)

            ' Allocate context for MIL.MimLocatePeak1D.
            MIL.MimAlloc(MilSystem, MIL.M_LOCATE_PEAK_1D_CONTEXT, MIL.M_DEFAULT, MilContext)

            ' Allocate result for MIL.MimLocatePeak1D.
            MIL.MimAllocResult(MilSystem, MIL.M_DEFAULT, MIL.M_LOCATE_PEAK_1D_RESULT, MilLocatePeak)

            ' Allocate graphic list.
            MIL.MgraAllocList(MilSystem, MIL.M_DEFAULT, MilGraList)

            ' Select display.
            MIL.MdispSelect(MilDisplay, MilDisplayImage)
            MIL.MdispControl(MilDisplay, MIL.M_ASSOCIATED_GRAPHIC_LIST_ID, MilGraList)

            ' Print a message.
            Console.WriteLine()
            Console.WriteLine("EXTRACTING 3D IMAGE FROM A LASER LINE (SHEET-OF-LIGHT):")
            Console.WriteLine("--------------------------------------------------------")
            Console.WriteLine()
            Console.WriteLine("The position of a laser line is being extracted from an image")
            Console.WriteLine("to generate a depth image.")

            ' Open the sequence file for reading.
            MIL.MbufImportSequence(SEQUENCE_FILE, MIL.M_DEFAULT, MIL.M_NULL, MIL.M_NULL, CType(MIL.M_NULL, IntPtr), MIL.M_NULL, MIL.M_NULL, MIL.M_OPEN)

            ' Preprocess the context.
            MIL.MimLocatePeak1d(MilContext, MilImage, MilLocatePeak, MIL.M_NULL, MIL.M_NULL, MIL.M_NULL, MIL.M_PREPROCESS, MIL.M_DEFAULT)

            ' Read And process all images in the input sequence.
            MIL.MappTimer(MIL.M_DEFAULT, MIL.M_TIMER_READ + MIL.M_SYNCHRONOUS, PreviousTime)
            TotalProcessTime = 0.0

            For n = 0 To NumberOfImages - 1
                ' Read image from sequence.
                MIL.MbufImportSequence(SEQUENCE_FILE, MIL.M_DEFAULT, MIL.M_LOAD, MIL.M_NULL, MilImage, MIL.M_DEFAULT, 1, MIL.M_READ)

                ' Display the image.
                MIL.MbufCopy(MilImage, MilDisplayImage)

                ' Locate the peak in each column of the image.
                MIL.MappTimer(MIL.M_DEFAULT, MIL.M_TIMER_READ + MIL.M_SYNCHRONOUS, StartTime)

                MIL.MimLocatePeak1d(MilContext, MilImage, MilLocatePeak, LINE_WIDTH_AVERAGE, LINE_WIDTH_DELTA, MIN_CONTRAST, MIL.M_DEFAULT, MIL.M_DEFAULT)

                ' Draw extracted peaks.
                MIL.MgraColor(MIL.M_DEFAULT, MIL.M_COLOR_RED)
                MIL.MgraClear(MIL.M_DEFAULT, MilGraList)

                ' Draw peak's data to depth map.
                MIL.MimDraw(MIL.M_DEFAULT, MilLocatePeak, MIL.M_NULL, MilGraList, MIL.M_DRAW_PEAKS + MIL.M_CROSS, MIL.M_ALL, MIL.M_DEFAULT, MIL.M_DEFAULT)
                MIL.MimDraw(MIL.M_DEFAULT, MilLocatePeak, MIL.M_NULL, MilPosYImage, MIL.M_DRAW_DEPTH_MAP_ROW, n, MIL.M_NULL, MIL.M_FIXED_POINT + NB_FIXED_POINT)
                MIL.MimDraw(MIL.M_DEFAULT, MilLocatePeak, MIL.M_NULL, MilValImage, MIL.M_DRAW_INTENSITY_MAP_ROW, n, MIL.M_NULL, MIL.M_FIXED_POINT + NB_FIXED_POINT)
                MIL.MappTimer(MIL.M_DEFAULT, MIL.M_TIMER_READ + MIL.M_SYNCHRONOUS, EndTime)
                TotalProcessTime += EndTime - StartTime

                ' Wait to have a proper frame rate.
                WaitTime = 1.0 / FrameRate - (EndTime - PreviousTime)
                If WaitTime > 0 Then
                    MIL.MappTimer(MIL.M_DEFAULT, MIL.M_TIMER_WAIT, WaitTime)
                End If
                MIL.MappTimer(MIL.M_DEFAULT, MIL.M_TIMER_READ + MIL.M_SYNCHRONOUS, PreviousTime)
            Next

            MIL.MgraClear(MIL.M_DEFAULT, MilGraList)

            ' Close the sequence file.
            MIL.MbufImportSequence(SEQUENCE_FILE, MIL.M_DEFAULT, MIL.M_NULL, MIL.M_NULL, CType(MIL.M_NULL, IntPtr), MIL.M_NULL, MIL.M_NULL, MIL.M_CLOSE)

            ' Pause to show the result.
            Console.WriteLine("{0} images processed in {1,7:f2} s ({2,7:f2} ms/image).", CInt(NumberOfImages), TotalProcessTime, TotalProcessTime / CDbl(NumberOfImages) * 1000.0)
            Console.WriteLine("Press <Enter> to continue.")
            Console.WriteLine()
            Console.ReadKey()

            Console.WriteLine("The reconstructed images are being displayed.")

            ' Draw extracted peak position in each column of each image.
            Dim VisualizationDelayMsec As Integer
            VisualizationDelayMsec = 10

            For n = 0 To NumberOfImages - 1
                MIL.MbufClear(MilImage, 0)
                MIL.MimDraw(MIL.M_DEFAULT, MilPosYImage, MilValImage, MilImage, MIL.M_DRAW_PEAKS + MIL.M_1D_COLUMNS + MIL.M_LINES, n, 1, MIL.M_FIXED_POINT + NB_FIXED_POINT)

                ' Display the result image.
                MIL.MbufCopy(MilImage, MilDisplayImage)

                Thread.Sleep(VisualizationDelayMsec)
            Next

            ' Pause to show the result.
            Console.WriteLine("Press <Enter> to continue.")
            Console.WriteLine()
            Console.ReadKey()

            ' Try to allocate 3D display
            Dim MilDisplay3D As MIL_ID = Alloc3dDisplayId(MilSystem)
            If MilDisplay3D <> MIL.M_NULL Then
                MIL.McalUniform(MilPosYImage, 0.0, 0.0, M3D_MESH_SCALING_X, M3D_MESH_SCALING_Y, 0.0, MIL.M_DEFAULT)
                MIL.McalControl(MilPosYImage, MIL.M_GRAY_LEVEL_SIZE_Z, M3D_MESH_SCALING_Z)
                Dim ContainerId As MIL_ID = MIL.MbufAllocContainer(MilSystem, MIL.M_PROC Or MIL.M_DISP, MIL.M_DEFAULT, CType(MIL.M_NULL, IntPtr))
                MIL.MbufConvert3d(MilPosYImage, ContainerId, MIL.M_NULL, MIL.M_DEFAULT, MIL.M_DEFAULT)
                Dim Reflectance As MIL_ID = MIL.MbufAllocComponent(ContainerId, 1, SizeX, NumberOfImages, 8 + MIL.M_UNSIGNED, MIL.M_IMAGE, MIL.M_COMPONENT_REFLECTANCE, CType(MIL.M_NULL, IntPtr))
                MIL.MbufCopy(MilValImage, Reflectance)
                Console.WriteLine("The depth buffer is displayed using 3D MIL.")
                Console.WriteLine("Press <R> on the display window to stop/start the rotation.")
                Console.WriteLine()

                ' Hide Mil Display.
                MIL.MdispControl(MilDisplay, MIL.M_WINDOW_SHOW, MIL.M_DISABLE)
                MIL.M3ddispSelect(MilDisplay3D, ContainerId, MIL.M_SELECT, MIL.M_DEFAULT)
                AutoRotate3dDisplay(MilDisplay3D)

                ' Pause to show the result.
                Console.WriteLine("Press <Enter> to end.")
                Console.ReadKey()
                MIL.M3ddispFree(MilDisplay3D)
                MIL.MbufFree(ContainerId)
            Else
                Console.WriteLine("The depth buffer is displayed using 2D MIL.")

                ' Find the remapping for result buffers.
                MIL.MimAlloc(MilSystem, MIL.M_STATISTICS_CONTEXT, MIL.M_DEFAULT, MilStatContext)
                MIL.MimAllocResult(MilSystem, MIL.M_DEFAULT, MIL.M_STATISTICS_RESULT, MilExtreme)

                MIL.MimControl(MilStatContext, MIL.M_STAT_MIN, MIL.M_ENABLE)
                MIL.MimControl(MilStatContext, MIL.M_STAT_MAX, MIL.M_ENABLE)
                MIL.MimControl(MilStatContext, MIL.M_CONDITION, MIL.M_NOT_EQUAL)
                MIL.MimControl(MilStatContext, MIL.M_COND_LOW, &HFFFF)

                MIL.MimStatCalculate(MilStatContext, MilPosYImage, MilExtreme, MIL.M_DEFAULT)
                MIL.MimGetResult(MilExtreme, MIL.M_STAT_MIN + MIL.M_TYPE_MIL_INT, ExtremePosY(0))
                MIL.MimGetResult(MilExtreme, MIL.M_STAT_MAX + MIL.M_TYPE_MIL_INT, ExtremePosY(1))

                MIL.MimFree(MilExtreme)
                MIL.MimFree(MilStatContext)

                ' Free the display And reallocate a New one of the proper dimension for results.
                MIL.MbufFree(MilDisplayImage)
                MIL.MbufAlloc2d(MilSystem, CType(CDbl(SizeX) * If(M3D_MESH_SCALING_X > 0, M3D_MESH_SCALING_X, -M3D_MESH_SCALING_X), MIL_INT),
                            CType(CDbl(NumberOfImages) * M3D_MESH_SCALING_Y, MIL_INT), 8 + MIL.M_UNSIGNED, MIL.M_IMAGE + MIL.M_PROC + MIL.M_DISP, MilDisplayImage)

                MIL.MdispSelect(MilDisplay, MilDisplayImage)

                ' Display the height buffer.
                MIL.MimClip(MilPosYImage, MilPosYImage, MIL.M_GREATER, ExtremePosY(1), MIL.M_NULL, ExtremePosY(1), MIL.M_NULL)
                MIL.MimArith(MilPosYImage, ExtremePosY(0), MilPosYImage, MIL.M_SUB_CONST)
                MIL.MimArith(MilPosYImage, (ExtremePosY(1) - ExtremePosY(0)) / 255 + 1, MilPosYImage, MIL.M_DIV_CONST)
                MIL.MimResize(MilPosYImage, MilDisplayImage, MIL.M_FILL_DESTINATION, MIL.M_FILL_DESTINATION, MIL.M_BILINEAR)

                ' Pause to show the result.
                Console.WriteLine("Press <Enter> to end.")
                Console.ReadKey()
            End If

            ' Free all allocations.
            MIL.MimFree(MilLocatePeak)
            MIL.MimFree(MilContext)
            MIL.MbufFree(MilImage)
            MIL.MgraFree(MilGraList)
            MIL.MbufFree(MilDisplayImage)
            MIL.MbufFree(MilPosYImage)
            MIL.MbufFree(MilValImage)
            MIL.MappFreeDefault(MilApplication, MilSystem, MilDisplay, MIL.M_NULL, MIL.M_NULL)
        End Sub

      ' ****************************************************************************
      ' Allocates a 3D display and returns its MIL identifier. 
      ' ****************************************************************************
      Private Shared Function Alloc3dDisplayId(ByVal MilSystem As MIL_ID) As MIL_ID
            MIL.MappControl(MIL.M_DEFAULT, MIL.M_ERROR, MIL.M_PRINT_DISABLE)
            Dim MilDisplay3D As MIL_ID = MIL.M3ddispAlloc(MilSystem, MIL.M_DEFAULT, "M_DEFAULT", MIL.M_DEFAULT, CType(MIL.M_NULL, IntPtr))
            MIL.MappControl(MIL.M_DEFAULT, MIL.M_ERROR, MIL.M_PRINT_ENABLE)

            If MilDisplay3D = MIL.M_NULL Then
                Console.WriteLine()
                Console.WriteLine("The current system does not support the 3D display.")
                Console.WriteLine()
            End If

            Return MilDisplay3D
        End Function

      ' ***************************************************************************
      ' Auto rotate the 3D object
      ' ***************************************************************************
      Private Shared Sub AutoRotate3dDisplay(ByVal MilDisplay As MIL_ID)
            MIL.M3ddispControl(MilDisplay, MIL.M_AUTO_ROTATE, MIL.M_ENABLE)
        End Sub
    End Class
End Namespace