' VL53L5CX Driver for MMBasic (PicoMite) v0.7.3
' --------------------------------------
' Copyright (c) 2025-26 Thomas Euler
' MIT Licence
'
' Many functions are a direct port of the underlying API functions from the
' `vl53l5cx_api.cpp` library (`SparkFun_VL53L5CX_Arduino_Library`) written by
' SparkFun Electronics, November 2024
'
' v0.7.0 (2026-01-01)
' - LInput command for reading binary files accelerates loading firmware
' v0.7.1
' - Using structures (Types) for internal variables
' - Main program to test subroutine towards library
' v0.7.2
' - Before loading firmware, check if sensor has already initialized to
'   shorten startup time
' - BytesToInt16 improved, now faster
' - ToF functions tested, including calibration
' v0.7.3
' - ToF functions removed; turned into a library
'
' Unsolved issues:
' ---------------
' - 8x8 data is not correctly scaled (approx. 4x larger); this is currently
'   "heuristically" corrected
' - 4x4 mode does not generate data
'
' Requirements:
' ------------
' File structure on SD card:
'   /vl53l5cx/firmware.bin  (86016 bytes)
'   /vl53l5cx/config.bin    (972 bytes)
'   /vl53l5cx/xtalk.bin     (776 bytes)
'   /vl53l5cx/nvm_cmd.bin   (40 bytes)
'
' I/O pins:
' - GP12 (SDA) and GP13 (SCL) for I2C2
'
' List of available target status:
'   0 - Ranging data are not updated
'   1 - Signal rate too low on SPAD array
'   2 - Target phase
'   3 - Sigma estimator too high
'   4 - Target consistency failed
'   5 - Range valid
'   6 - Wrap around not performed (typically the first range)
'   7 - Rate consistency failed
'   8 - Signal rate too low for the current target
'   9 - Range valid with large pulse (may be due to a merged target)
'  10 - Range valid, but no target detected at previous range
'  11 - Measurement consistency failed
'  12 - Target blurred by another one, due to sharpener
'  13 - Target detected but inconsistent data. Frequently happens for
'       secondary targets.
' 255 - No target detected (only if number of targets detected is enabled)
'
' ----------------------------------------------------------------------------
Option Base 0
Option explicit
Option Default Integer
Option Escape

Print "VL53L5CX sensor driver in library"
Print "| Hardware requirements: I2C (GP12, GP13)"

' ----------------------------------------------------------------------------
' Constants
Const VL53_I2C_SDA          = MM.Info(PinNo GP12)
Const VL53_I2C_SCL          = MM.Info(PinNo GP13)
Const VL53_I2C_ADDR         = &H29
Const VL53_BUF_SIZE         = 16384    ' Buffer size for reading
Const VL53_I2C_CHUNK        = 192      ' I2C write reliable chunk size
Const VL53_I2C_FREQ         = 1000
Const VL53_DEV_ID           = &HF0
Const VL53_REV_ID           = &H02
Const VL53_RES_4x4          = 16
Const VL53_RES_8x8          = 64
Const VL53_RMODE_CONTINOUS  = 1
Const VL53_RMODE_AUTONOMOUS = 3
Const VL53_VERBOSE          = 0

' Register map
Const VL53_REG_SOFT_RESET   = &H0000
Const VL53_REG_PAGE_SEL     = &H7FFF
Const VL53_REG_CMD_STAT     = &H2C00
Const VL53_REG_BOOT_STAT    = &H0006
Const VL53_REG_SYSTEM_STAT  = &H00E5
Const VL53_KUI_CMD_START    = &H2C04
Const VL53_KUI_CMD_END      = &H2FFF
Const VL53_KDCI_FREQ_HZ     = &H5458
Const VL53_KDCI_ZONE_CONFIG = &H5450
Const VL53_KDCI_DSS_CONFIG  = &HAD38
Const VL53_KDCI_RANGE_MODE  = &HAD30

' Firmware section and buffer sizes
Const VL53_FW_PART1_SIZE    = &H8000   ' 32KB
Const VL53_FW_PART2_SIZE    = &H8000   ' 32KB
Const VL53_FW_PART3_SIZE    = &H5000   ' 20KB
Const VL53_KNMV_DATA_SIZE   = 492
Const VL53_KOFFS_DATA_SIZE  = 488
Const VL53_KTEMP_BUF_SIZE   = 1440
Const VL53_KXTALK_DATA_SIZE = 776
Const VL53_KCONFIG_SIZE     = 972

' Internal structure
Type TVL53
 stream_count As integer
 dread_size As integer
 kBHTypeShift As integer
 kBHTypeMask As integer
 kBHSizeShift As integer
 kBHSizeMask As integer
 kBHIdxShift As integer
 kBHIdxMask As integer
 frame_count As Integer
End Type

' Ranging data pixel
Type TVL53Pixel
 dist_mm As integer  ' distance in [mm]
 t_state As integer  ' pixel state (see list above)
 n_trgts As integer  ' # of targets detected
End Type

' ----------------------------------------------------------------------------
Sub VL53.test _res%, _freq%
 ' Testing the sensor
 Dim Integer res = 0, iFr
 If _res% = 0 Then _res% = VL53_RES_8x8
 If _freq% = 0 Then _freq% = 5

 VL53.init
 Pause 500

 tof_params.dxy = Int(Sqr(_res%))
 tof_params.freq_Hz = _freq%

 res = VL53.set_freq_Hz(_freq%)
 Print "Ranging frequency [Hz]=";VL53.get_freq_Hz()
 res = VL53.set_resolution(_res%)
 Print "Resolution=";VL53.get_resolution()
 res = VL53.set_ranging_mode(VL53_RMODE_CONTINOUS)
 res = VL53.get_ranging_mode()
 Print "Ranging mode=";Choice(res = 1, "continuous", "autonomous")

 res = VL53.start_ranging()
 Print "Start ranging ->";res
 Pause 200
 For iFr=0 To 199
   VL53.update_data
   Struct Print vl53_data()
   Pause 500
 Next
 res = VL53.stop_ranging()
 Print "Stop ranging ->";res
 VL53.close
 Print "Done."
End Sub

