import math
import numpy as np

# Normalize a 1 dimension vector
def norm1d(a):
    if len(a.shape) != 1:
        return -1
    else:
        l = a.shape[0]
        s = 0 #sum
        for i in range(0,l):
            s+=a[i]**2
        s = math.sqrt(s)
        v = np.zeros(l)
        for i in range(0,l):
            v[i] = a[i]/s
        return v
    
# Converting a vector from Cartesian coordianate to spherical
# theta:0~180 (zenith), phi: 0~180 for left, 0~-180 for right (azimuth)
def to_sphere(a):
    x = a[0]
    y = a[1]
    z = a[2]
    r = math.sqrt(x**2+y**2+z**2)
    if r == 0:
        return (0.0,0.0,0.0)
    theta = math.degrees(math.acos(z/r))
    # phi is from -180~+180
    if x != 0.0:
        phi = math.degrees(math.atan(y/x))
    else:
        if y > 0:
            phi = 90
        else:
            phi = -90
    if x < 0 and y > 0:
        phi = 180+phi
    elif x < 0 and y < 0:
        phi = -180+phi
    else:
        phi = phi
    return (r, theta, phi)

def coordinate2laban(theta, phi):
    laban = ['Forward', 'Normal']
    
    #find direction, phi, (-180,180]
    #forward
    if (phi <= 22.5 and phi >= 0) or (phi < 0 and phi > -22.5):
        laban[0] = 'Forward'
    elif (phi <= 67.5 and phi > 22.5):
        laban[0] = 'Left Forward'
    elif (phi <= 112.5 and phi > 67.5):
        laban[0] = 'Left'
    elif (phi <= 157.5 and phi > 112.5):
        laban[0] = 'Left Backward'
    elif (phi <= -157.5 and phi > -180) or (phi <= 180 and phi > 157.5):
        laban[0] = 'Backward'
    elif (phi <= -112.5 and phi > -157.5):
        laban[0] = 'Right Backward'
    elif (phi <= -67.5 and phi > -112.5):
        laban[0] = 'Right'
    else:
        laban[0] = 'Right Forward'
    
    # find height, theta, [0,180]
    # place high
    if theta < 22.5:
        laban=['Place','High']
    # high
    elif theta < 67.5:
        laban[1] = 'High'
    # normal/mid
    elif theta < 112.5:
        laban[1] = 'Normal'
    # low
    elif theta < 157.5:
        laban[1] = 'Low'
    # place low
    else:
        laban = ['Place','Low']

    return laban

