/*
 * vec_mat.h - vector and matrix defination & calculation
 *
 *  Copyright (c) 2017 Intel Corporation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * Author: Zong Wei <wei.zong@intel.com>
 */

#ifndef XCAM_VECTOR_MATRIX_H
#define XCAM_VECTOR_MATRIX_H

#include <xcam_std.h>
#include <cmath>


namespace XCam {

#ifndef PI
#define PI 3.14159265358979323846
#endif

#ifndef FLT_EPSILON
#define FLT_EPSILON 1.19209290e-07F // float
#endif

#ifndef DBL_EPSILON
#define DBL_EPSILON 2.2204460492503131e-16 // double
#endif

#ifndef DEGREE_2_RADIANS
#define DEGREE_2_RADIANS(x) (((x) * PI) / 180.0)
#endif

#ifndef RADIANS_2_DEGREE
#define RADIANS_2_DEGREE(x) (((x) * 180.0) / PI)
#endif

#define XCAM_VECT2_OPERATOR_VECT2(op)                       \
    Vector2<T> operator op (const Vector2<T>& b) const {    \
        return Vector2<T>(x op b.x, y op b.y);              \
    }                                                       \
    Vector2<T> &operator op##= (const Vector2<T>& b) {      \
        x op##= b.x;  y op##= b.y; return *this;            \
    }

#define XCAM_VECT2_OPERATOR_SCALER(op)                      \
    Vector2<T> operator op (const T& b) const {             \
        return Vector2<T>(x op b, y op b);                  \
    }                                                       \
    Vector2<T> &operator op##= (const T& b) {               \
        x op##= b;  y op##= b; return *this;                \
    }

template<class T>
class Vector2
{
public:

    T x;
    T y;

    Vector2 () : x(0), y(0) {};
    Vector2 (T _x, T _y) : x(_x), y(_y) {};

    template <typename New>
    Vector2<New> convert_to () const {
        Vector2<New> ret((New)(this->x), (New)(this->y));
        return ret;
    }

    Vector2<T>& operator = (const Vector2<T>& rhs)
    {
        x = rhs.x;
        y = rhs.y;
        return *this;
    }

    template <typename Other>
    Vector2<T>& operator = (const Vector2<Other>& rhs)
    {
        x = rhs.x;
        y = rhs.y;
        return *this;
    }

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

    XCAM_VECT2_OPERATOR_VECT2 (+)
    XCAM_VECT2_OPERATOR_VECT2 (-)
    XCAM_VECT2_OPERATOR_VECT2 (*)
    XCAM_VECT2_OPERATOR_VECT2 ( / )
    XCAM_VECT2_OPERATOR_SCALER (+)
    XCAM_VECT2_OPERATOR_SCALER (-)
    XCAM_VECT2_OPERATOR_SCALER (*)
    XCAM_VECT2_OPERATOR_SCALER ( / )

    bool operator == (const Vector2<T>& rhs) const {
        return (x == rhs.x) && (y == rhs.y);
    }

    void reset () {
        this->x = (T) 0;
        this->y = (T) 0;
    }

    void set (T _x, T _y) {
        this->x = _x;
        this->y = _y;
    }

    T magnitude () const {
        return (T) sqrtf(x * x + y * y);
    }

    float distance (const Vector2<T>& vec) const {
        return sqrtf((vec.x - x) * (vec.x - x) + (vec.y - y) * (vec.y - y));
    }

    T dot (const Vector2<T>& vec) const {
        return (x * vec.x + y * vec.y);
    }

    inline Vector2<T> lerp (T weight, const Vector2<T>& vec) const {
        return (*this) + (vec - (*this)) * weight;
    }

};

template<class T, uint32_t N>
class VectorN
{
public:

    VectorN ();
    VectorN (T x);
    VectorN (T x, T y);
    VectorN (T x, T y, T z);
    VectorN (T x, T y, T z, T w);
    VectorN (VectorN<T, 3> vec3, T w);

    inline VectorN<T, N>& operator = (const VectorN<T, N>& rhs);
    inline VectorN<T, N> operator - () const;
    inline bool operator == (const VectorN<T, N>& rhs) const;

    inline T& operator [] (uint32_t index) {
        XCAM_ASSERT(index >= 0 && index < N);
        return data[index];
    }
    inline const T& operator [] (uint32_t index) const {
        XCAM_ASSERT(index >= 0 && index < N);
        return data[index];
    }

    inline VectorN<T, N> operator + (const T rhs) const;
    inline VectorN<T, N> operator - (const T rhs) const;
    inline VectorN<T, N> operator * (const T rhs) const;
    inline VectorN<T, N> operator / (const T rhs) const;
    inline VectorN<T, N> operator += (const T rhs);
    inline VectorN<T, N> operator -= (const T rhs);
    inline VectorN<T, N> operator *= (const T rhs);
    inline VectorN<T, N> operator /= (const T rhs);

    inline VectorN<T, N> operator + (const VectorN<T, N>& rhs) const;
    inline VectorN<T, N> operator - (const VectorN<T, N>& rhs) const;
    inline VectorN<T, N> operator * (const VectorN<T, N>& rhs) const;
    inline VectorN<T, N> operator / (const VectorN<T, N>& rhs) const;
    inline VectorN<T, N> operator += (const VectorN<T, N>& rhs);
    inline VectorN<T, N> operator -= (const VectorN<T, N>& rhs);
    inline VectorN<T, N> operator *= (const VectorN<T, N>& rhs);
    inline VectorN<T, N> operator /= (const VectorN<T, N>& rhs);

