/*
 * Copyright (c) Hisilicon Technologies Co., Ltd. 2021-2023. All rights reserved.
 * Description: Math module
 * Author: Hisilicon
 * Created: 2021.04.16
 */

#ifndef IMAGEKIT_IMAGE_MATH_H
#define IMAGEKIT_IMAGE_MATH_H

#include <math.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <float.h>

/* -----------------------------------------------------------------------------------
 * ***** IMAGE_MATH_ALIGNAS
 */
#if !defined(IMAGE_MATH_ALIGNAS)
#if defined(__GNUC__) || defined(__clang__)
#define IMAGE_MATH_ALIGNAS(n) __attribute__((aligned(n)))
#elif defined(__CC_ARM)
#define IMAGE_MATH_ALIGNAS(n) __align(n)
#else
#error Need to define IMAGE_MATH_ALIGNAS
#endif
#endif

/* -------------------------------------------------------------------------------------
 * ***** IMAGE_MATH_ASSERT
 *
 * Independent debug break implementation for IMAGE_Math.h.
 */
#if !defined(IMAGE_MATH_DEBUG_BREAK)
#if defined(_DEBUG)
#define IMAGE_MATH_DEBUG_BREAK __builtin_trap()
#else
#define IMAGE_MATH_DEBUG_BREAK ((void)0)
#endif
#endif


/* -------------------------------------------------------------------------------------
 * ***** IMAGE_MATH_ASSERT
 *
 * Independent IMAGE_MATH_ASSERT implementation for IMAGE_Math.h.
 */
#if !defined(IMAGE_MATH_ASSERT)
#if defined(_DEBUG)
#define IMAGE_MATH_ASSERT(p)    \
    if (!(p)) {                 \
        IMAGE_MATH_DEBUG_BREAK; \
    }
#else
#define IMAGE_MATH_ASSERT(p) ((void)0)
#endif
#endif


/* -------------------------------------------------------------------------------------
 * ***** IMAGE_MATH_STATIC_ASSERT
 *
 * Independent IMAGE_MATH_ASSERT implementation for IMAGE_Math.h.
 */
#if !defined(IMAGE_MATH_STATIC_ASSERT)
#if defined(__cplusplus) && ((defined(_MSC_VER) && (defined(_MSC_VER) >= 1600)) || \
    defined(__GXX_EXPERIMENTAL_CXX0X__) || (__cplusplus >= 201103L))
#define IMAGE_MATH_STATIC_ASSERT static_assert
#else
#if !defined(IMAGE_SA_UNUSED)
#if defined(__GNUC__) || defined(__clang__)
#define IMAGE_SA_UNUSED __attribute__((unused))
#else
#define IMAGE_SA_UNUSED
#endif
#define IMAGE_SA_PASTE(a, b) a##b
#define IMAGE_SA_HELP(a, b) IMAGE_SA_PASTE(a, b)
#endif

#define IMAGE_MATH_STATIC_ASSERT(expression, msg) \
    typedef char IMAGE_SA_HELP(compileTimeAssert, __LINE__)[((expression) != 0) ? 1 : -1] IMAGE_SA_UNUSED
#endif
#endif

namespace PhoenixImage {
/* -----------------------------------------------------------------------------------
 * ***** Simple Math Structures
 *
 */

/* A 2D vector with float components. */
typedef struct IMAGE_MATH_ALIGNAS(4) ImageVector2f_
{
    float x, y;
} ImageVector2f;

/* A 4x4 matrix with float elements. */
typedef struct IMAGE_MATH_ALIGNAS(4) ImageMatrix4f_ {
    float m[4][4];
} ImageMatrix4f;

typedef struct IMAGE_MATH_ALIGNAS(4) ImageMatrix4d_ {
    double m[4][4];
} ImageMatrix4d;


/* -------------------------------------------------------------------------------------
 * ***** Constants for 3D world/axis definitions.
 *
 * Definitions of axes for coordinate and rotation conversions.
 */
enum Axis {
    AXIS_X = 0,
    AXIS_Y = 1,
    AXIS_Z = 2
};

/* RotateDirection describes the rotation direction around an axis, interpreted as follows:
 *  CW  - Clockwise while looking "down" from positive axis towards the origin.
 *  CCW - Counter-clockwise while looking from the positive axis towards the origin,
 *        which is in the negative axis direction.
 *  CCW is the default for the RHS coordinate system. Oculus standard RHS coordinate
 *  system defines Y up, X right, and Z back (pointing out from the screen). In this
 *  system ROTATE_CCW around Z will specifies counter-clockwise rotation in XY plane.
 */
enum RotateDirection {
    ROTATE_CCW = 1,
    ROTATE_CW = -1
};

/* Constants for right handed and left handed coordinate systems */
enum HandedSystem {
    HANDED_R = 1,
    HANDED_L = -1
};

/* AxisDirection describes which way the coordinate axis points. Used by WorldAxes. */
enum AxisDirection {
    AXIS_UP = 2,
    AXIS_DOWN = -2,
    AXIS_RIGHT = 1,
    AXIS_LEFT = -1,
    AXIS_IN = 3,
    AXIS_OUT = -3
};

struct WorldAxes {
    AxisDirection axisX, axisY, axisZ;

