Source code for mds_api.mds_utils

"""
   Utility functions of MDS API
"""
import numpy
import re



def get_degrees(deg: str) -> float:
    """ Utility function: get degrees in float format. 
        If you give argument as float, you get the same float.
        If you give argument as string, it must be degrees-minutes-seconds and E or W or N or S in the end. 
            You then get a numeric representation of it as float. For example: 45o35'43''E or 33deg15'53''S 
    """
    if (isinstance(deg, float)):
        return deg
    elif (isinstance(deg, int)):
        return float(deg)
    elif (isinstance(deg, str)):
        last_symbol = deg.upper()[-1]
        deg_parts = list(map(int, re.findall(r'\d+', deg)))
        result_deg = 0
        if len(deg_parts) > 0:
            result_deg = (float(deg_parts[0]))
        if len(deg_parts) > 1:
            result_deg += (float(deg_parts[1])) / 60.0
        if len(deg_parts) > 2:
            result_deg += (float(deg_parts[2])) / 3600.0

        if (last_symbol == 'S') or (last_symbol == 'W'):
            result_deg = -result_deg

       # print(f"===== DEGREES: input = {deg} output = {result_deg}")
        return result_deg
        # split the string by non-numeric characters
    else:
        raise TypeError("Error: deg must be float or string")

    


def extract_pos_vel(dict):
    """
        extract position and velocity from config dict.

        Return tuple (name, position, velocity)
    """
    name = dict["name"]
    pos = dict["localPosition"]
    vel = dict["localVelocity"]
    print(name + ":")
    print("Local position = " + pos + " m")
    print("Local velocity = " + vel + " m/s")

    return name, string_to_vector(pos), string_to_vector(vel)



def vector_to_string(vec):
    """
        :meta private:
    """
    string = "(" + str(vec[0]) + ", " + str(vec[1]) + ", " + str(vec[2]) + ")"

    return string


def quaternion_to_string(quat):
    """
        :meta private:
    """
    string = "(" + str(quat[0]) + ", " + str(quat[1]) + ", " + str(quat[2]) + ", " + str(quat[3]) + ")"

    return string




def string_to_vector(str):
    """
        Conversion from string to vector. Converts to numpy array.
    """
    vec = str.strip('()')
    vec = vec.split(',')
    vec = [float(i) for i in vec]
    return numpy.array(vec, dtype=numpy.float64)





def string_to_quaternion(str):
    """
        Conversion from string to quaternion
    """
    quat = str.strip('()')
    quat = quat.split(',')
    quat = [float(i) for i in quat]

    return numpy.array(quat, dtype=numpy.float64)




def quaternion_multiply(q1, q2):
    """ Multiply quaternions
    """
    # For scalar last quaternions
    x1, y1, z1, w1 = q1
    x2, y2, z2, w2 = q2
    x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
    y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
    z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
    w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
    return numpy.array([x, y, z, w], dtype=numpy.float64)




def quaternion_conjugate(q):
    x, y, z, w = q
    return numpy.array([-x, -y, -z, w], dtype=numpy.float64)


def rotate_vector_by_quaternion(vector, quaternion):
    # Vector as pure quaternion
    vec_quat = [vector[0], vector[1], vector[2], 0]
    # Apply rotation
    q_conj = quaternion_conjugate(quaternion)
    tmp = quaternion_multiply(quaternion, vec_quat)
    rotated = quaternion_multiply(tmp, q_conj)
    # Return only the vector part
    return rotated[:3]