# License: Apache 2.0. See LICENSE file in root directory.
# Copyright(c) 2021 Intel Corporation. All Rights Reserved.

import sys, os, re, platform
try:
    from rspy import log
except ModuleNotFoundError:
    if __name__ != '__main__':
        raise
    #
    def usage():
        ourname = os.path.basename( sys.argv[0] )
        print( 'Syntax: devices [actions|flags]' )
        print( '        Control the LibRS devices connected' )
        print( 'Actions (only one)' )
        print( '        --list         Enumerate devices (default action)' )
        print( '        --recycle      Recycle all' )
        print( 'Flags:' )
        print( '        --all          Enable all port [requires acroname]' )
        print( '        --port <#>     Enable only this port [requires acroname]' )
        print( '        --ports        Show physical port for each device (rather than the RS string)' )
        sys.exit(2)
    #
    # We need to tell Python where to look for rspy
    rspy_dir = os.path.dirname( os.path.abspath( __file__ ))
    py_dir   = os.path.dirname( rspy_dir )
    sys.path.append( py_dir )
    from rspy import log
#
# And where to look for pyrealsense2
from rspy import repo
pyrs_dir = repo.find_pyrs_dir()
sys.path.insert( 1, pyrs_dir )


# We need both pyrealsense2 and acroname. We can work without acroname, but
# without rs no devices at all will be returned.
try:
    import pyrealsense2 as rs
    log.d( rs )
    #
    try:
        from rspy import acroname
    except ModuleNotFoundError:
        # Error should have already been printed
        # We assume there's no brainstem library, meaning no acroname either
        log.d( 'sys.path=', sys.path )
        acroname = None
    #
    sys.path = sys.path[:-1]  # remove what we added
except ModuleNotFoundError:
    log.w( 'No pyrealsense2 library is available! Running as if no cameras available...' )
    import sys
    log.d( 'sys.path=', sys.path )
    rs = None
    acroname = None

import time

_device_by_sn = dict()
_context = None
_acroname_hubs = set()


class Device:
    def __init__( self, sn, dev ):
        self._sn = sn
        self._dev = dev
        self._name = None
        if dev.supports( rs.camera_info.name ):
            self._name = dev.get_info( rs.camera_info.name )
        self._product_line = None
        if dev.supports( rs.camera_info.product_line ):
            self._product_line = dev.get_info( rs.camera_info.product_line )
        self._physical_port = dev.supports( rs.camera_info.physical_port ) and dev.get_info( rs.camera_info.physical_port ) or None
        self._usb_location = None
        try:
            self._usb_location = _get_usb_location( self._physical_port )
        except Exception as e:
            log.e( 'Failed to get usb location:', e )
        self._port = None
        if acroname:
            try:
                self._port = _get_port_by_loc( self._usb_location )
            except Exception as e:
                log.e( 'Failed to get device port:', e )
                log.d( '    physical port is', self._physical_port )
                log.d( '    USB location is', self._usb_location )
        self._removed = False

    @property
    def serial_number( self ):
        return self._sn

    @property
    def name( self ):
        return self._name

    @property
    def product_line( self ):
        return self._product_line

    @property
    def physical_port( self ):
        return self._physical_port

    @property
    def usb_location( self ):
        return self._usb_location

    @property
    def port( self ):
        return self._port

    @property
    def handle( self ):
        return self._dev

    @property
    def enabled( self ):
        return self._removed is False


def wait_until_all_ports_disabled( timeout = 5 ):
    """
    Waits for all ports to be disabled
    """
    for retry in range( timeout ):
        if len( enabled() ) == 0:
            return True
        time.sleep( 1 )
    log.w( 'Timed out waiting for 0 devices' )
    return False


