// Copyright (c) 2021, NVIDIA CORPORATION. All rights reserved.
//
// NVIDIA CORPORATION and its licensors retain all intellectual property
// and proprietary rights in and to this software, related documentation
// and any modifications thereto.  Any use, reproduction, disclosure or
// distribution of this software and related documentation without an express
// license agreement from NVIDIA CORPORATION is strictly prohibited.
//

#include <OgnInvertMatrixDatabase.h>
#include <omni/math/linalg/matrix.h>

#include <exception>

using omni::math::linalg::matrix3d;
using omni::math::linalg::matrix4d;

namespace omni
{
namespace graph
{
namespace nodes
{

void computeMatrix4Inverse(const double input[16], double result[16])
{
    auto& output = *reinterpret_cast<matrix4d*>(result);
    output = reinterpret_cast<const matrix4d*>(input)->GetInverse();
}
void computeMatrix3Inverse(const double input[9], double result[9])
{
    auto& output = *reinterpret_cast<matrix3d*>(result);
    output = reinterpret_cast<const matrix3d*>(input)->GetInverse();
}

class OgnInvertMatrix
{
public:
    static bool compute(OgnInvertMatrixDatabase& db)
    {
        auto& matrixInput = db.inputs.matrix();
        auto& matrixOutput = db.outputs.invertedMatrix();

        if (auto matrix = matrixInput.get<double[9]>())
        {
            if (auto output = matrixOutput.get<double[9]>())
            {
                computeMatrix3Inverse(*matrix, *output);
            }
        }
        else if (auto matrix = matrixInput.get<double[16]>())
        {
            if (auto output = matrixOutput.get<double[16]>())
            {
                computeMatrix4Inverse(*matrix, *output);
            }
        }
        else if (auto matrices = matrixInput.get<double[][9]>())
        {
            if (auto output = matrixOutput.get<double[][9]>())
            {
                output->resize(matrices->size());
                for (size_t i = 0; i < matrices.size(); i++)
                {
                    computeMatrix3Inverse((*matrices)[i], (*output)[i]);
                }
            }
        }
        else if (auto matrices = matrixInput.get<double[][16]>())
        {
            if (auto output = matrixOutput.get<double[][16]>())
            {
                output->resize(matrices->size());
                for (size_t i = 0; i < matrices.size(); i++)
                {
                    computeMatrix4Inverse((*matrices)[i], (*output)[i]);
                }
            }
        }
        else
        {
            db.logWarning("OgnInvertMatrix: Failed to resolve input types");
        }

        return true;
    }

    static void onConnectionTypeResolve(const NodeObj& node) {
        std::array<AttributeObj, 2> attrs {
            node.iNode->getAttributeByToken(node, inputs::matrix.token()),
            node.iNode->getAttributeByToken(node, outputs::invertedMatrix.token())
        };
        node.iNode->resolveCoupledAttributes(node, attrs.data(), attrs.size());
    }
};

REGISTER_OGN_NODE()

} // namespace nodes
} // namespace graph
} // namespace omni