' =============================================================================
' Initialization/finalization of sensor
' -----------------------------------------------------------------------------
Sub VL53.init
 ' Initialize sensor and load firmware
 Local Integer res, n, m, tmp, status = 0, i, p_tmp, p2
 Local string path$
 Local Float t_ms = Timer

 ' Some global data buffers and variables
 Dim Integer vl53_offs_data(VL53_KOFFS_DATA_SIZE -1)
 Dim Integer vl53_xtalk_data(VL53_KXTALK_DATA_SIZE -1)
 Dim Integer vl53_tmp_buf(VL53_KTEMP_BUF_SIZE -1)
 p_tmp = Peek(VarAddr vl53_tmp_buf())
 Dim _vl53 As TVL53
 _vl53.kBHTypeShift = 0
 _vl53.kBHTypeMask  = &H000F << _vl53.kBHTypeShift
 _vl53.kBHSizeShift = 4
 _vl53.kBHSizeMask  = &H0FFF << _vl53.kBHSizeShift
 _vl53.kBHIdxShift  = 16
 _vl53.kBHIdxMask   = &HFFFF << _vl53.kBHIdxShift
 Dim vl53_data(VL53_RES_8x8 -1) As TVL53Pixel

 ' Setup I2C
 Print "Opening VL53L5CX ..."
 SetPin VL53_I2C_SDA, VL53_I2C_SCL, I2C
 I2C OPEN VL53_I2C_FREQ, 1000

 ' Check sensor presence
 I2C Write VL53_I2C_ADDR, 0, 1, 0
 If MM.I2C = 0 And VL53.isAlife(1) Then
   Print "| Sensor detected."
 Else
   Print "| Error: Sensor not found at &H"; Hex$(VL53_I2C_ADDR)
   I2C CLOSE
   Exit Sub
 EndIf
 Pause 500

 ' Check if sensor already initialized, if so, exit
 res = VL53.get_resolution()
 If res = 16 Or res = 64 Then
   Print "| Sensor already initialized"
   Exit Sub
 EndIf

 ' Initializing sensor
 Print "| Initializing sensor ..."

 ' Sw reboot sequence
 _WriteI2CReg16 VL53_REG_PAGE_SEL, &H00
 _WriteI2CReg16 &H0009, &H04
 _WriteI2CReg16 &H000F, &H40
 _WriteI2CReg16 &H000A, &H03
 tmp = _ReadI2CReg16(&H7FFF)
 _WriteI2CReg16 &H000C, &H01
 _WriteI2CReg16 &H0101, &H00
 _WriteI2CReg16 &H0102, &H00
 _WriteI2CReg16 &H010A, &H01
 _WriteI2CReg16 &H4002, &H01
 _WriteI2CReg16 &H4002, &H00
 _WriteI2CReg16 &H010A, &H03
 _WriteI2CReg16 &H0103, &H01
 _WriteI2CReg16 &H000C, &H00
 _WriteI2CReg16 &H000F, &H43
 Pause 1
 _WriteI2CReg16 &H000F, &H40
 _WriteI2CReg16 &H000A, &H01
 Pause 100

 ' Wait for sensor booted (several ms required to get sensor ready)
 ' (self.poll_for_answer(1, 0,  &H06, 0xff, 1))
 _WriteI2CReg16 VL53_REG_PAGE_SEL, &H00
 status = status Or VL53._waitForStatus(1, 0, &H06, &HFF, 1)
 _WriteI2CReg16 &H000E, &H01
 _WriteI2CReg16 VL53_REG_PAGE_SEL, &H02

 ' Enable Fw access
 ' (self.poll_for_answer(1, 0, &H21, &H10, &H10))
 _WriteI2CReg16 &H03, &H0D
 _WriteI2CReg16 VL53_REG_PAGE_SEL, &H01
 status = status Or VL53._waitForStatus(1, 0, &H21, &H10, &H10)
 _WriteI2CReg16 VL53_REG_PAGE_SEL, &H00

 ' Enable host access to GO1
 _WriteI2CReg16 &H000C, &H01

 ' Power ON status
 _WriteI2CReg16 VL53_REG_PAGE_SEL, &H00
 _WriteI2CReg16 &H0101, &H00
 _WriteI2CReg16 &H0102, &H00
 _WriteI2CReg16 &H010A, &H01
 _WriteI2CReg16 &H4002, &H01
 _WriteI2CReg16 &H4002, &H00
 _WriteI2CReg16 &H010A, &H03
 _WriteI2CReg16 &H0103, &H01
 _WriteI2CReg16 &H400F, &H00
 _WriteI2CReg16 &H021A, &H43
 _WriteI2CReg16 &H021A, &H03
 _WriteI2CReg16 &H021A, &H01
 _WriteI2CReg16 &H021A, &H00
 _WriteI2CReg16 &H0219, &H00
 _WriteI2CReg16 &H021B, &H00

 ' Wake up MCU
 _WriteI2CReg16 VL53_REG_PAGE_SEL, &H00
 _WriteI2CReg16 &H000C, &H00
 _WriteI2CReg16 VL53_REG_PAGE_SEL, &H01
 _WriteI2CReg16 &H0020, &H07
 _WriteI2CReg16 &H0020, &H06

 ' Load firmware in three sections
 Print "| Loading firmware ..."
 path$ = "/vl53l5cx/firmware.bin"
 _WriteI2CReg16 VL53_REG_PAGE_SEL, &H09
 n = 0
 m = VL53_FW_PART1_SIZE
 res = VL53._loadFirmwareSect(Path$, 0, n, m, 1)
 If Not(res) Then GoTo VL53.LoadError

 _WriteI2CReg16 VL53_REG_PAGE_SEL, &H0A
 n = VL53_FW_PART1_SIZE
 m = VL53_FW_PART2_SIZE
 res = VL53._LoadFirmwareSect(Path$, 0, n, m)
 If Not(res) Then GoTo VL53.LoadError

 _WriteI2CReg16 VL53_REG_PAGE_SEL, &H0B
 n = VL53_FW_PART1_SIZE +VL53_FW_PART2_SIZE
 m = VL53_FW_PART3_SIZE
 res = VL53._LoadFirmwareSect(Path$, 0, n, m, 2)
 If Not(res) Then GoTo VL53.LoadError

 ' Check if FW correctly downloaded
 ' (self.poll_for_answer(1, 0, &H21, &H10, &H10))
 _WriteI2CReg16 VL53_REG_PAGE_SEL, &H02
 _WriteI2CReg16 &H0003, &H0D
 _WriteI2CReg16 VL53_REG_PAGE_SEL, &H01
 status = status Or VL53._waitForStatus(1, 0, &H21, &H10, &H10)
 _WriteI2CReg16 VL53_REG_PAGE_SEL, &H00
 _WriteI2CReg16 &H000C, &H01

 ' Reset MCU And wait boot
 ' (self.poll_for_answer(1, 0, &H06, &Hff, &H00))
 _WriteI2CReg16 VL53_REG_PAGE_SEL, &H00
 _WriteI2CReg16 &H0114, &H00
 _WriteI2CReg16 &H0115, &H00
 _WriteI2CReg16 &H0116, &H42
 _WriteI2CReg16 &H0117, &H00
 _WriteI2CReg16 &H000B, &H00
 _WriteI2CReg16 &H000C, &H00
 _WriteI2CReg16 &H000B, &H01
 status = status Or VL53._waitForStatus(1, 0, &H06, &HFF, 0)
 _WriteI2CReg16 VL53_REG_PAGE_SEL, &H02

 ' Get offset NVM data and store them into the offset buffer
 ' (self.poll_for_answer(4, 0, self.kUiCmdStatus, 0xff, 0x02))
 path$ = "/vl53l5cx/nvm_cmd.bin"
 res = VL53._loadSmallFile(Path$, &H2FD8, 40)
 If Not(res) Then GoTo VL53.LoadError
 res = VL53._waitForStatus(4, 0, VL53_REG_CMD_STAT, &HFF, &H02)
 If res Then
   Print "| Error: NVM command timeout"
   GoTo VL53.LoadError
 EndIf
 _ReadI2CReg16Mult VL53_KUI_CMD_START, vl53_tmp_buf(), VL53_KNMV_DATA_SIZE
 p2 = Peek(VarAddr vl53_offs_data())
 Memory Copy Integer p_tmp, p2, VL53_KOFFS_DATA_SIZE
 res = _sendOffsetData(VL53_RES_4X4)

 ' Set default Xtalk shape, send Xtalk to sensor
 path$ = "/vl53l5cx/xtalk.bin"
 res = VL53._loadSmallFile(Path$, 0, VL53_KXTALK_DATA_SIZE, 1)
 If Not(res) Then
   Print "| Error: Timeout when loading calibration data"
   GoTo VL53.LoadError
 EndIf
 p2 = Peek(VarAddr vl53_xtalk_data())
 Memory Copy Integer p_tmp, p2, VL53_KXTALK_DATA_SIZE
 res = _sendXTalkData(VL53_RES_4x4)
 If res Then GoTo VL53.LoadError

 ' Send default configuration to VL53L5CX firmware
 ' (self.poll_for_answer(4, 1, self.kUiCmdStatus, 0xff, 0x03))
 path$ = "/vl53l5cx/conf.bin"
 res = VL53._loadSmallFile(Path$, &H2C34, VL53_KCONFIG_SIZE)
 If Not(res) Then GoTo VL53.LoadError
 res = VL53._waitForStatus(4, 1, VL53_REG_CMD_STAT, &HFF, &H03)
 If res Then
   Print "| Error: Timeout when loading configuration"
   GoTo VL53.LoadError
 EndIf

 ' Set to kNbTargetPerZone=1 and single_range=0x01
 Local Integer buf(3) = (1, 0, &H01, 0), srange = &H01
 res = _DCIWriteData(buf(), &HCF78, 4)
 Memory Unpack Peek(VarAddr srange), buf(), 4, 8
 res = _DCIWriteData(buf(), &HCD5C, 4)

 Print "| Done in ";Str$((Timer -t_ms)/1000,0,1);" sec."
 Exit Sub