    template <typename NEW> inline
    VectorN<NEW, N> convert_to () const;

    inline void zeros ();
    inline void set (T x, T y);
    inline void set (T x, T y, T z);
    inline void set (T x, T y, T z, T w);
    inline T magnitude () const;
    inline float distance (const VectorN<T, N>& vec) const;
    inline T dot (const VectorN<T, N>& vec) const;
    inline VectorN<T, N> lerp (T weight, const VectorN<T, N>& vec) const;

private:
    T data[N];

};


template<class T, uint32_t N> inline
VectorN<T, N>::VectorN ()
{
    for (uint32_t i = 0; i < N; i++) {
        data[i] = 0;
    }
}

template<class T, uint32_t N> inline
VectorN<T, N>::VectorN (T x) {
    data[0] = x;
}

template<class T, uint32_t N> inline
VectorN<T, N>::VectorN (T x, T y) {
    if (N >= 2) {
        data[0] = x;
        data[1] = y;
    }
}

template<class T, uint32_t N> inline
VectorN<T, N>::VectorN (T x, T y, T z) {
    if (N >= 3) {
        data[0] = x;
        data[1] = y;
        data[2] = z;
    }
}

template<class T, uint32_t N> inline
VectorN<T, N>::VectorN (T x, T y, T z, T w) {
    if (N >= 4) {
        data[0] = x;
        data[1] = y;
        data[2] = z;
        data[3] = w;
    }
}

template<class T, uint32_t N> inline
VectorN<T, N>::VectorN (VectorN<T, 3> vec3, T w) {
    if (N >= 4) {
        data[0] = vec3.data[0];
        data[1] = vec3.data[1];
        data[2] = vec3.data[2];
        data[3] = w;
    }
}

template<class T, uint32_t N> inline
VectorN<T, N>& VectorN<T, N>::operator = (const VectorN<T, N>& rhs) {
    for (uint32_t i = 0; i < N; i++) {
        data[i] = rhs.data[i];
    }

    return *this;
}

template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator - () const {
    for (uint32_t i = 0; i < N; i++) {
        data[i] = -data[i];
    }

    return *this;
}

template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator + (const T rhs) const {
    VectorN<T, N> result;

    for (uint32_t i = 0; i < N; i++) {
        result.data[i] = data[i] + rhs;
    }
    return result;
}

template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator - (const T rhs) const {
    VectorN<T, N> result;

    for (uint32_t i = 0; i < N; i++) {
        result.data[i] = data[i] - rhs;
    }
    return result;
}

template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator * (const T rhs) const {
    VectorN<T, N> result;

    for (uint32_t i = 0; i < N; i++) {
        result.data[i] = data[i] * rhs;
    }
    return result;
}

template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator / (const T rhs) const {
    VectorN<T, N> result;

    for (uint32_t i = 0; i < N; i++) {
        result.data[i] = data[i] / rhs;
    }
    return result;
}

template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator += (const T rhs) {
    for (uint32_t i = 0; i < N; i++) {
        data[i] += rhs;
    }
    return *this;
}

template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator -= (const T rhs) {
    for (uint32_t i = 0; i < N; i++) {
        data[i] -= rhs;
    }
    return *this;
}

template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator *= (const T rhs) {
    for (uint32_t i = 0; i < N; i++) {
        data[i] *= rhs;
    }
    return *this;
}

template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator /= (const T rhs) {
    for (uint32_t i = 0; i < N; i++) {
        data[i] /= rhs;
    }
    return *this;
}

template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator + (const VectorN<T, N>& rhs) const {
    VectorN<T, N> result;

    for (uint32_t i = 0; i < N; i++) {
        result.data[i] = data[i] + rhs.data[i];
    }
    return result;
}

template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator - (const VectorN<T, N>& rhs) const {
    VectorN<T, N> result;

    for (uint32_t i = 0; i < N; i++) {
        result.data[i] = data[i] - rhs.data[i];
    }
    return result;
}

template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator * (const VectorN<T, N>& rhs) const {
    VectorN<T, N> result;

    for (uint32_t i = 0; i < N; i++) {
        result.data[i] = data[i] * rhs.data[i];
    }
    return result;
}

template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator / (const VectorN<T, N>& rhs) const {
    VectorN<T, N> result;

    for (uint32_t i = 0; i < N; i++) {
        result.data[i] = data[i] / rhs.data[i];
    }
    return result;
}

template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator += (const VectorN<T, N>& rhs) {

    for (uint32_t i = 0; i < N; i++) {
        data[i] += rhs.data[i];
    }
    return *this;
}

template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator -= (const VectorN<T, N>& rhs) {

    for (uint32_t i = 0; i < N; i++) {
        data[i] -= rhs.data[i];
    }
    return *this;
}

template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator *= (const VectorN<T, N>& rhs) {

    for (uint32_t i = 0; i < N; i++) {
        data[i] *= rhs.data[i];
    }
    return *this;
}

template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator /= (const VectorN<T, N>& rhs) {

    for (uint32_t i = 0; i < N; i++) {
        data[i] /= rhs.data[i];
    }
    return *this;
}

template<class T, uint32_t N> inline
bool VectorN<T, N>::operator == (const VectorN<T, N>& rhs) const {
    for (uint32_t i = 0; i < N; i++) {
        if (data[i] != rhs[i]) {
            return false;
        }
    }
    return true;
}

