# Copyright 2025 PostalPortal LLC and Netsyms Technologies LLC
#
# Redistribution and use in source and binary forms, with or
# without modification, are permitted provided that
# the following conditions are met:
#   1. Redistributions of source code must retain
#      the above copyright notice, this list of conditions
#      and the following disclaimer.
#   2. Redistributions in binary form must reproduce
#      the above copyright notice, this list of conditions
#      and the following disclaimer in the documentation
#      and/or other materials provided with the distribution.
#   3. Neither the name of the copyright holder
#       nor the names of its contributors may be used
#       to endorse or promote products derived from
#       this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
# CONTRIBUTORS “AS IS” AND ANY EXPRESS OR IMPLIED WARRANTIES,
# INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
# MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
# CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
# GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
# BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
# OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
# TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
# WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
# THE POSSIBILITY OF SUCH DAMAGE.

#
# Interface between an ultrasonic rangefinder and USB HID, for dimensioning packages.
# Requires the usb_device_hid MicroPython package/library to be installed.
#

# HID report bytes to PC:
# 1. Status: 0x01 if fault detected, 0x02 if at zero,
#    0x03 if distance unstable, 0x04 if OK, 0x05 if reading under zero,
#    0x06 if outside max sensor range, 0x07 if below minimum range,
#    0x08 if bad command from host, 0x09 if distance too large to fit in two bytes, 0x00 for other errors.
#    If status is 0x01, 0x06, 0x07, 0x08, 0x09, or 0x00, bytes 2-6 will be 0x00.
# 2. Units: 0x01 for mm, 0x02 for cm, 0x03 for in, 0x04 for feet
# 3. Sign: 0x00 if measurement positive or zero, 0x01 if negative (under zero)
# 4. First byte of distance to target
# 5. Second byte of distance to target
# 6. Fractional part of distance, .00 to .99 (0x00 to 0x63)
#
# Distance bytes allow transmitting distances from 0.00 to 65535.99 (0x0000 0x00 to 0xffff 0x63)
# Sign byte also allows distances under zero, down to -65535

# HID report from PC:
# First byte:
# 0x00: Request re-zeroing (use currently read distance as the distance to the empty measuring platform)
# 0x01: Set units to mm
# 0x02: Set units to cm
# 0x03: Set units to in
# 0x04: Set units to ft
# 0xAC: Set ambient temperature for calibration to the following byte in degrees C (ignored on US-100 sensor)

import sys
import select
import math
from machine import Pin, UART, time_pulse_us, reset, bootloader
from time import sleep, sleep_us, ticks_diff, ticks_ms
import usb.device
from micropython import const
from usb.device.hid import HIDInterface
import ujson as json
from dimensioner_utils import *
from dimensioner_screen import updateDisplay, errorDisplay, bootDisplay, clearDisplay
from config import *


class USBHIDInterface(HIDInterface):
    def __init__(self):
        super().__init__(
            bytes([
                0x06, 0x69, 0xff,       # Usage Page (Vendor Defined)
                0x09, 0x01,             # Usage (Vendor Usage 1)
                0x19, 0x01,
                0x29, 0x01,
                0xa1, 0x01,             # Collection (Application)
                0x15, 0x00,             #   Logical Minimum (0)
                0x26, 0xff, 0x00,       #   Logical Maximum (255)
                0x85, 0x02,             #   Report ID 2 to host
                0x09, 0x01,             #     Usage (Vendor Usage 1)
                0x95, 0x08,             #     Report Count (1 bit)
                0x75, 0x06,             #     Report Size (48 bits/6 bytes)
                0x81, 0x00,             #     Input (Data,Ary,Abs)
                0x85, 0x02,             #   Report ID 2 from host
                0x09, 0x01,             #     Usage (Vendor Usage 1)
                0x95, 0x08,             #     Report Count (1 bit)
                0x75, 0x02,             #     Report Size (16 bits/2 bytes)
                0x91, 0x01,             #     Output (Data,Ary,Abs)
                0xc0                    # End Collection
            ]),
            set_report_buf=bytearray(3),
            protocol=const(0x00),
            interface_str=INTERFACE,
        )

    def on_set_report(self, report_data, _report_id, _report_type):
        if _report_id != 2:
                return
        try:
            if report_data[1] == 0x00:
                # Command to set zero distance
                setZeroDistance()
                printSerial("A0: ZEROED")
            elif report_data[1] > 0x00 and report_data[0] < 0x05:
                # Set units to send measurements in
                setUnits(getUnitsFromByte(report_data[1]))
                printSerial("A2: UNIT: "+reportUnits)
            elif report_data[1] == 0xAC:
                # Set ambient temperature in degrees C
                setAmbientTemp(report_data[2])
                printSerial("A1: TEMP: "+str(ambientTempC))
            elif report_data[1] == 0x4D:
                errorDisplay(0x4D)
                printSerial("Soft resetting to MicroPython REPL, power cycle to undo. Goodbye for now!")
                machine.reset()
            elif report_data[1] == 0x50:
                errorDisplay(0x50)
                printSerial("Entering firmware update mode, power cycle to undo. Goodbye for now!")
                machine.bootloader()
            else:
                # Can't understand command
                send_data(b'\x02\x08\x00\x00\x00\x00\x00')
                printSerial("E8: Unsupported Command from Host")
                errorDisplay(8)
        except:
            send_data(b'\x02\x08\x00\x00\x00\x00\x00')
            printSerial("E8: Unsupported Command from Host")
            errorDisplay(8)

    def send_data(self, data):
        self.send_report(data)
            