def map_unknown_ports():
    """
    Fill in unknown ports in devices by enabling one port at a time, finding out which device
    is there.
    """
    if not acroname:
        return
    global _device_by_sn
    devices_with_unknown_ports = [device for device in _device_by_sn.values() if device.port is None]
    if not devices_with_unknown_ports:
        return
    #
    ports = acroname.ports()
    known_ports = [device.port for device in _device_by_sn.values() if device.port is not None]
    unknown_ports = [port for port in ports if port not in known_ports]
    try:
        log.d( 'mapping unknown ports', unknown_ports, '...' )
        log.debug_indent()
        #log.d( "active ports:", ports )
        #log.d( "- known ports:", known_ports )
        #log.d( "= unknown ports:", unknown_ports )
        #
        for known_port in known_ports:
            if known_port not in ports:
                log.e( "A device was found on port", known_port, "but the port is not reported as used by Acroname!" )
        #
        if len( unknown_ports ) == 1:
            device = devices_with_unknown_ports[0]
            log.d( '... port', unknown_ports[0], 'has', device.handle )
            device._port = unknown_ports[0]
            return
        #
        acroname.disable_ports( ports )
        wait_until_all_ports_disabled()
        #
        # Enable one port at a time to try and find what device is connected to it
        n_identified_ports = 0
        for port in unknown_ports:
            #
            log.d( 'enabling port', port )
            acroname.enable_ports( [port], disable_other_ports=True )
            sn = None
            for retry in range( 5 ):
                if len( enabled() ) == 1:
                    sn = list( enabled() )[0]
                    break
                time.sleep( 1 )
            if not sn:
                log.d( 'could not recognize device in port', port )
            else:
                device = _device_by_sn.get( sn )
                if device:
                    log.d( '... port', port, 'has', device.handle )
                    device._port = port
                    n_identified_ports += 1
                    if len( devices_with_unknown_ports ) == n_identified_ports:
                        #log.d( 'no more devices; stopping' )
                        break
                else:
                    log.w( "Device with serial number", sn, "was found in port", port,
                            "but was not in context" )
            acroname.disable_ports( [port] )
            wait_until_all_ports_disabled()
    finally:
        log.debug_unindent()


def query( monitor_changes = True ):
    """
    Start a new LRS context, and collect all devices
    :param monitor_changes: If True, devices will update dynamically as they are removed/added
    """
    global rs
    if not rs:
        return
    #
    # Before we can start a context and query devices, we need to enable all the ports
    # on the acroname, if any:
    if acroname:
        if not acroname.hub:
            acroname.connect()  # MAY THROW!
            acroname.enable_ports( sleep_on_change = 5 )  # make sure all connected!
            if platform.system() == 'Linux':
                global _acroname_hubs
                _acroname_hubs = set( acroname.find_all_hubs() )
    #
    # Get all devices, and store by serial-number
    global _device_by_sn, _context, _port_to_sn
    _context = rs.context()
    _device_by_sn = dict()
    try:
        log.d( 'discovering devices ...' )
        log.debug_indent()
        for retry in range(3):
            try:
                devices = _context.query_devices()
                break
            except RuntimeError as e:
                log.d( 'FAILED to query devices:', e )
                if retry > 1:
                    log.e( 'FAILED to query devices', retry + 1, 'times!' )
                    raise
                else:
                    time.sleep( 1 )
        for dev in devices:
            # The FW update ID is always available, it seems, and is the ASIC serial number
            # whereas the Serial Number is the OPTIC serial number and is only available in
            # non-recovery devices. So we use the former...
            sn = dev.get_info( rs.camera_info.firmware_update_id )
            device = Device( sn, dev )
            _device_by_sn[sn] = device
            log.d( '... port {}:'.format( device.port is None and '?' or device.port ), sn, dev )
    finally:
        log.debug_unindent()
    #
    if monitor_changes:
        _context.set_devices_changed_callback( _device_change_callback )


def _device_change_callback( info ):
    """
    Called when librealsense detects a device change (see query())
    """
    global _device_by_sn
    for device in _device_by_sn.values():
        if device.enabled  and  info.was_removed( device.handle ):
            log.d( 'device removed:', device.serial_number )
            device._removed = True
    for handle in info.get_new_devices():
        sn = handle.get_info( rs.camera_info.firmware_update_id )
        log.d( 'device added:', sn, handle )
        if sn in _device_by_sn:
            device = _device_by_sn[sn]
            device._removed = False
            device._dev = handle     # Because it has a new handle!
        else:
            # shouldn't see new devices...
            log.d( 'new device detected!?' )
            _device_by_sn[sn] = Device( sn, handle )


