# AUTOGENERATED! DO NOT EDIT! File to edit: ../06_uwb.ipynb.

# %% auto 0
__all__ = ['UWB']

# %% ../06_uwb.ipynb 4
import yaml
import pypozyx
from pypozyx import NetworkID, Coordinates, PozyxSerial, DeviceCoordinates, DeviceRange, PozyxConstants
from pypozyx.core import PozyxException

# %% ../06_uwb.ipynb 5
class UWB():
    def __init__(self, port = None):
        self.port = port
        self._network_id = None
        self._pozyx_handler = None        
        self._pose = None
        self._env_config = None
        
        #TODO: Make height parameterized
        self._height = 500
        
    @property
    def network_id(self):
        return self._network_id
    
    @network_id.setter
    def network_id(self, value=None):
        if value is None:
            self._network_id = NetworkID()
        else:
            self._network_id = NetworkID(value)
        
    @property
    def port(self):
        return self._port
    
    @port.setter
    def port(self, port):
        self._port = port
        
    @property
    def pose(self):
        return self._pose
    
    @pose.setter
    def pose(self, value=None):
        if value is None:
            self._pose = Coordinates()
        else:
            self._pose.x = value[0]
            self._pose.y = value[1]
            self._pose.z = value[2]
    
    @property
    def env_config(self):
        return self._env_config
    
    def load_env_config(self, config_file_path: str) -> bool:
        """
        Load UWB anchors' environment config.
        
        Args:
            config_file_path (str): The environment config file path.
            
        Returns:
            bool: True for success, False for failure.
        """
        with open(config_file_path, 'r') as config_file:
            try:
                self._env_config = yaml.safe_load(config_file)
            except yaml.YAMLError as ex:
                print(ex)
                return False
        return True
    
    def connect(self):
        if self.port is None:
            #TODO:
            pass
        else:
            try:
                self.pozyx_handler = PozyxSerial(self.port)
            except PozyxException:
                #TODO:
                pass
    
    def write_device(self):
        ANCHOR_FLAG = 1
        self.pozyx_handler.clearDevices()
        for anchor_name, config in self.env_config:
            coordinate = Coordinates(config['x'], config['y'], config['z'])
            device_coordinate = DeviceCoordinates(config['id'], ANCHOR_FLAG, coordinate)
            self.pozyx_handler.addDevice(device_coordinate)
            self.pozyx_handler.setSelectionOfAnchorsAutomatic(len(self.env_config))
    
    #TODO: Seperate localize of differ dimensions and height
    def localize(self, dest_id):
        self.pozyx_handler.doPosition(self.pose, PozyxConstants.DIMENSION_3D, self._height, PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY, )
        
    def range(self, dest_id):
        ranges = DeviceRange()
        self.pozyx_handler.doRanging(dest_id, ranges)
        return ranges