    WorldAxes(AxisDirection x, AxisDirection y, AxisDirection z) : axisX(x), axisY(y), axisZ(z)
    {
        IMAGE_MATH_ASSERT(abs(x) != abs(y) && abs(y) != abs(z) && abs(z) != abs(x));
    }
};

/* Forward-declare our templates. */
template<class T> class Vector2;
template<class T> class Matrix4;

/* CompatibleTypes::Type is used to lookup a compatible C-version of a C++ class. */
template<class C> struct CompatibleTypes {
    // Declaration here seems necessary for MSVC; specializations are
    // used instead.
    typedef struct {} Type;
};

// Specializations providing CompatibleTypes::Type value.
template<> struct CompatibleTypes<Vector2<float>>  { typedef ImageVector2f Type;};
template<> struct CompatibleTypes<Matrix4<float>>  { typedef ImageMatrix4f Type;};
template<> struct CompatibleTypes<Matrix4<double>> { typedef ImageMatrix4d Type;};

/* ------------------------------------------------------------------------------------//
 * ***** Math
 *
 * Math class contains constants and functions. This class is a template specialized
 * per type, with Math<float> and Math<double> being distinct.
 */
template<class T> class Math {
public:
    /* By default, support explicit conversion to float. This allows Vector2<int> to
     * compile, for example.
     */
    typedef float OtherFloatType;

    static int Tolerance()
    {
        return 0;
    }  /* Default value so integer types compile */
};

/* ------------------------------------------------------------------------------------//
 * ***** double constants
 */
#define MATH_DOUBLE_PI 3.14159265358979323846
#define MATH_DOUBLE_PIOVER2 (0.5 * MATH_DOUBLE_PI)

/* ------------------------------------------------------------------------------------//
 * ***** float constants
 */
#define MATH_FLOAT_PI float(MATH_DOUBLE_PI)

/* -------------------------------------------------------------------------------------
 * ***** Vector2<>
 *
 * Vector2f (Vector2d) represents a 2-dimensional vector or point in space,
 * consisting of coordinates x and y
 */

template<class T>
class Vector2 {
public:
    typedef T ElementType;
    static const size_t ElementCount = 2;

    T x, y;

    Vector2() : x(0), y(0) { }
    Vector2(T x_, T y_) : x(x_), y(y_) { }
    explicit Vector2(T s) : x(s), y(s) { }

    // C-interop support.
    typedef  typename CompatibleTypes<Vector2<T> >::Type CompatibleType;

    Vector2(const CompatibleType& s) : x(s.x), y(s.y) {  }

    operator const CompatibleType& () const
    {
        IMAGE_MATH_STATIC_ASSERT(sizeof(Vector2<T>) == sizeof(CompatibleType), "sizeof(Vector2<T>) failure");
        return reinterpret_cast<const CompatibleType&>(*this);
    }

    bool     operator== (const Vector2& b) const  { return x == b.x && y == b.y; }
    bool     operator!= (const Vector2& b) const  { return x != b.x || y != b.y; }

    Vector2  operator+  (const Vector2& b) const
    {
        return Vector2(x + b.x, y + b.y);
    }

    Vector2& operator+= (const Vector2& b)
    {
        x += b.x;
        y += b.y;
        return *this;
    }