template <class T, uint32_t N>
template <typename NEW>
VectorN<NEW, N> VectorN<T, N>::convert_to () const {
    VectorN<NEW, N> result;

    for (uint32_t i = 0; i < N; i++) {
        result[i] = (NEW)(this->data[i]);
    }
    return result;
}

template <class T, uint32_t N> inline
void VectorN<T, N>::zeros () {
    for (uint32_t i = 0; i < N; i++) {
        data[i] = (T)(0);
    }
}

template<class T, uint32_t N> inline
void VectorN<T, N>::set (T x, T y) {
    if (N >= 2) {
        data[0] = x;
        data[1] = y;
    }
}

template<class T, uint32_t N> inline
void VectorN<T, N>::set (T x, T y, T z) {
    if (N >= 3) {
        data[0] = x;
        data[1] = y;
        data[2] = z;
    }
}

template<class T, uint32_t N> inline
void VectorN<T, N>::set (T x, T y, T z, T w) {
    if (N >= 4) {
        data[0] - x;
        data[1] = y;
        data[2] = z;
        data[3] = w;
    }
}

template<class T, uint32_t N> inline
T VectorN<T, N>::magnitude () const {
    T result = 0;

    for (uint32_t i = 0; i < N; i++) {
        result += (data[i] * data[i]);
    }
    return (T) sqrtf(result);
}

template<class T, uint32_t N> inline
float VectorN<T, N>::distance (const VectorN<T, N>& vec) const {
    T result = 0;

    for (uint32_t i = 0; i < N; i++) {
        result += (vec.data[i] - data[i]) * (vec.data[i] - data[i]);
    }
    return sqrtf(result);
}

template<class T, uint32_t N> inline
T VectorN<T, N>::dot (const VectorN<T, N>& vec) const {
    T result = 0;

    for (uint32_t i = 0; i < N; i++) {
        result += (vec.data[i] * data[i]);
    }
    return result;
}

template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::lerp (T weight, const VectorN<T, N>& vec) const {
    return (*this) + (vec - (*this)) * weight;
}

// NxN matrix in row major order
template<class T, uint32_t N>
class MatrixN
{
public:
    MatrixN ();
    MatrixN (VectorN<T, 2> a, VectorN<T, 2> b);
    MatrixN (VectorN<T, 3> a, VectorN<T, 3> b, VectorN<T, 3> c);
    MatrixN (VectorN<T, 4> a, VectorN<T, 4> b, VectorN<T, 4> c, VectorN<T, 4> d);

    inline void zeros ();
    inline void eye ();

    inline T& at (uint32_t row, uint32_t col) {
        XCAM_ASSERT(row >= 0 && row < N);
        XCAM_ASSERT(col >= 0 && col < N);

        return data[row * N + col];
    };
    inline const T& at (uint32_t row, uint32_t col) const {
        XCAM_ASSERT(row >= 0 && row < N);
        XCAM_ASSERT(col >= 0 && col < N);

        return data[row * N + col];
    };

    inline T& operator () (uint32_t row, uint32_t col) {
        return at (row, col);
    };
    inline const T& operator () (uint32_t row, uint32_t col) const {
        return at (row, col);
    };

    inline MatrixN<T, N>& operator = (const MatrixN<T, N>& rhs);
    inline MatrixN<T, N> operator - () const;
    inline MatrixN<T, N> operator + (const MatrixN<T, N>& rhs) const;
    inline MatrixN<T, N> operator - (const MatrixN<T, N>& rhs) const;
    inline MatrixN<T, N> operator * (const T a) const;
    inline MatrixN<T, N> operator / (const T a) const;
    inline VectorN<T, N> operator * (const VectorN<T, N>& rhs) const;
    inline MatrixN<T, N> operator * (const MatrixN<T, N>& rhs) const;
    inline MatrixN<T, N> transpose ();
    inline MatrixN<T, N> inverse ();
    inline T trace ();

private:
    inline MatrixN<T, 2> inverse (const MatrixN<T, 2>& mat);
    inline MatrixN<T, 3> inverse (const MatrixN<T, 3>& mat);
    inline MatrixN<T, 4> inverse (const MatrixN<T, 4>& mat);

private:
    T data[N * N];

};

// NxN matrix in row major order
template<class T, uint32_t N>
MatrixN<T, N>::MatrixN () {
    eye ();
}

template<class T, uint32_t N>
MatrixN<T, N>::MatrixN (VectorN<T, 2> a, VectorN<T, 2> b) {
    if (N == 2) {
        data[0] = a[0];
        data[1] = a[1];
        data[2] = b[0];
        data[3] = b[1];
    } else {
        eye ();
    }
}

template<class T, uint32_t N>
MatrixN<T, N>::MatrixN (VectorN<T, 3> a, VectorN<T, 3> b, VectorN<T, 3> c) {
    if (N == 3) {
        data[0]  = a[0];
        data[1] = a[1];
        data[2] = a[2];
        data[3]  = b[0];
        data[4] = b[1];
        data[5] = b[2];
        data[6]  = c[0];
        data[7] = c[1];
        data[8] = c[2];
    } else {
        eye ();
    }
}

