# =============================================================================
# Copyright © 2025 NaturalPoint, Inc. All Rights Reserved.
#
# THIS SOFTWARE IS GOVERNED BY THE OPTITRACK PLUGINS EULA AVAILABLE AT
#  https://www.optitrack.com/about/legal/eula.html AND/OR FOR DOWNLOAD WITH
# THE APPLICABLE SOFTWARE FILE(S) (“PLUGINS EULA”). BY DOWNLOADING, INSTALLING,
# ACTIVATING AND/OR OTHERWISE USING THE SOFTWARE, YOU ARE AGREEING THAT YOU
# HAVE READ, AND THAT YOU AGREE TO COMPLY WITH AND ARE BOUND BY, THE PLUGINS
# EULA AND ALL APPLICABLE LAWS AND REGULATIONS. IF YOU DO NOT AGREE TO BE
# BOUND BY THE PLUGINS EULA, THEN YOU MAY NOT DOWNLOAD, INSTALL, ACTIVATE OR
# OTHERWISE USE THE SOFTWARE AND YOU MUST PROMPTLY DELETE OR RETURN IT. IF YOU
# ARE DOWNLOADING, INSTALLING, ACTIVATING AND/OR OTHERWISE USING THE SOFTWARE
# ON BEHALF OF AN ENTITY, THEN BY DOING SO YOU REPRESENT AND WARRANT THAT YOU
# HAVE THE APPROPRIATE AUTHORITY TO ACCEPT THE PLUGINS EULA ON BEHALF OF SUCH
# ENTITY.
# See license file in root directory for additional governing terms and
# information.
# =============================================================================


# OptiTrack NatNet direct depacketization sample for Python 3.x
#


# Uses the Python NatNetClient.py library to establish a connection (by
# creating a NatNetClient), and receive data via a NatNet connection and
# decode it using the NatNetClient library.


import copy
import hashlib
import random

K_SKIP = [0, 0, 1]
K_FAIL = [0, 1, 0]
K_PASS = [1, 0, 0]


def get_tab_str(tab_str, level):
    out_tab_str = ""
    loop_range = range(0, level)
    for _ in loop_range:
        out_tab_str += tab_str
    return out_tab_str


def add_lists(totals, totals_tmp):
    totals[0] += totals_tmp[0]
    totals[1] += totals_tmp[1]
    totals[2] += totals_tmp[2]
    return totals


def test_hash(test_name, test_hash_str, test_object):
    out_str = test_object.get_as_string()
    out_hash_str = hashlib.sha1(out_str.encode()).hexdigest()
    ret_value = True
    if test_hash_str == out_hash_str:
        print("[PASS]:%s" % test_name)
    else:
        print("[FAIL]:%s test_hash_str != out_hash_str" % test_name)
        print("test_hash_str=%s" % test_hash_str)
        print("out_hash_str=%s" % out_hash_str)
        print("out_str =\n%s" % out_str)
        ret_value = False
    return ret_value


def test_hash2(test_name, test_hash_str, test_object, generator_string,
               run_test=True):
    ret_value = K_FAIL
    out_str = "FAIL"
    out_str2 = ""
    indent_string = "       "
    if not run_test:
        ret_value = K_SKIP
        out_str = "SKIP"
    elif test_object is None:
        out_str = "FAIL"
        ret_value = K_FAIL
        out_str2 = "%sERROR: test_object was None" % indent_string
    else:
        obj_out_hash_str = ""
        if str(type(test_object)) != 'NoneType':
            obj_out_str = test_object.get_as_string()
            obj_out_hash_str = hashlib.sha1(obj_out_str.encode()).hexdigest()

        if test_hash_str == obj_out_hash_str:
            out_str = "PASS"
            ret_value = K_PASS
        else:
            out_str2 += "%s%s test_hash_str != out_hash_str\n" % (
                indent_string, test_name)
            out_str2 += "%stest_hash_str=%s\n" % (indent_string, test_hash_str)
            out_str2 += "%sobj_out_hash_str=%s\n" % (indent_string,
                                                     obj_out_hash_str)
            out_str2 += "%sUpdated Test Entry:\n" % (indent_string)
            out_str2 += "%s[\"%s\", \"%s\", \"%s\", True],\n" % (
                indent_string, test_name, obj_out_hash_str, generator_string)
            out_str2 += "%sobj_out_str =\n%s" % (indent_string, obj_out_str)
            ret_value = K_FAIL
    print("[%s]:%s" % (out_str, test_name))

    if len(out_str2):
        print("%s" % out_str2)
    return ret_value


def get_as_string(input_str):
    if type(input_str) is str:
        return input_str
    else:
        return input_str.decode('utf-8')