def all():
    """
    :return: A set of all device serial-numbers at the time of query()
    """
    global _device_by_sn
    return _device_by_sn.keys()


def enabled():
    """
    :return: A set of all device serial-numbers that are currently enabled
    """
    global _device_by_sn
    return { device.serial_number for device in _device_by_sn.values() if device.enabled }


def by_product_line( product_line, ignored_products ):
    """
    :param product_line: The product line we're interested in, as a string ("L500", etc.)
    :param ignored_products: List of products we want to ignore. e.g. ['D455', 'D457', etc.]
    :return: A set of device serial-numbers
    """
    global _device_by_sn
    result = set()
    for device in _device_by_sn.values():
        if device.product_line == product_line:
            for ignored_product in ignored_products:
                if ignored_product in device.name:
                    break
            else:
                result.add(device.serial_number)
    return result


def by_name( name, ignored_products ):
    """
    :param name: Part of the product name to search for ("L515" would match "Intel RealSense L515")
    :param ignored_products: List of products we want to ignore. e.g. ['D455', 'D457', etc.]
    :return: A set of device serial-numbers
    """
    global _device_by_sn
    result = set()
    ignored_list_as_str = " ".join(ignored_products)
    if name not in ignored_list_as_str:
        for device in _device_by_sn.values():
            if device.name and device.name.find( name ) >= 0:
                result.add(device.serial_number)
    return result

def _get_sns_from_spec( spec, ignored_products ):
    """
    Helper function for by_configuration. Yields all serial-numbers matching the given spec
    :param spec: A product name/line (as a string) we want to get serial number of
    :param ignored_products: List of products we want to ignore. e.g. ['D455', 'D457', etc.]
    :return: A set of device serial-numbers
    """
    if spec.endswith( '*' ):
        for sn in by_product_line( spec[:-1], ignored_products ):
            yield sn
    else:
        for sn in by_name( spec, ignored_products ):
            yield sn


def expand_specs( specs ):
    """
    Given a collection of configuration specs, expand them into actual serial numbers.
    Specs can be loaded from a file: see load_specs_from_file()
    :param specs: a collection of specs
    :return: a set of serial-numbers
    """
    expanded = set()
    for spec in specs:
        sns = {sn for sn in _get_sns_from_spec( spec )}
        if sns:
            expanded.update( sns )
        else:
            # maybe the spec is a specific serial-number?
            if get(spec):
                expanded.add( spec )
            else:
                log.d( 'unknown spec:', spec )
    return expanded


def load_specs_from_file( filename ):
    """
    Loads a set of specs from a file:
        - Comments (#) are removed
        - Each word in the file is a spec
    :param filename: the path to the text file we want to load
    :return: a set of specs that can then be expanded to a set of serial-numbers (see expand_specs())
    """
    from rspy import file
    exceptions = set()
    for line, comment in file.split_comments( filename ):
        specs = line.split()
        if specs:
            log.d( '...', specs, comment and ('  # ' + comment) or '', )
            exceptions.update( specs )
    return exceptions


def by_configuration( config, exceptions = None ):
    """
    Yields the serial numbers fitting the given configuration. If configuration includes an 'each' directive
    will yield all fitting serial numbers one at a time. Otherwise yields one set of serial numbers fitting the configuration

    :param config: A test:device line collection of arguments (e.g., [L515 D400*])
    :param exceptions: A collection of serial-numbers that serve as exceptions that will never get matched

    If no device matches the configuration devices specified, a RuntimeError will be
    raised!
    """
    exceptions = exceptions or set()
    # split the current config to two lists:
    #     1) new_config (the wanted products)
    #     2) ignored_products (strings starting with !)
    # For example: "each(D400*) !D457" ---> new_config = ['each(D400*)'], ignored_products = ['D457']
    new_config = []
    ignored_products = []
    for p in config:
        if p[0] == '!':
            ignored_products.append(p[1:])  # remove the '!'
        else:
            new_config.append(p)

    if len( new_config ) > 0 and re.fullmatch( r'each\(.+\)', new_config[0], re.IGNORECASE ):
        spec = new_config[0][5:-1]
        for sn in _get_sns_from_spec( spec, ignored_products ):
            if sn not in exceptions:
                yield { sn }
    else:
        sns = set()
        for spec in new_config:
            old_len = len(sns)
            for sn in _get_sns_from_spec( spec, ignored_products ):
                if sn in exceptions:
                    continue
                if sn not in sns:
                    sns.add( sn )
                    break
            new_len = len(sns)
            if new_len == old_len:
                error = 'no device matches configuration "' + spec + '"'
                if old_len:
                    error += ' (after already matching ' + str(sns) + ')'
                if ignored_products:
                    error += ' (!' + str(ignored_products) + ')'
                if exceptions:
                    error += ' (-' + str(exceptions) + ')'
                raise RuntimeError( error )
        if sns:
            yield sns


