import sys
import os.path
import inspect
import numpy as np
import math

from SofaPython import Quaternion

def frameLandmarks(frame):
    """ @return a list of the required landmarks to compute @a frame
    """
    frameFunc = getattr(sys.modules[__name__], frame)
    return inspect.getargspec(frameFunc).args

def compute(frame, landmarks, missingLandmarks=None):
    """ compute the requested frame based on landmarks
    deduce landmarks names from frame() argument names and call frame() with the positions stored in landmarks dictionnary
    append the missing landmarks name to missingLandmarks if not None
    """
    args={}
    frameFunc = getattr(sys.modules[__name__], frame)
    canCompute=True
    for landmarkName in inspect.getargspec(frameFunc).args:
        if not landmarkName in landmarks:
            if not missingLandmarks is None:
                missingLandmarks.append(landmarkName)
            else:
                print "boneFrameISB.compute - missing landmark:", landmarkName
            canCompute=False
            continue
        args[landmarkName] = landmarks[landmarkName]
    if canCompute:
        return frameFunc(**args)
    else:
        return None

def list():
    """ @return a list of the defined frames
    """
    frames=[]
    for (name,func) in inspect.getmembers(sys.modules[__name__], inspect.isfunction):
        # remove exceptions
        if name[0]=="_" or name in ("list", "compute", "frameLandmarks", "LowerVertebra", "UpperVertebra"):
            continue
        frames.append(name)
    return frames

# returns the femur coordinate systems based on ISB recommendations
# inputs: hip joint center (HJC), left/right anterior superior iliac spine, left/right posterior superior iliac spine
# output: center+quaternion
def _Pelvis_Hip(HJC,r_ASIS,l_ASIS,r_PSIS,l_PSIS):
    m_PSIS = (r_PSIS+l_PSIS)*0.5
    w=r_ASIS-l_ASIS
    w/=np.linalg.norm(w)
    v=np.cross(r_ASIS-m_PSIS,l_ASIS-m_PSIS)
    v/=np.linalg.norm(v)
    u=np.cross(v,w)
    q= Quaternion.from_matrix(np.array([u,v,w]).transpose())
    return [HJC[0],HJC[1],HJC[2],q[0],q[1],q[2],q[3]]

def Pelvis_Right_Hip(Cotyle_R, anterior_superior_iliac_spine_R, anterior_superior_iliac_spine_L, posterior_superior_iliac_spine_R, posterior_superior_iliac_spine_L):
    return _Pelvis_Hip(Cotyle_R, anterior_superior_iliac_spine_R, anterior_superior_iliac_spine_L, posterior_superior_iliac_spine_R, posterior_superior_iliac_spine_L)

def Pelvis_Left_Hip(Cotyle_L, anterior_superior_iliac_spine_R, anterior_superior_iliac_spine_L, posterior_superior_iliac_spine_R, posterior_superior_iliac_spine_L):
    return _Pelvis_Hip(Cotyle_L, anterior_superior_iliac_spine_R, anterior_superior_iliac_spine_L, posterior_superior_iliac_spine_R, posterior_superior_iliac_spine_L)



# returns the femur coordinate systems based on ISB recommendations
# inputs: hip joint center (HJC), lateral and medial femoral epicondyles (FE_L,FE_M), and side (eg. "right")
# output: center+quaternion
def _Femur_Knee(HJC,FE_L,FE_M,side):
    MFE = (FE_L+FE_M)*0.5
    v=HJC-MFE
    v/=np.linalg.norm(v)
    u=np.cross(HJC-FE_L,HJC-FE_M)
    u/=np.linalg.norm(u)
    if side!="right":
        u=-u
    w=np.cross(u,v)
    q=Quaternion.from_matrix(np.array([u,v,w]).transpose())
    return [MFE[0],MFE[1],MFE[2],q[0],q[1],q[2],q[3]]