def get_data_sub_packet_type(new_data):
    out_string = ""
    data_type = type(new_data)
    if data_type == MarkerSetDescription:
        out_string = "Type: 0 Markerset\n"
    elif data_type == RigidBodyDescription:
        out_string = "Type: 1 Rigid Body\n"
    elif data_type == SkeletonDescription:
        out_string = "Type: 2 Skeleton\n"
    elif data_type == ForcePlateDescription:
        out_string = " Type: 3 Force Plate\n"
    elif data_type == DeviceDescription:
        out_string = "Type: 4 Device\n"
    elif data_type == CameraDescription:
        out_string = "Type: 5 Camera\n"
    elif data_type == AssetDescription:
        out_string = "Type: 6 Asset\n"
    elif data_type == IMUDescription:
        out_string = "Type: 7 IMU\n"
    elif data_type == GPIODescription:
        out_string = "Type: 8 GPIO\n"
    elif data_type == AnchorDescription:
        out_string = "Type: 9 Anchor\n"
    elif data_type is None:
        out_string = "Type: None\n"
    else:
        out_string = "Type: Unknown %s\n" % str(data_type)
    return out_string


# cMarkerSetDescription
class MarkerSetDescription:
    def __init__(self):
        self.marker_set_name = "Not Set"
        self.marker_names_list = []

    def set_name(self, new_name):
        self.marker_set_name = new_name

    def get_num_markers(self):
        return len(self.marker_names_list)

    def add_marker_name(self, marker_name):
        self.marker_names_list.append(copy.copy(marker_name))
        return self.get_num_markers()

    def get_as_string(self, tab_str="  ", level=0):
        out_tab_str = get_tab_str(tab_str, level)
        out_tab_str2 = get_tab_str(tab_str, level+1)
        out_tab_str3 = get_tab_str(tab_str, level+2)
        out_string = ""
        out_string += "%sMarkerset Name: %s\n" % (
            out_tab_str, get_as_string(self.marker_set_name))
        num_markers = len(self.marker_names_list)
        out_string += "%sMarker Count   : %d\n" % (out_tab_str2, num_markers)
        for i in range(num_markers):
            out_string += "%s%3.1d Marker Name: %s\n" % (
                out_tab_str3, i, get_as_string(self.marker_names_list[i]))
        return out_string


class RBMarker:
    def __init__(self, marker_name="", active_label=0, pos=[0.0, 0.0, 0.0]):
        self.marker_name = marker_name
        self.active_label = active_label
        self.pos = pos

    def get_as_string(self, tab_str="  ", level=0):
        out_tab_str = get_tab_str(tab_str, level)
        out_string = ""
        out_string += """%sMarker Label: %s Position: [%3.2f %3.2f %3.2f] %s
        \n""" % \
            (out_tab_str, self.active_label,
             self.pos[0], self.pos[1], self.pos[2],
             self.marker_name)
        return out_string


class RigidBodyDescription:
    def __init__(self, sz_name="", new_id=0, parent_id=0, pos=[0.0, 0.0, 0.0]):
        self.sz_name = sz_name
        self.id_num = new_id
        self.parent_id = parent_id
        self.pos = pos
        self.rb_marker_list = []
        self.rb_num = -1

    def set_name(self, new_name):
        self.sz_name = new_name

    def set_id(self, new_id):
        self.id_num = new_id

    def set_parent_id(self, parent_id):
        self.parent_id = parent_id

    def set_pos(self, p_x, p_y, p_z):
        self.pos = [p_x, p_y, p_z]

    def get_num_markers(self):
        return len(self.rb_marker_list)

    def add_rb_marker(self, new_rb_maker):
        self.rb_marker_list.append(copy.deepcopy(new_rb_maker))
        return self.get_num_markers()

    def get_as_string(self, tab_str="  ", level=0):
        out_tab_str = get_tab_str(tab_str, level)
        out_tab_str2 = get_tab_str(tab_str, level+1)
        out_string = ""
        out_string += "%sRigid Body        :" % (out_tab_str)
        if (self.rb_num > -1):
            out_string += " %d\n" % (self.rb_num)
        out_string += "\n"
        out_string += "%sRigid Body Name   : %s\n" % (
            out_tab_str, get_as_string(self.sz_name))
        out_string += "%sRigid Body ID     : %d\n" % (out_tab_str, self.id_num)
        out_string += "%sParent ID         : %d\n" % (
            out_tab_str, self.parent_id)
        out_string += "%sPosition          : [%3.2f, %3.2f, %3.2f]\n" % (
            out_tab_str, self.pos[0], self.pos[1], self.pos[2])
        num_markers = len(self.rb_marker_list)
        out_string += "%sNumber of Markers : %d\n" % (out_tab_str, num_markers)
        # loop over markers
        for i in range(num_markers):
            out_string += "%s%i %s" % (
                out_tab_str2, i,
                self.rb_marker_list[i].get_as_string(tab_str, 0))
        return out_string


