"""
Description:
    General math methods

Authors:
    Bianca Rudareanu <Bianca.Rudareanu@rockstarlondon.com>
    David Vega <david.vega@rockstargames.com>
"""

import pyfbsdk as mobu

import math
import numpy

import clr

clr.AddReference("RSG.Base")

from RSG.Base import Math

ZERO_VECTOR = numpy.array([0, 0, 0])

# --- Math Methods ---


def IsFloat(value):
    """
    checks if the value is a float

    Return:
        Boolean
    """
    try:
        float(value)
        return True

    except ValueError:
        return False


def ListToFBMatrix(*values):
    """
    converts a list to a FBMatrix

    Arguments:
        *values (list[int/float, etc.]: values to convert into a matrix. Up to a max of 16 values are accepted)

    Return:
        FBMatrix
    """
    matrix = mobu.FBMatrix()
    for eachNum, each in enumerate(values):
        matrix[eachNum] = each
    return matrix


def Add(*values):
    """
    Adds the values together
        *values (list[int/float, etc.]: values to add)
    Return:
        int/float
    """
    result = 0
    for value in values:
        result += value
    return result


def Multiply(*values):
    """
    Multiplies the values together
        *values (list[int/float, etc.]: values to multiply)
    Return:
        int/float
    """
    result = 1
    for value in values:
        result *= value
    return result


def Length(*values):
    """
    Gets the length of values of a list

    Arguments:
        *values (list[int/float, etc.]): numbers to combine together and get a length from

    Return:
        float
    """
    norm = 0
    for each in values:
        norm += each * each
    norm = float(math.sqrt(norm))
    return norm


def Normalize(*values):
    """
    Normalizes the values of a list

    Arguments:
        *values (list[int/float, etc.]): numbers to normalize
    """
    norm = Length(*values)
    return [each / norm for each in values]


def DotProduct(*vectors):
    """
    Gets the dot product of the given numbers
    Arguments:
        vectors(list): list of vectors to get their dot product
    Return:
        int/float
    """
    return Add(*[Multiply(*values) for values in zip(*vectors)])


def CrossProduct(vectorA, vectorB):
    """
    Get the cross product from two vectors

    Arguments:
        vectorA (list[float/int, float/int, float/int]): vector to get cross product from
        vectorB (list[float/int, float/int, float/int]): vector to get cross product from

    Return:
        list[float, float, float]
    """
    return [vectorA[1] * vectorB[2] - vectorA[2] * vectorB[1],
            vectorA[2] * vectorB[0] - vectorA[0] * vectorB[2],
            vectorA[0] * vectorB[1] - vectorA[1] * vectorB[0]]


def AngleBetweenVectors(vectorA, vectorB):
    """
    Returns the angle between the passed vectors.

    Arguments:
        vectorA = first vector
        vectorB = second vector

    Return:
        Returns the angle between the passed vectors. Returns value in degrees not radians.
    """

    # If the vectors are equivalent then the angle will be zero
    if vectorA == vectorB:
        return 0.0
    else:
        lengths = vectorA.Length() * vectorB.Length()
        if lengths > 0.0: # Protects against passing two vectors that have no length
            return numpy.degrees(math.acos(vectorA.DotProduct(vectorB) / lengths))
        else:
            return 0.0


def Angle(vectorA, vectorB):
    """
    Returns the angle between the passed vectors. This method uses methods from this math lib to
    get the dot prodcut rather than assume the data being passed in is from the RSG.Math libs.

    Arguments:
        vectorA = first vector
        vectorB = second vector

    Return:
        Returns the angle between the passed vectors. Returns value in degrees not radians.
    """
    if vectorA == vectorB:
        return 0.0
    else:
        lengths = Length(*vectorA) * Length(*vectorB)
        if lengths > 0.0: # Protects against passing two vectors that have no length
            result = DotProduct(vectorA, vectorB) / lengths
            print result
            return numpy.degrees(numpy.arccos(result))
        else:
            return 0.0

# --- Matrix Methods ---