def _Femur_Hip(HJC,FE_L,FE_M,side):
    jcs = _Femur_Knee(HJC,FE_L,FE_M,side)
    return [HJC[0],HJC[1],HJC[2],jcs[3],jcs[4],jcs[5],jcs[6]]

def Right_Femur_Hip(femoral_head_center_R, lateral_femoral_epicondyles_R, medial_femoral_epicondyles_R):
    return _Femur_Hip(femoral_head_center_R, lateral_femoral_epicondyles_R, medial_femoral_epicondyles_R,"right")

def Left_Femur_Hip(femoral_head_center_L, lateral_femoral_epicondyles_L, medial_femoral_epicondyles_L):
    return _Femur_Hip(femoral_head_center_L, lateral_femoral_epicondyles_L, medial_femoral_epicondyles_L,"left")

# returns the tibia ankle coordinate system based on Grood & Suntay 83
# inputs: lateral and medial femoral epicondyles (FE_L,FE_M), Tip of the medial and lateral malleolus (MM and LM), The most medial and lateral points on the border of the tibial condyle (MC and LC) and side (eg. "right")
# output: center+quaternion
def _Tibia_Knee(FE_L,FE_M,LC,MC,LM,MM,side):
    MFE = (FE_L+FE_M)*0.5
    IC = (MC+LC)*0.5
    IM = (LM+MM)*0.5
    v=IC-IM
    v/=np.linalg.norm(v)
    u=np.cross(MC-IM,LC-IM)
    u/=np.linalg.norm(u)
    if side!="right":
        u=-u
    w=np.cross(u,v)
    q=Quaternion.from_matrix(np.array([u,v,w]).transpose())
    return [MFE[0],MFE[1],MFE[2],q[0],q[1],q[2],q[3]]

# returns the tibia ankle coordinate system based on ISB recommendations
# inputs: Tip of the medial and lateral malleolus (MM and LM), The most medial and lateral points on the border of the tibial condyle (MC and LC) and side (eg. "right")
# output: center+quaternion
def _Tibia_Ankle(LC,MC,LM,MM,side):
    IC = (MC+LC)*0.5
    IM = (LM+MM)*0.5
    w=LM-MM
    w/=np.linalg.norm(w)
    u=np.cross(IC-LM,IC-MM)
    u/=np.linalg.norm(u)
    if side!="right":
        w=-w
        u=-u
    v=np.cross(w,u)
    q=Quaternion.from_matrix(np.array([u,v,w]).transpose())
    return [IM[0],IM[1],IM[2],q[0],q[1],q[2],q[3]]

# returns a patella coordinate system based on 4 landmarks
# inputs: proximal,distal,lateral and medial landmarks, and side (eg. "right")
# output: center+quaternion
def _Patella(patD,patP,patL,patM,side):
    center = (patD+patP+patL+patM)*0.25
    w=patL-patM
    w/=np.linalg.norm(w)
    u=np.cross(patM-patD,patL-patD)
    u/=np.linalg.norm(u)
    if side!="right":
        w=-w
        u=-u
    v=np.cross(w,u)
    q=Quaternion.from_matrix(np.array([u,v,w]).transpose())
    return [center[0],center[1],center[2],q[0],q[1],q[2],q[3]]

def _Scapula(GH, AA, AI, TS, side):
    """ compute the scapula coordinate system based on ISB recommendation
    /!\ change the joint center from AA to GH, seems to be a mistake in the paper
    """
    w = AA-TS
    w/=np.linalg.norm(w)
    if side!="right":
        w=-w
    u=np.cross(AA-TS, AI-TS)
    u/=np.linalg.norm(u)
    if side!="right":
        u=-u
    v=np.cross(w,u)
    q=Quaternion.from_matrix(np.array([u,v,w]).transpose())
    return [GH[0],GH[1],GH[2],q[0],q[1],q[2],q[3]]