def raw2sphere(joint_xyz_array):
    wrist = joint_xyz_array[20]
    thumb_cmc = joint_xyz_array[3]
    thumb_mcp = joint_xyz_array[2]
    thumb_ip = joint_xyz_array[1]
    thumb_tip = joint_xyz_array[0]
    index_finger_mcp = joint_xyz_array[7]
    index_finger_pip = joint_xyz_array[6]
    index_finger_dip = joint_xyz_array[5]
    index_finger_tip = joint_xyz_array[4]
    middle_finger_mcp = joint_xyz_array[11]
    middle_finger_pip = joint_xyz_array[10]
    middle_finger_dip = joint_xyz_array[9]
    middle_finger_tip = joint_xyz_array[8]
    ring_finger_mcp = joint_xyz_array[15]
    ring_finger_pip = joint_xyz_array[14]
    ring_finger_dip = joint_xyz_array[13]
    ring_finger_tip = joint_xyz_array[12]
    pinky_mcp = joint_xyz_array[19]
    pinky_pip = joint_xyz_array[18]
    pinky_dip = joint_xyz_array[17]
    pinky_tip = joint_xyz_array[16]
    
    palm = middle_finger_mcp - wrist
    wrist_thumb_cmc = thumb_cmc - wrist
    thumb_cmc_mcp = thumb_mcp - thumb_cmc
    thumb_mcp_ip = thumb_ip - thumb_mcp
    thumb_ip_tip = thumb_tip - thumb_ip
    index_finger_mcp_pip = index_finger_pip - index_finger_mcp
    index_finger_pip_dip = index_finger_dip - index_finger_pip
    index_finger_dip_tip = index_finger_tip - index_finger_dip
    middle_index = index_finger_mcp - middle_finger_mcp
    middle_finger_mcp_pip = middle_finger_pip - middle_finger_mcp
    middle_finger_pip_dip = middle_finger_dip - middle_finger_pip
    middle_finger_dip_tip = middle_finger_tip - middle_finger_dip
    ring_finger_mcp_pip = ring_finger_pip - ring_finger_mcp
    ring_finger_pip_dip = ring_finger_dip - ring_finger_pip
    ring_finger_dip_tip = ring_finger_tip - ring_finger_dip
    middle_ring = ring_finger_mcp - middle_finger_mcp
    pinky_mcp_pip = pinky_pip - pinky_mcp
    pinky_pip_dip = pinky_dip - pinky_pip
    pinky_dip_tip = pinky_tip - pinky_dip
    ring_pinky = pinky_mcp - ring_finger_mcp
    
    sh = np.zeros((3, 3))
    v1 = middle_finger_mcp - wrist
    v2 = index_finger_mcp - pinky_mcp
    sh[0] = np.cross(v1, v2)    #x axis
    sh[1] = v1  #y axis
    sh[2] = np.cross(sh[0], sh[1])  #z axis
    nv = np.zeros((3, 3))
    nv[0] = norm1d(sh[0])
    nv[1] = norm1d(sh[1])
    nv[2] = norm1d(sh[2])
    # generate the rotation matrix for
    conv = np.linalg.inv(np.transpose(nv))
    palm_sph = to_sphere(np.dot(conv, palm))
    wrist_thumb_cmc_sph = to_sphere(np.dot(conv, wrist_thumb_cmc))
    thumb_cmc_mcp_sph = to_sphere(np.dot(conv, thumb_cmc_mcp))
    thumb_mcp_ip_sph = to_sphere(np.dot(conv, thumb_mcp_ip))
    thumb_ip_tip_sph = to_sphere(np.dot(conv, thumb_ip_tip))
    index_finger_mcp_pip_sph = to_sphere(np.dot(conv, index_finger_mcp_pip))
    index_finger_pip_dip_sph = to_sphere(np.dot(conv, index_finger_pip_dip))
    index_finger_dip_tip_sph = to_sphere(np.dot(conv, index_finger_dip_tip))
    middle_index_sph = to_sphere(np.dot(conv, middle_index))
    middle_finger_mcp_pip_sph = to_sphere(np.dot(conv, middle_finger_mcp_pip))
    middle_finger_pip_dip_sph = to_sphere(np.dot(conv, middle_finger_pip_dip))
    middle_finger_dip_tip_sph = to_sphere(np.dot(conv, middle_finger_dip_tip))
    ring_finger_mcp_pip_sph = to_sphere(np.dot(conv, ring_finger_mcp_pip))
    ring_finger_pip_dip_sph = to_sphere(np.dot(conv, ring_finger_pip_dip))
    ring_finger_dip_tip_sph = to_sphere(np.dot(conv, ring_finger_dip_tip))
    middle_ring_sph = to_sphere(np.dot(conv, middle_ring))
    pinky_mcp_pip_sph = to_sphere(np.dot(conv, pinky_mcp_pip))
    pinky_pip_dip_sph = to_sphere(np.dot(conv, pinky_pip_dip))
    pinky_dip_tip_sph = to_sphere(np.dot(conv, pinky_dip_tip))
    ring_pinky_sph = to_sphere(np.dot(conv, ring_pinky))
    
    return {
        'palm': palm_sph, 'wrist thumb': wrist_thumb_cmc_sph, 'thumb_cmc mcp': thumb_cmc_mcp_sph, 'thumb_mcp ip': thumb_mcp_ip_sph,
        'thumb ip_tip': thumb_ip_tip_sph, 'index_mcp pip': index_finger_mcp_pip_sph, 'index_pip dip': index_finger_pip_dip_sph,
        'index_dip tip': index_finger_dip_tip_sph, 'middle index': middle_index_sph, 'middle_mcp pip': middle_finger_mcp_pip_sph,
        'middle_pip dip': middle_finger_pip_dip_sph, 'middle_dip tip': middle_finger_dip_tip_sph, 'ring_mcp pip': ring_finger_mcp_pip_sph,
        'ring_pip dip': ring_finger_pip_dip_sph, 'ring_dip tip': ring_finger_dip_tip_sph, 'middle ring': middle_ring_sph,
        'pinky_mcp pip': pinky_mcp_pip_sph, 'pinky_pip dip': pinky_pip_dip_sph, 'pinky_dip tip': pinky_dip_tip_sph, 'ring pinky': ring_pinky_sph
    }
    
def xyz2laban(joint_xyz_list):
    joint_xyz_array = np.array(joint_xyz_list)
    res_angle_dict = raw2sphere(joint_xyz_array)
    res_laban_dict = {}
    for key in res_angle_dict:
        res_laban_dict[key] = coordinate2laban(res_angle_dict[key][1], res_angle_dict[key][2])
    return res_laban_dict