VL53.LoadError:
 Print "| Error: Firmware load failed"
 I2C CLOSE
End Sub


Sub VL53.close
 I2C Close
 Print "VL53L5CX closed."
End Sub

' -----------------------------------------------------------------------------
' Method to check if sensor is alife
' -----------------------------------------------------------------------------
Function VL53.isAlife(log%)
 ' Returns True if device is alife
 Local integer devID, revID
 _WriteI2CReg16 VL53_REG_PAGE_SEL, &H00
 devID = _ReadI2CReg16(0)
 revID = _ReadI2CReg16(1)
 _WriteI2CReg16 VL53_REG_PAGE_SEL, &H02
 If log% Then
   Print "| Device ID=";Hex$(devID,4);" revision ID=";Hex$(revID,4)
 EndIf
 VL53.isAlife = (devID = VL53_DEV_ID) And (revID = VL53_REV_ID)
End Function

' -----------------------------------------------------------------------------
' Methods for setting/getting sensor parameters (frequency, resolution, mode)
' -----------------------------------------------------------------------------
Function VL53.get_freq_Hz()
 ' Returns the current ranging frequency in Hz
 Local integer stat
 stat = _DCIReadData(vl53_tmp_buf(), VL53_KDCI_FREQ_HZ, 4)
 VL53.get_freq_Hz = vl53_tmp_buf(1)
End Function


Function VL53.set_freq_Hz(freq%)
 ' Sets current ranging frequency in Hz, returns True if successful
 Local integer stat, tmp(1) = (freq%, 0)
 stat = _DCIReplaceData(vl53_tmp_buf(), VL53_KDCI_FREQ_HZ, 4, tmp(), 1, 1)
 VL53.set_freq_Hz = (stat = 0)
End Function

' - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
Function VL53.get_resolution()
 ' Returns the current resolution with 16 for 4x4 and 64 for 8x8
 Local integer stat
 stat = _DCIReadData(vl53_tmp_buf(), VL53_KDCI_ZONE_CONFIG, 8)
 VL53.get_resolution = vl53_tmp_buf(0) *vl53_tmp_buf(1)
End Function


Function VL53.set_resolution(resol%)
 ' Sets resolution, returns 0 if successful
 Local integer stat = 0
 If resol% = VL53_RES_4x4 Then
   stat = _DCIReadData(vl53_tmp_buf(), VL53_KDCI_DSS_CONFIG, 16)
   vl53_tmp_buf(&H04) = 64
   vl53_tmp_buf(&H06) = 64
   vl53_tmp_buf(&H09) = 4
   stat = stat Or _DCIWriteData(vl53_tmp_buf(), VL53_KDCI_DSS_CONFIG, 16)
   stat = stat Or _DCIReadData(vl53_tmp_buf(), VL53_KDCI_ZONE_CONFIG, 8)
   vl53_tmp_buf(&H00) = 4
   vl53_tmp_buf(&H01) = 4
   vl53_tmp_buf(&H04) = 8
   vl53_tmp_buf(&H05) = 8
   stat = stat Or _DCIWriteData(vl53_tmp_buf(), VL53_KDCI_ZONE_CONFIG, 8)
 Else If resol% = VL53_RES_8x8 Then
   stat = _DCIReadData(vl53_tmp_buf(), VL53_KDCI_DSS_CONFIG, 16)
   vl53_tmp_buf(&H04) = 16
   vl53_tmp_buf(&H06) = 16
   vl53_tmp_buf(&H09) = 1
   stat = stat Or _DCIWriteData(vl53_tmp_buf(), VL53_KDCI_DSS_CONFIG, 16)
   stat = stat Or _DCIReadData(vl53_tmp_buf(), VL53_KDCI_ZONE_CONFIG, 8)
   vl53_tmp_buf(&H00) = 8
   vl53_tmp_buf(&H01) = 8
   vl53_tmp_buf(&H04) = 4
   vl53_tmp_buf(&H05) = 4
   stat = stat Or _DCIWriteData(vl53_tmp_buf(), VL53_KDCI_ZONE_CONFIG, 8)
 Else
   Print "Error: Invalid resolution"
 EndIf
 stat = stat Or _sendOffsetData(resol%)
 stat = stat Or _sendXTalkData(resol%)
 VL53.set_resolution = stat