    Vector2  operator-  (const Vector2& b) const
    {
        return Vector2(x - b.x, y - b.y);
    }

    Vector2& operator-= (const Vector2& b)
    {
        x -= b.x;
        y -= b.y;
        return *this;
    }

    Vector2  operator*  (const Vector2& b) const
    {
        return Vector2(x * b.x, y * b.y);
    }

    Vector2& operator*= (const Vector2& b)
    {
        x *= b.x;
        y *= b.y;
        return *this;
    }

    Vector2  operator/  (const Vector2& b) const
    {
        return Vector2(x / b.x, y / b.y);
    }

    Vector2& operator/= (const Vector2& b)
    {
        x /= b.x;
        y /= b.y;
        return *this;
    }

    Vector2  operator- () const
    {
        return Vector2(-x, -y);
    }

    // Scalar multiplication/division scales vector.
    Vector2  operator*  (T s) const
    {
        return Vector2(x*s, y*s);
    }

    friend Vector2 operator*  (T s, const Vector2& b)
    {
        return b * s;
    }

    Vector2& operator*= (T s)
    {
        x *= s;
        y *= s;
        return *this;
    }

    Vector2  operator/  (T s) const
    {
        T rcp = T(1)/s;
        return Vector2(x*rcp, y*rcp);
    }
    Vector2& operator/= (T s)
    {
        T rcp = T(1)/s;
        x *= rcp;
        y *= rcp;
        return *this;
    }

    // Access element by index
    T& operator[] (int idx)
    {
        IMAGE_MATH_ASSERT(0 <= idx && idx < ElementCount);
        return *(&x + idx);
    }

    // Dot product
    // Used to calculate angle q between two vectors among other things,
    // as (A dot B) = |a||b|cos(q).
    T        Dot(const Vector2& b) const
    {
        return x * b.x + y * b.y;
    }