template<class T, uint32_t N>
MatrixN<T, N>::MatrixN (VectorN<T, 4> a, VectorN<T, 4> b, VectorN<T, 4> c, VectorN<T, 4> d) {
    if (N == 4) {
        data[0]  = a[0];
        data[1]  = a[1];
        data[2]  = a[2];
        data[3]  = a[3];
        data[4]  = b[0];
        data[5]  = b[1];
        data[6]  = b[2];
        data[7]  = b[3];
        data[8]  = c[0];
        data[9]  = c[1];
        data[10] = c[2];
        data[11] = c[3];
        data[12] = d[0];
        data[13] = d[1];
        data[14] = d[2];
        data[15] = d[3];
    } else {
        eye ();
    }
}

template<class T, uint32_t N> inline
void MatrixN<T, N>::zeros () {
    for (uint32_t i = 0; i < N * N; i++) {
        data[i] = 0;
    }
}

template<class T, uint32_t N> inline
void MatrixN<T, N>::eye () {
    zeros ();
    for (uint32_t i = 0; i < N; i++) {
        data[i * N + i] = 1;
    }
}

template<class T, uint32_t N> inline
MatrixN<T, N>& MatrixN<T, N>::operator = (const MatrixN<T, N>& rhs) {
    for (uint32_t i = 0; i < N * N; i++) {
        data[i] = rhs.data[i];
    }
    return *this;
}

template<class T, uint32_t N> inline
MatrixN<T, N> MatrixN<T, N>::operator - () const {
    MatrixN<T, N> result;
    for (uint32_t i = 0; i < N * N; i++) {
        result.data[i] = -data[i];
    }
    return result;
}

template<class T, uint32_t N> inline
MatrixN<T, N> MatrixN<T, N>::operator + (const MatrixN<T, N>& rhs) const {
    MatrixN<T, N> result;
    for (uint32_t i = 0; i < N * N; i++) {
        result.data[i] = data[i] + rhs.data[i];
    }
    return result;
}

template<class T, uint32_t N> inline
MatrixN<T, N> MatrixN<T, N>::operator - (const MatrixN<T, N>& rhs) const {
    MatrixN<T, N> result;
    for (uint32_t i = 0; i < N * N; i++) {
        result.data[i] = data[i] - rhs.data[i];
    }
    return result;
}

template<class T, uint32_t N> inline
MatrixN<T, N> MatrixN<T, N>::operator * (const T a) const {
    MatrixN<T, N> result;
    for (uint32_t i = 0; i < N * N; i++) {
        result.data[i] = data[i] * a;
    }
    return result;
}

template<class T, uint32_t N> inline
MatrixN<T, N> MatrixN<T, N>::operator / (const T a) const {
    MatrixN<T, N> result;
    for (uint32_t i = 0; i < N * N; i++) {
        result.data[i] = data[i] / a;
    }
    return result;
}

template<class T, uint32_t N> inline
MatrixN<T, N> MatrixN<T, N>::operator * (const MatrixN<T, N>& rhs) const {
    MatrixN<T, N> result;
    result.zeros ();

    for (uint32_t i = 0; i < N; i++) {
        for (uint32_t j = 0; j < N; j++) {
            T element = 0;
            for (uint32_t k = 0; k < N; k++) {
                element += at(i, k) * rhs(k, j);
            }
            result(i, j) = element;
        }
    }
    return result;
}

template<class T, uint32_t N> inline
VectorN<T, N> MatrixN<T, N>::operator * (const VectorN<T, N>& rhs) const {
    VectorN<T, N> result;
    for (uint32_t i = 0; i < N; i++) {  // row
        for (uint32_t j = 0; j < N; j++) {  // col
            result.data[i] = data[i * N + j] * rhs.data[j];
        }
    }
    return result;
}

template<class T, uint32_t N> inline
MatrixN<T, N> MatrixN<T, N>::transpose () {
    MatrixN<T, N> result;
    for (uint32_t i = 0; i < N; i++) {
        for (uint32_t j = 0; j <= N; j++) {
            result.data[i * N + j] = data[j * N + i];
        }
    }
    return result;
}

// if the matrix is non-invertible, return identity matrix
template<class T, uint32_t N> inline
MatrixN<T, N> MatrixN<T, N>::inverse () {
    MatrixN<T, N> result;

    result = inverse (*this);
    return result;
}

template<class T, uint32_t N> inline
T MatrixN<T, N>::trace () {
    T t = 0;
    for ( uint32_t i = 0; i < N; i++ ) {
        t += data(i, i);
    }
    return t;
}

template<class T, uint32_t N> inline
MatrixN<T, 2> MatrixN<T, N>::inverse (const MatrixN<T, 2>& mat)
{
    MatrixN<T, 2> result;

    T det = mat(0, 0) * mat(1, 1) - mat(0, 1) * mat(1, 0);

    if (det == (T)0) {
        return result;
    }

    result(0, 0) = mat(1, 1);
    result(0, 1) = -mat(0, 1);
    result(1, 0) = -mat(1, 0);
    result(1, 1) = mat(0, 0);

    return result * (1.0f / det);
}