class SkeletonDescription:
    def __init__(self, name="", new_id=0):
        self.name = name
        self.id_num = new_id
        self.rigid_body_description_list = []

    def set_name(self, new_name):
        self.name = new_name

    def set_id(self, new_id):
        self.id_num = new_id

    def add_rigid_body_description(self, rigid_body_description):
        self.rigid_body_description_list.append(
            copy.deepcopy(rigid_body_description))
        return len(self.rigid_body_description_list)

    def get_as_string(self, tab_str="  ", level=0):
        out_tab_str = get_tab_str(tab_str, level)
        out_tab_str2 = get_tab_str(tab_str, level+1)
        out_string = ""
        out_string += "%sName                    : %s\n" % (
            out_tab_str, get_as_string(self.name))
        out_string += "%sID                      : %d\n" % (
            out_tab_str, self.id_num)
        num_bones = len(self.rigid_body_description_list)
        out_string += "%sRigid Body (Bone) Count : %d\n" % (
            out_tab_str, num_bones)
        for i in range(num_bones):
            out_string += "%sRigid Body (Bone) %d\n" % (out_tab_str2, i)
            out_string += self.rigid_body_description_list[i].get_as_string(
                tab_str, level+2)
        return out_string


class ForcePlateDescription:
    def __init__(self, new_id=0, serial_number=""):
        self.id_num = new_id
        self.serial_number = serial_number
        self.width = 0
        self.length = 0
        self.position = [0.0, 0.0, 0.0]
        self.cal_matrix = [[0.0 for col in range(12)] for row in range(12)]
        self.corners = [[0.0 for col in range(3)] for row in range(4)]
        self.plate_type = 0
        self.channel_data_type = 0
        self.channel_list = []

    def set_id(self, new_id):
        self.id_num = new_id

    def set_serial_number(self, serial_number):
        self.serial_number = serial_number

    def set_dimensions(self, width, length):
        self.width = width
        self.length = length

    def set_origin(self, p_x, p_y, p_z):
        self.position = [p_x, p_y, p_z]

    def set_cal_matrix(self, cal_matrix):
        self.cal_matrix = cal_matrix

    def set_corners(self, corners):
        self.corners = corners

    def set_plate_type(self, plate_type):
        self.plate_type = plate_type

    def set_channel_data_type(self, channel_data_type):
        self.channel_data_type = channel_data_type

    def add_channel_name(self, channel_name):
        self.channel_list.append(copy.deepcopy(channel_name))
        return len(self.channel_list)

    def get_cal_matrix_as_string(self, tab_str="", level=0):
        """Get force plate calibration matrix as string"""
        out_tab_str = get_tab_str(tab_str, level)
        out_tab_str2 = get_tab_str(tab_str, level+1)
        out_string = ""
        out_string += "%sCal Matrix:\n" % out_tab_str
        for i in range(0, 12):
            out_string += """%s%2.1d %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e
             %3.3e %3.3e %3.3e %3.3e %3.3e\n""" % \
                    (out_tab_str2, i,
                     self.cal_matrix[i][0], self.cal_matrix[i][1],
                     self.cal_matrix[i][2], self.cal_matrix[i][3],
                     self.cal_matrix[i][4], self.cal_matrix[i][5],
                     self.cal_matrix[i][6], self.cal_matrix[i][7],
                     self.cal_matrix[i][8], self.cal_matrix[i][9],
                     self.cal_matrix[i][10], self.cal_matrix[i][11])
        return out_string

    def get_corners_as_string(self, tab_str="", level=0):
        """Get force plate corner positions as a string"""
        # Corners 4x3 floats
        out_tab_str = get_tab_str(tab_str, level)
        out_tab_str2 = get_tab_str(tab_str, level+1)
        out_string = ""
        out_string += "%sCorners:\n" % out_tab_str
        for i in range(0, 4):
            out_string += "%s%2.1d %3.3e %3.3e %3.3e\n" % \
                    (out_tab_str2, i,
                     self.corners[i][0],
                     self.corners[i][1],
                     self.corners[i][2])
        return out_string

    def get_as_string(self, tab_str="  ", level=0):
        """Get force plate description as a class"""
        out_tab_str = get_tab_str(tab_str, level)
        out_string = ""
        out_string += "%sID                      : %d\n" % (
            out_tab_str, self.id_num)
        out_string += "%sSerial Number           : %s\n" % (
            out_tab_str,
            get_as_string(self.serial_number))
        out_string += "%sWidth                   : %3.2f\n" % (
            out_tab_str, self.width)
        out_string += "%sLength                  : %3.2f\n" % (
            out_tab_str, self.length)
        out_string += "%sOrigin                  : [%3.2f, %3.2f, %3.2f]\n" % (
            out_tab_str,
            self.position[0], self.position[1], self.position[2])
        out_string += self.get_cal_matrix_as_string(tab_str, level)
        out_string += self.get_corners_as_string(tab_str, level)

        out_string += "%sPlate Type                : %d\n" % (
            out_tab_str, self.plate_type)
        out_string += "%sChannel Data Type         : %d\n" % (
            out_tab_str, self.channel_data_type)
        num_channels = len(self.channel_list)
        out_string += "%sNumber of Channels        : %d\n" % (
            out_tab_str, num_channels)
        # Channel Names list of NoC strings
        out_tab_str2 = get_tab_str(tab_str, level+1)
        for channel_num in range(num_channels):
            out_string += "%sChannel Name %d: %s\n" % (
                out_tab_str2, channel_num, get_as_string(
                    self.channel_list[channel_num]))

        return out_string