def GetOrthonormalMatrixVectors(vectorA, vectorB, upVector=1):
    """
    Get the values of the vectors from a Orthonormal Matrix generated by the two vectors provided
    An Orthonormal Matrix is a matrix where each vector that makes it up is normalized and are 90
    degrees from each other.

    Arguments:
        vectorA  (list[int/float, int/float, int/float]): vector to generate matrix from
        vectorB  (list[int/float, int/float, int/float]): vector to generate matrix from
        upVector (int 1 or 2):  the vector to use as the up vector, 0 is for the first vector (Vector A) and 1 is for
                                the second vector (Vector B)
    Return:
        list[[int/float, int/float, int/float], [int/float, int/float, int/float], [int/float, int/float, int/float]]
    """
    # Normalize Vectors so values are from 0 to 1
    vectorA = Normalize(*vectorA)
    vectorB = Normalize(*vectorB)
    print vectorA, vectorB
    # Get a perpendicular vector from vectors a & b
    vectorC = CrossProduct(vectorA, vectorB)

    # Get an up vector perpendicular to vector c and the vector that isn't suppose to be an upvector
    # This is just to ensure that all the vectors are 90 degrees from each other
    if upVector:
        vectorB = CrossProduct(vectorA, vectorC)
        vectorA = CrossProduct(vectorB, vectorC)
    else:
        vectorA = CrossProduct(vectorB, vectorC)
        vectorB = CrossProduct(vectorA, vectorC)

    return vectorA, vectorB, vectorC


def FBQuaternionToFBMatrix(pInputFBQuaternion, pOutputFBMatrix):
    """
    Conversion from Quat to Matrix

    Arguments:
        pInputFBQuaternion (pyfbsdk.FBQuaternion): Quaternion from Motion Builder
        pOutputFBMatrix (pyfbsdk.FBQuaternion): Matrix from Motion Builer to update

    """
    pOutputFBMatrix.Identity()

    RandomNumber = 1.4142135623730950488016887242097

    tx = RandomNumber * pInputFBQuaternion[0]
    ty = RandomNumber * pInputFBQuaternion[1]
    tz = RandomNumber * pInputFBQuaternion[2]
    tw = RandomNumber * pInputFBQuaternion[3]

    # A - [ 0][ 1][ 2][ 3]
    # B - [ 4][ 5][ 6][ 7]
    # C - [ 8][ 9][10][11]
    # D - [12][13][14][15]

    # a.y
    pOutputFBMatrix[1] = tx * ty + tz * tw
    # b.x
    pOutputFBMatrix[4] = tx * ty - tz * tw

    # a.z
    pOutputFBMatrix[2] = tx * tz - ty * tw
    # c.x
    pOutputFBMatrix[8] = tx * tz + ty * tw

    # b.z
    pOutputFBMatrix[6] = ty * tz + tx * tw
    # c.y
    pOutputFBMatrix[9] = ty * tz - tx * tw

    ty *= ty # need squares along diagonal
    tz *= tz
    tx *= tx

    # a.x
    pOutputFBMatrix[0] = 1.0 - (ty + tz)
    # b.y
    pOutputFBMatrix[5] = 1.0 - (tz + tx)
    # c.z
    pOutputFBMatrix[10] = 1.0 - (ty + tx)


def GameQuaterionToFBMatrix(w, x, y, z,):
    """
    Converts quaternion coordinates to euler rotations for the given rotation order.

    Arguments:
        x (int/float): first value of the quaternion
        y (int/float): second value of the quaternion
        z (int/float): third value of the quaternion
        w (int/float): fourth value of the quaternion
        order (string): the rotation order to use when return the euler rotation

    Return:
        list[float, float, float]
    """

    # Swizzle and negate some components of the input quaternion
    lKeyQuat = mobu.FBVector4d(w, x, y, z)

    # Get the input matrix from this
    lInputMatrix = mobu.FBMatrix()
    FBQuaternionToFBMatrix(lKeyQuat, lInputMatrix)

    # Create first adjustment matrix (unwind about the Y 180)
    lAdjustmentMatrixA = mobu.FBMatrix()
    mobu.FBRotationToMatrix(lAdjustmentMatrixA, mobu.FBVector3d(0, -180, 0))

    # Multiple these matrices together to an intermediate matrix
    lIntermediateMatrix = lInputMatrix
    mobu.FBMatrixMult(lIntermediateMatrix, lAdjustmentMatrixA, lInputMatrix)

    # Create first adjustment matrix (unwind by a further 90 in the Y axis)
    # I don't know why we have to do this - there is nothing in the export
    # code I could see which would require this.
    lAdjustmentMatrixB = mobu.FBMatrix()
    mobu.FBRotationToMatrix(lAdjustmentMatrixB, mobu.FBVector3d(0, -90, 0))

    # Multiple these matrices together to get the final rotation matrix
    lFinalMatrix = mobu.FBMatrix()
    mobu.FBMatrixMult(lFinalMatrix, lIntermediateMatrix, lAdjustmentMatrixB)

    return lFinalMatrix