usbinterface = USBHIDInterface()
if USBHID_ENABLED:
    usb.device.get().active(False)
    usb.device.get().config(usbinterface, builtin_driver=True, manufacturer_str=MANU, product_str=PROD, id_vendor=VID, id_product=PID)
    usb.device.get().active(True)

zeroButtonPin = Pin(ZERO_BUTTON_PIN, Pin.IN, Pin.PULL_UP)
unitsButtonPin = Pin(UNITS_BUTTON_PIN, Pin.IN, Pin.PULL_UP)

bootDisplay(FIRMWARE_VERSION)
printSerial(PROD)
printSerial("Version " + FIRMWARE_VERSION)
printSerial("Sensor: " + SENSOR_TYPE)

zeroDistance = 500
reportUnits = "in"
ambientTempC = 20

def saveSettings():
    global zeroDistance, reportUnits, ambientTempC
    jsondata = {"zeroDistance": zeroDistance, "reportUnits": reportUnits, "ambientTempC": ambientTempC}
    try:
        with open('settings.json', 'w') as f:
            json.dump(jsondata, f)
    except:
        printSerial("E0: Could not save settings to flash memory.")
    
def loadSettings():
    global zeroDistance, reportUnits, ambientTempC
    try:
        with open('settings.json', 'r') as f:
            data = json.load(f)
            zeroDistance = data["zeroDistance"]
            reportUnits = data["reportUnits"]
            ambientTempC = data["ambientTempC"]
            printConfig()
    except:
        printSerial("Warning: Could not load settings from flash memory. Saving current values instead.")
        saveSettings()

def printConfig():
    printSerial("I0: Zero: ", end="")
    printSerial(zeroDistance, end="mm\n")
    printSerial("I1: Units: " + reportUnits)
    printSerial("I2: Temp C: ", end="")
    printSerial(getAmbientTemp())

def getAmbientTemp():
    if SENSOR_TYPE == "US-100":
        # Ignore the set value, we have a sensor for this
        return getUS100Temp()
    else:
        return ambientTempC
    
def sendToDisplay(measurement, unstable = False):
    updateDisplay(measurement, reportUnits, getAmbientTemp(), unstable)

def setUnits(units):
    global reportUnits
    reportUnits = units.lower()
    saveSettings()

def setAmbientTemp(tempC):
    global ambientTempC
    ambientTempC = tempC
    saveSettings()

def setZeroDistance():
    global zeroDistance
    try:
        zeroDistance = getAverageDistanceMM()
        saveSettings()
    except DeltaError:
        pass
    
def getUS100Temp():
    sensorUart.write(b'\x50')
    time = ticks_ms()
    buffer = bytearray(1)
    while not sensorUart.any():
        if ticks_diff(ticks_ms(), time) > 100:
            return -1
        sleep_us(100)
    sensorUart.readinto(buffer, 1)
    return buffer[0] - 45

def getDistanceMM():
    if SENSOR_TYPE == "PING":
        pin = Pin(SENSOR_PIN, Pin.OUT)
        pin.value(0)
        sleep_us(5)
        pin.value(1)
        sleep_us(5)
        pin.value(0)
        pin = Pin(SENSOR_PIN, Pin.IN, Pin.PULL_DOWN)
        pulseDuration = time_pulse_us(pin, 1, 500000)
        if pulseDuration < 0:
            # -1 or -2 means we timed out waiting for the pulse
            return pulseDuration
        pulseDuration = pulseDuration / 2 # Convert round-trip time to one-way time
        speedOfSoundMetersPerSecond = (331.5 + (0.6 * ambientTempC))
        speedOfSoundMMPerUS = speedOfSoundMetersPerSecond / 1000.0
        return pulseDuration * speedOfSoundMMPerUS
    elif SENSOR_TYPE == "US-100":
        sensorUart.write(b'\x55')
        time = ticks_ms()
        buffer = bytearray(2)
        while not sensorUart.any():
            if ticks_diff(ticks_ms(), time) > 100:
                return -1
            sleep_us(100)
        sensorUart.readinto(buffer, 2)
        return buffer[0] * 256 + buffer[1]
        
    return 0

def getAverageDistanceMM(samples = SAMPLE_SIZE):
    i = 0
    total = 0.0
    minV = 999999
    maxV = 0
    while i < samples:
        sample = getDistanceMM()
        if sample < 0:
            return sample
        if i == 0:
            minV = sample
            maxV = sample
        if sample < minV:
            minV = sample
        if sample > maxV:
            maxV = sample
        total = total + sample
        i = i + 1
        sleep_us(2000) # PING datasheet says wait a minimum of 200us between measurements
    if abs(maxV - minV) > UNSTABLE_DELTA_MM:
        raise DeltaError
    return total / samples



