io_import_scene_mhx.py

        ('PTongue', (0,0.0))], 
    'SH' : [('PMouth', (-0.6,0)),
        ('PUpLip', (0,-0.5)),
        ('PLoLip', (0,0.5)),
        ('PJaw', (0,0)),
        ('PTongue', (0,0.0))], 
    'EE' : [('PMouth', (0.3,0)),
        ('PUpLip', (0,-0.3)),
        ('PLoLip', (0,0.3)),
        ('PJaw', (0,0.025)),
        ('PTongue', (0,0.0))], 
    'AH' : [('PMouth', (-0.1,0)),
        ('PUpLip', (0,-0.4)),
        ('PLoLip', (0,0)),
        ('PJaw', (0,0.35)),
        ('PTongue', (0,0.0))], 
    'EH' : [('PMouth', (0.1,0)),
        ('PUpLip', (0,-0.2)),
        ('PLoLip', (0,0.2)),
        ('PJaw', (0,0.2)),
        ('PTongue', (0,0.0))], 
    'TH' : [('PMouth', (0,0)),
        ('PUpLip', (0,-0.5)),
        ('PLoLip', (0,0.5)),
        ('PJaw', (-0.2,0.1)),
        ('PTongue', (0,-0.6))], 
    'L' : [('PMouth', (0,0)),
        ('PUpLip', (0,-0.2)),
        ('PLoLip', (0,0.2)),
        ('PJaw', (0.2,0.2)),
        ('PTongue', (0,-0.8))], 
    'G' : [('PMouth', (0,0)),
        ('PUpLip', (0,-0.1)),
        ('PLoLip', (0,0.1)),
        ('PJaw', (-0.3,0.1)),
        ('PTongue', (0,-0.6))], 

    'Blink' : [('PUpLid', (0,1.0)), ('PLoLid', (0,-1.0))], 
    'Unblink' : [('PUpLid', (0,0)), ('PLoLid', (0,0))], 
})
"""
bodyLanguageVisemes = ({
    'Rest' : [
        ('MouthWidth_L', 0), 
        ('MouthWidth_R', 0), 
        ('MouthNarrow_L', 0), 
        ('MouthNarrow_R', 0), 
        ('LipsPart', 0.6), 
        ('UpLipsMidHeight', 0), 
        ('LoLipsMidHeight', 0), 
        ('LoLipsIn', 0),
        ('MouthOpen', 0), 
        ('TongueBackHeight', 0),
        ('TongueHeight', 0),
        ], 
    'Etc' : [
        ('MouthWidth_L', 0), 
        ('MouthWidth_R', 0), 
        ('MouthNarrow_L', 0), 
        ('MouthNarrow_R', 0), 
        ('LipsPart', 0.4), 
        ('UpLipsMidHeight', 0), 
        ('LoLipsMidHeight', 0), 
        ('LoLipsIn', 0),
        ('MouthOpen', 0), 
        ('TongueBackHeight', 0),
        ('TongueHeight', 0),
        ], 
    'MBP' : [
        ('MouthWidth_L', 0), 
        ('MouthWidth_R', 0), 
        ('MouthNarrow_L', 0), 
        ('MouthNarrow_R', 0), 
        ('LipsPart', 0), 
        ('UpLipsMidHeight', 0), 
        ('LoLipsMidHeight', 0), 
        ('LoLipsIn', 0),
        ('MouthOpen', 0), 
        ('TongueBackHeight', 0),
        ('TongueHeight', 0),
        ], 
    'OO' : [
        ('MouthWidth_L', 0), 
        ('MouthWidth_R', 0), 
        ('MouthNarrow_L', 1.0), 
        ('MouthNarrow_R', 1.0), 
        ('LipsPart', 0), 
        ('UpLipsMidHeight', 0), 
        ('LoLipsMidHeight', 0), 
        ('LoLipsIn', 0),
        ('MouthOpen', 0.4), 
        ('TongueBackHeight', 0),
        ('TongueHeight', 0),
        ], 
    'O' : [
        ('MouthWidth_L', 0), 
        ('MouthWidth_R', 0), 
        ('MouthNarrow_L', 0.9), 
        ('MouthNarrow_R', 0.9), 
        ('LipsPart', 0), 
        ('UpLipsMidHeight', 0), 
        ('LoLipsMidHeight', 0), 
        ('LoLipsIn', 0),
        ('MouthOpen', 0.8), 
        ('TongueBackHeight', 0),
        ('TongueHeight', 0),
        ], 
    'R' : [
        ('MouthWidth_L', 0), 
        ('MouthWidth_R', 0), 
        ('MouthNarrow_L', 0.5), 
        ('MouthNarrow_R', 0.5), 
        ('LipsPart', 0), 
        ('UpLipsMidHeight', 0.2), 
        ('LoLipsMidHeight', -0.2), 
        ('LoLipsIn', 0),
        ('MouthOpen', 0), 
        ('TongueBackHeight', 0),
        ('TongueHeight', 0),
        ], 
    'FV' : [
        ('MouthWidth_L', 0.2), 
        ('MouthWidth_R', 0.2), 
        ('MouthNarrow_L', 0), 
        ('MouthNarrow_R', 0), 
        ('LipsPart', 1.0), 
        ('UpLipsMidHeight', 0), 
        ('LoLipsMidHeight', 0.3), 
        ('LoLipsIn', 0.6),
        ('MouthOpen', 0), 
        ('TongueBackHeight', 0),
        ('TongueHeight', 0),
        ], 
    'S' : [
        ('MouthWidth_L', 0), 
        ('MouthWidth_R', 0), 
        ('MouthNarrow_L', 0), 
        ('MouthNarrow_R', 0), 
        ('LipsPart', 0), 
        ('UpLipsMidHeight', 0.5), 
        ('LoLipsMidHeight', -0.7), 
        ('LoLipsIn', 0),
        ('MouthOpen', 0), 
        ('TongueBackHeight', 0),
        ('TongueHeight', 0),
        ], 
    'SH' : [
        ('MouthWidth_L', 0.8), 
        ('MouthWidth_R', 0.8), 
        ('MouthNarrow_L', 0), 
        ('MouthNarrow_R', 0), 
        ('LipsPart', 0), 
        ('UpLipsMidHeight', 1.0), 
        ('LoLipsMidHeight', 0), 
        ('LoLipsIn', 0),
        ('MouthOpen', 0), 
        ('TongueBackHeight', 0),
        ('TongueHeight', 0),
        ], 
    'EE' : [
        ('MouthWidth_L', 0.2), 
        ('MouthWidth_R', 0.2), 
        ('MouthNarrow_L', 0), 
        ('MouthNarrow_R', 0), 
        ('LipsPart', 0), 
        ('UpLipsMidHeight', 0.6), 
        ('LoLipsMidHeight', -0.6), 
        ('LoLipsIn', 0),
        ('MouthOpen', 0.5), 
        ('TongueBackHeight', 0),
        ('TongueHeight', 0),
        ], 
    'AH' : [
        ('MouthWidth_L', 0), 
        ('MouthWidth_R', 0), 
        ('MouthNarrow_L', 0), 
        ('MouthNarrow_R', 0), 
        ('LipsPart', 0), 
        ('UpLipsMidHeight', 0.4), 
        ('LoLipsMidHeight', 0), 
        ('LoLipsIn', 0),
        ('MouthOpen', 0.7), 
        ('TongueBackHeight', 0),
        ('TongueHeight', 0),
        ], 
    'EH' : [
        ('MouthWidth_L', 0), 
        ('MouthWidth_R', 0), 
        ('MouthNarrow_L', 0), 
        ('MouthNarrow_R', 0), 
        ('LipsPart', 0), 
        ('UpLipsMidHeight', 0.5), 
        ('LoLipsMidHeight', -0.6), 
        ('LoLipsIn', 0),
        ('MouthOpen', 0.25), 
        ('TongueBackHeight', 0),
        ('TongueHeight', 0),
        ], 
    'TH' : [
        ('MouthWidth_L', 0), 
        ('MouthWidth_R', 0), 
        ('MouthNarrow_L', 0), 
        ('MouthNarrow_R', 0), 
        ('LipsPart', 0), 
        ('UpLipsMidHeight', 0), 
        ('LoLipsMidHeight', 0), 
        ('LoLipsIn', 0),
        ('MouthOpen', 0.2), 
        ('TongueBackHeight', 1.0),
        ('TongueHeight', 1.0)
        ], 
    'L' : [
        ('MouthWidth_L', 0), 
        ('MouthWidth_R', 0), 
        ('MouthNarrow_L', 0), 
        ('MouthNarrow_R', 0), 
        ('LipsPart', 0), 
        ('UpLipsMidHeight', 0.5), 
        ('LoLipsMidHeight', -0.5), 
        ('LoLipsIn', 0),
        ('MouthOpen', -0.2), 
        ('TongueBackHeight', 1.0),
        ('TongueHeight', 1.0),
        ], 
    'G' : [
        ('MouthWidth_L', 0), 
        ('MouthWidth_R', 0), 
        ('MouthNarrow_L', 0), 
        ('MouthNarrow_R', 0), 
        ('LipsPart', 0), 
        ('UpLipsMidHeight', 0.5), 
        ('LoLipsMidHeight', -0.5), 
        ('LoLipsIn', 0),
        ('MouthOpen', -0.2), 
        ('TongueBackHeight', 1.0),
        ('TongueHeight', 0),
        ], 

    'Blink' : [
        ('UpLidUp_L', 1), 
        ('UpLidUp_R', 1), 
        ('LoLidDown_L', 1),
        ('LoLidDown_R', 1)
        ], 
        
    'Unblink' : [
        ('UpLidUp_L', 0), 
        ('UpLidUp_R', 0), 
        ('LoLidDown_L', 0),
        ('LoLidDown_R', 0)
        ], 
})

VisemePanelBones = {
    'MouthOpen' :       ('PJaw', (0,0.25)),
    'UpLipsMidHeight' : ('PUpLipMid', (0,-0.25)),
    'LoLipsMidHeight' : ('PLoLipMid', (0,-0.25)),
    'LoLipsIn':         ('PLoLipMid', (-0.25,0)),
    'MouthWidth_L' :    ('PMouth_L', (0.25,0)),
    'MouthWidth_R' :    ('PMouth_R', (-0.25,0)),
    'MouthNarrow_L' :   ('PMouth_L', (-0.25,0)),
    'MouthNarrow_R' :   ('PMouth_R', (0.25,0)),
    'LipsPart' :        ('PMouthMid', (0, -0.25)),    
    'TongueBackHeight': ('PTongue', (-0.25, 0)),
    'TongueHeight' :    ('PTongue', (0, -0.25)),
    
    'UpLidUp_L' :       ('PUpLid_L', (0,1.0)),
    'UpLidUp_R' :       ('PUpLid_R', (0,1.0)),
    'LoLidDown_L' :     ('PLoLid_L', (0,-1.0)), 
    'LoLidDown_R' :     ('PLoLid_R', (0,-1.0)), 
}    

VisemeList = [
    ('Rest', 'Etc', 'AH'),
    ('MBP', 'OO', 'O'),
    ('R', 'FV', 'S'),
    ('SH', 'EE', 'EH'),
    ('TH', 'L', 'G')
]

#
#   makeVisemes(ob, scn):
#   class VIEW3D_OT_MhxMakeVisemesButton(bpy.types.Operator):
#

def makeVisemes(ob, scn):
    if ob.type != 'MESH':
        print("Active object %s is not a mesh" % ob)
        return
    if not ob.data.shape_keys:
        print("%s has no shapekeys" % ob)
        return
    try:
        ob.data.shape_keys.key_blocks["VIS_Rest"]
        print("Visemes already created")
        return
    except:
        pass        

    verts = ob.data.vertices            
    for (vis,shapes) in bodyLanguageVisemes.items():
        if vis in ['Blink', 'Unblink']:
            continue
        vkey = ob.shape_key_add(name="VIS_%s" % vis)  
        print(vkey.name)
        for n,v in enumerate(verts):
            vkey.data[n].co = v.co
        for (name,value) in shapes:
            if name[-2:] == "_R":
                continue
            skey = ob.data.shape_keys.key_blocks[name]
            factor = 0.75*value
            for n,v in enumerate(verts):
                vkey.data[n].co += factor*(skey.data[n].co - v.co)
    print("Visemes made")                    
    return
           
class VIEW3D_OT_MhxMakeVisemesButton(bpy.types.Operator):
    bl_idname = "mhx.make_visemes"
    bl_label = "Generate viseme shapekeys"
    bl_options = {'UNDO'}

    def execute(self, context):
        makeVisemes(context.object, context.scene)
        return{'FINISHED'}    
       
#
#    mohoVisemes
#    magpieVisemes
#

mohoVisemes = dict({
    'rest' : 'Rest', 
    'etc' : 'Etc', 
    'AI' : 'AH', 
    'O' : 'O', 
    'U' : 'OO', 
    'WQ' : 'AH', 
    'L' : 'L', 
    'E' : 'EH', 
    'MBP' : 'MBP', 
    'FV' : 'FV', 
})

magpieVisemes = dict({
    "CONS" : "t,d,k,g,T,D,s,z,S,Z,h,n,N,j,r,tS", 
    "AI" : "i,&,V,aU,I,0,@,aI", 
    "E" : "eI,3,e", 
    "O" : "O,@U,oI", 
    "UW" : "U,u,w", 
    "MBP" : "m,b,p", 
    "L" : "l", 
    "FV" : "f,v", 
    "Sh" : "dZ", 
})

#
#    setViseme(context, vis, setKey, frame):
#    setBoneLocation(context, pbone, loc, mirror, setKey, frame):
#    class VIEW3D_OT_MhxVisemeButton(bpy.types.Operator):
#

def getVisemeSet(context, rig):
    try:
        visset = rig['MhxVisemeSet']
    except:
        return bodyLanguageVisemes
    if visset == 'StopStaring':
        return stopStaringVisemes
    elif visset == 'BodyLanguage':
        return bodyLanguageVisemes
    else:
        raise MhxError("Unknown viseme set %s" % visset)


def setViseme(context, vis, setKey, frame):
    (rig, mesh) = getMhxRigMesh(context.object)
    isPanel = False
    isProp = False
    shapekeys = None
    scale = 0.75
    if rig.MhxShapekeyDrivers:
        try:
            scale *= rig.pose.bones['PFace'].bone.length
            isPanel = True
        except:
            isProp = True
    elif mesh:
        shapekeys = mesh.data.shape_keys.key_blocks
    visemes = getVisemeSet(context, rig)
    for (skey, value) in visemes[vis]:
        if isPanel:
            (b, (x,z)) = VisemePanelBones[skey]
            loc = mathutils.Vector((float(x*value),0,float(z*value)))            
            pb = rig.pose.bones[b]
            pb.location = loc*scale
            if setKey or context.tool_settings.use_keyframe_insert_auto:
                for n in range(3):
                    pb.keyframe_insert('location', index=n, frame=frame, group=pb.name)
        elif isProp:
            skey = '&_' + skey
            try:
                prop = rig[skey]
            except:
                continue
            rig[skey] = value*scale
            if setKey or context.tool_settings.use_keyframe_insert_auto:
                rig.keyframe_insert('["%s"]' % skey, frame=frame, group="Visemes")    
        elif shapekeys:
            try:
                shapekeys[skey].value = value*scale
            except:
                continue
            if setKey or context.tool_settings.use_keyframe_insert_auto:
                shapekeys[skey].keyframe_insert("value", frame=frame)            
    updatePose(context)
    return
    

class VIEW3D_OT_MhxVisemeButton(bpy.types.Operator):
    bl_idname = 'mhx.pose_viseme'
    bl_label = 'Viseme'
    bl_options = {'UNDO'}
    viseme = StringProperty()

    def invoke(self, context, event):
        setViseme(context, self.viseme, False, context.scene.frame_current)
        return{'FINISHED'}



#
#    openFile(context, filepath):
#    readMoho(context, filepath, offs):
#    readMagpie(context, filepath, offs):
#

def openFile(context, filepath):
    (path, fileName) = os.path.split(filepath)
    (name, ext) = os.path.splitext(fileName)
    return open(filepath, "rU")

def readMoho(context, filepath, offs):
    (rig, mesh) = getMhxRigMesh(context.object)
    context.scene.objects.active = rig
    bpy.ops.object.mode_set(mode='POSE')    
    fp = openFile(context, filepath)        
    for line in fp:
        words= line.split()
        if len(words) < 2:
            pass
        else:
            vis = mohoVisemes[words[1]]
            setViseme(context, vis, True, int(words[0])+offs)
    fp.close()
    setInterpolation(rig)
    updatePose(context)
    print("Moho file %s loaded" % filepath)
    return

def readMagpie(context, filepath, offs):
    rig,mesh = getMhxRigMesh(context.object)
    context.scene.objects.active = rig
    bpy.ops.object.mode_set(mode='POSE')    
    fp = openFile(context, filepath)        
    for line in fp: 
        words= line.split()
        if len(words) < 3:
            pass
        elif words[2] == 'X':
            vis = magpieVisemes[words[3]]
            setViseme(context, vis, True, int(words[0])+offs)
    fp.close()
    setInterpolation(rig)
    updatePose(context)
    print("Magpie file %s loaded" % filepath)
    return

# 
#    class VIEW3D_OT_MhxLoadMohoButton(bpy.types.Operator):
#

class VIEW3D_OT_MhxLoadMohoButton(bpy.types.Operator):
    bl_idname = "mhx.pose_load_moho"
    bl_label = "Moho (.dat)"
    bl_options = {'UNDO'}
    filepath = StringProperty(subtype='FILE_PATH')
    startFrame = IntProperty(name="Start frame", description="First frame to import", default=1)

    def execute(self, context):
        import bpy, os, mathutils
        readMoho(context, self.properties.filepath, self.properties.startFrame-1)        
        return{'FINISHED'}    

    def invoke(self, context, event):
        context.window_manager.fileselect_add(self)
        return {'RUNNING_MODAL'}    

#
#    class VIEW3D_OT_MhxLoadMagpieButton(bpy.types.Operator):
#

class VIEW3D_OT_MhxLoadMagpieButton(bpy.types.Operator):
    bl_idname = "mhx.pose_load_magpie"
    bl_label = "Magpie (.mag)"
    filepath = StringProperty(subtype='FILE_PATH')
    startFrame = IntProperty(name="Start frame", description="First frame to import", default=1)

    def execute(self, context):
        import bpy, os, mathutils
        readMagpie(context, self.properties.filepath, self.properties.startFrame-1)        
        return{'FINISHED'}    

    def invoke(self, context, event):
        context.window_manager.fileselect_add(self)
        return {'RUNNING_MODAL'}    

#
#    class MhxLipsyncPanel(bpy.types.Panel):
#

class MhxLipsyncPanel(bpy.types.Panel):
    bl_label = "MHX Lipsync"
    bl_space_type = "VIEW_3D"
    bl_region_type = "UI"
    bl_options = {'DEFAULT_CLOSED'}
    
    @classmethod
    def poll(cls, context):
        return pollMhx(context.object)

    def draw(self, context):
        rig,mesh = getMhxRigMesh(context.object)
        if not rig:
            return

        layout = self.layout        
        layout.label(text="Visemes")
        for (vis1, vis2, vis3) in VisemeList:
            row = layout.row()
            row.operator("mhx.pose_viseme", text=vis1).viseme = vis1
            row.operator("mhx.pose_viseme", text=vis2).viseme = vis2
            row.operator("mhx.pose_viseme", text=vis3).viseme = vis3
        layout.separator()
        row = layout.row()
        row.operator("mhx.pose_viseme", text="Blink").viseme = 'Blink'
        row.operator("mhx.pose_viseme", text="Unblink").viseme = 'Unblink'
        layout.label(text="Load file")
        row = layout.row()
        row.operator("mhx.pose_load_moho")
        row.operator("mhx.pose_load_magpie")
        layout.operator("mhx.update")
        layout.separator()
        layout.operator("mhx.make_visemes")
        return
        
#
#   updatePose(context):
#   class VIEW3D_OT_MhxUpdateButton(bpy.types.Operator):
#

def updatePose(context):
    scn = context.scene
    scn.frame_current = scn.frame_current
    bpy.ops.object.posemode_toggle()
    bpy.ops.object.posemode_toggle()
    return

class VIEW3D_OT_MhxUpdateButton(bpy.types.Operator):
    bl_idname = "mhx.update"
    bl_label = "Update"

    def execute(self, context):
        updatePose(context)
        return{'FINISHED'}    
        

###################################################################################    
#
#    Expression panel
#
###################################################################################    
#
#    class VIEW3D_OT_MhxResetExpressionsButton(bpy.types.Operator):
#

class VIEW3D_OT_MhxResetExpressionsButton(bpy.types.Operator):
    bl_idname = "mhx.pose_reset_expressions"
    bl_label = "Reset expressions"
    bl_options = {'UNDO'}

    def execute(self, context):
        rig,mesh = getMhxRigMesh(context.object)
        props = getShapeProps(rig)
        for (prop, name) in props:
            rig[prop] = 0.0
        updatePose(context)
        return{'FINISHED'}    

#
#    class VIEW3D_OT_MhxKeyExpressionButton(bpy.types.Operator):
#

class VIEW3D_OT_MhxKeyExpressionsButton(bpy.types.Operator):
    bl_idname = "mhx.pose_key_expressions"
    bl_label = "Key expressions"
    bl_options = {'UNDO'}

    def execute(self, context):
        rig,mesh = getMhxRigMesh(context.object)
        props = getShapeProps(rig)
        frame = context.scene.frame_current
        for (prop, name) in props:
            rig.keyframe_insert('["%s"]' % prop, frame=frame)
        updatePose(context)
        return{'FINISHED'}    
#
#    class VIEW3D_OT_MhxPinExpressionButton(bpy.types.Operator):
#

class VIEW3D_OT_MhxPinExpressionButton(bpy.types.Operator):
    bl_idname = "mhx.pose_pin_expression"
    bl_label = "Pin"
    bl_options = {'UNDO'}
    expression = StringProperty()

    def execute(self, context):
        rig,mesh = getMhxRigMesh(context.object)
        props = getShapeProps(rig)
        if context.tool_settings.use_keyframe_insert_auto:
            frame = context.scene.frame_current
            for (prop, name) in props:
                old = rig[prop]
                if prop == self.expression:
                    rig[prop] = 1.0
                else:
                    rig[prop] = 0.0
                if abs(rig[prop] - old) > 1e-3:
                    rig.keyframe_insert('["%s"]' % prop, frame=frame)
        else:                    
            for (prop, name) in props:
                if prop == self.expression:
                    rig[prop] = 1.0
                else:
                    rig[prop] = 0.0
        updatePose(context)
        return{'FINISHED'}    

#
#   getShapeProps(ob):        
#

def getShapeProps(rig):
    props = []        
    plist = list(rig.keys())
    plist.sort()
    for prop in plist:
        if prop[0] == '*':
            props.append((prop, prop[1:]))
    return props                

#
#    class MhxExpressionsPanel(bpy.types.Panel):
#

class MhxExpressionsPanel(bpy.types.Panel):
    bl_label = "MHX Expressions"
    bl_space_type = "VIEW_3D"
    bl_region_type = "UI"
    bl_options = {'DEFAULT_CLOSED'}
    
    @classmethod
    def poll(cls, context):
        return pollMhx(context.object)

    def draw(self, context):
        layout = self.layout
        rig,mesh = getMhxRigMesh(context.object)
        if not rig:
            print("No MHX rig found")
            return
        if not rig.MhxShapekeyDrivers:
            layout.label("No shapekey drivers.")
            layout.label("Set expression values in mesh context instead")
            return
        props = getShapeProps(rig)
        if not props:
            return
        layout.label(text="Expressions")
        layout.operator("mhx.pose_reset_expressions")
        layout.operator("mhx.pose_key_expressions")
        #layout.operator("mhx.update")
        layout.separator()
        for (prop, name) in props:
            row = layout.split(0.85)
            row.prop(rig, '["%s"]' % prop, text=name)
            row.operator("mhx.pose_pin_expression", text="", icon='UNPINNED').expression = prop
        return

#########################################
#
#   FK-IK snapping. 
#
#########################################

def getPoseMatrix(mat, pb):
    restInv = pb.bone.matrix_local.inverted()
    if pb.parent:
        parInv = pb.parent.matrix.inverted()
        parRest = pb.parent.bone.matrix_local
        return restInv * (parRest * (parInv * mat))
    else:
        return restInv * mat

        
def getGlobalMatrix(mat, pb):
    gmat = pb.bone.matrix_local * mat
    if pb.parent:
        parMat = pb.parent.matrix
        parRest = pb.parent.bone.matrix_local
        return parMat * (parRest.inverted() * gmat)
    else:
        return gmat


def matchPoseTranslation(pb, fkPb, auto):
    mat = getPoseMatrix(fkPb.matrix, pb)
    insertLocation(pb, mat, auto)
    

def insertLocation(pb, mat, auto):    
    pb.location = mat.to_translation()
    if auto:
        pb.keyframe_insert("location", group=pb.name)
    bpy.ops.object.mode_set(mode='OBJECT')
    bpy.ops.object.mode_set(mode='POSE')


def matchPoseRotation(pb, fkPb, auto):
    mat = getPoseMatrix(fkPb.matrix, pb)
    insertRotation(pb, mat, auto)
    

def insertRotation(pb, mat, auto):    
    q = mat.to_quaternion()
    if pb.rotation_mode == 'QUATERNION':
        pb.rotation_quaternion = q
        if auto:
            pb.keyframe_insert("rotation_quaternion", group=pb.name)
    else:
        pb.rotation_euler = q.to_euler(pb.rotation_mode)
        if auto:
            pb.keyframe_insert("rotation_euler", group=pb.name)
    bpy.ops.object.mode_set(mode='OBJECT')
    bpy.ops.object.mode_set(mode='POSE')


def matchPoseReverse(pb, fkPb, auto):
    bpy.ops.object.mode_set(mode='OBJECT')
    bpy.ops.object.mode_set(mode='POSE')
    gmat = fkPb.matrix * Matrix.Rotation(math.pi, 4, 'Z')
    offs = pb.bone.length * fkPb.matrix.col[1]    
    gmat[0][3] += offs[0]
    gmat[1][3] += offs[1]
    gmat[2][3] += offs[2]    
    mat = getPoseMatrix(gmat, pb)
    pb.matrix_basis = mat
    insertLocation(pb, mat, auto)
    insertRotation(pb, mat, auto)
    

def matchPoseScale(pb, fkPb, auto):
    mat = getPoseMatrix(fkPb.matrix, pb)
    pb.scale = mat.to_scale()
    if auto:
        pb.keyframe_insert("scale", group=pb.name)
    bpy.ops.object.mode_set(mode='OBJECT')
    bpy.ops.object.mode_set(mode='POSE')


def fk2ikArm(context, suffix):
    rig = context.object
    auto = context.scene.tool_settings.use_keyframe_insert_auto
    print("Snap FK Arm%s" % suffix)
    (uparmIk, loarmIk, elbow, elbowPt, wrist) = getSnapBones(rig, "ArmIK", suffix)
    (uparmFk, loarmFk, elbowPtFk, handFk) = getSnapBones(rig, "ArmFK", suffix)

    matchPoseRotation(uparmFk, uparmFk, auto)
    matchPoseScale(uparmFk, uparmFk, auto)

    matchPoseRotation(loarmFk, loarmFk, auto)
    matchPoseScale(loarmFk, loarmFk, auto)

    if rig["&HandFollowsWrist" + suffix]:
        matchPoseRotation(handFk, wrist, auto)
        matchPoseScale(handFk, wrist, auto)
    return


def ik2fkArm(context, suffix):
    rig = context.object
    scn = context.scene
    auto = scn.tool_settings.use_keyframe_insert_auto
    print("Snap IK Arm%s" % suffix)
    (uparmIk, loarmIk, elbow, elbowPt, wrist) = getSnapBones(rig, "ArmIK", suffix)
    (uparmFk, loarmFk, elbowPtFk, handFk) = getSnapBones(rig, "ArmFK", suffix)

    #rig["&ElbowFollowsShoulder" + suffix] = False
    #rig["&ElbowFollowsWrist" + suffix] = False
    
    matchPoseTranslation(wrist, handFk, auto)
    matchPoseRotation(wrist, handFk, auto)  
    matchPoseTranslation(elbow, elbowPtFk, auto)
    matchPoseTranslation(elbowPt, elbowPtFk, auto)
    setInverse(rig, elbowPt)
    return


def fk2ikLeg(context, suffix):
    rig = context.object
    auto = context.scene.tool_settings.use_keyframe_insert_auto
    print("Snap FK Leg%s" % suffix)
    (uplegIk, lolegIk, kneePt, ankleIk, legIk, legFk, footIk, toeIk) = getSnapBones(rig, "LegIK", suffix)
    (uplegFk, lolegFk, kneePtFk, footFk, toeFk) = getSnapBones(rig, "LegFK", suffix)

    matchPoseRotation(uplegFk, uplegFk, auto)
    matchPoseScale(uplegFk, uplegFk, auto)

    matchPoseRotation(lolegFk, lolegFk, auto)
    matchPoseScale(lolegFk, lolegFk, auto)
    return


def ik2fkLeg(context, suffix):
    rig = context.object
    scn = context.scene
    auto = scn.tool_settings.use_keyframe_insert_auto
    print("Snap IK Leg%s" % suffix)
    (uplegIk, lolegIk, kneePt, ankleIk, legIk, legFk, footIk, toeIk) = getSnapBones(rig, "LegIK", suffix)
    (uplegFk, lolegFk, kneePtFk, footFk, toeFk) = getSnapBones(rig, "LegFK", suffix)

    #rig["&KneeFollowsHip" + suffix] = False
    #rig["&KneeFollowsFoot" + suffix] = False
    
    legIkToAnkle = rig["&LegIkToAnkle" + suffix]
    if legIkToAnkle:
        matchPoseTranslation(ankleIk, footFk, auto)
    matchPoseTranslation(legIk, legFk, auto)
    matchPoseRotation(legIk, legFk, auto)  
    matchPoseReverse(toeIk, toeFk, auto)
    matchPoseReverse(footIk, footFk, auto)
    setInverse(rig, ankleIk)
    matchPoseTranslation(kneePt, kneePtFk, auto)
    setInverse(rig, kneePt)
    if not legIkToAnkle:
        matchPoseTranslation(ankleIk, footFk, auto)
    return
   
#
#   setInverse(rig, pb):
#

def setInverse(rig, pb):
    rig.data.bones.active = pb.bone
    pb.bone.select = True
    bpy.ops.object.mode_set(mode='OBJECT')
    bpy.ops.object.mode_set(mode='POSE')
    for cns in pb.constraints:
        if cns.type == 'CHILD_OF':
            bpy.ops.constraint.childof_set_inverse(constraint=cns.name, owner='BONE')
    bpy.ops.object.mode_set(mode='OBJECT')
    bpy.ops.object.mode_set(mode='POSE')
    return


"""           
def clearInverse(rig, pb):
    rig.data.bones.active = pb.bone
    pb.bone.select = True
    bpy.ops.object.mode_set(mode='OBJECT')
    bpy.ops.object.mode_set(mode='POSE')
    for cns in pb.constraints:
        if cns.type == 'CHILD_OF':
            bpy.ops.constraint.childof_clear_inverse(constraint=cns.name, owner='BONE')
    bpy.ops.object.mode_set(mode='OBJECT')
    bpy.ops.object.mode_set(mode='POSE')
    return