class DeviceDescription:
    """Device Description class"""
    def __init__(self, new_id, name, serial_number,
                 device_type, channel_data_type):
        self.id_num = new_id
        self.name = name
        self.serial_number = serial_number
        self.device_type = device_type
        self.channel_data_type = channel_data_type
        self.channel_list = []

    def set_id(self, new_id):
        """Set the device id"""
        self.id_num = new_id

    def set_name(self, name):
        """Set the Device name"""
        self.name = name

    def add_channel_name(self, channel_name):
        """Add channel name to channel_list"""
        self.channel_list.append(channel_name)
        return len(self.channel_list)

    def get_as_string(self, tab_str="  ", level=0):
        """Get Device Description as string"""
        out_tab_str = get_tab_str(tab_str, level)
        out_tab_str2 = get_tab_str(tab_str, level+1)
        out_string = ""
        out_string += "%sID                 : %5.1d\n" % (
            out_tab_str, self.id_num)
        out_string += "%sName               : %s\n" % (
            out_tab_str, get_as_string(self.name))
        out_string += "%sSerial Number      : %s\n" % (
            out_tab_str, get_as_string(self.serial_number))
        out_string += "%sDevice Type        : %d\n" % (
            out_tab_str, self.device_type)
        out_string += "%sChannel Data Type  : %d\n" % (
            out_tab_str, self.channel_data_type)
        num_channels = len(self.channel_list)
        out_string += "%sNumber of Channels : %d\n" % (
            out_tab_str, num_channels)
        for i in range(num_channels):
            out_string += "%sChannel %2.1d Name : %s\n" % (
                out_tab_str2, i, get_as_string(self.channel_list[i]))
        return out_string


class CameraDescription:
    """Camera Description class"""
    def __init__(self, name, position_vec3, orientation_quat):
        self.name = name
        self.position = position_vec3
        self.orientation = orientation_quat

    def get_as_string(self, tab_str="..", level=0):
        """Get Camera Description as a string"""
        out_tab_str = get_tab_str(tab_str, level)
        out_string = ""
        out_string += "%sName        : %s\n" % (
            out_tab_str, get_as_string(self.name))
        out_string += "%sPosition    : [%3.2f, %3.2f, %3.2f]\n" % \
            (out_tab_str, self.position[0], self.position[1], self.position[2])
        out_string += "%sOrientation : [%3.2f, %3.2f, %3.2f, %3.2f]\n" % \
            (out_tab_str, self.orientation[0], self.orientation[1],
             self.orientation[2], self.orientation[3])
        return out_string


class AnchorDescription:
    """Anchor Description class"""
    def __init__(self, name, position_vec3, id):
        self.name = name
        self.position = position_vec3
        self.id = id

    def get_as_string(self, tab_str="..", level=0):
        """Get Anchor Description as a string"""
        out_tab_str = get_tab_str(tab_str, level)
        out_string = f"{out_tab_str}Name       : {self.name}\n"
        out_string += f'{out_tab_str}Position   : [{self.position[0]:3.2f}, {self.position[1]:3.2f}, {self.position[2]:3.2f}]\n' #type: ignore  # noqa E501
        out_string += f"{out_tab_str}ID         : {self.id} \n"
        return out_string


class MarkerDescription:
    """Marker Description class"""
    def __init__(self, name, marker_id, position, marker_size, marker_params):
        self.name = name
        self.marker_id = marker_id
        self.position = position
        self.marker_size = marker_size
        self.marker_params = marker_params

    def get_as_string(self, tab_str="..", level=0):
        """Get Marker Description as a string"""
        out_tab_str = get_tab_str(tab_str, level)
        out_string = ""
        out_string += "%sName        : %s\n" % (
            out_tab_str, get_as_string(self.name))
        out_string += "%sID          : %d\n" % (
            out_tab_str, self.marker_id)
        out_string += "%sPosition    : [%3.2f, %3.2f, %3.2f]\n" % \
            (out_tab_str, self.position[0], self.position[1], self.position[2])
        out_string += "%sSize          : %3.2f\n" % (
            out_tab_str, self.marker_size[0])
        out_string += "%sParams        : %d\n" % (
            out_tab_str, self.marker_params)

        return out_string