def get_first( sns ):
    """
    Throws if no serial-numbers are available!
    :param sns: An iterable list of serial-numbers. If None, defaults to all enabled() devices
    :return: The first Device in the given serial-numbers
    """
    return get( next( iter( sns or enabled() )))


def get( sn ):
    """
    :param sn: The serial-number of the requested device
    :return: The Device object with the given SN, or None
    """
    global _device_by_sn
    return _device_by_sn.get(sn)


def get_by_port( port ):
    """
    Return the Device at the given port number. Note that the device may not be enabled!
    :param sn: The port of the requested device
    :return: The Device object, or None
    """
    global _device_by_sn
    for sn in all():
        device = get( sn )
        if device.port == port:
            return device
    return None


def recovery():
    """
    :return: A set of all device serial-numbers that are in recovery mode
    """
    global _device_by_sn
    return { device.serial_number for device in _device_by_sn.values() if device.handle.is_update_device() }


def enable_only( serial_numbers, recycle = False, timeout = 5 ):
    """
    Enable only the devices corresponding to the given serial-numbers. This can work either
    with or without Acroname: without, the devices will simply be HW-reset, but other devices
    will still be present.

    NOTE: will raise an exception if any SN is unknown!

    :param serial_numbers: A collection of serial-numbers to enable - all others' ports are
                           disabled and will no longer be usable!
    :param recycle: If False, the devices will not be reset if they were already enabled. If
                    True, the devices will be recycled by disabling the port, waiting, then
                    re-enabling
    :param timeout: The maximum seconds to wait to make sure the devices are indeed online
    """
    if acroname:
        #
        ports = [ get( sn ).port for sn in serial_numbers ]
        #
        if recycle:
            #
            log.d( 'recycling ports via acroname:', ports )
            #
            acroname.disable_ports( acroname.ports() )
            _wait_until_removed( serial_numbers, timeout = timeout )
            #
            acroname.enable_ports( ports )
            #
        else:
            #
            acroname.enable_ports( ports, disable_other_ports = True )
        #
        _wait_for( serial_numbers, timeout = timeout )
        #
    elif recycle:
        #
        hw_reset( serial_numbers )
        #
    else:
        log.d( 'no acroname; ports left as-is' )


def enable_all():
    """
    Enables all ports on an Acroname -- without an Acroname, this does nothing!
    """
    if acroname:
        acroname.enable_ports()


def _wait_until_removed( serial_numbers, timeout = 5 ):
    """
    Wait until the given serial numbers are all offline

    :param serial_numbers: A collection of serial-numbers to wait until removed
    :param timeout: Number of seconds of maximum wait time
    :return: True if all have come offline; False if timeout was reached
    """
    while True:
        have_devices = False
        enabled_sns = enabled()
        for sn in serial_numbers:
            if sn in enabled_sns:
                have_devices = True
                break
        if not have_devices:
            return True
        #
        if timeout <= 0:
            return False
        timeout -= 1
        time.sleep( 1 )