def fixAnkle(rig, suffix, scn):
    layers = list(rig.data.layers)
    try:
        rig.data.layers = 32*[True]
        setInverse(rig, rig.pose.bones["Ankle" + suffix])
        scn.frame_current = scn.frame_current
    finally:
        rig.data.layers = layers
    return


def clearAnkle(rig, suffix, scn):
    layers = list(rig.data.layers)
    try:
        rig.data.layers = 32*[True]
        clearInverse(rig, rig.pose.bones["Ankle" + suffix])
        scn.frame_current = scn.frame_current
    finally:
        rig.data.layers = layers
    return


class VIEW3D_OT_FixAnkleButton(bpy.types.Operator):
    bl_idname = "mhx.fix_ankle"
    bl_label = "Fix ankle"
    bl_description = "Set inverse for ankle Child-of constraints"
    bl_options = {'UNDO'}
    suffix = StringProperty()

    def execute(self, context):
        fixAnkle(context.object, self.suffix, context.scene)
        return{'FINISHED'}    


class VIEW3D_OT_ClearAnkleButton(bpy.types.Operator):
    bl_idname = "mhx.clear_ankle"
    bl_label = "Clear ankle"
    bl_description = "Clear inverse for ankle Child-of constraints"
    bl_options = {'UNDO'}
    suffix = StringProperty()

    def execute(self, context):
        clearAnkle(context.object, self.suffix, context.scene)
        return{'FINISHED'}    
