/*
* Copyright (c) 2022-2023 NVIDIA CORPORATION. All rights reserved
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/ 

#pragma once

#include "sl.h"
#include "sl_consts.h"

namespace sl
{

inline void matrixMul(float4x4& result, const float4x4& a, const float4x4& b)
{
    // Alias raw pointers over the input matrices
    const float* pA = &a[0].x;
    const float* pB = &b[0].x;

    result[0].x = (float)((pA[0] * pB[0]) + (pA[1] * pB[4]) + (pA[2] * pB[8]) + (pA[3] * pB[12]));
    result[0].y = (float)((pA[0] * pB[1]) + (pA[1] * pB[5]) + (pA[2] * pB[9]) + (pA[3] * pB[13]));
    result[0].z = (float)((pA[0] * pB[2]) + (pA[1] * pB[6]) + (pA[2] * pB[10]) + (pA[3] * pB[14]));
    result[0].w = (float)((pA[0] * pB[3]) + (pA[1] * pB[7]) + (pA[2] * pB[11]) + (pA[3] * pB[15]));

    result[1].x = (float)((pA[4] * pB[0]) + (pA[5] * pB[4]) + (pA[6] * pB[8]) + (pA[7] * pB[12]));
    result[1].y = (float)((pA[4] * pB[1]) + (pA[5] * pB[5]) + (pA[6] * pB[9]) + (pA[7] * pB[13]));
    result[1].z = (float)((pA[4] * pB[2]) + (pA[5] * pB[6]) + (pA[6] * pB[10]) + (pA[7] * pB[14]));
    result[1].w = (float)((pA[4] * pB[3]) + (pA[5] * pB[7]) + (pA[6] * pB[11]) + (pA[7] * pB[15]));

    result[2].x = (float)((pA[8] * pB[0]) + (pA[9] * pB[4]) + (pA[10] * pB[8]) + (pA[11] * pB[12]));
    result[2].y = (float)((pA[8] * pB[1]) + (pA[9] * pB[5]) + (pA[10] * pB[9]) + (pA[11] * pB[13]));
    result[2].z = (float)((pA[8] * pB[2]) + (pA[9] * pB[6]) + (pA[10] * pB[10]) + (pA[11] * pB[14]));
    result[2].w = (float)((pA[8] * pB[3]) + (pA[9] * pB[7]) + (pA[10] * pB[11]) + (pA[11] * pB[15]));

    result[3].x = (float)((pA[12] * pB[0]) + (pA[13] * pB[4]) + (pA[14] * pB[8]) + (pA[15] * pB[12]));
    result[3].y = (float)((pA[12] * pB[1]) + (pA[13] * pB[5]) + (pA[14] * pB[9]) + (pA[15] * pB[13]));
    result[3].z = (float)((pA[12] * pB[2]) + (pA[13] * pB[6]) + (pA[14] * pB[10]) + (pA[15] * pB[14]));
    result[3].w = (float)((pA[12] * pB[3]) + (pA[13] * pB[7]) + (pA[14] * pB[11]) + (pA[15] * pB[15]));
}

inline void matrixFullInvert(float4x4& result, const float4x4& mat)
{
    // Matrix inversion code from https://stackoverflow.com/questions/1148309/inverting-a-4x4-matrix
    // Alias raw pointers over the input matrix and the result
    const float* pMat = &mat[0].x;
    float* pResult = &result[0].x;

    pResult[0] = pMat[5] * pMat[10] * pMat[15] - pMat[5] * pMat[11] * pMat[14] - pMat[9] * pMat[6] * pMat[15] + pMat[9] * pMat[7] * pMat[14] + pMat[13] * pMat[6] * pMat[11] - pMat[13] * pMat[7] * pMat[10];
    pResult[4] = -pMat[4] * pMat[10] * pMat[15] + pMat[4] * pMat[11] * pMat[14] + pMat[8] * pMat[6] * pMat[15] - pMat[8] * pMat[7] * pMat[14] - pMat[12] * pMat[6] * pMat[11] + pMat[12] * pMat[7] * pMat[10];
    pResult[8] = pMat[4] * pMat[9] * pMat[15] - pMat[4] * pMat[11] * pMat[13] - pMat[8] * pMat[5] * pMat[15] + pMat[8] * pMat[7] * pMat[13] + pMat[12] * pMat[5] * pMat[11] - pMat[12] * pMat[7] * pMat[9];
    pResult[12] = -pMat[4] * pMat[9] * pMat[14] + pMat[4] * pMat[10] * pMat[13] + pMat[8] * pMat[5] * pMat[14] - pMat[8] * pMat[6] * pMat[13] - pMat[12] * pMat[5] * pMat[10] + pMat[12] * pMat[6] * pMat[9];
    pResult[1] = -pMat[1] * pMat[10] * pMat[15] + pMat[1] * pMat[11] * pMat[14] + pMat[9] * pMat[2] * pMat[15] - pMat[9] * pMat[3] * pMat[14] - pMat[13] * pMat[2] * pMat[11] + pMat[13] * pMat[3] * pMat[10];
    pResult[5] = pMat[0] * pMat[10] * pMat[15] - pMat[0] * pMat[11] * pMat[14] - pMat[8] * pMat[2] * pMat[15] + pMat[8] * pMat[3] * pMat[14] + pMat[12] * pMat[2] * pMat[11] - pMat[12] * pMat[3] * pMat[10];
    pResult[9] = -pMat[0] * pMat[9] * pMat[15] + pMat[0] * pMat[11] * pMat[13] + pMat[8] * pMat[1] * pMat[15] - pMat[8] * pMat[3] * pMat[13] - pMat[12] * pMat[1] * pMat[11] + pMat[12] * pMat[3] * pMat[9];
    pResult[13] = pMat[0] * pMat[9] * pMat[14] - pMat[0] * pMat[10] * pMat[13] - pMat[8] * pMat[1] * pMat[14] + pMat[8] * pMat[2] * pMat[13] + pMat[12] * pMat[1] * pMat[10] - pMat[12] * pMat[2] * pMat[9];
    pResult[2] = pMat[1] * pMat[6] * pMat[15] - pMat[1] * pMat[7] * pMat[14] - pMat[5] * pMat[2] * pMat[15] + pMat[5] * pMat[3] * pMat[14] + pMat[13] * pMat[2] * pMat[7] - pMat[13] * pMat[3] * pMat[6];
    pResult[6] = -pMat[0] * pMat[6] * pMat[15] + pMat[0] * pMat[7] * pMat[14] + pMat[4] * pMat[2] * pMat[15] - pMat[4] * pMat[3] * pMat[14] - pMat[12] * pMat[2] * pMat[7] + pMat[12] * pMat[3] * pMat[6];
    pResult[10] = pMat[0] * pMat[5] * pMat[15] - pMat[0] * pMat[7] * pMat[13] - pMat[4] * pMat[1] * pMat[15] + pMat[4] * pMat[3] * pMat[13] + pMat[12] * pMat[1] * pMat[7] - pMat[12] * pMat[3] * pMat[5];
    pResult[14] = -pMat[0] * pMat[5] * pMat[14] + pMat[0] * pMat[6] * pMat[13] + pMat[4] * pMat[1] * pMat[14] - pMat[4] * pMat[2] * pMat[13] - pMat[12] * pMat[1] * pMat[6] + pMat[12] * pMat[2] * pMat[5];
    pResult[3] = -pMat[1] * pMat[6] * pMat[11] + pMat[1] * pMat[7] * pMat[10] + pMat[5] * pMat[2] * pMat[11] - pMat[5] * pMat[3] * pMat[10] - pMat[9] * pMat[2] * pMat[7] + pMat[9] * pMat[3] * pMat[6];
    pResult[7] = pMat[0] * pMat[6] * pMat[11] - pMat[0] * pMat[7] * pMat[10] - pMat[4] * pMat[2] * pMat[11] + pMat[4] * pMat[3] * pMat[10] + pMat[8] * pMat[2] * pMat[7] - pMat[8] * pMat[3] * pMat[6];
    pResult[11] = -pMat[0] * pMat[5] * pMat[11] + pMat[0] * pMat[7] * pMat[9] + pMat[4] * pMat[1] * pMat[11] - pMat[4] * pMat[3] * pMat[9] - pMat[8] * pMat[1] * pMat[7] + pMat[8] * pMat[3] * pMat[5];
    pResult[15] = pMat[0] * pMat[5] * pMat[10] - pMat[0] * pMat[6] * pMat[9] - pMat[4] * pMat[1] * pMat[10] + pMat[4] * pMat[2] * pMat[9] + pMat[8] * pMat[1] * pMat[6] - pMat[8] * pMat[2] * pMat[5];

    float det = pMat[0] * pResult[0] + pMat[1] * pResult[4] + pMat[2] * pResult[8] + pMat[3] * pResult[12];
    if (det != 0.f)
    {
        det = 1.0f / det;

        for (int i = 0; i < 16; ++i)
        {
            pResult[i] *= det;
        }
    }
}

// Specialised lightweight matrix invert when the matrix is known to be orthonormal
inline void matrixOrthoNormalInvert(float4x4& result, const float4x4& mat)
{
    // Transpose the first 3x3
    result[0].x = mat[0].x;
    result[0].y = mat[1].x;
    result[0].z = mat[2].x;
    result[1].x = mat[0].y;
    result[1].y = mat[1].y;
    result[1].z = mat[2].y;
    result[2].x = mat[0].z;
    result[2].y = mat[1].z;
    result[2].z = mat[2].z;

    // Invert the translation
    result[3].x = -((mat[3].x * mat[0].x) + (mat[3].y * mat[0].y) + (mat[3].z * mat[0].z));
    result[3].y = -((mat[3].x * mat[1].x) + (mat[3].y * mat[1].y) + (mat[3].z * mat[1].z));
    result[3].z = -((mat[3].x * mat[2].x) + (mat[3].y * mat[2].y) + (mat[3].z * mat[2].z));

    // Fill in the remaining constants
    result[0].w = 0.0f;
    result[1].w = 0.0f;
    result[2].w = 0.0f;
    result[3].w = 1.0f;
}

inline void vectorNormalize(float3& v)
{
    float k = 1.f / sqrtf((v.x * v.x) + (v.y * v.y) + (v.z * v.z));
    v.x *= k;
    v.y *= k;
    v.z *= k;
}

inline void vectorCrossProduct(float3& result, const float3& a, const float3& b)
{
    result.x = a.y * b.z - a.z * b.y;
    result.y = a.z * b.x - a.x * b.z;
    result.z = a.x * b.y - a.y * b.x;
}

// Calculate a cameraToPrevCamera matrix from cameraToWorld and cameraToWorldPrev matrices
// but do so in such a way as to avoid precision issues.
//
// Traditionally, you might go something like this...
//
//    worldToCameraPrev = invert(cameraToWorldPrev)
//    cameraToPrevCamera = cameraToWorld * worldToCameraPrev
//
// But if you do that, you will subject yourself to fp32 precision issues if the camera is
// any kind of reasonable distance from the origin, because you'll end up adding small
// numbers to large numbers due to the large translations.
//
// But the camera's absolute position in the world doesn't matter at all to the result.
// What we're interested in is the camera's motion.
// So if we add the same thing to the translations of cameraToWorld and cameraToWorldPrev
// then we should get the same result.
// If we choose to subtract the current camera's translation in world space, then we will
// change a potentially very large translation value into a very small one - thereby
// sidestepping the precision issues.
inline void calcCameraToPrevCamera(float4x4& outCameraToPrevCamera, const float4x4& cameraToWorld, const float4x4& cameraToWorldPrev)
{
    // Create translated versions of cameraToWorld and cameraToWorldPrev, translated to
    // so that the current camera is effectively at the world origin.
    // CC == 'Camera-Centred'
    float4x4 cameraToCcWorld = cameraToWorld;
    cameraToCcWorld[3] = float4(0, 0, 0, 1);
    float4x4 cameraToCcWorldPrev = cameraToWorldPrev;
    cameraToCcWorldPrev[3].x -= cameraToWorld[3].x;
    cameraToCcWorldPrev[3].y -= cameraToWorld[3].y;
    cameraToCcWorldPrev[3].z -= cameraToWorld[3].z;

    // We can use an optimised invert if we assume that the camera matrix is orthonormal
    float4x4 ccWorldToCameraPrev;
    matrixOrthoNormalInvert(ccWorldToCameraPrev, cameraToCcWorldPrev);
    matrixMul(outCameraToPrevCamera, cameraToCcWorld, ccWorldToCameraPrev);
}

// Calculate some of the matrix fields in Constants
// This can be used to validate what the app is providing, or tease out precision issues
// The matrices that are recalculated are...
// - clipToCameraView
// - clipToPrevClip
// - prevClipToClip
inline void recalculateCameraMatrices(Constants& values)
{
    // Form a camera-to-world matrix from the camera fields
    vectorNormalize(values.cameraRight);
    vectorNormalize(values.cameraFwd);
    vectorCrossProduct(values.cameraUp, values.cameraFwd, values.cameraRight);
    vectorNormalize(values.cameraUp);
    float4x4 cameraViewToWorld = {
        float4(values.cameraRight.x, values.cameraRight.y, values.cameraRight.z, 0.f),
        float4(values.cameraUp.x,    values.cameraUp.y,    values.cameraUp.z,    0.f),
        float4(values.cameraFwd.x,   values.cameraFwd.y,   values.cameraFwd.z,   0.f),
        float4(values.cameraPos.x,   values.cameraPos.y,   values.cameraPos.z, 1.f)
    };
    // ********* DO NOT USE THIS IN ANYTHING PROPER *********
    // Crap storage of cameraViewToWorldPrev and cameraViewToClipPrev
    // These should be provided by the app, or stored by association with the view index.
    static float4x4 cameraViewToWorldPrev = {
        float4(1, 0, 0, 0),
        float4(0, 1, 0, 0),
        float4(0, 0, 1, 0),
        float4(0, 0, 0, 1),
    };
    static float4x4 cameraViewToClipPrev = {
        float4(1, 0, 0, 0),
        float4(0, 1, 0, 0),
        float4(0, 0, 1, 0),
        float4(0, 0, 0, 1),
    };
    matrixFullInvert(values.clipToCameraView, values.cameraViewToClip);

    float4x4 cameraViewToPrevCameraView;
    calcCameraToPrevCamera(cameraViewToPrevCameraView, cameraViewToWorld, cameraViewToWorldPrev);

    float4x4 clipToPrevCameraView;
    matrixMul(clipToPrevCameraView, values.clipToCameraView, cameraViewToPrevCameraView);
    matrixMul(values.clipToPrevClip, clipToPrevCameraView, cameraViewToClipPrev);
    matrixFullInvert(values.prevClipToClip, values.clipToPrevClip);

    // ********* DO NOT USE THIS IN ANYTHING PROPER *********
    cameraViewToWorldPrev = cameraViewToWorld;
    cameraViewToClipPrev = values.cameraViewToClip;
}

}