class AssetDescription:
    """Asset Description class"""
    def __init__(self, name, assetType, assetID, rigidbodyArray, markerArray):
        self.name = name
        self.assetType = assetType
        self.assetID = assetID
        self.rigidbodyArray = rigidbodyArray
        self.markerArray = markerArray

    def get_as_string(self, tab_str="..", level=0):
        """Get Asset Description as a string"""
        out_tab_str = get_tab_str(tab_str, level)
        out_string = ""
        # out_string += "Asset Description\n"
        out_string += "%sName       : %s\n" % (
            out_tab_str, get_as_string(self.name))
        out_string += "%sType       : %d\n" % (
            out_tab_str, self.assetType)
        out_string += "%sID         : %d\n" % (
            out_tab_str, self.assetID)

        rbCount = 0
        out_string += "%sRigidBody (Bone) Count : %d\n" % (
            out_tab_str, len(self.rigidbodyArray))
        for rigidbody in self.rigidbodyArray:
            out_string += "%sRigidBody (Bone) %d:\n" % (out_tab_str, rbCount)
            out_string += rigidbody.get_as_string(tab_str, level+1)
            rbCount += 1

        markerCount = 0
        out_string += "%sMarker Count : %d\n" % (
            out_tab_str, len(self.markerArray))
        for marker in self.markerArray:
            out_string += "%sMarker %d:\n" % (out_tab_str, markerCount)
            out_string += marker.get_as_string(tab_str, level+1)
            markerCount += 1

        return out_string


class IMUDescription:
    """IMU Descriptions Class"""
    order_num = 0

    def __init__(self, name, imu_id, sensor_fused, rb_id):
        self.name = name
        self.imu_id = imu_id
        self.sensor_fused = sensor_fused
        self.rb_id = rb_id

    def get_as_string(self, tab_str="..", level=0):
        """Get IMU Description As String"""
        out_tab_str = get_tab_str(tab_str, level)
        out_string = f"{out_tab_str}IMU Tag Name:   {self.name}\n"
        out_string += f"{out_tab_str}IMU ID:         {self.imu_id}\n"
        out_string += f"{out_tab_str}Sensor Fused:   {self.sensor_fused}\n"
        out_string += f"{out_tab_str}Rigid Body ID:  {self.rb_id}\n"
        return out_string


class GPIODescription:
    """GPIO Descriptions Class"""
    order_num = 0

    def __init__(self, name, gpio_id, port_list=[]):
        self.name = name
        self.gpio_id = gpio_id
        self.port_list = port_list
        self.gpio_count = len(port_list)

    def get_as_string(self, tab_str="..", level=0):
        """Get GPIO Description as String"""
        out_tab_str = get_tab_str(tab_str, level)
        out_string = f"{out_tab_str}GPIO Name:      {self.name}\n"
        out_string += f"{out_tab_str}GPIO ID:        {self.gpio_id}\n"
        out_string += f"{out_tab_str}GPIO Port Count:{self.gpio_count}\n"
        for i, port in enumerate(self.port_list):
            out_string += f"{out_tab_str}GPIO Port {i}: {self.port_list[i]}\n"
        return out_string