"""        
#
#
#

SnapBones = {
    "ArmFK" : ["UpArm", "LoArm", "ElbowPTFK", "Hand"],
    "ArmIK" : ["UpArmIK", "LoArmIK", "Elbow", "ElbowPT", "Wrist"],
    "LegFK" : ["UpLeg", "LoLeg", "KneePTFK", "Foot", "Toe"],
    "LegIK" : ["UpLegIK", "LoLegIK", "KneePT", "Ankle", "LegIK", "LegFK", "FootRev", "ToeRev"],
}


def getSnapBones(rig, key, suffix):
    names = SnapBones[key]
    pbones = []
    for name in names:
        pb = rig.pose.bones[name+suffix]
        pbones.append(pb)
    return tuple(pbones)


class VIEW3D_OT_MhxSnapFk2IkButton(bpy.types.Operator):
    bl_idname = "mhx.snap_fk_ik"
    bl_label = "Snap FK"
    bl_options = {'UNDO'}
    data = StringProperty()    

    def execute(self, context):
        bpy.ops.object.mode_set(mode='POSE')
        rig = context.object
        (prop, old) = setSnapProp(rig, self.data, 1.0, context, False)
        if prop[:4] == "&Arm":
            fk2ikArm(context, prop[-2:])
        elif prop[:4] == "&Leg":
            fk2ikLeg(context, prop[-2:])
        restoreSnapProp(rig, prop, old, context)
        return{'FINISHED'}    


class VIEW3D_OT_MhxSnapIk2FkButton(bpy.types.Operator):
    bl_idname = "mhx.snap_ik_fk"
    bl_label = "Snap IK"
    bl_options = {'UNDO'}
    data = StringProperty()    

    def execute(self, context):
        bpy.ops.object.mode_set(mode='POSE')
        rig = context.object
        (prop, old) = setSnapProp(rig, self.data, 0.0, context, True)
        if prop[:4] == "&Arm":
            ik2fkArm(context, prop[-2:])
        elif prop[:4] == "&Leg":
            ik2fkLeg(context, prop[-2:])
        restoreSnapProp(rig, prop, old, context)
        return{'FINISHED'}    


def setSnapProp(rig, data, value, context, isIk):
    words = data.split()
    prop = words[0]
    oldValue = rig[prop]
    rig[prop] = value
    ik = int(words[1])
    fk = int(words[2])
    extra = int(words[3])
    oldIk = rig.data.layers[ik]