template<class T, uint32_t N> inline
MatrixN<T, 3> MatrixN<T, N>::inverse (const MatrixN<T, 3>& mat)
{
    MatrixN<T, 3> result;

    T det = mat(0, 0) * mat(1, 1) * mat(2, 2) +
            mat(1, 0) * mat(2, 1) * mat(0, 2) +
            mat(2, 0) * mat(0, 1) * mat(1, 2) -
            mat(0, 0) * mat(2, 1) * mat(1, 2) -
            mat(1, 0) * mat(0, 1) * mat(2, 2) -
            mat(2, 0) * mat(1, 1) * mat(0, 2);

    if (det == (T)0) {
        return result;
    }

    result(0, 0) = mat(1, 1) * mat(2, 2) - mat(1, 2) * mat(2, 1);
    result(1, 0) = mat(1, 2) * mat(2, 0) - mat(1, 0) * mat(2, 2);
    result(2, 0) = mat(1, 0) * mat(2, 1) - mat(1, 1) * mat(2, 0);
    result(0, 1) = mat(0, 2) * mat(2, 1) - mat(0, 1) * mat(2, 2);
    result(1, 1) = mat(0, 0) * mat(2, 2) - mat(0, 2) * mat(2, 0);
    result(2, 1) = mat(0, 1) * mat(2, 0) - mat(0, 0) * mat(2, 1);
    result(0, 2) = mat(0, 1) * mat(1, 2) - mat(0, 2) * mat(1, 1);
    result(1, 2) = mat(0, 2) * mat(1, 0) - mat(0, 0) * mat(1, 2);
    result(2, 2) = mat(0, 0) * mat(1, 1) - mat(0, 1) * mat(1, 0);

    return result * (1.0f / det);
}