# cDataDescriptions
# Full data descriptions
class DataDescriptions():
    """Data Descriptions class"""
    order_num = 0

    def __init__(self):
        self.data_order_dict = {}
        self.marker_set_list = []
        self.rigid_body_list = []
        self.skeleton_list = []
        self.asset_list = []
        self.force_plate_list = []
        self.device_list = []
        self.camera_list = []
        self.imu_list = []
        self.gpio_list = []
        self.anchor_list = []

    def generate_order_name(self):
        """Generate the name for the order list based on the current length of
        the list"""
        # should be a one up counter instead of based on length of
        # data_order_dict
        order_name = f"data_{self.order_num:3d}"
        self.order_num += 1
        return order_name

    # Add Markerset
    def add_marker_set(self, new_marker_set):
        """Add a Markerset"""
        order_name = self.generate_order_name()

        # generate order entry
        pos = len(self.marker_set_list)
        self.data_order_dict[order_name] = ("marker_set_list", pos)
        self.marker_set_list.append(copy.deepcopy(new_marker_set))

    # Add Rigid Body
    def add_rigid_body(self, new_rigid_body):
        """Add a rigid body"""
        order_name = self.generate_order_name()

        # generate order entry
        pos = len(self.rigid_body_list)
        self.data_order_dict[order_name] = ("rigid_body_list", pos)
        self.rigid_body_list.append(copy.deepcopy(new_rigid_body))

    # Add a skeleton
    def add_skeleton(self, new_skeleton):
        """Add a skeleton"""
        order_name = self.generate_order_name()

        # generate order entry
        pos = len(self.skeleton_list)
        self.data_order_dict[order_name] = ("skeleton_list", pos)
        self.skeleton_list.append(copy.deepcopy(new_skeleton))

    # Add an asset
    def add_asset(self, new_asset):
        """Add an asset"""
        order_name = self.generate_order_name()

        # generate order entry
        pos = len(self.asset_list)
        self.data_order_dict[order_name] = ("asset_list", pos)
        self.asset_list.append(copy.deepcopy(new_asset))

    # Add a force plate
    def add_force_plate(self, new_force_plate):
        """Add a force plate"""
        order_name = self.generate_order_name()

        # generate order entry
        pos = len(self.force_plate_list)
        self.data_order_dict[order_name] = ("force_plate_list", pos)
        self.force_plate_list.append(copy.deepcopy(new_force_plate))

    def add_device(self, newdevice):
        """ add_device - Add a device"""
        order_name = self.generate_order_name()

        # generate order entry
        pos = len(self.device_list)
        self.data_order_dict[order_name] = ("device_list", pos)
        self.device_list.append(copy.deepcopy(newdevice))

    def add_camera(self, newcamera):
        """ Add a new camera """
        order_name = self.generate_order_name()

        # generate order entry
        pos = len(self.camera_list)
        self.data_order_dict[order_name] = ("camera_list", pos)
        self.camera_list.append(copy.deepcopy(newcamera))

    def add_imu(self, newimu):
        """Add new IMU"""
        order_name = self.generate_order_name()

        # generate order entry
        pos = len(self.imu_list)
        self.data_order_dict[order_name] = ("imu_list", pos)
        self.imu_list.append(copy.deepcopy(newimu))

    def add_gpio(self, newgpio):
        """Adds new GPIO device"""
        order_name = self.generate_order_name()

        # generate order entry
        pos = len(self.gpio_list)
        self.data_order_dict[order_name] = ("gpio_list", pos)
        self.gpio_list.append(copy.deepcopy(newgpio))

    def add_anchor(self, newanchor):
        """Adds new Anchor"""
        order_name = self.generate_order_name()
        # generate order entry
        pos = len(self.anchor_list)
        self.data_order_dict[order_name] = ("anchor_list", pos)
        self.anchor_list.append(copy.deepcopy(newanchor))

    def add_data(self, new_data):
        """Add data based on data type"""
        data_type = type(new_data)
        if data_type == MarkerSetDescription:
            self.add_marker_set(new_data)
        elif data_type == RigidBodyDescription:
            self.add_rigid_body(new_data)
        elif data_type == SkeletonDescription:
            self.add_skeleton(new_data)
        elif data_type == ForcePlateDescription:
            self.add_force_plate(new_data)
        elif data_type == DeviceDescription:
            self.add_device(new_data)
        elif data_type == CameraDescription:
            self.add_camera(new_data)
        elif data_type == AssetDescription:
            self.add_asset(new_data)
        elif data_type == IMUDescription:
            self.add_imu(new_data)
        elif data_type == GPIODescription:
            self.add_gpio(new_data)
        elif data_type == AnchorDescription:
            self.add_anchor(new_data)
        elif data_type is None:
            data_type = None
        else:
            print("ERROR: Type %s unknown" % str(data_type))

    def get_object_from_list(self, list_name, pos_num):
        """Determine list name and position of the object"""
        ret_value = None
        if (list_name == "marker_set_list") and \
           (pos_num < len(self.marker_set_list)):
            ret_value = self.marker_set_list[pos_num]

        elif (list_name == "rigid_body_list") and \
             (pos_num < len(self.rigid_body_list)):
            ret_value = self.rigid_body_list[pos_num]

        elif (list_name == "skeleton_list") and \
             (pos_num < len(self.skeleton_list)):
            ret_value = self.skeleton_list[pos_num]

        elif (list_name == "asset_list") and \
             (pos_num < len(self.asset_list)):
            ret_value = self.asset_list[pos_num]

        elif (list_name == "force_plate_list") and \
             (pos_num < len(self.force_plate_list)):
            ret_value = self.force_plate_list[pos_num]

        elif (list_name == "device_list") and \
             (pos_num < len(self.device_list)):
            ret_value = self.device_list[pos_num]

        elif (list_name == "camera_list") and \
             (pos_num < len(self.camera_list)):
            ret_value = self.camera_list[pos_num]

        elif (list_name == "imu_list") and \
             (pos_num < len(self.imu_list)):
            ret_value = self.imu_list[pos_num]

        elif (list_name == "gpio_list") and \
             (pos_num < len(self.gpio_list)):
            ret_value = self.gpio_list[pos_num]

        elif (list_name == "anchor_list") and \
             (pos_num < len(self.anchor_list)):
            ret_value = self.anchor_list[pos_num]

        else:
            ret_value = None

        return ret_value

    def get_as_string(self, tab_str="  ", level=0):
        """Ensure data comes back as a string"""
        out_tab_str = get_tab_str(tab_str, level)
        out_tab_str2 = get_tab_str(tab_str, level+1)
        out_tab_str3 = get_tab_str(tab_str, level+2)
        out_string = ""
        num_data_sets = len(self.data_order_dict)
        out_string += "%sDataset Count: %d\n" % (out_tab_str, num_data_sets)
        i = 0
        for tmp_key, tmp_value in self.data_order_dict.items():
            # tmp_name,tmp_num=self.data_order_dict[data_set]
            tmp_name = tmp_value[0]
            tmp_num = tmp_value[1]
            tmp_object = self.get_object_from_list(tmp_name, tmp_num)
            out_string += "%sDataset %3.1d\n" % (out_tab_str2, i)
            tmp_string = get_data_sub_packet_type(tmp_object)
            if tmp_string != "":
                out_string += "%s%s" % (out_tab_str2, tmp_string)
            # outputs keys for looking up objects
            # out_string += "%s%s %s %d\n" % (
            #    out_tab_str2, data_set, tmp_name, tmp_num)
            if tmp_object is not None:
                out_string += tmp_object.get_as_string(tab_str, level+2)
            else:
                out_string += "%s%s %s %s not found\n" % (
                    out_tab_str3, tmp_key, tmp_name, tmp_num)
            out_string += "\n"
            i += 1

        return out_string