    // Linearly interpolates from this vector to another.
    // Factor should be between 0.0 and 1.0, with 0 giving full value to this.
    Vector2 Lerp(const Vector2& b, T f) const
    {
        return *this * (T(1) - f) + b * f;
    }
};

typedef Vector2<float>  Vector2f;
typedef Vector2<double> Vector2d;

/* -------------------------------------------------------------------------------------
 * ***** Matrix4
 *
 * Matrix4 is a 4x4 matrix used for 3d transformations and projections.
 * Translation stored in the last column.
 * The matrix is stored in row-major order in memory, meaning that values
 * of the first row are stored before the next one.
 *
 * The arrangement of the matrix is chosen to be in Right-Handed
 * coordinate system and counterclockwise rotations when looking down
 * the axis
 *
 * Transformation Order:
 *   - Transformations are applied from right to left, so the expression
 *     M1 * M2 * M3 * V means that the vector V is transformed by M3 first,
 *     followed by M2 and M1.
 *
 * Coordinate system: Right Handed
 *
 * Rotations: Counterclockwise when looking down the axis. All angles are in radians.
 *
 *  | sx   01   02   tx |    // First column  (sx, 10, 20): Axis X basis vector.
 *  | 10   sy   12   ty |    // Second column (01, sy, 21): Axis Y basis vector.
 *  | 20   21   sz   tz |    // Third columnt (02, 12, sz): Axis Z basis vector.
 *  | 30   31   32   33 |
 *
 *  The basis vectors are first three columns.
 */
template<class T> class Matrix4 {
public:
    typedef T ElementType;
    static const size_t dimension = 4;

    T A[4][4];

    enum NoInitType { NoInit};

    /* Construct with no memory initialization. */
    Matrix4(NoInitType) {}

    /* By default, we construct identity matrix. */
    Matrix4()
    {
        A[0][0] = T(1);
        A[1][1] = T(1);
        A[2][2] = T(1);
        A[3][3] = T(1);

        A[0][1] = T(0);
        A[1][0] = T(0);
        A[2][3] = T(0);
        A[3][1] = T(0);

        A[0][2] = T(0);
        A[1][2] = T(0);
        A[2][0] = T(0);
        A[3][2] = T(0);

        A[0][3] = T(0);
        A[1][3] = T(0);
        A[2][1] = T(0);
        A[3][0] = T(0);
    }

    Matrix4(T m11, T m12, T m13, T m14,
            T m21, T m22, T m23, T m24,
            T m31, T m32, T m33, T m34,
            T m41, T m42, T m43, T m44)
    {
        A[0][0] = m11;
        A[0][1] = m12;
        A[0][2] = m13;
        A[0][3] = m14;

        A[1][0] = m21;
        A[1][1] = m22;
        A[1][2] = m23;
        A[1][3] = m24;

        A[2][0] = m31;
        A[2][1] = m32;
        A[2][2] = m33;
        A[2][3] = m34;

        A[3][0] = m41;
        A[3][1] = m42;
        A[3][2] = m43;
        A[3][3] = m44;
    }

    Matrix4(T m11, T m12, T m13, T m21, T m22, T m23, T m31, T m32, T m33)
    {
        A[0][0] = m11;
        A[0][1] = m12;
        A[0][2] = m13;
        A[0][3] = T(0);

        A[1][0] = m21;
        A[1][1] = m22;
        A[1][2] = m23;
        A[1][3] = T(0);

        A[2][0] = m31;
        A[2][1] = m32;
        A[2][2] = m33;
        A[2][3] = T(0);

        A[3][0] = T(0);
        A[3][1] = T(0);
        A[3][2] = T(0);
        A[3][3] = T(1);
    }

    /* C-interop support. */
    Matrix4(const typename CompatibleTypes<Matrix4<T>>::Type &s)
    {
        IMAGE_MATH_STATIC_ASSERT(sizeof(s) == sizeof(Matrix4), "sizeof(s) == sizeof(Matrix4)");
        for (int x = 0; x < 4; x++) /* 4 * 4 matrix */
            for (int y = 0; y < 4; y++) /* 4 * 4 matrix */
                A[x][y] = (T)s.m[x][y];
    }

    operator typename CompatibleTypes<Matrix4<T>>::Type() const
    {
        typename CompatibleTypes<Matrix4<T>>::Type result;
        IMAGE_MATH_STATIC_ASSERT(sizeof(result) == sizeof(Matrix4), "sizeof(result) == sizeof(Matrix4)");
        for (int x = 0; x < 4; x++) /* 4 * 4 matrix */
            for (int y = 0; y < 4; y++) /* 4 * 4 matrix */
                result.m[x][y] = (T)A[x][y];

        return result;
    }

    static Matrix4 Identity()
    {
        return Matrix4();
    }

    void SetIdentity()
    {
        A[0][0] = T(1);
        A[1][1] = T(1);
        A[2][2] = T(1);
        A[3][3] = T(1);

        A[0][1] = T(0);
        A[1][0] = T(0);
        A[2][3] = T(0);
        A[3][1] = T(0);

        A[0][2] = T(0);
        A[1][2] = T(0);
        A[2][0] = T(0);
        A[3][2] = T(0);

        A[0][3] = T(0);
        A[1][3] = T(0);
        A[2][1] = T(0);
        A[3][0] = T(0);
    }

    bool operator == (const Matrix4 &b) const
    {
        bool isEqual = true;
        for (int x = 0; x < 4; x++)  /* 4 * 4 matrix */
            for (int y = 0; y < 4; y++)  /* 4 * 4 matrix */
                isEqual &= (A[x][y] == b.A[x][y]);

        return isEqual;
    }

    Matrix4 operator + (const Matrix4 &b) const
    {
        Matrix4 result(*this);
        result += b;
        return result;
    }

    Matrix4 &operator += (const Matrix4 &b)
    {
        for (int x = 0; x < 4; x++)  /* 4 * 4 matrix */
            for (int y = 0; y < 4; y++)  /* 4 * 4 matrix */
                A[x][y] += b.A[x][y];
        return *this;
    }

    Matrix4 operator - (const Matrix4 &b) const
    {
        Matrix4 result(*this);
        result -= b;
        return result;
    }

    Matrix4 &operator -= (const Matrix4 &b)
    {
        for (int x = 0; x < 4; x++)  /* 4 * 4 matrix */
            for (int y = 0; y < 4; y++)  /* 4 * 4 matrix */
                A[x][y] -= b.A[x][y];
        return *this;
    }

    // Multiplies two matrices into destination with minimum copying.
    static Matrix4 &Multiply(Matrix4 *d, const Matrix4 &a, const Matrix4 &b)
    {
        IMAGE_MATH_ASSERT((d != &a) && (d != &b));
        int i = 0;
        do {
            d->A[i][0] = a.A[i][0] * b.A[0][0] + a.A[i][1] * b.A[1][0] + a.A[i][2] * b.A[2][0] + a.A[i][3] * b.A[3][0];
            d->A[i][1] = a.A[i][0] * b.A[0][1] + a.A[i][1] * b.A[1][1] + a.A[i][2] * b.A[2][1] + a.A[i][3] * b.A[3][1];
            d->A[i][2] = a.A[i][0] * b.A[0][2] + a.A[i][1] * b.A[1][2] + a.A[i][2] * b.A[2][2] + a.A[i][3] * b.A[3][2];
            d->A[i][3] = a.A[i][0] * b.A[0][3] + a.A[i][1] * b.A[1][3] + a.A[i][2] * b.A[2][3] + a.A[i][3] * b.A[3][3];
        } while ((++i) < 4);  /* 4 * 4 matrix */

        return *d;
    }

    Matrix4 operator*(const Matrix4 &b) const
    {
        Matrix4 result(Matrix4::NoInit);
        Multiply(&result, *this, b);
        return result;
    }

    Matrix4 &operator *= (const Matrix4 &b)
    {
        return Multiply(this, Matrix4(*this), b);
    }

    Matrix4 operator*(T s) const
    {
        Matrix4 result(*this);
        result *= s;
        return result;
    }

    Matrix4 &operator *= (T s)
    {
        for (int x = 0; x < 4; x++) /* 4 * 4 matrix */
            for (int y = 0; y < 4; y++)  /* 4 * 4 matrix */
                A[x][y] *= s;
        return *this;
    }

    Matrix4 operator / (T s) const
    {
        Matrix4 result(*this);
        result /= s;
        return result;
    }

    Matrix4 &operator /= (T s)
    {
        for (int x = 0; x < 4; x++) /* 4 * 4 matrix */
            for (int y = 0; y < 4; y++)  /* 4 * 4 matrix */
                A[x][y] /= s;
        return *this;
    }

    Matrix4 Transposed() const
    {
        return Matrix4(A[0][0], A[1][0], A[2][0], A[3][0], A[0][1], A[1][1], A[2][1], A[3][1], A[0][2], A[1][2],
            A[2][2], A[3][2], A[0][3], A[1][3], A[2][3], A[3][3]);
    }

    void Transpose()
    {
        *this = Transposed();
    }

    T SubDet(const size_t *rows, const size_t *cols) const
    {
        return A[rows[0]][cols[0]] *
            (A[rows[1]][cols[1]] * A[rows[2]][cols[2]] - A[rows[1]][cols[2]] * A[rows[2]][cols[1]]) -
            A[rows[0]][cols[1]] *
            (A[rows[1]][cols[0]] * A[rows[2]][cols[2]] - A[rows[1]][cols[2]] * A[rows[2]][cols[0]]) +
            A[rows[0]][cols[2]] *
            (A[rows[1]][cols[0]] * A[rows[2]][cols[1]] - A[rows[1]][cols[1]] * A[rows[2]][cols[0]]);
    }

    T Cofactor(size_t I, size_t J) const
    {
        const size_t indices[4][3] = {{1, 2, 3}, {0, 2, 3}, {0, 1, 3}, {0, 1, 2}};
        return ((I + J) & 1) ? -SubDet(indices[I], indices[J]) : SubDet(indices[I], indices[J]);
    }

    T Determinant() const
    {
        return A[0][0] * Cofactor(0, 0) + A[0][1] * Cofactor(0, 1) + A[0][2] * Cofactor(0, 2) +
            A[0][3] * Cofactor(0, 3);
    }

    Matrix4 Adjugated() const
    {
        return Matrix4(Cofactor(0, 0), Cofactor(1, 0), Cofactor(2, 0), Cofactor(3, 0), Cofactor(0, 1), Cofactor(1, 1),
            Cofactor(2, 1), Cofactor(3, 1), Cofactor(0, 2), Cofactor(1, 2), Cofactor(2, 2), Cofactor(3, 2),
            Cofactor(0, 3), Cofactor(1, 3), Cofactor(2, 3), Cofactor(3, 3));
    }

    Matrix4 Inverted() const
    {
        T det = Determinant();
        IMAGE_MATH_ASSERT(det != 0);
        return Adjugated() * (T(1) / det);
    }

    void Invert()
    {
        *this = Inverted();
    }

    /* Matrix to Euler Angles conversion
     * a,b,c, are the YawPitchRoll angles to be returned
     * rotation a around axis A1
     * is followed by rotation b around axis A2
     * is followed by rotation c around axis A3
     * rotations are CCW or CW (D) in LH or RH coordinate system (S)
     */
    template<Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S> void ToEulerAngles(T *a, T *b, T *c) const
    {
        IMAGE_MATH_STATIC_ASSERT((A1 != A2) && (A2 != A3) && (A1 != A3), "(A1 != A2) && (A2 != A3) && (A1 != A3)");

        T psign = T(-1);
        if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3)) /* average value for 3: A1¡¢A2¡¢A3 */
            psign = T(1);

