import bge
from math import radians
from mathutils import Quaternion, Vector, Matrix

armature = "Char4"

def get_bone_pose_matrix_cleaned(bone):
    offset_m4 = (Matrix.Translation(bone.location) * Quaternion(bone.rotation_quaternion).to_matrix().to_4x4())
    return bone.pose_matrix * offset_m4.inverted()

def zero_constraints(own, scene):
    own['rd_incer'] = 0
    armature = scene.objects['Char4']
    for const in armature.constraints:
        
        if 'rd_cl' in str(const):
            const.enforce = 0
            print(const)
        if 'rd_cr' in str(const):
            const.enforce = 0   
    for x in own['rd_rb'].groupMembers:
        x.suspendDynamics(True)

    
def update_rb(physBody, scene):
    armature = scene.objects['Char4']
    master = armature.parent
    master = scene.objects['control_cube.002']
    
    for pB in physBody.groupMembers: 
        if 'bone' in pB:   
            bone_name = pB["bone"]
            bone = armature.channels[bone_name]   
            pos = (bone.pose_head)+master.worldPosition
            rot = Vector(master.worldOrientation.to_euler()) * Vector(bone.channel_matrix.to_euler())    
            pose_bone = bone
            obj = armature
            matrix_final = obj.worldTransform * get_bone_pose_matrix_cleaned(bone)
            pB.worldTransform = matrix_final            
             
def max_constraints(own, scene, physBody):            
    armature = scene.objects['Char4']
    if armature.parent != None:
        master = armature.parent
        if not armature.parent.isSuspendDynamics:
            pass
    armature.removeParent()
    physBody.restoreDynamics()
    for pB in physBody.groupMembers:
        pB.restoreDynamics()
        if 'bone' in pB:
            bone_name = pB["bone"]
            bone = armature.channels[bone_name]
            if 'init' not in pB:
                pB["origin_pos"] = pB.worldPosition
                pB["origin_rot"] = Vector(pB.worldOrientation.to_euler())  
            pos = (bone.pose_head)+master.worldPosition
            rot = Vector(bone.channel_matrix.to_euler())   
                    
            for const in armature.constraints:
                if pB['bone'] in str(const) and ('rd_cl' in str(const) or 'rd_cr' in str(const)):
                    const.target = pB
                    const.enforce = 1
                    #print('enforcing', pB, const)
                else:
                    pass
        else:
            #print('no bone in', pB)            
            pass
    
    