# cDataDescriptions END


def generate_marker_set_description(set_num=0):
    """generate_marker_set_description - Testing functions"""
    marker_set_description = MarkerSetDescription()
    marker_set_description.set_name("MarkerSetName%3.3d" % set_num)
    marker_set_description.add_marker_name("MarkerName%3.3d_0" % set_num)
    marker_set_description.add_marker_name("MarkerName%3.3d_1" % set_num)
    marker_set_description.add_marker_name("MarkerName%3.3d_2" % set_num)
    marker_set_description.add_marker_name("MarkerName%3.3d_3" % set_num)
    return marker_set_description


def generate_rb_marker(marker_num=0):
    """generate_rb_marker - Generate rigid body marker based on marker
    number"""
    marker_num_mod = marker_num % 4
    marker_name = "RBMarker_%3.3d" % marker_num
    marker_active_label = marker_num+10000
    marker_pos = [1.0, 4.0, 9.0]
    if marker_num_mod == 1:
        marker_pos = [1.0, 8.0, 27.0]
    elif marker_num_mod == 2:
        marker_pos = [3.1, 4.1, 5.9]
    elif marker_num_mod == 3:
        marker_pos = [1.0, 3.0, 6.0]

    return RBMarker(marker_name, marker_active_label, marker_pos)


def generate_rigid_body_description(rbd_num=0):
    """generate_rigid_body_description - Generate Rigid Body Description
    Data"""
    rbd = RigidBodyDescription()
    rbd.set_name("rigidBodyDescription_%3.3d" % rbd_num)
    rbd.set_id(3141)
    rbd.set_parent_id(314)
    rbd.set_pos(1, 4, 9)
    rbd.add_rb_marker(generate_rb_marker(0))
    rbd.add_rb_marker(generate_rb_marker(1))
    rbd.add_rb_marker(generate_rb_marker(2))

    return rbd


def generate_skeleton_description(skeleton_num=0):
    """generate_skeleton_description -Generate Test SkeletonDescription Data"""
    skel_desc = SkeletonDescription(
        "SkeletonDescription_%3.3d" % skeleton_num, skeleton_num)
    # generate some rigid bodies to add
    skel_desc.add_rigid_body_description(generate_rigid_body_description(0))
    skel_desc.add_rigid_body_description(generate_rigid_body_description(1))
    skel_desc.add_rigid_body_description(generate_rigid_body_description(2))
    skel_desc.add_rigid_body_description(generate_rigid_body_description(3))
    skel_desc.add_rigid_body_description(generate_rigid_body_description(5))
    skel_desc.add_rigid_body_description(generate_rigid_body_description(7))
    return skel_desc


def generate_force_plate_description(force_plate_num=0):
    """generate_force_plate_description - Generate Test ForcePlateDescription
    Data"""
    fp_id = force_plate_num
    random.seed(force_plate_num)

    serial_number = "S/N_%5.5d" % random.randint(0, 99999)
    width = random.random()*10
    length = random.random()*10
    origin = [
        (random.random()*100), (random.random()*100), (random.random() * 100)]
    corners = [[0.0, 0.0, 0.0],
               [0.0, 1.0, 0.0],
               [1.0, 1.0, 0.0],
               [1.0, 0.0, 0.0]]

    fp_desc = ForcePlateDescription(fp_id, serial_number)
    fp_desc.set_dimensions(width, length)
    fp_desc.set_origin(origin[0], origin[1], origin[2])
    # fp_desc.set_cal_matrix(cal_matrix)
    fp_desc.set_corners(corners)
    for i in range(3):
        fp_desc.add_channel_name("channel_%3.3d" % i)
    return fp_desc


def generate_device_description(dev_num=0):
    """generate_device_description- Generate Test DeviceDescription Data"""
    new_id = 0
    name = "Device%3.3d" % dev_num
    serial_number = "SerialNumber%3.3d" % dev_num
    device_type = dev_num % 4
    channel_data_type = dev_num % 5
    dev_desc = DeviceDescription(
        new_id, name, serial_number, device_type, channel_data_type)
    for i in range(channel_data_type+3):
        dev_desc.add_channel_name("channel_name_%2.2d" % i)
    return dev_desc


def generate_imu_description(imu_num=0):
    name = f"IMU_{imu_num:d}"
    new_id = imu_num
    sensor_fused = 0
    rb_id = 0
    imu_desc = IMUDescription(name, new_id, sensor_fused, rb_id)
    return imu_desc


