/**
 * Copyright 2019,2020 Sony Corporation
 */

#ifndef XR_MATH_H_
#define XR_MATH_H_

#include <math.h>

struct SrdXrQuaternionf {
    float x;

    float y;

    float z;

    float w;

    SrdXrQuaternionf()
        : x(0.f), y(0.f), z(0.f), w(0.f) {}

    SrdXrQuaternionf(float in_x, float in_y, float in_z, float in_w)
        : x(in_x), y(in_y), z(in_z), w(in_w) {}
};

struct SrdXrVector3f {
    float x;

    float y;

    float z;

    SrdXrVector3f()
        : x(0.f), y(0.f), z(0.f) {}

    SrdXrVector3f(float in_x, float in_y, float in_z)
        : x(in_x), y(in_y), z(in_z) {}

    SrdXrVector3f operator +(SrdXrVector3f a) {
        return SrdXrVector3f(x + a.x, y + a.y, z + a.z);
    }
    SrdXrVector3f operator -(SrdXrVector3f a) {
        return SrdXrVector3f(x - a.x, y - a.y, z - a.z);
    }

    SrdXrVector3f operator *(float a) {
        return SrdXrVector3f(x * a, y * a, z * a);
    }
    SrdXrVector3f operator /(float a) {
        return SrdXrVector3f(x / a, y / a, z / a);
    }

    float Dot(SrdXrVector3f a) {
        return x * a.x + y * a.y + z * a.z;
    }

    void Normalize() {
        float length = sqrtf(x * x + y * y + z * z);
        x /= length;
        y /= length;
        z /= length;
    }
};

struct SrdXrVector4f {
    float x;

    float y;

    float z;

    float w;

    SrdXrVector4f()
        : x(0.f), y(0.f), z(0.f), w(0.f) {}

    SrdXrVector4f(float in_x, float in_y, float in_z, float in_w)
        : x(in_x), y(in_y), z(in_z), w(in_w) {}

    float operator *(SrdXrVector4f a) {
        return x * a.x + y * a.y + z * a.z + w * a.w;
    }
};

struct SrdXrMatrix4x4f {
    float matrix[4][4];

    SrdXrMatrix4x4f() {
        matrix[0][0] = 0.f; matrix[0][1] = 0.f; matrix[0][2] = 0.f; matrix[0][3] = 0.f;
        matrix[1][0] = 0.f; matrix[1][1] = 0.f; matrix[1][2] = 0.f; matrix[1][3] = 0.f;
        matrix[2][0] = 0.f; matrix[2][1] = 0.f; matrix[2][2] = 0.f; matrix[2][3] = 0.f;
        matrix[3][0] = 0.f; matrix[3][1] = 0.f; matrix[3][2] = 0.f; matrix[3][3] = 0.f;
    }

    SrdXrMatrix4x4f(SrdXrVector4f in_x, SrdXrVector4f in_y, SrdXrVector4f in_z, SrdXrVector4f in_w) {
        matrix[0][0] = in_x.x; matrix[0][1] = in_x.y; matrix[0][2] = in_x.z; matrix[0][3] = in_x.w;
        matrix[1][0] = in_y.x; matrix[1][1] = in_y.y; matrix[1][2] = in_y.z; matrix[1][3] = in_y.w;
        matrix[2][0] = in_z.x; matrix[2][1] = in_z.y; matrix[2][2] = in_z.z; matrix[2][3] = in_z.w;
        matrix[3][0] = in_w.x; matrix[3][1] = in_w.y; matrix[3][2] = in_w.z; matrix[3][3] = in_w.w;
    }

    SrdXrMatrix4x4f operator *(SrdXrMatrix4x4f a) {
        SrdXrVector4f m_0 = SrdXrVector4f(matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3]);
        SrdXrVector4f m_1 = SrdXrVector4f(matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][3]);
        SrdXrVector4f m_2 = SrdXrVector4f(matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]);
        SrdXrVector4f m_3 = SrdXrVector4f(matrix[3][0], matrix[3][1], matrix[3][2], matrix[3][3]);

        SrdXrVector4f a_0 = SrdXrVector4f(a.matrix[0][0], a.matrix[1][0], a.matrix[2][0], a.matrix[3][0]);
        SrdXrVector4f a_1 = SrdXrVector4f(a.matrix[0][1], a.matrix[1][1], a.matrix[2][1], a.matrix[3][1]);
        SrdXrVector4f a_2 = SrdXrVector4f(a.matrix[0][2], a.matrix[1][2], a.matrix[2][2], a.matrix[3][2]);
        SrdXrVector4f a_3 = SrdXrVector4f(a.matrix[0][3], a.matrix[1][3], a.matrix[2][3], a.matrix[3][3]);

        float m_00 = m_0 * a_0, m_01 = m_0 * a_1, m_02 = m_0 * a_2, m_03 = m_0 * a_3;
        float m_10 = m_1 * a_0, m_11 = m_1 * a_1, m_12 = m_1 * a_2, m_13 = m_1 * a_3;
        float m_20 = m_2 * a_0, m_21 = m_2 * a_1, m_22 = m_2 * a_2, m_23 = m_2 * a_3;
        float m_30 = m_3 * a_0, m_31 = m_3 * a_1, m_32 = m_3 * a_2, m_33 = m_3 * a_3;

        return SrdXrMatrix4x4f(SrdXrVector4f(m_00, m_01, m_02, m_03),
                               SrdXrVector4f(m_10, m_11, m_12, m_13),
                               SrdXrVector4f(m_20, m_21, m_22, m_23),
                               SrdXrVector4f(m_30, m_31, m_32, m_33));
    }
};

#endif // XR_MATH_H_