def _wait_for( serial_numbers, timeout = 5 ):
    """
    Wait until the given serial numbers are all online

    :param serial_numbers: A collection of serial-numbers to wait for
    :param timeout: Number of seconds of maximum wait time
    :return: True if all have come online; False if timeout was reached
    """
    did_some_waiting = False
    #
    # In Linux, we don't have an active notification mechanism - we query devices every 5 seconds
    # (see POLLING_DEVICES_INTERVAL_MS) - so we add extra timeout
    if timeout and platform.system() == 'Linux':
        timeout += 5
    #
    while True:
        #
        have_all_devices = True
        enabled_sns = enabled()
        for sn in serial_numbers:
            if sn not in enabled_sns:
                have_all_devices = False
                break
        #
        if have_all_devices:
            if did_some_waiting:
                # Wait an extra second, just in case -- let the devices properly power up
                #log.d( 'all devices powered up' )
                time.sleep( 1 )
            return True
        #
        if timeout <= 0:
            if did_some_waiting:
                log.d( 'timed out' )
            return False
        timeout -= 1
        time.sleep( 1 )
        did_some_waiting = True


def hw_reset( serial_numbers, timeout = 5 ):
    """
    Recycles the given devices manually, using a hardware-reset (rather than any acroname port
    reset). The devices are sent a HW-reset command and then we'll wait until they come back
    online.

    NOTE: will raise an exception if any SN is unknown!

    :param serial_numbers: A collection of serial-numbers to reset
    :param timeout: Maximum # of seconds to wait for the devices to come back online
    :return: True if all devices have come back online before timeout
    """
    for sn in serial_numbers:
        dev = get( sn ).handle
        dev.hardware_reset()
    #
    _wait_until_removed( serial_numbers )
    #
    return _wait_for( serial_numbers, timeout = timeout )


###############################################################################################
import platform
if 'windows' in platform.system().lower():
    #
    def _get_usb_location( physical_port ):
        """
        Helper method to get windows USB location from registry
        """
        if not physical_port:
            return None
        # physical port example:
        #   \\?\usb#vid_8086&pid_0b07&mi_00#6&8bfcab3&0&0000#{e5323777-f976-4f5b-9b55-b94699c46e44}\global
        #
        re_result = re.match( r'.*\\(.*)#vid_(.*)&pid_(.*)(?:&mi_(.*))?#(.*)#', physical_port, flags = re.IGNORECASE )
        dev_type = re_result.group(1)
        vid = re_result.group(2)
        pid = re_result.group(3)
        mi = re_result.group(4)
        unique_identifier = re_result.group(5)
        #
        import winreg
        if mi:
            registry_path = "SYSTEM\CurrentControlSet\Enum\{}\VID_{}&PID_{}&MI_{}\{}".format(
                dev_type, vid, pid, mi, unique_identifier
                )
        else:
            registry_path = "SYSTEM\CurrentControlSet\Enum\{}\VID_{}&PID_{}\{}".format(
                dev_type, vid, pid, unique_identifier
                )
        try:
            reg_key = winreg.OpenKey( winreg.HKEY_LOCAL_MACHINE, registry_path )
        except FileNotFoundError:
            log.e( 'Could not find registry key for port:', registry_path )
            log.e( '    usb location:', physical_port )
            return None
        result = winreg.QueryValueEx( reg_key, "LocationInformation" )
        # location example: 0000.0014.0000.016.003.004.003.000.000
        # and, for T265: Port_#0002.Hub_#0006
        return result[0]
    #
    def _get_port_by_loc( usb_location ):
        """
        """
        if usb_location:
            #
            # T265 locations look differently...
            match = re.fullmatch( r'Port_#(\d+)\.Hub_#(\d+)', usb_location, re.IGNORECASE )
            if match:
                # We don't know how to get the port from these yet!
                return None #int(match.group(2))
            else:
                split_location = [int(x) for x in usb_location.split('.')]
                # only the last two digits are necessary
                return acroname.get_port_from_usb( split_location[-5], split_location[-4] )
    #