End Function

' - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
Function VL53.get_ranging_mode()
 ' Returns the current ranging mode
 Local Integer stat
 stat = _DCIReadData(vl53_tmp_buf(), VL53_KDCI_RANGE_MODE, 8)
 If vl53_tmp_buf(1) = &H01 Then
   VL53.get_ranging_mode = VL53_RMODE_CONTINOUS
 Else
   VL53.get_ranging_mode = VL53_RMODE_AUTONOMOUS
 EndIf
End Function


Function VL53.set_ranging_mode(_mode%)
 ' Set the ranging mode. Two modes are `continuous` and `autonomous
 Local Integer stat, sr(3) = (0, 0, 0, 0)
 stat = _DCIReadData(vl53_tmp_buf(), VL53_KDCI_RANGE_MODE, 8)
 If _mode% = VL53_RMODE_CONTINOUS Then
   vl53_tmp_buf(1) = &H01
   vl53_tmp_buf(3) = &H03
 Else If _mode% = VL53_RMODE_AUTONOMOUS Then
   vl53_tmp_buf(1) = &H03
   vl53_tmp_buf(3) = &H02
   sr(0) = &H01
 Else
   stat = 1
 EndIf
 If stat = 0 Then
   stat = stat Or _DCIWriteData(vl53_tmp_buf(), VL53_KDCI_RANGE_MODE, 8)
   stat = stat Or _DCIWriteData(sr(), &HCD5C, 4)
 EndIf
 VL53.set_ranging_mode = stat
End Function

' -----------------------------------------------------------------------------
' Methods for starting/stoping ranging and checking for/retrieving new data
' -----------------------------------------------------------------------------
Function VL53.start_ranging()
 ' Starts ranging ...
 Local Integer stat = 0, op_size = 12, TrgtPerZone = 1
 Local Integer bh_ptr_type, bh_ptr_size, bh_ptr_idx, resol, i, j
 Local Integer hconf(1) = (0, 0), buf(4* op_size -1)
 Local Integer cmd(3) = (0, &H03, 0, 0), output(op_size -1)
 Local Integer output_bh_enable(3) = (&H07, 0, 0, &HC0000000)

 resol = VL53.get_resolution()
 _vl53.stream_count = 255
 _vl53.dread_size = 0
 _vl53.frame_count = 0
 output(0)  = &H0000000D
 output(1)  = &H54B400C0
 output(2)  = &H54C00040
 output(3)  = &H54D00104
 output(4)  = &H55D00404
 output(5)  = &HCF7C0401
 output(6)  = &HCFBC0404
 output(7)  = &HD2BC0402
 output(8)  = &HD33C0402
 output(9)  = &HD43C0401
 output(10) = &HD47C0401
 output(11) = &HCC5008C0

 ' Enable selected outputs
 /*
 Inc output_bh_enable(0), 8    ' VL53L5CX_ENABLE_AMBIENT_PER_SPAD
 Inc output_bh_enable(0), 16   ' VL53L5CX_ENABLE_NB_SPADS_ENABLED
 Inc output_bh_enable(0), 64   ' VL53L5CX_ENABLE_SIGNAL_PER_SPAD
 Inc output_bh_enable(0), 128  ' VL53L5CX_ENABLE_RANGE_SIGMA_MM
 Inc output_bh_enable(0), 512  ' VL53L5CX_ENABLE_REFLECTANCE_PERCENT
 Inc output_bh_enable(0), 2048 ' VL53L5CX_ENABLE_MOTION_INDICATOR
 */
 Inc output_bh_enable(0), 32   ' VL53L5CX_ENABLE_NB_TARGET_DETECTED
 Inc output_bh_enable(0), 256  ' VL53L5CX_ENABLE_DISTANCE_MM
 Inc output_bh_enable(0), 1024 ' VL53L5CX_ENABLE_TARGET_STATUS

 ' Send output addresses
 For i=0 To 11
   j = (output_bh_enable(i \32) And (1 << (i Mod 32))) = 0
   If (output(i) = 0) Or j Then Continue For

   bh_ptr_type = (output(i) And _vl53.kBHTypeMask) >> _vl53.kBHTypeShift
   bh_ptr_size = (output(i) And _vl53.kBHSizeMask) >> _vl53.kBHSizeShift
   bh_ptr_idx  = (output(i) And _vl53.kBHIdxMask)  >> _vl53.kBHIdxShift

   If (bh_ptr_type >= &H01) And (bh_ptr_type < &H0d) Then
     If (bh_ptr_idx >= &H54D0) And (bh_ptr_idx < &H54D0 +960) Then
       bh_ptr_size = resol
       output(i) = output(i) And (INV _vl53.kBHSizeMask)
       output(i) = output(i) Or (resol << _vl53.kBHSizeShift)
     Else
       bh_ptr_size = resol *TrgtPerZone
       output(i) = output(i) And (INV _vl53.kBHSizeMask)
       output(i) = output(i) Or ((resol *TrgtPerZone) << _vl53.kBHSizeShift)
     EndIf
     Inc _vl53.dread_size, bh_ptr_type *bh_ptr_size
   Else
     Inc _vl53.dread_size, bh_ptr_size
   EndIf
   Inc _vl53.dread_size, 4
 Next
 Inc _vl53.dread_size, 20

 _uint32ArrayToBytes output(), buf(), op_size
 stat = stat Or _DCIWriteData(buf(), &HCD78, op_size *4)

 hconf(0) = _vl53.dread_size
 hconf(1) = i +2
 _uint32ArrayToBytes hconf(), buf(), 2
 stat = stat Or _DCIWriteData(buf(), &HCD60, 8)
 _uint32ArrayToBytes output_bh_enable(), buf(), 4
 stat = stat Or _DCIWriteData(buf(), &HCD68, 16)

 ' Start xshut bypass (interrupt mode)
 _WriteI2CReg16 &H7FFF, &H00
 _WriteI2CReg16 &H0009, &H05
 _WriteI2CReg16 &H7FFF, &H02

 ' Start ranging session
 _WriteI2CReg16Mult VL53_KUI_CMD_END -(4 -1), cmd(), 4
 stat = stat Or VL53._waitForStatus(4, 1, VL53_REG_CMD_STAT, &HFF, &H03)

 VL53.start_ranging = stat
End Function