        T pm = psign * A[A1][A3];
        T singularityRadius = Math<T>::SingularityRadius();
        if (pm < T(-1) + singularityRadius) { // South pole singularity
            *a = T(0);
            *b = -S * D * ((T)MATH_DOUBLE_PIOVER2);
            *c = S * D * atan2(psign * A[A2][A1], A[A2][A2]);
        } else if (pm > T(1) - singularityRadius) { // North pole singularity
            *a = T(0);
            *b = S * D * ((T)MATH_DOUBLE_PIOVER2);
            *c = S * D * atan2(psign * A[A2][A1], A[A2][A2]);
        } else { // Normal case (nonsingular)
            *a = S * D * atan2(-psign * A[A2][A3], A[A3][A3]);
            *b = S * D * asin(pm);
            *c = S * D * atan2(-psign * A[A1][A2], A[A1][A1]);
        }
    }

    /* Matrix to Euler Angles conversion
     * a,b,c, are the YawPitchRoll angles to be returned
     * rotation a around axis A1
     * is followed by rotation b around axis A2
     * is followed by rotation c around axis A1
     * rotations are CCW or CW (D) in LH or RH coordinate system (S)
     */
    template<Axis A1, Axis A2, RotateDirection D, HandedSystem S> void ToEulerAnglesABA(T *a, T *b, T *c) const
    {
        IMAGE_MATH_STATIC_ASSERT(A1 != A2, "A1 != A2");

        /* Determine the axis that was not supplied */
        int m = 3 - A1 - A2;

        T psign = T(-1);
        if ((A1 + 1) % 3 == A2) /* average value for 3: A1¡¢A2¡¢A3 */
            psign = T(1);

        T c2 = A[A1][A1];
        T singularityRadius = Math<T>::SingularityRadius();
        if (c2 < T(-1) + singularityRadius) { // South pole singularity
            *a = T(0);
            *b = S * D * ((T)MATH_DOUBLE_PI);
            *c = S * D * atan2(-psign * A[A2][m], A[A2][A2]);
        } else if (c2 > T(1) - singularityRadius) { // North pole singularity
            *a = T(0);
            *b = T(0);
            *c = S * D * atan2(-psign * A[A2][m], A[A2][A2]);
        } else { // Normal case (nonsingular)
            *a = S * D * atan2(A[A2][A1], -psign * A[m][A1]);
            *b = S * D * acos(c2);
            *c = S * D * atan2(A[A1][A2], psign * A[A1][m]);
        }
    }

    /* Creates a matrix that converts the vertices from one coordinate system
     * to another.
     */
    static Matrix4 AxisConversion(const WorldAxes &to, const WorldAxes &from)
    {
        /* Holds axis values from the 'to' structure */
        int toArray[3] = { to.axisX, to.axisY, to.axisZ};

        /* The inverse of the toArray */
        int inv[4];
        inv[0] = inv[abs(to.axisX)] = 0;
        inv[abs(to.axisY)] = 1;
        inv[abs(to.axisZ)] = 2;

        Matrix4 m(0, 0, 0, 0, 0, 0, 0, 0, 0);

        /* Only three values in the matrix need to be changed to 1 or -1. */
        m.A[inv[abs(from.axisX)]][0] = T(from.axisX / toArray[inv[abs(from.axisX)]]);
        m.A[inv[abs(from.axisY)]][1] = T(from.axisY / toArray[inv[abs(from.axisY)]]);
        m.A[inv[abs(from.axisZ)]][2] = T(from.axisZ / toArray[inv[abs(from.axisZ)]]);
        return m;
    }

    /* Creates a matrix for translation by vector */
    static Matrix4 Translation(T x, T y, T z = T(0))
    {
        Matrix4 t;
        t.A[0][3] = x;
        t.A[1][3] = y;
        t.A[2][3] = z;
        return t;
    }

    /* Creates a matrix for scaling by vector */
    static Matrix4 Scaling(T x, T y, T z)
    {
        Matrix4 t;
        t.A[0][0] = x;
        t.A[1][1] = y;
        t.A[2][2] = z;
        return t;
    }

    /* Creates a matrix for scaling by constant */
    static Matrix4 Scaling(T s)
    {
        Matrix4 t;
        t.A[0][0] = s;
        t.A[1][1] = s;
        t.A[2][2] = s;
        return t;
    }

    /* Simple L1 distance in R^12 */
    T Distance(const Matrix4 &m2) const
    {
        T d = fabs(A[0][0] - m2.A[0][0]) + fabs(A[0][1] - m2.A[0][1]);
        d += fabs(A[0][2] - m2.A[0][2]) + fabs(A[0][3] - m2.A[0][3]);
        d += fabs(A[1][0] - m2.A[1][0]) + fabs(A[1][1] - m2.A[1][1]);
        d += fabs(A[1][2] - m2.A[1][2]) + fabs(A[1][3] - m2.A[1][3]);
        d += fabs(A[2][0] - m2.A[2][0]) + fabs(A[2][1] - m2.A[2][1]);
        d += fabs(A[2][2] - m2.A[2][2]) + fabs(A[2][3] - m2.A[2][3]);
        d += fabs(A[3][0] - m2.A[3][0]) + fabs(A[3][1] - m2.A[3][1]);
        d += fabs(A[3][2] - m2.A[3][2]) + fabs(A[3][3] - m2.A[3][3]);
        return d;
    }

    /* Creates a rotation matrix rotating around the X axis by 'angle' radians.
     * Just for quick testing.  Not for final API.  Need to remove case.
     */
    static Matrix4 RotationAxis(Axis A, T angle, RotateDirection d, HandedSystem s)
    {
        T sina = s * d * sin(angle);
        T cosa = cos(angle);

        switch (A) {
            case AXIS_X:
                return Matrix4(1, 0, 0, 0, cosa, -sina, 0, sina, cosa);
            case AXIS_Y:
                return Matrix4(cosa, 0, sina, 0, 1, 0, -sina, 0, cosa);
            case AXIS_Z:
                return Matrix4(cosa, -sina, 0, sina, cosa, 0, 0, 0, 1);
            default:
                return Matrix4();
        }
    }

    /* Creates a rotation matrix rotating around the X axis by 'angle' radians.
     * Rotation direction is depends on the coordinate system:
     * RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
     *                        while looking in the negative axis direction. This is the
     *                       same as looking down from positive axis values towards origin.
     * LHS: Positive angle values rotate clock-wise (CW), while looking in the
     *       negative axis direction.
     */
    static Matrix4 RotationX(T angle)
    {
        T sina = sin(angle);
        T cosa = cos(angle);
        return Matrix4(1, 0, 0, 0, cosa, -sina, 0, sina, cosa);
    }

    /* Creates a rotation matrix rotating around the Y axis by 'angle' radians.
     * Rotation direction is depends on the coordinate system:
     *  RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
     *                        while looking in the negative axis direction. This is the
     *                        same as looking down from positive axis values towards origin.
     *  LHS: Positive angle values rotate clock-wise (CW), while looking in the
     *       negative axis direction.
     */
    static Matrix4 RotationY(T angle)
    {
        T sina = sin(angle);
        T cosa = cos(angle);
        return Matrix4(cosa, 0, sina, 0, 1, 0, -sina, 0, cosa);
    }

    /* Creates a rotation matrix rotating around the Z axis by 'angle' radians.
     * Rotation direction is depends on the coordinate system:
     *  RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
     *                        while looking in the negative axis direction. This is the
     *                        same as looking down from positive axis values towards origin.
     *  LHS: Positive angle values rotate clock-wise (CW), while looking in the
     *       negative axis direction.
     */
    static Matrix4 RotationZ(T angle)
    {
        T sina = sin(angle);
        T cosa = cos(angle);
        return Matrix4(cosa, -sina, 0, sina, cosa, 0, 0, 0, 1);
    }

    /* PerspectiveRH creates a right-handed perspective projection matrix that can be
     * used with the Oculus sample renderer.
     *  yfov   - Specifies vertical field of view in radians.
     *  aspect - Screen aspect ration, which is usually width/height for square pixels.
     *           Note that xfov = yfov * aspect.
     *  znear  - Absolute value of near Z clipping clipping range.
     *  zfar   - Absolute value of far  Z clipping clipping range (larger then near).
     * Even though RHS usually looks in the direction of negative Z, positive values
     * are expected for znear and zfar.
     */
    static Matrix4 PerspectiveRH(T yfov, T aspect, T znear, T zfar)
    {
        Matrix4 m;
        T tanHalfFov = tan(yfov * T(0.5));

        m.A[0][0] = T(1) / (aspect * tanHalfFov);
        m.A[1][1] = T(1) / tanHalfFov;
        m.A[2][2] = zfar / (znear - zfar);
        m.A[3][2] = T(-1);
        m.A[2][3] = (zfar * znear) / (znear - zfar);
        m.A[3][3] = T(0);

        /* Note: Post-projection matrix result assumes Left-Handed coordinate system,
         *       with Y up, X right and Z forward. This supports positive z-buffer values.
         * This is the case even for RHS coordinate input.
         */
        return m;
    }

    /* PerspectiveLH creates a left-handed perspective projection matrix that can be
     * used with the Oculus sample renderer.
     *  yfov   - Specifies vertical field of view in radians.
     *  aspect - Screen aspect ration, which is usually width/height for square pixels.
     *           Note that xfov = yfov * aspect.
     *  znear  - Absolute value of near Z clipping clipping range.
     *  zfar   - Absolute value of far  Z clipping clipping range (larger then near).
     */
    static Matrix4 PerspectiveLH(T yfov, T aspect, T znear, T zfar)
    {
        Matrix4 m;
        T tanHalfFov = tan(yfov * T(0.5));

        m.A[0][0] = T(1) / (aspect * tanHalfFov);
        m.A[1][1] = T(1) / tanHalfFov;
        m.A[2][2] = zfar / (zfar - znear);
        m.A[3][2] = T(-1);
        m.A[2][3] = (zfar * znear) / (znear - zfar);
        m.A[3][3] = T(0);

        /* Note: Post-projection matrix result assumes Left-Handed coordinate system,
         *       with Y up, X right and Z forward. This supports positive z-buffer values.
         * This is the case even for RHS coordinate input.
         */
        return m;
    }

    static Matrix4 PerspectiveLR(T yfov, T aspect, T znear, T zfar)
    {
        Matrix4 m;
        T tanHalfFov = tan(yfov * T(0.5));

        m.A[0][0] = T(1) / tanHalfFov;
        m.A[1][1] = T(1) * aspect / tanHalfFov;
        m.A[2][2] = -(zfar + znear) / (zfar - znear);
        m.A[3][2] = T(-1);
        m.A[2][3] = (zfar * znear) / (znear - zfar);
        m.A[3][3] = T(0);

        /* Note: Post-projection matrix result assumes Left-Handed coordinate system,
         *       with Y up, X right and Z forward. This supports positive z-buffer values.
         * This is the case even for RHS coordinate input.
         */

        return m;
    }

    static Matrix4 Ortho2D(T w, T h)
    {
        Matrix4 m;
        m.A[0][0] = T(2.0) / w;
        m.A[1][1] = T(-2.0) / h;
        m.A[0][3] = T(-1.0);
        m.A[1][3] = T(1.0);
        m.A[2][2] = T(0);
        return m;
    }
};

typedef Matrix4<float> Matrix4f;
typedef Matrix4<double> Matrix4d;
}


#endif