def main():
    cont = bge.logic.getCurrentController()
    own = cont.owner
    scene = bge.logic.getCurrentScene()
    sens = cont.sensors['rd_key']
    dict = bge.logic.globalDict
    
    if 'rdinited' not in own:
        own['rdinited'] = True
        own['ragdoll_active'] = False
        pB = scene.objectsInactive["npc_ed_arm_physBody"]  
        own['rd_rb'] = scene.addObject(pB)
        own['rd_rb'].suspendDynamics()
        own['rd_set_vel'] = False
        own['rd_vel'] = None
        own['rd_incer'] = 0
        zero_constraints(own, scene)    

    #if (sens.positive) or (dict['aBut'] == 1 and dict['bBut'] == 1) or own['fall'] == True:
    if (sens.positive) or own['fall'] == True:
        #print('option1')
        own['fall'] = False
        if own['ragdoll_active'] == False:
            #own['fall'] = 1
            own['fall'] = True
            print('do ragdoll')
            incer = -1
            update_rb(own['rd_rb'], scene)   
            # while incer < 20:
            #     incer += 1
            #     scene.objects[armature].stopAction(incer)
            pB = scene.objectsInactive["npc_ed_arm_physBody"]        
            physBody = own['rd_rb']
            physBody.worldPosition = own.worldPosition
            physBody.worldOrientation = own.worldOrientation
            own['rd_vel'] = own.worldLinearVelocity.copy()
            print('setting linvel', own['rd_vel'])
            own['rd_set_vel']  = True
            physBody.name = 'ragdollll'
            physBody['to_delete'] = True            
            own['rd_to_delete'] = physBody
            own['ragdoll_active'] = True
            max_constraints(own, scene, physBody)
            own.visible = False
            #own.visible = True
            own['throw_deck'] = False
            cont.actuators['Camera'].height = 2.0
            #cont.actuators['Camera'].min = 14.0
            #cont.actuators['Camera'].max = 18.0
            cont.actuators['Camera'].damping = .000001
            cont.activate(cont.actuators['Camera']) 
            own.suspendDynamics(True)

            
    else:

        if own['ragdoll_active'] == True:
            #pass
            own['rd_incer'] += 1
            # scene.objects[armature].stopAction(1)
            # scene.objects[armature].stopAction(2)
            # scene.objects[armature].stopAction(3)
            # scene.objects[armature].stopAction(7)
            #scene.objects[armature].update()
            
            #cont.actuators['Camera'].object = scene.objects['ragdoll_parent']
            
            #scene.objects['control_cube.002'].worldPosition = scene.objects['control_cube.002'].worldPosition.lerp(scene.objects['ragdoll_parent'].worldPosition, .01)
            
            #scene.objects['control_cube.002'].worldOrientation = scene.objects['ragdoll_parent'].worldOrientation.copy()
            #scene.objects['control_cube.002'].applyRotation([-3.14/2,3.14/2, 0], True)
            scene.objects['control_cube.002'].worldPosition = scene.objects['npc_ed_arm_phys_master'].worldPosition.copy()
            scene.objects['control_cube.002'].worldOrientation = scene.objects['npc_ed_arm_phys_master'].worldOrientation.copy()
            scene.objects['control_cube.002'].applyRotation([3.14/2,3.14/2, 0], True)
            #print('doing the active thing')


            wp = scene.objects['camCube'].worldPosition - scene.objects['ragdoll_parent'].worldPosition
            wp = wp * .9
            cont.actuators['Camera'].height = 2.0
            cont.actuators['Camera'].min = 4.0
            cont.actuators['Camera'].max = 8.0
            cont.actuators['Camera'].damping = .000001
            cont.activate(cont.actuators['Camera']) 
            #print(own['rd_set_vel'], 'vel setter')
            if own['stance']:
                own['requestAction'] = 'reg_walk'
            else:    
                own['requestAction'] = 'fak_walk'
            if own['rd_set_vel'] == True:
                #print(own['rd_incer'], 'rdincer', own['rd_vel'])
                
                scene.objects['npc_ed_arm_phys_master'].setLinearVelocity(own['rd_vel'] * 20, False)
                own['rd_set_vel'] = False
                
                if own['rd_incer'] > 20:
                    own['rd_incer'] += 1
                    #print(
                    #scene.objects['npc_ed_arm_phys_master'].setLinearVelocity(own['rd_vel'] * 4000, False)
                    #scene.objects['npc_ed_arm_phys_master'].worldLinearVelocity.x = 100
                    #scene.objects['npc_ed_arm_phys_master'].applyForce([5000,0,0], False)
                    for pB in own['rd_rb'].groupMembers:
                        #print('setting', pB, own['rd_vel'])
                        
                        try:
                            #pass
                            #pB.applyForce([5000,0,500], False)
                            #pB.worldLinearVelocity.x = 100
                            #pB.worldLinearVelocity = own['rd_vel']
                            pB.setLinearVelocity(own['rd_vel'], False)
                            #print('adding force to', pB, pB.worldLinearVelocity)
                        except:
                            print('cant add force to ', pB)
                else:
                    own['rd_incer'] = 0
                    own['rd_set_vel'] = False
            
            if dict['yBut'] == 1 or dict['kb_q'] == 2:                
                print('turn off ragdoll')
                zero_constraints(own, scene)
                scene.objects[armature].setParent(own, False, True)
                own.restoreDynamics()
                for x in own['rd_rb'].groupMembers:
                    x.suspendDynamics(True)
                own['ragdoll_active'] = False    
                #print(scene.objects[armature].parent, 'parent')
                cont.actuators['Camera'].object = scene.objects['camCube']
                cont.activate(cont.actuators['Camera']) 
                own.worldPosition = scene.objects['npc_ed_arm_phys_master'].worldPosition
                own.worldPosition.z += .8#4
                cont.activate(cont.actuators['walk']) 
                own['getonboard'] = True
                dict['walk'] = 0
                own['fall'] = False
                own.alignAxisToVect([0,0,1], 2, .99)
            if dict['lUD'] < -.08 and own['rd_incer'] > 200:
                physBody = own['rd_rb']
                excludes = ['npc_ed_arm_phys_hand.R', 'npc_ed_arm_phys_hand.L', 'npc_ed_arm_phys_forearm.fk.L', 'npc_ed_arm_phys_forearm.fk.R', 'npc_ed_arm_phys_upper_arm.fk.L', 'npc_ed_arm_phys_upper_arm.fk.R']
                #excludes = []
                # for x in physBody.groupMembers:
                #     print(x.name)
                                    
                #     if x.name not in excludes:
                #         #pass
                #         x.applyForce([0,0,15], False)
                #     else:
                #         x.applyForce([0,0,100], False)   
                #         #pass 

                # scene.objects['npc_ed_arm_phys_master'].applyForce([0,0,500], False)
                scene.objects['npc_ed_arm_phys_head'].applyForce([0,0,400], False)
                # scene.objects['npc_ed_arm_phys_chest'].applyForce([0,0,200], False)
                scene.objects['npc_ed_arm_phys_chest-1'].applyForce([0,0,200], False)
                # scene.objects['npc_ed_arm_phys_spine'].applyForce([0,0,200], False)
                scene.objects['npc_ed_arm_phys_neck'].applyForce([0,0,200], False)
                scene.objects['npc_ed_arm_phys_forearm.fk.L'].applyForce([0,0,30], False)
                scene.objects['npc_ed_arm_phys_forearm.fk.R'].applyForce([0,0,30], False)
                
                # scene.objects['npc_ed_arm_phys_deltoid.L'].applyForce([0,0,200], False)
                # scene.objects['npc_ed_arm_phys_deltoid.R'].applyForce([0,0,200], False)
                # scene.objects['npc_ed_arm_phys_clavicle.L'].applyForce([0,0,200], False)
                # scene.objects['npc_ed_arm_phys_clavicle.R'].applyForce([0,0,200], False)

            if dict['lLR'] < -.04:
                scene.objects['npc_ed_arm_phys_spine'].applyRotation([0,.1,0], True)
                scene.objects['npc_ed_arm_phys_head'].applyRotation([0,.1,0], True)
            if dict['lLR'] > .04:
                scene.objects['npc_ed_arm_phys_spine'].applyRotation([0,-.1,0], True)
                scene.objects['npc_ed_arm_phys_head'].applyRotation([0,-.1,0], True)
                
            # if dict['rUD'] > .08 and own['rd_incer'] > 200:
            #     scene.objects['npc_ed_arm_phys_master'].applyForce([0,600,0], True)
        else:
            update_rb(own['rd_rb'], scene)   
            #pass