# Copyright (c) 2023 Acroname Inc. - All Rights Reserved
#
# This file is part of the BrainStem development package.
# See file LICENSE or go to https://acroname.com/software/brainstem-development-kit for full license details.

import sys

import brainstem
#for easy access to error constants
from brainstem.result import Result
from brainstem import _BS_C
import time

# Create USBHub3c object and connecting to the first module found
print('\nCreating USBHub3c stem and connecting to first module found')
chub = brainstem.stem.USBHub3c()

#Locate and connect to the first object you find on USB
#Easy way: 1=USB, 2=TCPIP
result = chub.discoverAndConnect(brainstem.link.Spec.USB)
#Locate and connect to a specific module (replace with your devices Serial Number (hex))
#result = chub.discoverAndConnect(brainstem.link.Spec.USB, 0x66F4859B)

#Verify we are connected
if result == (Result.NO_ERROR):
    result = chub.system.getSerialNumber()
    print("Connected to USBHub3c with serial number: 0x%08X" % result.value)
else:
    #If we are not connected there is nothing we can do.
    print ('Could not find a module.\n')
    sys.exit(1)


# VDM Commands are a minimum of 8 bytes chunked into 4 byte blocks
# First 4 bytes are SOP
#   0 = SOP
#   1 = SOP'
#   2 = SOP''
#   3 = SOP' Debug
#   4 = SOP'' Debug
# Second 4 bytes are the VDM header according to the USB PD Standard
# Any subsequent sets of 4 bytes are the attached VDO's
# Example:
# SOP           0x00000000
# VDM Header    0xFF00A001
# NO VDO'S

# Discover Identity Command
buffer = [0x00, 0x00, 0x00, 0x00,   0x01, 0xA0, 0x00, 0xFF]
err = chub.pd[0].set_UEIBytes(_BS_C.powerdeliveryVDM, buffer)
print(f"Send Discover Identity VDM to Port 1, err {err}")

#Disconnect from device.
chub.disconnect()