from pyfbsdk import *
from pyfbsdk_additions import *


# Game Euler to Matrix to runtime Game Matrix
def convertEulertoGame(lInputMatrix):
    
    lMatrixResult = FBMatrix()
    lMatrixResult[0] = lInputMatrix[0]
    lMatrixResult[1] = lInputMatrix[1]
    lMatrixResult[2] = lInputMatrix[2]
    
    lMatrixResult[4] = lInputMatrix[8]
    lMatrixResult[5] = lInputMatrix[9]
    lMatrixResult[6] = lInputMatrix[10]
    
    lMatrixResult[8] = lInputMatrix[4]*-1
    lMatrixResult[9] = lInputMatrix[5]*-1
    lMatrixResult[10] = lInputMatrix[6]*-1
    #print 
    #print lMatrixResult
    return lMatrixResult
 


def convertMobuToGame(lInputMatrix):
    '''
        Input Game Matrix
    '''
    lIntermediateMatrix = _rotToGame( lInputMatrix )

    ladjustMatrix = FBMatrix([
        lIntermediateMatrix[0],   -lIntermediateMatrix[2],   -lIntermediateMatrix[1],   0,
        -lIntermediateMatrix[4],   lIntermediateMatrix[6],   lIntermediateMatrix[5],   0,
        lIntermediateMatrix[8],   -lIntermediateMatrix[10],  - lIntermediateMatrix[9],   0,
        -lIntermediateMatrix[12],  lIntermediateMatrix[13],  -lIntermediateMatrix[14],   0

    ])


    return lInputMatrix


def convertGameToMobu(lInputMatrix):
    '''
        Input Game Matrix
        Taken from Rag bank
        Camera/Renderer/CamMtx/WorldMtx
    '''
    # Swap 2nd and 3rd rows around and then negate every 3 inputs offeset by 1.
    
    ladjustMatrix = FBMatrix([
        lInputMatrix[0],   -lInputMatrix[2],    -lInputMatrix[1],   0,
        -lInputMatrix[4],   lInputMatrix[6],    lInputMatrix[5],   0,
        lInputMatrix[8],   -lInputMatrix[10],   -lInputMatrix[9],   0,
        -lInputMatrix[12],  lInputMatrix[13],   -lInputMatrix[14],   0
        
    ])

    return _rotToMobu( ladjustMatrix )
    
def _rotToMobu( lInputMatrix ):
    '''
        We need to rotate -180, then another- 90 to orientate the the World
    '''
        
    lIntermediateMatrix = _rotMatrix( lInputMatrix,  0, -180, 0, True )
    lFinalMatrix = _rotMatrix( lIntermediateMatrix,  0, -90, 0, False ) 
   
    return lFinalMatrix
    
    
    
def _rotToGame( lInputMatrix ):
    '''
        We need to rotate 180, then another 90 to orientate the the World
    '''
    
    lIntermediateMatrix = _rotMatrix(lInputMatrix, 0, 180, 0, True)
    lFinalMatrix = _rotMatrix(lIntermediateMatrix, 0, 90, 0, False)
        
    return lFinalMatrix
    
    
def _rotMatrix( lInputMatrix, rotX=0, rotY=0, rotZ=0, AB=True ):
    '''
        We need to rotate 180, then another 90 to orientate the the World
        
        AB or BA is Multipluy order
    '''
    
    lAdjustmentMatrix = FBMatrix()
    FBRotationToMatrix( lAdjustmentMatrix, FBVector3d( rotX, rotY, rotZ ) )
    
    #Multiple these matrices together to our final Matrix    
    lFinalMatrix = FBMatrix()
    if (AB):
        FBMatrixMult( lFinalMatrix, lAdjustmentMatrix , lInputMatrix)
    else:
        FBMatrixMult( lFinalMatrix, lInputMatrix, lAdjustmentMatrix)
    
    
    return lFinalMatrix