template<class T, uint32_t N> inline
MatrixN<T, 4> MatrixN<T, N>::inverse (const MatrixN<T, 4>& mat)
{
    MatrixN<T, 4> result;

    T det =  mat(0, 3) * mat(1, 2) * mat(2, 1) * mat(3, 1) -
             mat(0, 2) * mat(1, 3) * mat(2, 1) * mat(3, 1) -
             mat(0, 3) * mat(1, 1) * mat(2, 2) * mat(3, 1) +
             mat(0, 1) * mat(1, 3) * mat(2, 2) * mat(3, 1) +
             mat(0, 2) * mat(1, 1) * mat(2, 3) * mat(3, 1) -
             mat(0, 1) * mat(1, 2) * mat(2, 3) * mat(3, 1) -
             mat(0, 3) * mat(1, 2) * mat(2, 0) * mat(3, 1) +
             mat(0, 2) * mat(1, 3) * mat(2, 0) * mat(3, 1) +
             mat(0, 3) * mat(1, 0) * mat(2, 2) * mat(3, 1) -
             mat(0, 0) * mat(1, 3) * mat(2, 2) * mat(3, 1) -
             mat(0, 2) * mat(1, 0) * mat(2, 3) * mat(3, 1) +
             mat(0, 0) * mat(1, 2) * mat(2, 3) * mat(3, 1) +
             mat(0, 3) * mat(1, 1) * mat(2, 0) * mat(3, 2) -
             mat(0, 1) * mat(1, 3) * mat(2, 0) * mat(3, 2) -
             mat(0, 3) * mat(1, 0) * mat(2, 1) * mat(3, 2) +
             mat(0, 0) * mat(1, 3) * mat(2, 1) * mat(3, 2) +
             mat(0, 1) * mat(1, 0) * mat(2, 3) * mat(3, 2) -
             mat(0, 0) * mat(1, 1) * mat(2, 3) * mat(3, 2) -
             mat(0, 2) * mat(1, 1) * mat(2, 0) * mat(3, 3) +
             mat(0, 1) * mat(1, 2) * mat(2, 0) * mat(3, 3) +
             mat(0, 2) * mat(1, 0) * mat(2, 1) * mat(3, 3) -
             mat(0, 0) * mat(1, 2) * mat(2, 1) * mat(3, 3) -
             mat(0, 1) * mat(1, 0) * mat(2, 2) * mat(3, 3) +
             mat(0, 0) * mat(1, 1) * mat(2, 2) * mat(3, 3);

    if (det == (T)0) {
        return result;
    }

    result(0, 0) = mat(1, 2) * mat(2, 3) * mat(3, 1) -
                   mat(1, 3) * mat(2, 2) * mat(3, 1) +
                   mat(1, 3) * mat(2, 1) * mat(3, 2) -
                   mat(1, 1) * mat(2, 3) * mat(3, 2) -
                   mat(1, 2) * mat(2, 1) * mat(3, 3) +
                   mat(1, 1) * mat(2, 2) * mat(3, 3);

    result(0, 1) = mat(0, 3) * mat(2, 2) * mat(3, 1) -
                   mat(0, 2) * mat(2, 3) * mat(3, 1) -
                   mat(0, 3) * mat(2, 1) * mat(3, 2) +
                   mat(0, 1) * mat(2, 3) * mat(3, 2) +
                   mat(0, 2) * mat(2, 1) * mat(3, 3) -
                   mat(0, 1) * mat(2, 2) * mat(3, 3);

    result(0, 2) = mat(0, 2) * mat(1, 3) * mat(3, 1) -
                   mat(0, 3) * mat(1, 2) * mat(3, 1) +
                   mat(0, 3) * mat(1, 1) * mat(3, 2) -
                   mat(0, 1) * mat(1, 3) * mat(3, 2) -
                   mat(0, 2) * mat(1, 1) * mat(3, 3) +
                   mat(0, 1) * mat(1, 2) * mat(3, 3);

    result(0, 3) = mat(0, 3) * mat(1, 2) * mat(2, 1) -
                   mat(0, 2) * mat(1, 3) * mat(2, 1) -
                   mat(0, 3) * mat(1, 1) * mat(2, 2) +
                   mat(0, 1) * mat(1, 3) * mat(2, 2) +
                   mat(0, 2) * mat(1, 1) * mat(2, 3) -
                   mat(0, 1) * mat(1, 2) * mat(2, 3);

    result(1, 0) = mat(1, 3) * mat(2, 2) * mat(3, 0) -
                   mat(1, 2) * mat(2, 3) * mat(3, 0) -
                   mat(1, 3) * mat(2, 0) * mat(3, 2) +
                   mat(1, 0) * mat(2, 3) * mat(3, 2) +
                   mat(1, 2) * mat(2, 0) * mat(3, 3) -
                   mat(1, 0) * mat(2, 2) * mat(3, 3);

    result(1, 1) = mat(0, 2) * mat(2, 3) * mat(3, 0) -
                   mat(0, 3) * mat(2, 2) * mat(3, 0) +
                   mat(0, 3) * mat(2, 0) * mat(3, 2) -
                   mat(0, 0) * mat(2, 3) * mat(3, 2) -
                   mat(0, 2) * mat(2, 0) * mat(3, 3) +
                   mat(0, 0) * mat(2, 2) * mat(3, 3);

    result(1, 2) = mat(0, 3) * mat(1, 2) * mat(3, 0) -
                   mat(0, 2) * mat(1, 3) * mat(3, 0) -
                   mat(0, 3) * mat(1, 0) * mat(3, 2) +
                   mat(0, 0) * mat(1, 3) * mat(3, 2) +
                   mat(0, 2) * mat(1, 0) * mat(3, 3) -
                   mat(0, 0) * mat(1, 2) * mat(3, 3);

    result(1, 3) = mat(0, 2) * mat(1, 3) * mat(2, 0) -
                   mat(0, 3) * mat(1, 2) * mat(2, 0) +
                   mat(0, 3) * mat(1, 0) * mat(2, 2) -
                   mat(0, 0) * mat(1, 3) * mat(2, 2) -
                   mat(0, 2) * mat(1, 0) * mat(2, 3) +
                   mat(0, 0) * mat(1, 2) * mat(2, 3);

    result(2, 0) = mat(1, 1) * mat(2, 3) * mat(3, 0) -
                   mat(1, 3) * mat(2, 1) * mat(3, 0) +
                   mat(1, 3) * mat(2, 0) * mat(3, 1) -
                   mat(1, 0) * mat(2, 3) * mat(3, 1) -
                   mat(1, 1) * mat(2, 0) * mat(3, 3) +
                   mat(1, 0) * mat(2, 1) * mat(3, 3);

    result(2, 1) = mat(0, 3) * mat(2, 1) * mat(3, 0) -
                   mat(0, 1) * mat(2, 3) * mat(3, 0) -
                   mat(0, 3) * mat(2, 0) * mat(3, 1) +
                   mat(0, 0) * mat(2, 3) * mat(3, 1) +
                   mat(0, 1) * mat(2, 0) * mat(3, 3) -
                   mat(0, 0) * mat(2, 1) * mat(3, 3);

    result(2, 2) = mat(0, 1) * mat(1, 3) * mat(3, 0) -
                   mat(0, 3) * mat(1, 1) * mat(3, 0) +
                   mat(0, 3) * mat(1, 0) * mat(3, 1) -
                   mat(0, 0) * mat(1, 3) * mat(3, 1) -
                   mat(0, 1) * mat(1, 0) * mat(3, 3) +
                   mat(0, 0) * mat(1, 1) * mat(3, 3);

    result(2, 3) = mat(0, 3) * mat(1, 1) * mat(2, 0) -
                   mat(0, 1) * mat(1, 3) * mat(2, 0) -
                   mat(0, 3) * mat(1, 0) * mat(2, 1) +
                   mat(0, 0) * mat(1, 3) * mat(2, 1) +
                   mat(0, 1) * mat(1, 0) * mat(2, 3) -
                   mat(0, 0) * mat(1, 1) * mat(2, 3);

    result(3, 0) = mat(1, 2) * mat(2, 1) * mat(3, 0) -
                   mat(1, 1) * mat(2, 2) * mat(3, 0) -
                   mat(1, 2) * mat(2, 0) * mat(3, 1) +
                   mat(1, 0) * mat(2, 2) * mat(3, 1) +
                   mat(1, 1) * mat(2, 0) * mat(3, 2) -
                   mat(1, 0) * mat(2, 1) * mat(3, 2);

    result(3, 1) = mat(1, 1) * mat(2, 2) * mat(3, 0) -
                   mat(1, 2) * mat(2, 1) * mat(3, 0) +
                   mat(1, 2) * mat(2, 0) * mat(3, 1) -
                   mat(1, 0) * mat(2, 2) * mat(3, 1) -
                   mat(1, 1) * mat(2, 0) * mat(3, 2) +
                   mat(1, 0) * mat(2, 1) * mat(3, 2);

    result(3, 2) = mat(0, 2) * mat(1, 1) * mat(3, 0) -
                   mat(0, 1) * mat(1, 2) * mat(3, 0) -
                   mat(0, 2) * mat(1, 0) * mat(3, 1) +
                   mat(0, 0) * mat(1, 2) * mat(3, 1) +
                   mat(0, 1) * mat(1, 0) * mat(3, 2) -
                   mat(0, 0) * mat(1, 1) * mat(3, 2);

    result(3, 3) = mat(0, 1) * mat(1, 2) * mat(2, 0) -
                   mat(0, 2) * mat(1, 1) * mat(2, 0) +
                   mat(0, 2) * mat(1, 0) * mat(2, 1) -
                   mat(0, 0) * mat(1, 2) * mat(2, 1) -
                   mat(0, 1) * mat(1, 0) * mat(2, 2) +
                   mat(0, 0) * mat(1, 1) * mat(2, 2);

    return result * (1.0f / det);
}

