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

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

# %% ../06_uwb.ipynb 4
import yaml
import serial
from serial.tools.list_ports import comports
import pypozyx
from pypozyx import PozyxSerial
from pypozyx import NetworkID
from pypozyx import Coordinates, DeviceCoordinates
from pypozyx import DeviceRange
from pypozyx import PozyxConstants
from pypozyx.core import PozyxException
from typing import List

# %% ../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
    
    @property
    def network_id_str(self) -> str:
        """A getter method of network id string

        Convert network id to string to show in readable.

        Returns:
            str: A string of id number in hexadecimal of The Pozyx
        """
        return str(self._network_id)

    @network_id.setter
    def network_id(self, value: int = None) -> None:
        """A setter method of port string

        Args:
            value (int, optional): A integer id number in hexadecimal or decimal of The Pozyx. Defaults to None.
        """
        if value is None:
            self._network_id = NetworkID()
        else:
            self._network_id = NetworkID(value)


# %% ../06_uwb.ipynb 6
class UWB(UWB):
    # pose
    @property
    def pose(self) -> List[float]:
        """A getter method of UWB pose

        Returns:
            list[float]: (pose.x, pose.y, pose.z)
        """
        return (self._pose.x, self._pose.y, self._pose.z)

    @pose.setter
    def pose(self, value: List[float] = None) -> None:
        """A setter method of UWB pose

        Args:
            value (List[float], optional): (pose.x, pose.y, pose.z) Defaults to None.
        """
        if value is None:
            self._pose = Coordinates()
        else:
            self._pose.x = value[0]
            self._pose.y = value[1]
            self._pose.z = value[2]


# %% ../06_uwb.ipynb 7
class UWB(UWB):
    # height
    @property
    def height(self) -> float:
        """A getter method of UWB pose height

        Returns:
            float: The default height for 2.5D localization.
        """
        return self._height

    @height.setter
    def height(self, value: float = 0) -> float:
        """A setter method of UWB pose height

        Args:
            value (int, optional): The default height for 2.5D localization.. Defaults to 0.
        """
        self._height = value


# %% ../06_uwb.ipynb 8
class UWB(UWB):
    # env_config
    @property
    def env_config(self) -> dict:
        """A getter method of environment config

        Returns:
            dict: The environment config in dict format.
        """
        return self._env_config


# %% ../06_uwb.ipynb 9
class UWB(UWB):
    # port_lost
    def port_list(self) -> List[str]:
        """A getter method of port list.

        Returns:
            List[str]: The list contains UWB port device path like `/dev/ttyACM0`.
        """
        return self._port_list


# %% ../06_uwb.ipynb 10
class UWB(UWB):
    # status
    @property
    def status(self) -> int:
        """A getter method of UWB status.

        Returns:
            int: The status got from Pozyx. 0 is success.
        """
        return self._status


# %% ../06_uwb.ipynb 11
class UWB(UWB):
    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


# %% ../06_uwb.ipynb 12
class UWB(UWB):
    def scan_port(self) -> None:
        """Scan all port connecting to host. Store port device path in port list.
        """
        self._port_list = []
        for port in comports():
            try:
                if "Pozyx Labs" in port.manufacturer:
                    self._port_list.append(port.device)
                    break
            except TypeError:
                pass
            try:
                if "Pozyx" in port.product:
                    self._port_list.append(port.device)
                    break
            except TypeError:
                pass

# %% ../06_uwb.ipynb 13
class UWB(UWB):
    def connect(self) -> bool:
        """Try to connect pozyx device.

        Returns:
            bool: Pozyx status
        """
        self._status = PozyxConstants.STATUS_SUCCESS
        if self.port is None:
            self.scan_port()
            if len(self._port_list) == 1:
                self.port = self._port_list[0]
                self._pozyx_handler = PozyxSerial(self.port)
                self._status &= self._pozyx_handler.getNetworkId(self._network_id)
            elif len(self._port_list) == 0:
                return False
            else:
                return False
        else:
            try:
                self._pozyx_handler = PozyxSerial(self.port)
                self._status &= self._pozyx_handler.getNetworkId(self._network_id)
                return True
            except PozyxException as ex:
                print(ex)
                return False


# %% ../06_uwb.ipynb 14
class UWB(UWB):
    def write_env_config(self) -> bool:
        """Write environment anchor location into Pozyx UWB device.

        Returns:
            bool: Pozyx status
        """
        self._status = PozyxConstants.STATUS_SUCCESS
        ANCHOR_FLAG = 1
        self._status &= self._pozyx_handler.clearDevices()
        for anchor_name, config in self.env_config.items():
            coordinate = Coordinates(config["x"], config["y"], config["z"])
            device_coordinate = DeviceCoordinates(config["id"], ANCHOR_FLAG, coordinate)
            self._status &= self._pozyx_handler.addDevice(device_coordinate)
        if len(self.env_config) > 4:
            self._status &= self._pozyx_handler.setSelectionOfAnchorsAutomatic(len(self.env_config))
        return self._status


# %% ../06_uwb.ipynb 15
class UWB(UWB):
    def localize_2_5D(self) -> bool:
        """Localize method in 2.5D. Need to know height.

        Returns:
            bool: Pozyx status
        """
        self._status &= self._pozyx_handler.doPositioning(
            self._pose,
            PozyxConstants.DIMENSION_2_5D,
            self._height,
            PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY,
        )
        return self._status


# %% ../06_uwb.ipynb 16
class UWB(UWB):
    def localize_3D(self)->bool:
        """Localize method in 3D. The height will be determined by Pozyx UWB device.

        Returns:
            bool: Pozyx status
        """
        self._status &= self._pozyx_handler.doPositioning(
            self._pose,
            PozyxConstants.DIMENSION_3D,
            self._height,
            PozyxConstants.POSITIONING_ALGORITHM_UWB_ONLY,
        )
        return self._status


# %% ../06_uwb.ipynb 17
class UWB(UWB):
    def range_from(self, dest_id) -> float:
        """Range method from this Pozyx UWB device to the destination Pozyx UWB device.

        Args:
            dest_id (_type_): The target Pozyx UWB device id want to be ranged.

        Returns:
            float: The range from this Pozyx UWB device to the destination Pozyx UWB device.
        """
        ranges = DeviceRange()
        self._pozyx_handler.doRanging(dest_id, ranges)
        return ranges