def EulerToMatrix(x, y, z):
    """
    Converts a euler rotation to a 4x3 matrix

    Arguments:
        x (int/float): x value in degrees
        y (int/float): y value in degrees
        z (int/float): z value in degrees

    Return:
        [int, etc.] 4x4 matrix represented as a list of 16 floats
    """
    sinX = math.sin(x)
    sinY = math.sin(y)
    sinZ = math.sin(z)
    cosX = math.cos(x)
    cosY = math.cos(y)
    cosZ = math.cos(z)

    return [cosY * cosX, -cosY * sinZ * cosX + sinY * sinX, cosY * sinZ * sinX + sinZ * cosX, 0,
                 sinZ, cosZ * cosY, -cosZ * sinX, 0,
           - sinY * cosZ, sinY * sinZ * cosX + cosY * sinX, -sinY * sinZ * sinX + cosY * cosX, 0]


# --- Quaternion Methods ---

def QuaternionToEuler(x, y, z, w, rotationOrder="xyz", invert=False):
    """
    Converts quaternion coordinates to euler rotations for the given rotation order.
    WORK IN PROGRESS

    Arguments:
        x (int/float): first value of the quaternion
        y (int/float): second value of the quaternion
        z (int/float): third value of the quaternion
        w (int/float): fourth value of the quaternion
        rotationOrder = string; the rotation order to use when return the euler rotation

    Return:
        tuple[float, float, float]
    """
    if invert:
        rotationOrder = "".join(reversed(rotationOrder))

    x, y, z, w = Normalize(x, y, z, w)

    xyz = {xyz: value * -1 for xyz, value in zip(rotationOrder.lower(), (x, y, z))}

    eulerX = math.degrees(math.atan2(-2 * (xyz["x"] * xyz["y"] - w * xyz["z"]),
                                     w * w + xyz["x"] * xyz["x"] - xyz["y"] * xyz["y"] - xyz["z"] * xyz["z"]))
    eulerY = math.degrees(math.asin(2 * (xyz["x"] * xyz["z"] + w * xyz["y"])))
    eulerZ = math.degrees(math.atan2(-2 * (xyz["y"] * xyz["z"] - w * xyz["x"]),
                                     w * w - xyz["x"] * xyz["x"] - xyz["y"] * xyz["y"] + xyz["z"] * xyz["z"]))

    return eulerX, eulerY, eulerZ


def FBQuaternionToEuler(x, y, z, w, rotationOrder="XYZ"):
    """
    Converts quaternion coordinates to euler rotations for the given rotation order.

    Arguments:
        x (int/float): first value of the quaternion
        y (int/float): second value of the quaternion
        z (int/float): third value of the quaternion
        w (int/float): fourth value of the quaternion
        order (string): the rotation order to use when return the euler rotation

    Return:
        list[float, float, float]
    """
    quaternion = Math.Quaternionf(x, y, z, w)
    # GTA does not support the TOEulers fucntion in latest source code, so need to add old function back
    if not getattr(quaternion, "ToEulers", False):
        r11 = -2*(x*y - w*z)
        r12 = w*w - x*x + y*y - z*z
        r21 = 2*(y*z + w*x)
        r31 = -2*(x*z - w*y)
        r32 = w*w - x*x - y*y + z*z

        res = mobu.FBVector3d()
        res[1] = math.degrees(math.atan2( r31, r32 ))
        res[0] = math.degrees(math.asin ( r21 ))
        res[2] = math.degrees(math.atan2( r11, r12 ))

        return res
    else:
        euler = quaternion.ToEulers(getattr(Math.EulerOrder, rotationOrder.upper()))
        return math.degrees(euler.X), math.degrees(euler.Y), math.degrees(euler.Z)