typedef VectorN<double, 2> Vec2d;
typedef VectorN<double, 3> Vec3d;
typedef VectorN<double, 4> Vec4d;
typedef MatrixN<double, 2> Mat2d;
typedef MatrixN<double, 3> Mat3d;
typedef MatrixN<double, 4> Mat4d;

typedef VectorN<float, 2> Vec2f;
typedef VectorN<float, 3> Vec3f;
typedef VectorN<float, 4> Vec4f;
typedef MatrixN<float, 3> Mat3f;
typedef MatrixN<float, 4> Mat4f;

template<class T>
class Quaternion
{
public:

    Vec3d v;
    T w;

    Quaternion () : v(0, 0, 0), w(0) {};
    Quaternion (const Quaternion<T>& q) : v(q.v), w(q.w) {};

    Quaternion (const Vec3d& vec, T _w) : v(vec), w(_w) {};
    Quaternion (const Vec4d& vec)  : v(vec[0], vec[1], vec[2]), w(vec[3]) {};
    Quaternion (T _x, T _y, T _z, T _w) : v(_x, _y, _z), w(_w) {};

    inline void reset () {
        v.zeros();
        w = (T) 0;
    }

    inline Quaternion<T>& operator = (const Quaternion<T>& rhs) {
        v = rhs.v;
        w = rhs.w;
        return *this;
    }

    inline Quaternion<T> operator + (const Quaternion<T>& rhs) const {
        const Quaternion<T>& lhs = *this;
        return Quaternion<T>(lhs.v + rhs.v, lhs.w + rhs.w);
    }

    inline Quaternion<T> operator - (const Quaternion<T>& rhs) const {
        const Quaternion<T>& lhs = *this;
        return Quaternion<T>(lhs.v - rhs.v, lhs.w - rhs.w);
    }

    inline Quaternion<T> operator * (T rhs) const {
        return Quaternion<T>(v * rhs, w * rhs);
    }

    inline Quaternion<T> operator * (const Quaternion<T>& rhs) const {
        const Quaternion<T>& lhs = *this;
        return Quaternion<T>(lhs.w * rhs.v[0] + lhs.v[0] * rhs.w + lhs.v[1] * rhs.v[2] - lhs.v[2] * rhs.v[1],
                             lhs.w * rhs.v[1] - lhs.v[0] * rhs.v[2] + lhs.v[1] * rhs.w + lhs.v[2] * rhs.v[0],
                             lhs.w * rhs.v[2] + lhs.v[0] * rhs.v[1] - lhs.v[1] * rhs.v[0] + lhs.v[2] * rhs.w,
                             lhs.w * rhs.w - lhs.v[0] * rhs.v[0] - lhs.v[1] * rhs.v[1] - lhs.v[2] * rhs.v[2]);
    }