' .............................................................................
Function VL53.check_data_ready()
 ' Checks if new data is ready, returns True if so.
 Local Integer check
 _ReadI2CReg16Mult 0, vl53_tmp_buf(), 4
 check = vl53_tmp_buf(0) <> _vl53.stream_count
 check = check And (vl53_tmp_buf(0) <> 255) And (vl53_tmp_buf(1) = 5)
 check = check And ((vl53_tmp_buf(2) And &H05) = &H05)
 If check And ((vl53_tmp_buf(3) And &H10) = &H10) Then
   _vl53.stream_count = vl53_tmp_buf(0)
   VL53.check_data_ready = 1
   Exit Function
 EndIf
 If VL53_VERBOSE Then
   Print "buf=";Hex$(vl53_tmp_buf(0),2);" ";Hex$(vl53_tmp_buf(1),2);
   Print " ";Hex$(vl53_tmp_buf(2),2);" ";Hex$(vl53_tmp_buf(3),2)
 EndIf
 VL53.check_data_ready = 0
End Function

' .............................................................................
Sub VL53.update_data()
 ' Gets the ranging data, using the selected output and resolution, and
 ' and saves it in `vl53_data().dist_mm`
'Local Float t = Timer
 Static Integer ptb = Peek(VarAddr vl53_tmp_buf()), bh, pbh = Peek(VarAddr bh)
 Static Integer tmp(VL53_RES_8x8 -1), p_t = Peek(VarAddr tmp())
 Static Integer ntg(VL53_RES_8x8 -1), p_n = Peek(VarAddr ntg())
 Local Integer bh_ptr_type, bh_ptr_size, bh_ptr_idx
 Local Integer stat = 0, i, msize, p

 ' Get the data
 _ReadI2CReg16Mult 0, vl53_tmp_buf(), _vl53.dread_size
 _vl53.stream_count = vl53_tmp_buf(0)
 _swapBuffer vl53_tmp_buf(), _vl53.dread_size
 Inc _vl53.frame_count, 1

 ' Start conversion at position 16 to avoid headers
 For i=16 To _vl53.dread_size -1 Step 4
   Memory Pack ptb +i*8, pbh, 4, 8
   bh_ptr_type = (bh And _vl53.kBHTypeMask) >> _vl53.kBHTypeShift
   bh_ptr_size = (bh And _vl53.kBHSizeMask) >> _vl53.kBHSizeShift
   bh_ptr_idx  = (bh And _vl53.kBHIdxMask)  >> _vl53.kBHIdxShift
   msize = bh_ptr_size
   If (bh_ptr_type > &H01) And (bh_ptr_type < &H0D) Then
     msize = msize *bh_ptr_type
   EndIf
   p = ptb +(i+4)*8

   ' Extract selected data
   If bh_ptr_idx = &HCF7C Then
     ' # of targets detected
     Memory Copy Integer p, p_n, msize
     Struct Insert ntg(), vl53_data().n_trgts

   Else If bh_ptr_idx = &HD33C Then
     ' Distances
     stat = _BytesToI16Arr(vl53_tmp_buf(), tmp(), i+4, msize)
     Struct Insert tmp(), vl53_data().dist_mm

   Else If bh_ptr_idx = &HD47C Then
     ' target status
     Memory Copy Integer p, p_t, msize
     Struct Insert tmp(), vl53_data().t_state
     Exit For
   EndIf
   ' Advance by payload size
   Inc i, msize
 Next

 ' Negate pixel distances for which no target was detected
 Math Scale ntg(), 2, ntg()
 Math Add ntg(), -1, ntg()
 Math C_Mult ntg(), vl53_data().dist_mm, vl53_data().dist_mm
't = Timer -t : Print "update_ranging_data t=";t
End Sub

' .............................................................................
Function VL53.stop_ranging()
 ' Stops a ranging session. It must be used when the sensor streams,
 ' after calling `VL53.start_ranging()`. Returns 0 if successful
 Local integer stat = 0, asf(3), tout = 0, tmp = 0
 _ReadI2CReg16Mult &H2FFC, asf(), 4
 If Not((asf(0)=0) And (asf(1)=0) And (asf(2)=4) And (asf(3)=&HFF)) Then
   _WriteI2CReg16 &H7FFF, &H00

   ' Provoke MCU stop
   _WriteI2CReg16 &H15, &H16
   _WriteI2CReg16 &H14, &H01

   ' Poll for G02 status 0 MCU stop
   Do While (((tmp And &H80) >> 7) = 0) And (tout < 500)
     tmp = _ReadI2CReg16(&H06)
     Pause 10
     Inc tout, 1
     If tout >= 500 Then stat = 255
   Loop
 EndIf

 ' Undo MCU stop
 _WriteI2CReg16 &H7FFF, &H00
 _WriteI2CReg16 &H14, &H00
 _WriteI2CReg16 &H15, &H00

 ' Stop xshut bypass
 _WriteI2CReg16 &H09, &H04
 _WriteI2CReg16 &H7FFF, &H02

 VL53.stop_ranging = stat
End Function

