// Copyright (c) 2021-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 <OgnSetMatrix4QuaternionDatabase.h>
#include <omni/math/linalg/matrix.h>
#include <omni/math/linalg/quat.h>

using omni::math::linalg::matrix4d;
using omni::math::linalg::quatd;

namespace omni
{
namespace graph
{
namespace nodes
{

namespace
{
void updateMatrix(const double input[16], double result[16], const double quaternion[4])
{
    matrix4d& resultMat = *reinterpret_cast<matrix4d*>(result);
    memcpy(resultMat.data(), input, sizeof(double) * 16);
    resultMat.SetRotateOnly(quatd(quaternion[3], quaternion[0], quaternion[1], quaternion[2]));
}
}

class OgnSetMatrix4Quaternion
{
public:
    static bool compute(OgnSetMatrix4QuaternionDatabase& db)
    {
        const auto& matrixInput = db.inputs.matrix();
        const auto& quaternionInput = db.inputs.quaternion();
        auto matrixOutput = db.outputs.matrix();
        bool failed = true;

        // Singular case        
        if (auto matrix = matrixInput.get<double[16]>())
        {
            if (auto quaternion = quaternionInput.get<double[4]>())
            {                    
                if (auto output = matrixOutput.get<double[16]>())
                {
                    updateMatrix(*matrix, *output, *quaternion);
                    failed = false;
                }
            }
        }
        // Array case
        else if (auto matrices = matrixInput.get<double[][16]>())
        {
            if (auto quaternions = quaternionInput.get<double[][4]>())
            {
                if (auto output = matrixOutput.get<double[][16]>())
                {
                    output->resize(matrices->size());
                    for (size_t i = 0; i < matrices.size(); i++)
                    {
                       updateMatrix((*matrices)[i], (*output)[i], (*quaternions)[i]);
                    }
                    failed = false;
                }
            }
        }
        else
        {
            db.logError("Input type for matrix input not supported");
            return false;
        }
        
        if (failed)
        {
            db.logError("Input and output depths need to align: Matrix input with depth %s, quaternion input with depth %s, and output with depth %s", 
                    matrixInput.type().arrayDepth, quaternionInput.type().arrayDepth, matrixOutput.type().arrayDepth);
            return false;
        }

        return true;
    }
    
    static void onConnectionTypeResolve(const NodeObj& node){
        // Resolve fully-coupled types for the 2 attributes
        std::array<AttributeObj, 2> attrs {
            node.iNode->getAttribute(node, OgnSetMatrix4QuaternionAttributes::inputs::matrix.m_name),
            node.iNode->getAttribute(node, OgnSetMatrix4QuaternionAttributes::outputs::matrix.m_name)
        };
        node.iNode->resolveCoupledAttributes(node, attrs.data(), attrs.size());
    }
};

REGISTER_OGN_NODE()

}
}
}