def PointOnVector(vector, multiplier=1, startVector=ZERO_VECTOR):
    """
    Gets a point on a given vector based on the given multiplier

    Arguments:
        vector (numpy.ndarray or list[int/float, etc]): vector to calculate point on
        multiplier (float): multiply the vector by this value
        starVector (numpy.ndarray): start point for the vector, default is (0, 0, 0)

    Return:
        numpy.ndarray()

    Example:
        PointOnVector((0, 5, 0), multiplier=0.5)
        # Returns (0, 2.5, 0)

        PointOnVector((0, 5, 0), multiplier=0.5, startVector=(1, 5, 0))
        # Returns (.5, 5, 0)

    """
    if not isinstance(vector, numpy.ndarray):
        numpy.array(vector)
    vector -= startVector
    vector *= multiplier
    return vector + startVector


def PointOnBezierCurve(percent=0, *points):
    """
    Return a point from a Bezier Curve
    Arguments:
        percent (float): point on the curve to get, 0 is 0% and 1.0 is 100%
        *points (list[numpy.ndarray, etc.]): list of control points for the curve
    """
    while len(points) > 1:
        newPoints = []
        for index, eachPoint in enumerate(points[:-1]):
            newPoint = PointOnVector(eachPoint, startVector=points[index + 1], multiplier=percent)
            newPoints.append(newPoint)
        points = newPoints

    return points


def BezierCurve(*points, **keywordParameters):
    """
    Return the points from a Bezier Curve
    Arguments:
        points (list[numpy.ndarray, etc.]): list of control points for the curve
        numberOfPoints (int): number of points on the curve that you want returned

    Return:
        [(float, float, float), (float, float, float), (float, float, float), etc.]
    """
    numberOfPoints = keywordParameters.get("numberOfPoints", 10)
    return [PointOnBezierCurve(index / float(numberOfPoints - 1), *points) for index in xrange(numberOfPoints)]


def Distance(pointA, pointB):
    """
    Given two lists of 3 (x,y,z) will compute the distance between the points

    example:
        print Distance([0, 0, 0], [1, 2, 3])

    Arguments:
        pointA (list[floats]): List of x,y,z values for the first point
        pointB (list[floats]): List of x,y,z values for the second point

    Return:
        float of the distance between the points
    """
    return math.sqrt(
                     math.pow((pointA[0] - pointB[0]), 2) +
                     math.pow((pointA[1] - pointB[1]), 2) +
                     math.pow((pointA[2] - pointB[2]), 2)
    )


def GetGlobalTransformationForModel(model):
    '''
    Returns the global transformation matrix of a given model
    '''
    mat = mobu.FBMatrix()
    model.GetMatrix(mat, mobu.FBModelTransformationType.kModelTransformation, True)

    geometricTransform = mobu.FBMatrix()

    mobu.FBTRSToMatrix(geometricTransform,
                        mobu.FBVector4d(model.GeometricTranslation[0],
                                        model.GeometricTranslation[1],
                                        model.GeometricTranslation[2],
                                        0),
                        model.GeometricRotation,
                        mobu.FBSVector(model.GeometricScaling[0],
                                       model.GeometricScaling[1],
                                       model.GeometricScaling[2]))

    globalModelMatrix = mat * geometricTransform

    return globalModelMatrix


def MayaMatrixSpaceToMotionBuilderMatrixSpace(mayaSpaceMatrix):
    """
    This will convert a matrix in maya space coords to motion builder space coords so it will
    be in the correct place with the correct scale and rotation applied to it.
    Returns a new matrix

    Arguments:
        mayaSpaceMatrix (FBMatrix): Matrix in maya space coords

    Return:
        a new FBMatrix which has been convert to mobu space

    """
    newMat = mobu.FBMatrix()
    newMat[0] = mayaSpaceMatrix[8] * -1
    newMat[1] = mayaSpaceMatrix[9] * -1
    newMat[2] = mayaSpaceMatrix[10] * -1

    newMat[4] = mayaSpaceMatrix[4]
    newMat[5] = mayaSpaceMatrix[5]
    newMat[6] = mayaSpaceMatrix[6]

    newMat[8] = mayaSpaceMatrix[0]
    newMat[9] = mayaSpaceMatrix[1]
    newMat[10] = mayaSpaceMatrix[2]

    newMat[12] = mayaSpaceMatrix[12]
    newMat[13] = mayaSpaceMatrix[13]
    newMat[14] = mayaSpaceMatrix[14]

    return newMat