    /*
                   --------
                  /    --
        |Qr| =  \/  Qr.Qr
    */
    inline T magnitude () const {
        return (T) sqrtf(w * w + v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
    }

    inline void normalize ()
    {
        T length = magnitude ();
        w = w / length;
        v = v / length;
    }

    inline Quaternion<T> conjugate (const Quaternion<T>& quat) const {
        return Quaternion<T>(-quat.v, quat.w);
    }

    inline Quaternion<T> inverse (const Quaternion<T>& quat) const {
        return conjugate(quat) * ( 1.0f / magnitude(quat));
    }

    inline Quaternion<T> lerp (T weight, const Quaternion<T>& quat) const {
        return Quaternion<T>(v.lerp(weight, quat.v), (1 - weight) * w + weight * quat.w);
    }

    inline Quaternion<T> slerp(T r, const Quaternion<T>& quat) const {
        Quaternion<T> ret;
        T cos_theta = w * quat.w + v[0] * quat.v[0] + v[1] * quat.v[1] + v[2] * quat.v[2];
        T theta = (T) acos(cos_theta);
        if (fabs(theta) < FLT_EPSILON)
        {
            ret = *this;
        }
        else
        {
            T sin_theta = (T) sqrt(1.0 - cos_theta * cos_theta);
            if (fabs(sin_theta) < FLT_EPSILON)
            {
                ret.w = 0.5 * w + 0.5 * quat.w;
                ret.v = v.lerp(0.5, quat.v);
            }
            else
            {
                T r0 = (T) sin((1.0 - r) * theta) / sin_theta;
                T r1 = (T) sin(r * theta) / sin_theta;

                ret.w = w * r0 + quat.w * r1;
                ret.v[0] = v[0] * r0 + quat.v[0] * r1;
                ret.v[1] = v[1] * r0 + quat.v[1] * r1;
                ret.v[2] = v[2] * r0 + quat.v[2] * r1;
            }
        }
        return ret;
    }

    static Quaternion<T> create_quaternion (Vec3d axis, T angle_rad) {
        T theta_over_two = angle_rad / (T) 2.0;
        T sin_theta_over_two = std::sin(theta_over_two);
        T cos_theta_over_two = std::cos(theta_over_two);
        return Quaternion<T>(axis * sin_theta_over_two, cos_theta_over_two);
    }

    static Quaternion<T> create_quaternion (Vec3d euler) {
        return create_quaternion(Vec3d(1, 0, 0), euler[0]) *
               create_quaternion(Vec3d(0, 1, 0), euler[1]) *
               create_quaternion(Vec3d(0, 0, 1), euler[2]);
    }

    static Quaternion<T> create_quaternion (const Mat3d& mat) {
        Quaternion<T> q;

        T trace, s;
        T diag1 = mat(0, 0);
        T diag2 = mat(1, 1);
        T diag3 = mat(2, 2);

        trace = diag1 + diag2 + diag3;

        if (trace >= FLT_EPSILON)
        {
            s = 2.0 * (T) sqrt(trace + 1.0);
            q.w = 0.25 * s;
            q.v[0] = (mat(2, 1) - mat(1, 2)) / s;
            q.v[1] = (mat(0, 2) - mat(2, 0)) / s;
            q.v[2] = (mat(1, 0) - mat(0, 1)) / s;
        }
        else
        {
            char max_diag = (diag1 > diag2) ? ((diag1 > diag3) ? 1 : 3) : ((diag2 > diag3) ? 2 : 3);

            if (max_diag == 1)
            {
                s = 2.0 * (T) sqrt(1.0 + mat(0, 0) - mat(1, 1) - mat(2, 2));
                q.w = (mat(2, 1) - mat(1, 2)) / s;
                q.v[0] = 0.25 * s;
                q.v[1] = (mat(0, 1) + mat(1, 0)) / s;
                q.v[2] = (mat(0, 2) + mat(2, 0)) / s;
            }
            else if (max_diag == 2)
            {
                s = 2.0 * (T) sqrt(1.0 + mat(1, 1) - mat(0, 0) - mat(2, 2));
                q.w = (mat(0, 2) - mat(2, 0)) / s;
                q.v[0] = (mat(0, 1) + mat(1, 0)) / s;
                q.v[1] = 0.25 * s;
                q.v[2] = (mat(1, 2) + mat(2, 1)) / s;
            }
            else
            {
                s = 2.0 * (T) sqrt(1.0 + mat(2, 2) - mat(0, 0) - mat(1, 1));
                q.w = (mat(1, 0) - mat(0, 1)) / s;
                q.v[0] = (mat(0, 2) + mat(2, 0)) / s;
                q.v[1] = (mat(1, 2) + mat(2, 1)) / s;
                q.v[2] = 0.25 * s;
            }
        }

        return q;
    }

    inline Vec4d rotation_axis () {
        Vec4d rot_axis;

        T cos_theta_over_two = w;
        rot_axis[4] = (T) std::acos( cos_theta_over_two ) * 2.0f;

        T sin_theta_over_two = (T) sqrt( 1.0 - cos_theta_over_two * cos_theta_over_two );
        if ( fabs( sin_theta_over_two ) < 0.0005 ) sin_theta_over_two = 1;
        rot_axis[0] = v[0] / sin_theta_over_two;
        rot_axis[1] = v[1] / sin_theta_over_two;
        rot_axis[2] = v[2] / sin_theta_over_two;

        return rot_axis;
    }

    /*
        psi=atan2(2.*(Q(:,1).*Q(:,4)-Q(:,2).*Q(:,3)),(Q(:,4).^2-Q(:,1).^2-Q(:,2).^2+Q(:,3).^2));
        theta=asin(2.*(Q(:,1).*Q(:,3)+Q(:,2).*Q(:,4)));
        phi=atan2(2.*(Q(:,3).*Q(:,4)-Q(:,1).*Q(:,2)),(Q(:,4).^2+Q(:,1).^2-Q(:,2).^2-Q(:,3).^2));
    */
    inline Vec3d euler_angles () {
        Vec3d euler;

        // atan2(2*(qx*qw-qy*qz) , qw2-qx2-qy2+qz2)
        euler[0] = atan2(2 * (v[0] * w - v[1] * v[2]),
                         w * w - v[0] * v[0] - v[1] * v[1] + v[2] * v[2]);

        // asin(2*(qx*qz + qy*qw)
        euler[1] = asin(2 * (v[0] * v[2] + v[1] * w));

        // atan2(2*(qz*qw- qx*qy) , qw2 + qx2 - qy2 - qz2)
        euler[2] = atan2(2 * (v[2] * w - v[0] * v[1]),
                         w * w + v[0] * v[0] - v[1] * v[1] - v[2] * v[2]);

        return euler;
    }

    inline Mat3d rotation_matrix () {
        Mat3d mat;

        T xx = v[0] * v[0];
        T xy = v[0] * v[1];
        T xz = v[0] * v[2];
        T xw = v[0] * w;

        T yy = v[1] * v[1];
        T yz = v[1] * v[2];
        T yw = v[1] * w;

        T zz = v[2] * v[2];
        T zw = v[2] * w;

        mat(0, 0) = 1 - 2 * (yy + zz);
        mat(0, 1) = 2 * (xy - zw);
        mat(0, 2) = 2 * (xz + yw);
        mat(1, 0) = 2 * (xy + zw);
        mat(1, 1) = 1 - 2 * (xx + zz);
        mat(1, 2) = 2 * (yz - xw);
        mat(2, 0) = 2 * (xz - yw);
        mat(2, 1) = 2 * (yz + xw);
        mat(2, 2) = 1 - 2 * (xx + yy);

        return mat;
    }
};


typedef Quaternion<double> Quaternd;

}

#endif //XCAM_VECTOR_MATRIX_H