def generate_gpio_description(gpio_num=0):
    name = f"GPIO_{gpio_num:d}"
    gpio_id = gpio_num + 123
    port_list = [1.0, 3.0, 0.0]
    gpio_desc = GPIODescription(name, gpio_id, port_list)
    return gpio_desc


def generate_camera_description(cam_num=0):
    """generate_camera_description - Generate Test CameraDescription data"""
    pos_vec3 = [1, 2, 3]
    orientation_quat = [1, 2, 3, 4]
    return CameraDescription(
        f"Camera_{cam_num:3d}", pos_vec3, orientation_quat)


def generate_anchor_description(anchor_num=0):
    """generate_anchor_description - Generate Test AnchorDescription data"""
    name = f"Anchor_{anchor_num}"
    pos_vec3 = [2.2, 4.4, 2.2]
    id = f"{anchor_num}"
    anchor_desc = AnchorDescription(name, pos_vec3, id)
    return anchor_desc


# generate_data_descriptions - Generate Test DataDescriptions
def generate_data_descriptions(data_desc_num=0):
    """Generate data descriptions"""
    data_descs = DataDescriptions()

    data_descs.add_data(generate_marker_set_description(data_desc_num+0))
    data_descs.add_data(generate_marker_set_description(data_desc_num+1))

    data_descs.add_data(generate_rigid_body_description(data_desc_num+0))
    data_descs.add_data(generate_rigid_body_description(data_desc_num+1))

    data_descs.add_skeleton(generate_skeleton_description(data_desc_num+3))
    data_descs.add_skeleton(generate_skeleton_description(data_desc_num+9))
    data_descs.add_skeleton(generate_skeleton_description(data_desc_num+27))

    data_descs.add_force_plate(
        generate_force_plate_description(data_desc_num+123))
    data_descs.add_force_plate(
        generate_force_plate_description(data_desc_num+87))
    data_descs.add_force_plate(
        generate_force_plate_description(data_desc_num+21))

    data_descs.add_device(generate_device_description(data_desc_num+0))
    data_descs.add_device(generate_device_description(data_desc_num+2))
    data_descs.add_device(generate_device_description(data_desc_num+4))

    data_descs.add_camera(generate_camera_description(data_desc_num+0))
    data_descs.add_camera(generate_camera_description(data_desc_num+10))
    data_descs.add_camera(generate_camera_description(data_desc_num+3))
    data_descs.add_camera(generate_camera_description(data_desc_num+7))
    return data_descs


# test_all - Test all the major classes
def test_all(run_test=True):
    """Test all the Data Description classes"""
    totals = [0, 0, 0]
    if run_test is True:
        test_cases = [
                    ["Test Markerset Description 0",
                     "d918228cc347bd0dac69dd02b1a5375a4421364f",
                     "generate_marker_set_description(0)", True],
                    ["Test RB Marker 0",
                     "df582ca7b764d889041b59ceb6a43251b68ca3be",
                     "generate_rb_marker(0)",
                     True],
                    ["Test Rigid Body Description 0",
                     "0ea7085657c391efe2fd349cc03f242247efbbe4",
                     "generate_rigid_body_description(0)",
                     True],
                    ["Test Skeleton Description 0",
                     "fa2a59e76f31c1d884f6554fe13e5cfcf31e703c",
                     "generate_skeleton_description(0)",
                     True],
                    ["Test Force Plate Description 0",
                     "798793a2fed302bc472b2636beff959901214be2",
                     "generate_force_plate_description(0)",
                     True],
                    ["Test Device Description 0",
                     "39b4fdda402bc73c0b1cd5c7f61599476aa9a926",
                     "generate_device_description(0)",
                     True],
                    ["Test Camera Description 0",
                     "614602c5d290bda3b288138d5e25516dd1e1e85a",
                     "generate_camera_description(0)",
                     True],
                    ["Test IMU Description 0",
                     "6497d51e81e40c4a36cb16285882c00bbf15fb75",
                     "generate_imu_description(0)", True],
                    ["Test Anchor Description 0",
                     "b331d578474570bf9b8b48f9d2b586eeedeb631a",
                     "generate_anchor_description(0)", True],
                    ["Test Data Description 0",
                     "98497c0ad84d3e002702aced7607602972ce4849",
                     "generate_data_descriptions(0)", True],
                    ["Test Data Description 0",
                     "b2fcffb251ae526e91ec9f65f5f2137f0d74db49",
                     "generate_data_descriptions(0)",
                     True],
                    ]
        num_tests = len(test_cases)
        for i in range(num_tests):
            data = eval(test_cases[i][2])
            totals_tmp = test_hash2(
                test_cases[i][0], test_cases[i][1], data,
                test_cases[i][2], test_cases[i][3])
            totals = add_lists(totals, totals_tmp)

    print("--------------------")
    print("[PASS] Count = %3.1d" % totals[0])
    print("[FAIL] Count = %3.1d" % totals[1])
    print("[SKIP] Count = %3.1d" % totals[2])

    return totals


if __name__ == "__main__":
    test_all(True)