else:
    #
    def _get_usb_location( physical_port ):
        """
        """
        if not physical_port:
            return None
        # physical port example:
        # u'/sys/devices/pci0000:00/0000:00:14.0/usb2/2-3/2-3.3/2-3.3.1/2-3.3.1:1.0/video4linux/video0'
        #
        split_location = physical_port.split( '/' )
        if len(split_location) > 4:
            port_location = split_location[-4]
        elif len(split_location) == 1:
            # Recovery devices may have only the relevant port: e.g., L515 Recovery has "2-2.4.4-84"
            port_location = physical_port
        else:
            raise RuntimeError( f"invalid physical port '{physical_port}'" )
        # location example: 2-3.3.1
        return port_location
    #
    def _get_port_by_loc( usb_location ):
        """
        """
        if usb_location:
            #
            # Devices connected thru an acroname will be in one of two sub-hubs under the acroname main
            # hub. Each is a 4-port hub with a different port (4 for ports 0-3, 3 for ports 4-7):
            #     /:  Bus 02.Port 1: Dev 1, Class=root_hub, Driver=xhci_hcd/6p, 10000M
            #         |__ Port 2: Dev 2, If 0, Class=Hub, Driver=hub/4p, 5000M                       <--- ACRONAME
            #             |__ Port 3: Dev 3, If 0, Class=Hub, Driver=hub/4p, 5000M
            #                 |__ Port X: Dev, If...
            #                 |__ Port Y: ...
            #             |__ Port 4: Dev 4, If 0, Class=Hub, Driver=hub/4p, 5000M
            #                 |__ Port Z: ...
            # (above is output from 'lsusb -t')
            # For the above acroname at '2-2' (bus 2, port 2), there are at least 3 devices:
            #     2-2.3.X
            #     2-2.3.Y
            #     2-2.4.Z
            # Given the two sub-ports (3.X, 3.Y, 4.Z), we can get the port number.
            # NOTE: some of our devices are hubs themselves! For example, the SR300 will show as '2-2.3.2.1' --
            # we must start a known hub or else the ports we look at are meaningless...
            #
            global _acroname_hubs
            for port in _acroname_hubs:
                if usb_location.startswith( port + '.' ):
                    match = re.search( r'^(\d+)\.(\d+)', usb_location[len(port)+1:] )
                    if match:
                        return acroname.get_port_from_usb( int(match.group(1)), int(match.group(2)) )


###############################################################################################
if __name__ == '__main__':
    import os, sys, getopt
    try:
        opts,args = getopt.getopt( sys.argv[1:], '',
            longopts = [ 'help', 'recycle', 'all', 'list', 'port=', 'ports' ])
    except getopt.GetoptError as err:
        print( '-F-', err )   # something like "option -a not recognized"
        usage()
    if args:
        usage()
    try:
        if acroname:
            if not acroname.hub:
                try:
                    acroname.connect()
                    if platform.system() == 'Linux':
                        _acroname_hubs = set( acroname.find_all_hubs() )
                except acroname.NoneFoundError as e:
                    # This can happen, e.g. on Jetson with D457...
                    log.d( 'connect() failed:', e )
                    acroname = None
        action = 'list'
        def get_handle(dev):
            return dev.handle
        def get_phys_port(dev):
            return dev.physical_port or "???"
        printer = get_handle
        for opt,arg in opts:
            if opt in ('--list'):
                action = 'list'
            elif opt in ('--ports'):
                printer = get_phys_port
            elif opt in ('--all'):
                if not acroname:
                    log.f( 'No acroname available' )
                acroname.enable_ports( sleep_on_change = 5 )
            elif opt in ('--port'):
                if not acroname:
                    log.f( 'No acroname available' )
                all_ports = acroname.all_ports()
                str_ports = arg.split(',')
                ports = [int(port) for port in str_ports if port.isnumeric() and int(port) in all_ports]
                if len(ports) != len(str_ports):
                    log.f( 'Invalid ports', str_ports )
                acroname.enable_ports( ports, disable_other_ports = True, sleep_on_change = 5 )
            elif opt in ('--recycle'):
                action = 'recycle'
            else:
                usage()
        if action == 'list':
            query()
            for sn in all():
                device = get( sn )
                print( '{port} {name:30} {sn:20} {handle}'.format(
                    sn = sn,
                    name = device.name,
                    port = device.port is None and '?' or device.port,
                    handle = printer(device)
                    ))
        elif action == 'recycle':
            log.f( 'Not implemented yet' )
    finally:
        #
        # Disconnect from the Acroname -- if we don't it'll crash on Linux...
        if acroname:
            acroname.disconnect()