def Right_Scapula(humeral_head_center_R, angulus_acromialis_R, scapulae_angulus_inferior_R, trigonum_scapulae_R):
    return _Scapula(humeral_head_center_R, angulus_acromialis_R, scapulae_angulus_inferior_R, trigonum_scapulae_R,"right")

def Left_Scapula(humeral_head_center_L, angulus_acromialis_L, scapulae_angulus_inferior_L, trigonum_scapulae_L):
    return _Scapula(humeral_head_center_L, angulus_acromialis_L, scapulae_angulus_inferior_L, trigonum_scapulae_L, "left")

def _Humerus(GH, EL, EM, US):
    """ compute the humerus coordinate system based on ISB recommendation (2nd option)
    output: center+quaternion
    """
    IE=(EL+EM)*0.5
    v=GH-IE
    v/=np.linalg.norm(v)
    Yf=IE-US
    w=np.cross(v,Yf)
    w/=np.linalg.norm(w)
    u=np.cross(v,w)
    q=Quaternion.from_matrix(np.array([u,v,w]).transpose())
    return [GH[0],GH[1],GH[2],q[0],q[1],q[2],q[3]]

def Right_Humerus(humeral_head_center_R, lateral_humeral_epicondyle_R, medial_humeral_epicondyle_R, ulnar_styloid_R):
    return _Humerus(humeral_head_center_R, lateral_humeral_epicondyle_R, medial_humeral_epicondyle_R, ulnar_styloid_R)
def Left_Humerus(humeral_head_center_L, lateral_humeral_epicondyle_L, medial_humeral_epicondyle_L, ulnar_styloid_L):
    return _Humerus(humeral_head_center_L, lateral_humeral_epicondyle_L, medial_humeral_epicondyle_L, ulnar_styloid_L)

def LowerVertebra(PUP, AUP, U_PLP, U_ALP):
    """ compute vertebra coordinate system (not based on ISB)
    landmarks: Posterior/Anterior Lower/Upper Plate
    U_ upper vertebra
    """
    origin = (PUP+AUP+U_PLP+U_ALP)*0.25
    u=AUP-PUP
    u/=np.linalg.norm(u)
    v=U_PLP-PUP
    v/=np.linalg.norm(v)
    w=np.cross(u,v)
    q=Quaternion.from_matrix(np.array([u,v,w]).transpose())
    return [origin[0],origin[1],origin[2],q[0],q[1],q[2],q[3]]

def UpperVertebra(PLP, ALP, L_PUP, L_AUP):
    """ compute vertebra coordinate system (not based on ISB)
    landmarks: Posterior/Anterior Lower/Upper Plate
    L_ lower vertebra
    """
    origin = (L_PUP+L_AUP+PLP+ALP)*0.25
    u=ALP-PLP
    u/=np.linalg.norm(u)
    v=PLP-L_PUP
    v/=np.linalg.norm(v)
    w=np.cross(u,v)
    q=Quaternion.from_matrix(np.array([u,v,w]).transpose())
    return [origin[0],origin[1],origin[2],q[0],q[1],q[2],q[3]]

def Pelvis_Tilt(femoral_head_center_R, femoral_head_center_L, L5_posterior_upper_plate, L5_anterior_upper_plate):
    """Boulay et al., 2006
    """
    origin = (femoral_head_center_R+femoral_head_center_L)*0.5
    mid_sacral = (L5_posterior_upper_plate+L5_anterior_upper_plate)*0.5
    w=mid_sacral-origin
    w/=np.linalg.norm(w)
    v=np.array([0,1,0])
    u=np.cross(v,w)
    q=Quaternion.from_matrix(np.array([u,v,w]).transpose())
    return [origin[0],origin[1],origin[2],q[0],q[1],q[2],q[3]]

def Pelvis_World(femoral_head_center_R, femoral_head_center_L):
    """Pelvis reference with world axis
    TODO: medio-lateral axis
    """
    origin = (femoral_head_center_R+femoral_head_center_L)*0.5
    return [origin[0],origin[1],origin[2],0,0,0,1]