' ============================================================================
' Internal methods
' -----------------------------------------------------------------------------
Function VL53._loadFirmwareSect(fname$, tAddr, fOffs, sectSize, flag)
 ' Load a section of firmware from file
 Local Integer bytesRead, totalRead, currAddr
 Local Integer i, j, writeSize, byteVal, res, n
 Local Integer DataBuf(VL53_BUF_SIZE \8 +1), pDB = Peek(VarAddr DataBuf())
 Local Integer writeBuf(VL53_I2C_CHUNK +2 -1)

 VL53._loadFirmwareSect = 0
 currAddr = tAddr
 totalRead = 0

 ' Check if file exists
 If Not(MM.Info(EXISTS FILE fname$)) Then
   Print "| Error: File `";fname$;"` not found"
   Exit Function
 EndIf

 ' Open file for binary reading
 Open fname$ For RANDOM As #1
 If flag = 1 Then Print "| Reading `";fname$;"` ";

 ' Read and write in chunks
 Do While totalRead < sectSize
   ' Calculate chunk size to read
   bytesRead = Min(VL53_BUF_SIZE, sectSize -totalRead)

   ' Read chunk from file
   Seek #1, fOffs +totalRead +1
   n = LInput(DataBuf(), #1, bytesRead)
   If n <> bytesRead Then
     Print "| Error: Size issue with file `";fname$;"`"
     Exit Function
   EndIf
   Memory Copy Integer pDB +8, pDB, bytesRead \8

   ' Write chunk to sensor via I2C
   j = 0
   Do While j < bytesRead
     writeSize = Min(VL53_I2C_CHUNK, bytesRead -j)

     ' Prepare I2C data
     writeBuf(0) = (currAddr >> 8) And &HFF
     writeBuf(1) = currAddr And &HFF
     For i=0 To writeSize -1
       writeBuf(i +2) = Peek(Var DataBuf(), j +i)
     Next

     ' Write to I2C
     I2C WRITE VL53_I2C_ADDR, 0, writeSize +2, writeBuf()
     res = MM.I2C
     If res <> 0 Then
       Print "| I2C write error ";res;" at 0x";Hex$(currAddr)
       Close #1
       VL53._loadFirmwareSect = 0
       Exit Function
     EndIf

     Inc currAddr, writeSize
     Inc j, writeSize
   Loop
   Inc totalRead, bytesRead

   ' Show progress
   If (totalRead Mod VL53_BUF_SIZE) = 0 Then Print ".";
 Loop

 Close #1
 If flag = 2 Then Print " Done"
 VL53._LoadFirmwareSect = 1
End Function

' -----------------------------------------------------------------------------
Function VL53._loadSmallFile(fname$, tAddr, fSize, toTmpBuf)
 ' Load small configuration file and write it to the I2C register or into
 ' `vl53_tmp_buf()`
 Local Integer n1, tmp1(fSize \8 +1)
 VL53._loadSmallFile = 1

 If Not(MM.Info(EXISTS FILE fname$)) Then
   Print "| Error: File `";fname$;"` not found"
   VL53._loadSmallFile = 0
   Exit Function
 EndIf

 ' Open file for binary reading
 Open fname$ For RANDOM As #1
 Print "| Reading `";fname$;"` ...";

 ' Read entire small file to buffer
 Seek #1, 1
 n1 = LInput(tmp1(), #1, fSize)
 If n1 <> fSize Then
   Print "| Error: Size issue with file `";fname$;"`"
   VL53._loadSmallFile = 0
   Exit Function
 EndIf
 Memory Unpack Peek(VarAddr tmp1()) +8, vl53_tmp_buf(), fSize, 8
 Close #1

 ' Write directly to I2C bus, if requested
 If Not(toTmpBuf) Then
   _WriteI2CReg16Mult tAddr, vl53_tmp_buf(), fsize
 EndIf
 Print " Done"
End Function

' -----------------------------------------------------------------------------
Function VL53._waitForStatus(n%, p%, reg%, mask%, vExp%)
 ' Wait for status register to reach expected value
 Local Integer stat = 0, j, buf(Max(n% -1, 1))
 Local Integer tout_ms = 2000
 If VL53_VERBOSE Then
   Print "_waitForStatus ";n%;" ";p%;" ";reg%;" ";mask%;" ";vExp%
 EndIf
 Do
   _ReadI2CReg16Mult reg%, buf(), n%
   If (n% >= 4) Then
     If (buf(2) >= &H7F) Then stat = stat Or 1
   EndIf
   If (buf(p%) And mask%) = vExp% Then
     VL53._waitForStatus = stat
     If VL53_VERBOSE Then Print "->ok"
     Exit Function
   EndIf
   Pause 10
   Inc tout_ms, -10
 Loop Until tout_ms <= 0

 stat = stat Or buf(2)
 VL53._waitForStatus = stat
 If VL53_VERBOSE Then Print "->timeout"
End Function

' -----------------------------------------------------------------------------
' Support functions
' -----------------------------------------------------------------------------
Function _sendXTalkData(res%)
 ' Set the Xtalk data from generic configuration, or user's calibration.
 ' (`vl53_tmp_buf()` already contains the xtalk_data)
 Local Integer stat1 = 0, i, j, v, p11, p21
 Local Integer sg(63)

 ' Copy xtalk data into temp buffer
 p11 = Peek(VarAddr vl53_xtalk_data())
 P21 = Peek(VarAddr vl53_tmp_buf())
 Memory Copy Integer p11, P21, VL53_KXTALK_DATA_SIZE

 If res% = VL53_RES_4x4 Then
   ' Data extrapolation is required for 4x4 Xtalk
   vl53_tmp_buf(&H08)    = &H0F
   vl53_tmp_buf(&H08 +1) = &H04
   vl53_tmp_buf(&H08 +2) = &H04
   vl53_tmp_buf(&H08 +3) = &H17
   vl53_tmp_buf(&H08 +4) = &H08
   vl53_tmp_buf(&H08 +5) = &H10
   vl53_tmp_buf(&H08 +6) = &H10
   vl53_tmp_buf(&H08 +7) = &H07

   vl53_tmp_buf(&H20)    = &H00
   vl53_tmp_buf(&H20 +1) = &H78
   vl53_tmp_buf(&H20 +2) = &H00
   vl53_tmp_buf(&H20 +3) = &H08
   vl53_tmp_buf(&H20 +4) = &H00
   vl53_tmp_buf(&H20 +5) = &H00
   vl53_tmp_buf(&H20 +6) = &H00
   vl53_tmp_buf(&H20 +7) = &H08
   _swapBuffer vl53_tmp_buf(), VL53_KXTALK_DATA_SIZE

   For i=0 To 63
     sg(i) = vl53_tmp_buf(&H34 +i)
   Next
   For j=0 To 3
     For i=0 To 3
       v = 0
       Inc v, sg((2 *i) +(16 *j) +0)
       Inc v, sg((2 *i) +(16 *j) +1)
       Inc v, sg((2 *i) +(16 *j) +8)
       Inc v, sg((2 *i) +(16 *j) +9)
       sg(i +(4 *j)) = v \4
     Next
   Next
   ' TODO: The cpp lib was a bit odd here, it appeared to memset well
   ' past the end of the arrays. Not sure if that was intentional or
   ' necessary
   ' It's possible these were located at known memory locations in the
   ' cpp lib and we wanted to zero out the stuff directly after them
   ' as well
   Memory Set Integer Peek(VarAddr sg()) +&H10 *8, 0, 64 -&H10
   For i=0 To 63
     vl53_tmp_buf(&H34 +i) = sg(i)
   Next
   _swapBuffer vl53_tmp_buf(), VL53_KXTALK_DATA_SIZE
   vl53_tmp_buf(&H134)    = &HA0
   vl53_tmp_buf(&H134 +1) = &HFC
   vl53_tmp_buf(&H134 +2) = &H01
   vl53_tmp_buf(&H134 +3) = &H00
   Memory Set Integer Peek(VarAddr vl53_tmp_buf()) +&H078 *8, 0, 4
 EndIf

 _WriteI2CReg16Mult &H2CF8, vl53_tmp_buf(), VL53_KXTALK_DATA_SIZE
 stat1 = stat1 Or VL53._waitForStatus(4, 1, VL53_REG_CMD_STAT, &HFF, &H03)
 _sendXTalkData = stat1
End Function

' - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
Function _sendOffsetData(resol%)
  ' Set the offset data gathered from NVM
 Local Integer stat1 = 0, i1, j1, v, p11, p21
 Local Integer sg(63), rg(63)

 ' Copy offset data into temp buffer
 p11 = Peek(VarAddr vl53_offs_data())
 p21 = Peek(VarAddr vl53_tmp_buf())
 Memory Copy Integer p11, p21, VL53_KOFFS_DATA_SIZE

 If resol% = VL53_RES_4x4 Then
   vl53_tmp_buf(&H10)    = &H0F
   vl53_tmp_buf(&H10 +1) = &H04
   vl53_tmp_buf(&H10 +2) = &H04
   vl53_tmp_buf(&H10 +3) = &H00
   vl53_tmp_buf(&H10 +4) = &H08
   vl53_tmp_buf(&H10 +5) = &H10
   vl53_tmp_buf(&H10 +6) = &H10
   vl53_tmp_buf(&H10 +7) = &H07
   _swapBuffer vl53_tmp_buf(), VL53_KOFFS_DATA_SIZE

   For i1=0 To 63
     sg(i1) = vl53_tmp_buf(&H03C +i1)
     rg(i1) = vl53_tmp_buf(&H140 +i1)
   Next

   For j1=0 To 3
     For i1=0 To 3
       v = 0
       Inc v, sg((2 *i1) +(16 *j1) +0)
       Inc v, sg((2 *i1) +(16 *j1) +1)
       Inc v, sg((2 *i1) +(16 *j1) +8)
       Inc v, sg((2 *i1) +(16 *j1) +9)
       sg(i1 +(4 *j1)) = v \4
       v = 0
       Inc v, rg((2 *i1) +(16 *j1) +0)
       Inc v, rg((2 *i1) +(16 *j1) +1)
       Inc v, rg((2 *i1) +(16 *j1) +8)
       Inc v, rg((2 *i1) +(16 *j1) +9)
       rg(i1 +(4 *j1)) = v \4
     Next
   Next
   ' TODO: The cpp lib was a bit odd here, it appeared to memset well
   ' past the end of the arrays. Not sure if that was intentional or
   ' necessary
   ' It's possible these were located at known memory locations in the
   ' cpp lib and we wanted to zero out the stuff directly after them
   ' as well
   Memory Set Integer Peek(VarAddr rg()) +&H10 *8, 0, 64 -&H10
   Memory Set Integer Peek(VarAddr sg()) +&H10 *8, 0, 64 -&H10
   For i1=0 To 63
     vl53_tmp_buf(&H03C +i1) = sg(i1)
     vl53_tmp_buf(&H140 +i1) = rg(i1)
   Next
   _swapBuffer vl53_tmp_buf(), VL53_KOFFS_DATA_SIZE
 EndIf

 For i1=0 To VL53_KOFFS_DATA_SIZE -4 -1
   vl53_tmp_buf(i1) = vl53_tmp_buf(i1 +8)
 Next
 vl53_tmp_buf(&H1E0)    = 0
 vl53_tmp_buf(&H1E0 +1) = 0
 vl53_tmp_buf(&H1E0 +2) = 0
 vl53_tmp_buf(&H1E0 +3) = &H0F
 vl53_tmp_buf(&H1E0 +4) = &H03
 vl53_tmp_buf(&H1E0 +5) = &H01
 vl53_tmp_buf(&H1E0 +6) = &H01
 vl53_tmp_buf(&H1E0 +7) = &HE4
 _WriteI2CReg16Mult &H2E18, vl53_tmp_buf(), VL53_KOFFS_DATA_SIZE
 stat1 = stat1 Or VL53._waitForStatus(4, 1, VL53_REG_CMD_STAT, &HFF, &H03)
 _sendOffsetData = stat1
End Function

' - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
Function _DCIReadData(_data%(), index%, n%)
 ' Read 'extra data' from DCI, returns 0 if o.k.
 Local integer stat2 = 0, rd_size = n% +12, i
 Local integer cmd(11) = (0, 0, 0, 0, 0, 0, 0, &H0F, 0, &H02, 0, &H08)
 cmd(0) = index% >> 8
 cmd(1) = index% And &HFF
 cmd(2) = (n% And &HFF0) >> 4
 cmd(3) = (n% And &HF) << 4

 ' Request data reading from FW
 _WriteI2CReg16Mult VL53_KUI_CMD_END -11, cmd(), 12
 stat2 = stat2 Or VL53._waitForStatus(4, 1, VL53_REG_CMD_STAT, &HFF, &H03)

 ' Read new data sent (4 bytes header + data_size + 8 bytes footer)
 _ReadI2CReg16Mult VL53_KUI_CMD_START, vl53_tmp_buf(), rd_size
 _swapBuffer vl53_tmp_buf(), rd_size
 For i=0 To n% -1 : _data%(i) = vl53_tmp_buf(i +4) : Next
 _DCIReadData = stat2
End Function


Function _DCIWriteData(_data%(), index%, n%)
 ' Write 'extra data' to DCI, returns 0 if o.k.
 Local integer stat2 = 0, addr, i2
 Local integer header(3), footer(7) = (0, 0, 0, &H0F, &H05, &H01, 0, 0)
 header(0) = index% >> 8
 header(1) = index% And &HFF
 header(2) = (n% And &HFF0) >> 4
 header(3) = (n% And &HF) << 4
 footer(6) = (n% +8) >> 8
 footer(7) = (n% +8) And &HFF
 addr = VL53_KUI_CMD_END -(n% +12) +1

 ' Copy data from array to FW format (+4 bytes to add header) and
 ' add headers and footer
 _swapBuffer _data%(), n%
 For i2=n% -1 To 0 Step -1 : vl53_tmp_buf(i2 +4) = _data%(i2) : Next
 For i2=0 To 3 : vl53_tmp_buf(i2) = header(i2) : Next
 For i2=0 To 7 : vl53_tmp_buf(i2 +4 +n%) = footer(i2) : Next

 ' Send data to FW
 _WriteI2CReg16Mult addr, vl53_tmp_buf(), n% +12
 stat2 = stat2 Or VL53._waitForStatus(4, 1, VL53_REG_CMD_STAT, &HFF, &H03)
 _swapBuffer _data%(), n%
 _DCIWriteData = stat2
End Function


Function _DCIReplaceData(_data%(), index%, n%, _data2%(), n2%, p%)
 ' Replace 'extra data' in DCI, returns 0 if successful
 Local integer stat1 = 0, i
 stat1 = stat1 Or _DCIReadData(_data%(), index%, n%)
 For i=p% To p% +n2% -1 : _data%(i) = _data2%(i -p%) : Next
 stat1 = stat1 Or _DCIWriteData(_data%(), index%, n%)
 _DCIReplaceData = stat1
End Function

' - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
Sub _WriteI2CReg16 reg%, _val%
 ' Write a single byte to a 16-bit register
 Local Integer _data(2)
 _data(0) = (reg% >> 8) And &HFF
 _data(1) = reg% And &HFF
 _data(2) = _val% And &HFF
 I2C WRITE VL53_I2C_ADDR, 0, 3, _data()
End Sub


Sub _WriteI2CReg16Mult reg%, vals%(), n%
 ' Write multiple bytes to a 16-bit register
 Local Integer chMaxSize = 128, _data(chMaxSize +1)
 Local Integer i3, j3, nCh = n% \chMaxSize +1
 Local Integer pCh, chLen, currReg, m
 If VL53_VERBOSE Then Print "_WriteI2CReg16Mult"
 For i3=0 To nCh -1
   ' Send data in chunks
   currReg = reg% +i3 *chMaxSize
   _data(0) = (currReg >> 8) And &HFF
   _data(1) = currReg And &HFF
   pCh = i3 *chMaxSize
   chLen = Min(n% -chMaxSize *i3, chMaxSize)
   For j3=0 To chLen -1
     ' Copy chunk to output array
     _data(2 +j3) = vals%(pCh +j3) And &HFF
   Next
   m = Choice(j3 < (chLen -1), 1, 0)
   I2C WRITE VL53_I2C_ADDR, m, chLen +2, _data()
 Next
 If VL53_VERBOSE Then _printByteArrayAsHex vals%(), n%
End Sub


Function _ReadI2CReg16(reg%)
 ' Read a single byte from a 16-bit register and returns the byte
 Local Integer _data(1), res3
 _data(0) = (reg% >> 8) And &HFF
 _data(1) = reg% And &HFF
 I2C WRITE VL53_I2C_ADDR, 0, 2, _data()
 I2C READ VL53_I2C_ADDR, 0, 1, res3
 _ReadI2CReg16 = res3
End Function


Sub _ReadI2CReg16Mult reg%, vals%(), n%
 ' Read multiple bytes from a 16-bit register into `vals%()`
 I2C WRITE VL53_I2C_ADDR, 0, 2, (reg% >> 8) And &HFF, reg% And &HFF
 I2C READ VL53_I2C_ADDR, 0, n%, vals%()
End Sub


Sub _swapBuffer buf%(), size%
 ' Reverse the sequence of groups of 4 values
 Local Integer tmp3(size% -1), n3 = size% /4
 Local Integer pb = Peek(VarAddr buf%()), pt = Peek(VarAddr tmp3())
 Memory Copy Integer pb,    pt+24, n3, 4, 4
 Memory Copy Integer pb+8,  pt+16, n3, 4, 4
 Memory Copy Integer pb+16, pt+8,  n3, 4, 4
 Memory Copy Integer pb+24, pt,    n3, 4, 4
 Memory Copy Integer pt, pb, size%
End Sub


Sub _uint32ArrayToBytes _uint32%(), _bytes%(), n_uint32%
 ' Converts a list of uint32 values to a list of bytes.
 Local Integer i3
 Math Set 0, _bytes%()
 For i3=0 To n_uint32% -1
   _bytes%(i3*4)    = _uint32(i3) And &HFF
   _bytes%(i3*4 +1) = (_uint32(i3) >> 8) And &HFF
   _bytes%(i3*4 +2) = (_uint32(i3) >> 16) And &HFF
   _bytes%(i3*4 +3) = _uint32(i3) >> 24
 Next
End Sub

/*
Function _BytesToUI32Arr(_bytes%(), _uint32%(), p%, n%)
 ' Converts a list of `n% bytes into uint32 values, starting from byte `p%
 ' Returns 0 if successful
 Local Integer nb, i3, tmp3(3)
 Local Integer pb3 = Peek(VarAddr _bytes%()) +p%
 Local Integer pt3 = Peek(VarAddr tmp3())
 nb = Choice(n% = 0, Bound(_bytes%()) +1 -p%, n%)
 If (nb Mod 4) <> 0 Then
   _BytesToUI32Arr = -1
 Else If (Bound(_uint32%()) +1) < (nb \4) Then
   _BytesToUI32Arr = -1
 Else
   For i3=0 To nb-1 Step 4
     Memory Copy pb3 +p% +i3*4, pt3, 4
     Memory Pack tmp3(), Peek(VarAddr _uint32%()) +i3\4, 4, 8
   Next
 EndIf
 _BytesToUI32Arr = 0
End Function
*/

Function _BytesToI16Arr(_bytes%(), _int16%(), p%, n%)
 ' Converts a list of `n%` bytes into int16 values, starting from byte `p%`;
 ' returns 0 if successful
'Local Integer i3
 Local Integer _pb = Peek(VarAddr _bytes%()) +p% *8
 Local Integer _pi = Peek(VarAddr _int16%())
 If (n% Mod 2) <> 0 Then
   _BytesToI16Arr = -1
 Else If (Bound(_int16%()) +1) < (n% \2) Then
   _BytesToI16Arr = -1
 Else
   Memory Copy _pb, _pi, n%, 16, 8
   Memory Copy _pb +8, _pi +1, n%, 16, 8
   /*
   _printByteArrayAsHex _bytes%(), n%, p%
   _printByteArrayAsHex _int16%(), n% \2
   For i3=0 To n%-1 Step 2
     _int16%(i3 \2) = (_bytes%(p% +i3 +1) << 8) Or _bytes%(p% +i3)
    'If _int16%(i3 \2) > 32767 Then Inc _int16%(i3 \2), -65536
   Next
   _printByteArrayAsHex _int16%(), n% \2
   */
   _BytesToI16Arr = 0
 EndIf
End Function

' - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
Sub _printByteArrayAsHex _array%(), _size%, _p%
 Local Integer i2
 For i2=_p% To _size% +_p% -1
   If (i2 Mod 16) = 0 Then Print Str$(i2, 3);": ";
   Print Hex$(_array%(i2), 2);" ";
   If ((i2 +1) Mod 16) = 0 Or i2 = (_size% -1) Then Print
 Next
End Sub


Sub _printMat m%(), title$, dy%, scl!
 ' Print `m%() as matrix, convert to matrix, if 1D, and scale it by `scl`
 Local Integer nx = Bound(m%(), 1), ny = Bound(m%(), 2)
 Local Integer ix, iy, nn = 0
 If scl! = 0 Then scl! = 1

 ' Make sure that data is in matrix form and scale
 If ny = 0 Then
   nn = nx +1
   nx = nn \dy% -1
   ny = dy% -1
 Else
   nn = (nx +1) *(ny +1)
 EndIf
 Local Integer tmp2d(nx, ny), pt = Peek(VarAddr tmp2d())
 Memory Copy Integer Peek(VarAddr m%()), pt, nn
 Local Float vals(nx, ny)
 Math Scale tmp2d(), scl!, vals()

 ' Print matrix
 Print "| ";title$
 For iy=0 To ny
   Print "| ";
   For ix=0 To nx : Print Str$(vals(ix, iy), 4, 1) +" "; : Next
   Print
 Next
End Sub

' -----------------------------------------------------------------------------