loadSettings()

if SENSOR_TYPE == "US-100":
    sensorUart = UART(UART_ID, baudrate=9600, tx=Pin(SENSOR_TX), rx=Pin(SENSOR_RX))
    sensorUart.init(bits=8, parity=None, stop=2)

try:
    while True:
        if zeroButtonPin.value() == 0:
            setZeroDistance()
        if unitsButtonPin.value() == 0:
            unitNumber = getUnitsAsByte(reportUnits)
            unitNumber = unitNumber + 1
            if unitNumber > 4:
                unitNumber = 1
            setUnits(getUnitsFromByte(unitNumber))
        buffLine = readlineSerial() # Read a command from the serial connection if available
        if buffLine:
            data = buffLine.strip().upper()
            try:
                if data == "":
                   pass
                elif data == "ZERO":
                    printSerial("A0: ZEROED")
                    setZeroDistance()
                elif data.startswith("TEMP:"):
                    setAmbientTemp(data[5:])
                    printSerial("A1: TEMP: "+ambientTempC)
                elif data.startswith("UNIT:"):
                    setUnits(data[5:])
                    printSerial("A2: UNIT: "+reportUnits)
                elif data == "?":
                    printConfig()
                elif data == "REPL":
                    errorDisplay(0x4D)
                    printSerial("REPL: Resetting to MicroPython REPL, power cycle to undo. Goodbye for now!")
                    sleep_us(500)
                    machine.reset()
                    break
                elif data == "UPDATE":
                    errorDisplay(0x50)
                    printSerial("FWUPD: Entering firmware update mode, power cycle to undo. Goodbye for now!")
                    sleep_us(500)
                    machine.bootloader()
                    break
                else:
                    errorDisplay(8)
                    printSerial("E8: Unsupported Command from Host: ", data)
            except:
                errorDisplay(8)
                printSerial("E8: Unsupported Command from Host")
        try:
            dist = getAverageDistanceMM() # Get distance to object
            if dist < 0:
                # distance measurement timed out, ultrasonic ping never came back?
                usbinterface.send_data(b'\x02\x01\x00\x00\x00\x00\x00')
                errorDisplay(1, "Sensor timeout")
                printSerial("E1: Sensor Timeout")
                continue
            size = zeroDistance - dist
            sizeInUnits = convertMMToUnits(size, reportUnits)
            reportData = bytearray([])
            if dist < SENSOR_MIN_MM:
                # Under minimum accurate range
                usbinterface.send_data(b'\x02\x07\x00\x00\x00\x00\x00')
                errorDisplay(7)
                printSerial("E7: Under Minimum Range")
                continue
            if dist > SENSOR_MAX_MM:
                # Above maximum accurate range
                usbinterface.send_data(b'\x02\x06\x00\x00\x00\x00\x00')
                errorDisplay(6)
                printSerial("E6: Over Maximum Range")
                continue
            if abs(sizeInUnits) > 65535:
                # Value too large to fit in available bytes
                usbinterface.send_data(b'\x02\x09\x00\x00\x00\x00\x00')
                errorDisplay(9)
                printSerial("E9: Value Too Large, Use Different Units")
                continue
            
            negativeByte = 0x00
            statusByte = 0x04
            if size < 0:
                negativeByte = 0x01
                statusByte = 0x05
            
            if size < ZERO_RANGE_MM and size > ZERO_RANGE_MM * -1:
                # Within zero range, send zero
                usbinterface.send_data(b'\x02\x02' + bytes([getUnitsAsByte(reportUnits), negativeByte]) + b'\x00\x00\x00')
                sendToDisplay(0)
                printSerial("S2: Zero")
                continue
            
            distanceBytes = math.floor(abs(sizeInUnits)).to_bytes(2, 'big') + round(abs(sizeInUnits) % 1 * 100).to_bytes(1, 'big')
            
            if USBHID_ENABLED:
                usbinterface.send_data(b'\x02' + bytes([statusByte, getUnitsAsByte(reportUnits), negativeByte]) + distanceBytes)
            
            if statusByte == 0x04:
                printSerial("S4: ",  end="")
            elif statusByte == 0x05:
                printSerial("S5: ",  end="")
            if size < 0:
                printSerial("-", end="")
            
            printSerial(abs(round(sizeInUnits, 2)), end="")
            printSerial(reportUnits.lower(), end=" ")
            printSerial("RAW:" + str(dist) + "mm")
            sendToDisplay(round(sizeInUnits, 2))
        except DeltaError:
            usbinterface.send_data(b'\x02\x03\x00\x00\x00\x00\x00')
            sendToDisplay(999, True)
            printSerial("E3: Unstable")
except KeyboardInterrupt:
    terminateThread = True
    clearDisplay()
    sys.exit()
